[Pick and Place using Inception_v3 in ROS] #01. Manipulator Code Review

Robotis에서는 Master Manipulator의 동작을 Slave Manipulator에서 따라하도록 하는 master_slave source가 존재한다.

이번에 Robotis의 Manipulator를 이용해서 작업을 할 일이 있어서,
해당 source를 분석하고자 한다.

이번 프로젝트의 목적은 하나의 Manipulator를 가지고 동작을 기억시켜 머신러닝을 이용해 해당 물체에 따라서 Grasping 작업을 진행시키는 것이다.

그러므로 내 경우에는 master_slave일 필요는 없으며, joint data를 가져오고, 가져온 joint data를 이용해 어떻게 Manipulator를 구동시키는지 확인하는 것이 목적이다.

---

PC Enviroment ( Labtop )

OS : Ubuntu 16.04
ROS : Kinetic
CPU : i8
Graphic Card : RTX 2070

Hardware
OpenManipulator(ROBOTIS)
U2D2 Interface

Software
Inception_v3

---

https://github.com/ROBOTIS-GIT/open_manipulator_applications/blob/master/open_manipulator_master_slave/src/open_manipulator_master.cpp

먼저 시작하기 전에, 해당 소스는 위 링크에서 발췌했음을 알린다.

open_manipulator_master.cpp

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402

/******************************************************************************* * Copyright 2018 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * *     http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *******************************************************************************/ /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ #include "open_manipulator_master_slave/open_manipulator_master.h" OpenManipulatorMaster::OpenManipulatorMaster(std::string usb_port, std::string baud_rate)     :node_handle_(""),      priv_node_handle_("~"),      service_call_period_(0.01),      mode_state_(MASTER_SLAVE_MODE),      buffer_index_(0) {   service_call_period_  = priv_node_handle_.param<double>("service_call_period", 0.010f);   dxl_id_.push_back(priv_node_handle_.param<int32_t>("joint1_id", 1));   dxl_id_.push_back(priv_node_handle_.param<int32_t>("joint2_id", 2));   dxl_id_.push_back(priv_node_handle_.param<int32_t>("joint3_id", 3));   dxl_id_.push_back(priv_node_handle_.param<int32_t>("joint4_id", 4));   dxl_id_.push_back(priv_node_handle_.param<int32_t>("gripper_id", 5));   goal_joint_position_.resize(NUM_OF_JOINT);   goal_tool_position_ = 0.0;   initOpenManipulator(usb_port, baud_rate, service_call_period_);   initServiceClient();   syncOpenManipulator(false); } OpenManipulatorMaster::~OpenManipulatorMaster() {   delete actuator_;   delete tool_;   if(ros::isStarted()) {     ros::shutdown();     ros::waitForShutdown();   } } void OpenManipulatorMaster::initServiceClient() {   goal_joint_space_path_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>("goal_joint_space_path");   goal_tool_control_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>("goal_tool_control"); } void OpenManipulatorMaster::initOpenManipulator(STRING usb_port, STRING baud_rate, double service_call_period) {   /*****************************************************************************   ** Initialize Manipulator Parameter   *****************************************************************************/   addWorld("world",   // world name            "joint1"); // child name   addJoint("joint1",  // my name            "world",   // parent name            "joint2",  // child name            math::vector3(0.012, 0.0, 0.017),                // relative position            math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation            Z_AXIS,        // axis of rotation            dxl_id_.at(0), // actuator id            M_PI,          // max joint limit (3.14 rad)            -M_PI);        // min joint limit (-3.14 rad)   addJoint("joint2",  // my name            "joint1",  // parent name            "joint3",  // child name            math::vector3(0.0, 0.0, 0.0595),                 // relative position            math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation            Y_AXIS,        // axis of rotation            dxl_id_.at(1), // actuator id            M_PI_2,        // max joint limit (1.67 rad)            -2.05);        // min joint limit (-2.05 rad)   addJoint("joint3",  // my name            "joint2",  // parent name            "joint4",  // child name            math::vector3(0.024, 0.0, 0.128),                // relative position            math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation            Y_AXIS,        // axis of rotation            dxl_id_.at(2), // actuator id            1.53,          // max joint limit (1.53 rad)            -M_PI_2);      // min joint limit (-1.67 rad)   addJoint("joint4",  // my name            "joint3",  // parent name            "gripper", // child name            math::vector3(0.124, 0.0, 0.0),                  // relative position            math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation            Y_AXIS,        // axis of rotation            dxl_id_.at(3), // actuator id            2.0,           // max joint limit (2.0 rad)            -1.8);         // min joint limit (-1.8 rad)   addTool("gripper",  // my name           "joint4",   // parent name           math::vector3(0.126, 0.0, 0.0),                  // relative position           math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation           dxl_id_.at(4),  // actuator id           0.010,          // max gripper limit (0.01 m)           -0.010,         // min gripper limit (-0.01 m)           -0.015);        // Change unit from `meter` to `radian`   /*****************************************************************************   ** Initialize Joint Actuator   *****************************************************************************/   actuator_ = new dynamixel::JointDynamixelProfileControl(service_call_period);   // Set communication arguments   STRING dxl_comm_arg[2] = {usb_port, baud_rate};   void *p_dxl_comm_arg = &dxl_comm_arg;   // Set joint actuator id   std::vector<uint8_t> jointDxlId;   jointDxlId.push_back(dxl_id_.at(0));   jointDxlId.push_back(dxl_id_.at(1));   jointDxlId.push_back(dxl_id_.at(2));   jointDxlId.push_back(dxl_id_.at(3));   addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg);   // Set joint actuator control mode   STRING joint_dxl_mode_arg = "position_mode";   void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg;   setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg);   /*****************************************************************************   ** Initialize Tool Actuator   *****************************************************************************/   tool_ = new dynamixel::GripperDynamixel();   uint8_t gripperDxlId = dxl_id_.at(4);   addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg);   // Set gripper actuator control mode   STRING gripper_dxl_mode_arg = "current_based_position_mode";   void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg;   setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg);   // Disable All Actuators   disableAllActuator(); } void OpenManipulatorMaster::syncOpenManipulator(bool recorded_state) {   log::println("Synchronizing Open Manipulators.", "GREEN");   double sync_path_time = 1.0;   receiveAllJointActuatorValue();   receiveAllToolActuatorValue();   if(recorded_state) // move to first pose of recorded buffer   {     setJointSpacePath(sync_path_time, record_buffer_.at(buffer_index_).joint_angle);     setToolPath(record_buffer_.at(buffer_index_).tool_position);   }   else // move to present master pose   {     setJointSpacePath(sync_path_time);     setToolPath();   }   ros::WallDuration sleep_time(sync_path_time);   sleep_time.sleep();   return; } bool OpenManipulatorMaster::setJointSpacePath(double path_time, std::vector<double> set_goal_joint_position) {   auto joint_name = getManipulator()->getAllActiveJointComponentName();   std::vector<double> joint_value;   if(set_goal_joint_position.size())  joint_value = set_goal_joint_position;   else   {     joint_value.push_back(getJointValue(joint_name.at(0)).position);     joint_value.push_back(getJointValue(joint_name.at(1)).position);     joint_value.push_back(getJointValue(joint_name.at(2)).position);     joint_value.push_back(getJointValue(joint_name.at(3)).position);   }   open_manipulator_msgs::SetJointPosition srv;   srv.request.joint_position.joint_name = joint_name;   for(int i = 0; i < NUM_OF_JOINT; i ++)   {     if(getManipulator()->checkJointLimit(joint_name.at(i), joint_value.at(i)))       srv.request.joint_position.position.push_back(joint_value.at(i));     else       srv.request.joint_position.position.push_back(goal_joint_position_.at(i));   }   goal_joint_position_ = srv.request.joint_position.position;   srv.request.path_time = path_time;   if(goal_joint_space_path_client_.call(srv))   {     return srv.response.is_planned;   }   return false; } bool OpenManipulatorMaster::setToolPath(double set_goal_tool_position) {   double tool_value;   if(set_goal_tool_position < -0.1)     tool_value = getAllToolValue().at(0).position;   else     tool_value = set_goal_tool_position;   open_manipulator_msgs::SetJointPosition srv;   srv.request.joint_position.joint_name.push_back("gripper");   if(getManipulator()->checkJointLimit("gripper", tool_value))     srv.request.joint_position.position.push_back(tool_value);   else     srv.request.joint_position.position.push_back(goal_tool_position_);   goal_tool_position_ = srv.request.joint_position.position.at(0);   if(goal_tool_control_client_.call(srv))   {     return srv.response.is_planned;   }   return false; } void OpenManipulatorMaster::publishCallback(const ros::TimerEvent&) {   static int count = 0;   if(!(count % 5))   {     printText();     if(kbhit())       setModeState(std::getchar());   }   count ++;   receiveAllJointActuatorValue();   receiveAllToolActuatorValue();   if(mode_state_ == MASTER_SLAVE_MODE)   {     setJointSpacePath(service_call_period_);     setToolPath();   }   else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)   {     setJointSpacePath(service_call_period_);     setToolPath();     WaypointBuffer temp;     temp.joint_angle = goal_joint_position_;     temp.tool_position = goal_tool_position_;     record_buffer_.push_back(temp);   }   else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)   {}   else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)   {     if(record_buffer_.size() > buffer_index_)     {       setJointSpacePath(service_call_period_, record_buffer_.at(buffer_index_).joint_angle);       setToolPath(record_buffer_.at(buffer_index_).tool_position);       buffer_index_ ++;     }   } } void OpenManipulatorMaster::setModeState(char ch) {   if(ch == '1')   {     syncOpenManipulator(false);     mode_state_ = MASTER_SLAVE_MODE;   }   else if(ch == '2')   {     syncOpenManipulator(false);     record_buffer_.clear();     mode_state_ = START_RECORDING_TRAJECTORY_MODE;   }   else if(ch == '3')   {     mode_state_ = STOP_RECORDING_TRAJECTORY_MODE;   }   else if(ch == '4')   {     buffer_index_ = 0;     if(record_buffer_.size()) syncOpenManipulator(true);     mode_state_ = PLAY_RECORDED_TRAJECTORY_MODE;   } } void OpenManipulatorMaster::printText() {   system("clear");   printf("\n");   printf("-----------------------------\n");   printf("Control Your OpenManipulator!\n");   printf("-----------------------------\n");   printf("Present Control Mode\n");   if(mode_state_ == MASTER_SLAVE_MODE)   {     printf("Master - Slave Mode\n");   }   else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)   {     printf("Start Recording Trajectory\n");     printf("Buffer Size : %d\n", (int)(record_buffer_.size()));   }   else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)   {     printf("Stop Recording Trajectory\n");     printf("Buffer Size : %d\n", (int)(record_buffer_.size()));   }   else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)   {     printf("Play Recorded Trajectory Mode\n");     printf("Buffer Size : %d\n", (int)(record_buffer_.size()));     printf("Buffer index : %d\n", buffer_index_);   }   printf("-----------------------------\n");   printf("1 : Master - Slave Mode\n");   printf("2 : Start Recording Trajectory\n");   printf("3 : Stop Recording Trajectory\n");   printf("4 : Play Recorded Trajectory\n");   printf("-----------------------------\n");   printf("Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n",          goal_joint_position_.at(0),          goal_joint_position_.at(1),          goal_joint_position_.at(2),          goal_joint_position_.at(3));   printf("Present Tool Position: %.3lf\n", goal_tool_position_);   printf("-----------------------------\n"); } bool OpenManipulatorMaster::kbhit() {   termios term;   tcgetattr(0, &term);   termios term2 = term;   term2.c_lflag &= ~ICANON;   tcsetattr(0, TCSANOW, &term2);   int byteswaiting;   ioctl(0, FIONREAD, &byteswaiting);   tcsetattr(0, TCSANOW, &term);   return byteswaiting > 0; } int main(int argc, char **argv) {   // Init ROS node   ros::init(argc, argv, "open_manipulator_master_slave");   ros::NodeHandle node_handle("");   std::string usb_port = "/dev/ttyUSB0";   std::string baud_rate = "1000000";   if (argc < 3)   {     log::error("Please set '-port_name' and  '-baud_rate' arguments for connected Dynamixels");     return 0;   }   else   {     usb_port = argv[1];     baud_rate = argv[2];   }   OpenManipulatorMaster open_manipulator_master(usb_port, baud_rate);   ros::Timer publish_timer = node_handle.createTimer(ros::Duration(open_manipulator_master.getServiceCallPeriod()), &OpenManipulatorMaster::publishCallback, &open_manipulator_master);   while (ros::ok())   {     ros::spinOnce();   }   return 0; } Colored by Color Scripter

cs

---

int main(int argc, char **argv)

{

  // Init ROS node

  ros::init(argc, argv, "open_manipulator_master_slave");

  ros::NodeHandle node_handle("");

  std::string usb_port = "/dev/ttyUSB0";

  std::string baud_rate = "1000000";

  if (argc < 3)

  {

    log::error("Please set '-port_name' and  '-baud_rate' arguments for connected Dynamixels");

    return 0;

  }

  else

  {

    usb_port = argv[1];

    baud_rate = argv[2];

  }

  OpenManipulatorMaster open_manipulator_master(usb_port, baud_rate);

  ros::Timer publish_timer = node_handle.createTimer(

ros::Duration(open_manipulator_master.getServiceCallPeriod()), 

&OpenManipulatorMaster::publishCallback, 

&open_manipulator_master);

  while (ros::ok())

  {

    ros::spinOnce();

  }

  return 0;

}

  // Init ROS node

  ros::init(argc, argv, "open_manipulator_master_slave");

해당 노드를 "open_manipulator_master_slave"란 이름으로 초기화한다.

ros::NodeHandle node_handle("");

node_handle도 초기화해준다.

  std::string usb_port = "/dev/ttyUSB0";

  std::string baud_rate = "1000000";

usb_port와 baud_rate를 정의해준다.

  if (argc < 3)

  {

    log::error("Please set '-port_name' and  '-baud_rate' arguments 
for connected Dynamixels");

    return 0;

  }

  else

  {

    usb_port = argv[1];

    baud_rate = argv[2];

  }

기본적으로 노드를 실행시킬 때, usb port 와 baudrate를 주어주게 되는데, 이 부분에서 해당 내용을 확인하게 된다.

 OpenManipulatorMaster open_manipulator_master(usb_port, baud_rate);

usb_port와 baud_rate의 값을 받아 OpenManipulatorMaster의 형태를 갖는 open_manipulator_master 객체를 생성한다.

  ros::Timer publish_timer = node_handle.createTimer(
ros::Duration(open_manipulator_master.getServiceCallPeriod()),
&OpenManipulatorMaster::publishCallback,
&open_manipulator_master);

publish_timer를 발동시킨다.

open_manipulator_master.h 에 존재하는 code를 보면 다음과 같다.

double getServiceCallPeriod() {return service_call_period_;}

그리고 생성자 함수를 살펴보면 다음과 같이 정의 되어있다.

  service_call_period_  = priv_node_handle_.param<double>("service_call_period", 

0.010f);

즉, 0.01초 간격으로 publishCallback 루틴이 open_manipulator_master를 파라미터로 하여 작동하는 것이다.

  while (ros::ok())

  {

    ros::spinOnce();

  }

먼저 기본적으로 main 문에서는 while문 안에서 ros::spinOnce(); 를 동작시킨다.
Callback 함수를 처리하는 루틴이다. 여기에선 publishCallback 함수를 의미한다.

---

void OpenManipulatorMaster::publishCallback(const ros::TimerEvent&)

{

  static int count = 0;

  if(!(count % 5))

  {

    printText();

    if(kbhit())

      setModeState(std::getchar());

  }

  count ++;

  receiveAllJointActuatorValue();

  receiveAllToolActuatorValue();

  if(mode_state_ == MASTER_SLAVE_MODE)

  {

    setJointSpacePath(service_call_period_);

    setToolPath();

  }

  else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)

  {

    setJointSpacePath(service_call_period_);

    setToolPath();

    WaypointBuffer temp;

    temp.joint_angle = goal_joint_position_;

    temp.tool_position = goal_tool_position_;

    record_buffer_.push_back(temp);

  }

  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)

  {}

  else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)

  {

    if(record_buffer_.size() > buffer_index_)

    {

      setJointSpacePath(service_call_period_, record_buffer_.at(buffer_index_).joint_angle);

      setToolPath(record_buffer_.at(buffer_index_).tool_position);

      buffer_index_ ++;

    }

  }

}

  static int count = 0;

  if(!(count % 5))

  {

    printText();

    if(kbhit())

      setModeState(std::getchar());

  }

  count ++;

카운트는 0부터 5까지는 나머지가 0이게 되므로 if문 안에 들어가게 된다.
들어가게 되면 먼저 printText() 함수가 작동한다.

void OpenManipulatorMaster::printText()

{

  system("clear");

  printf("\n");

  printf("-----------------------------\n");

  printf("Control Your OpenManipulator!\n");

  printf("-----------------------------\n");

  printf("Present Control Mode\n");

  if(mode_state_ == MASTER_SLAVE_MODE)

  {

    printf("Master - Slave Mode\n");

  }

  else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)

  {

    printf("Start Recording Trajectory\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

  }

  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)

  {

    printf("Stop Recording Trajectory\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

  }

  else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)

  {

    printf("Play Recorded Trajectory Mode\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

    printf("Buffer index : %d\n", buffer_index_);

  }

  printf("-----------------------------\n");

  printf("1 : Master - Slave Mode\n");

  printf("2 : Start Recording Trajectory\n");

  printf("3 : Stop Recording Trajectory\n");

  printf("4 : Play Recorded Trajectory\n");

  printf("-----------------------------\n");

  printf("Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n",

         goal_joint_position_.at(0),

         goal_joint_position_.at(1),

         goal_joint_position_.at(2),

         goal_joint_position_.at(3));

  printf("Present Tool Position: %.3lf\n", goal_tool_position_);

  printf("-----------------------------\n");

}

  if(mode_state_ == MASTER_SLAVE_MODE)

  {

    printf("Master - Slave Mode\n");

  }

MASTER_SLAVER_MODE는 아무작업도 하지 않는다.

  else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)

  {

    printf("Start Recording Trajectory\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

  }

START_RECORDING_TRAJECTORY_MODE는 record_buffer_ 의 크기를 출력한다.

  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)

  {

    printf("Stop Recording Trajectory\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

  }

STOP_RECORDING_TRAJECTORY_MODE 는 멈추는 것이다.

  else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)

  {

    printf("Play Recorded Trajectory Mode\n");

    printf("Buffer Size : %d\n", (int)(record_buffer_.size()));

    printf("Buffer index : %d\n", buffer_index_);

  }

 PLAY_RECORDED_TRAJECTORY_MODE는 record되었던 buffer의 크기와 index를 출력한다.

if(kbhit())

이 부분은 아래함수인데, 보류하자.

bool OpenManipulatorMaster::kbhit()

{

  termios term;

  tcgetattr(0, &term);

  termios term2 = term;

  term2.c_lflag &= ~ICANON;

  tcsetattr(0, TCSANOW, &term2);

  int byteswaiting;

  ioctl(0, FIONREAD, &byteswaiting);

  tcsetattr(0, TCSANOW, &term);

  return byteswaiting > 0;

}

      setModeState(std::getchar());

getchar()는 키보드의 입력을 기다리는 함수,
키보드가 입력되면 setModeState의 파라미터로 들어가게 된다.

  receiveAllJointActuatorValue();

  receiveAllToolActuatorValue();

모든 jointValue 들을 가져온다.

  if(mode_state_ == MASTER_SLAVE_MODE)

  {

    setJointSpacePath(service_call_period_);

    setToolPath();

  }

MASTER_SLAVE_MODE의 경우
setJointSpacePath와
setToolPath가 작동하게 된다.

아무래도 setJointSpacePath 함수 안에서 Slave를 움직이는 것 같다.

  else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE)

  {

    setJointSpacePath(service_call_period_);

    setToolPath();

    WaypointBuffer temp;

    temp.joint_angle = goal_joint_position_;

    temp.tool_position = goal_tool_position_;

    record_buffer_.push_back(temp);

  }

START_RECORDING_TRAJECTORY_MODE의 경우

setJointSpacePath와
setToolPath가 작동하고,

record_buffer_에 정보를 저장하게 된다.

이제 해당 정보를 어떻게 활용할지가 중요하겠다.

  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)

  {}

stop은 stop

  else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE)

  {

    if(record_buffer_.size() > buffer_index_)

    {

      setJointSpacePath(service_call_period_, record_buffer_.at(buffer_index_).joint_angle);

      setToolPath(record_buffer_.at(buffer_index_).tool_position);

      buffer_index_ ++;

    }

  }

이 부분이 저장된 record_buffer data를 이용해 매니퓰레이터를 제어하는 부분이다.

---

bool OpenManipulatorMaster::setJointSpacePath(double path_time,
std::vector<double> set_goal_joint_position)

{

  auto joint_name = getManipulator()->getAllActiveJointComponentName();

  std::vector<double> joint_value;

  if(set_goal_joint_position.size())  joint_value = set_goal_joint_position;

  else

  {

    joint_value.push_back(getJointValue(joint_name.at(0)).position);

    joint_value.push_back(getJointValue(joint_name.at(1)).position);

    joint_value.push_back(getJointValue(joint_name.at(2)).position);

    joint_value.push_back(getJointValue(joint_name.at(3)).position);

  }

  open_manipulator_msgs::SetJointPosition srv;

  srv.request.joint_position.joint_name = joint_name;

  for(int i = 0; i < NUM_OF_JOINT; i ++)

  {

    if(getManipulator()->checkJointLimit(joint_name.at(i), joint_value.at(i)))

      srv.request.joint_position.position.push_back(joint_value.at(i));

    else

      srv.request.joint_position.position.push_back(goal_joint_position_.at(i));

  }

  goal_joint_position_ = srv.request.joint_position.position;

  srv.request.path_time = path_time;

  if(goal_joint_space_path_client_.call(srv))

  {

    return srv.response.is_planned;

  }

  return false;

}

auto joint_name = getManipulator()->getAllActiveJointComponentName();

활성화 되어 있는 manipulator를 가져온다.
여기서 활성화란

roslaunch open_manipulator_controller open_manipulator_controller.launch dynamixel_usb_port:=/dev/ttyUSB0

위 명령어를 통해 활성화되는 slave manipulator를 의미한다.

  std::vector<double> joint_value;

  if(set_goal_joint_position.size())  joint_value = set_goal_joint_position;

set_goal_joint_position이 record_buffer_ 이므로 joint_value에 record_buffer_를 담는 것이다.

  else

  {

    joint_value.push_back(getJointValue(joint_name.at(0)).position);

    joint_value.push_back(getJointValue(joint_name.at(1)).position);

    joint_value.push_back(getJointValue(joint_name.at(2)).position);

    joint_value.push_back(getJointValue(joint_name.at(3)).position);

  }

만약 setJointSpacePath의 parameter에 set_goal_joint_position 이 존재하지 않는다면 joint_value에 현재 slave manipulator의 값을 담는다.

  open_manipulator_msgs::SetJointPosition srv;

  srv.request.joint_position.joint_name = joint_name;

이제 joint value를 이용해서 srv를 보내라는 작업을 할텐데,
이 때 joint_name이 위에서 정해진 slave manipulator의 name 이므로
slave manipulator로 보내게 될 것이다.

  for(int i = 0; i < NUM_OF_JOINT; i ++)

  {

이제 joint 개수만큼 반복문을 돌린다.

    if(getManipulator()->checkJointLimit(joint_name.at(i), joint_value.at(i)))

joint_name.at(i)은 현재 manipulator의 joint 값,
joint_value.at(i)는 목표로 하는 manipulator의 joint 값이다.
현재 manipulator가 목표 위치로 이동시 Limit이 존재하는지 확인하는 것이다.

srv.request.joint_position.position.push_back(joint_value.at(i));

만약 Limit에 걸리지 않는다면 srv에 joint_value의 위치를 담게 된다.

    else

      srv.request.joint_position.position.push_back(goal_joint_position_.at(i));

Limit에 걸린 경우 goal_joint_position 으로 이동하게 되는데,
goal_joint_position은 아래에서 정의된다.

  goal_joint_position_ = srv.request.joint_position.position;

  srv.request.path_time = path_time;

즉 '이 전'의 위치가 goal_joint_position이고,
'이 전'의 시간이 srv.request.path_time이 된다.

  if(goal_joint_space_path_client_.call(srv))

  {

    return srv.response.is_planned;

  }

  return false;

만약 goal_joint_space_path_client_.call(srv) 가 true일 경우, 즉, 작동할 경우.
plan 된 값을 전송하는 것 같다.

서비스 통신이 안되면 그냥 끝낸다.

---

void OpenManipulatorMaster::initServiceClient()

{

  goal_joint_space_path_client_ =
node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>
("goal_joint_space_path");

  goal_tool_control_client_ =
node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>
("goal_tool_control");

}

serviceClient가 여기서 만들어 진다 2개가.
용도는 Manipulator에게 작동을 하도록 하는 것이다.

---

이정도 보았으면, 된 것 같다.

내가 필요한 정보는 아래와 같기 때문이다.

1. joint data 찾기

2. data를 이용해 manipulator 구동하기