// it's for Latex

pages

[Pick and Place using Inception_v3 in ROS] #02. Making open_manipulator_recording_trajectory node

이제 #01 에서 리뷰한 코드를 기반으로 한 개의 Manipulator 만의 Trajectory( joint value ) 를 Recording 할 수 있는 source를 만들 것이다.



PC Enviroment ( Labtop )
OS : Ubuntu 16.04
ROS : Kinetic
CPU : i8
Graphic Card : RTX 2070

Hardware
OpenManipulator(ROBOTIS)
U2D2 Interface

Software
Inception_v3



1. Enviroment Setting

먼저 시작하기 전에 아래 링크에서 Open Manipulator를 구동하기 위한 환경을 세팅해준다.
ROS[Setup] 부분을 모두 진행해야 하며, 5.4에서는 U2D2 환경까지 완료하면 된다.
여기선 OpenCR은 사용하지 않는다.

http://emanual.robotis.com/docs/en/platform/openmanipulator_x/ros_setup/#ros-setup


2. Install Application Package


이제 기본적인 Manipulator 구동은 가능해졌지만, #01에서 본것과 같은 master_slave 같은 Package들은 따로 설치해야 한다.

http://emanual.robotis.com/docs/en/platform/openmanipulator_x/ros_applications/#ros-applications

위 링크를 참고하여 아래 패키지들을 설치하면 된다.


  $ sudo apt-get install ros-kinetic-ar-track-alvar ros-kinetic-ar-track-alvar-msgs ros-kinetic-image-proc

  $ cd ~/catkin_ws/src
  $ git clone https://github.com/ROBOTIS-GIT/open_manipulator_perceptions.git
  $ cd ~/catkin_ws && catkin_make

  $ cd ~/catkin_ws/src
  $ git clone https://github.com/ROBOTIS-GIT/open_manipulator_applications.git
  $ cd ~/catkin_ws && catkin_make


3. Making open_manipulator_recording_trajectory.cpp


이제 node에 사용될 cpp 파일을 만들자.

/home/catkin_ws/src/open_manipulator_applications/open_manipulator_master_slave/src

위의 위치로 이동하면 open_manipulator_master.cpp 파일이 존재한다.

해당 파일을 복사하여 이름만 open_manipulator_recording_trajectory.cpp 로 바꾸어준다.

그리고 아래의 내용으로 소스를 수정해준다.

#01에서 Review한 내용을 바탕으로 slave의 기능은 제외하고 Master의 Trajectory만을 Review할 수 있는 source이다.

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
/*******************************************************************************
* 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();
}
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.0120.00.017),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.00.00.0595),                 // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.0240.00.128),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1240.00.0),                  // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1260.00.0),                  // relative position
          math::convertRPYToRotationMatrix(0.00.00.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();
}
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_ == 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);
    //cout << "joint 0 : "<<temp.joint_angle[0] << endl;
    //cout << "joint 1 : "<<temp.joint_angle[1] << endl;
    //cout << "joint 2 : "<<temp.joint_angle[2] << endl;
    //cout << "joint 3 : "<<temp.joint_angle[3] << endl;
    //cout << "tool position : "<< temp.tool_position << endl;
  }
  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)
  {}
}
void OpenManipulatorMaster::setModeState(char ch)
{
  if(ch == '1')
  {
    record_buffer_.clear();
    mode_state_ = START_RECORDING_TRAJECTORY_MODE;
  }
  else if(ch == '2')
  {
    mode_state_ = STOP_RECORDING_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_ == 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()));
  }
  printf("-----------------------------\n");
  printf("1 : Start Recording Trajectory\n");
  printf("2 : Stop Recording 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;
}
cs


4. Save Trajectory Data to PC


이제 저장된 Trajectory Data를 PC에 저장할 것이다.
data를 txt 형태로 만드는 것이 목적이다.

https://www.youtube.com/watch?v=yzahMaUfL4A

위 링크에서 참조한 방법을 이용해 소스 몇가지를 추가한 소스는 다음과 같다.


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
/*******************************************************************************
* 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"
#include <iostream>
#include <fstream>
#include <string.h>
using namespace std;
// for Writing File  
ofstream file;
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();
}
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.0120.00.017),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.00.00.0595),                 // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.0240.00.128),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1240.00.0),                  // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1260.00.0),                  // relative position
          math::convertRPYToRotationMatrix(0.00.00.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();
}
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_ == 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);
    //cout << "joint 0 : "<<temp.joint_angle[0] << endl;
    //cout << "joint 1 : "<<temp.joint_angle[1] << endl;
    //cout << "joint 2 : "<<temp.joint_angle[2] << endl;
    //cout << "joint 3 : "<<temp.joint_angle[3] << endl;
    //cout << "tool position : "<< temp.tool_position << endl;
    // write File
    if(file.is_open()) {
       file << "j0:" << temp.joint_angle[0];
       file << "j1:" << temp.joint_angle[1];
       file << "j2:" << temp.joint_angle[2];
       file << "j3:" << temp.joint_angle[3];
       file << "j4:" << temp.joint_angle[4]; 
       file << "\n"
    }
    else {
       printf("fail");
    }
  }
  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)
  {
       file.close();
  }
}
void OpenManipulatorMaster::setModeState(char ch)
{
  if(ch == '1')
  {
    record_buffer_.clear();
    mode_state_ = START_RECORDING_TRAJECTORY_MODE;
  }
  else if(ch == '2')
  {
    mode_state_ = STOP_RECORDING_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_ == 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()));
  }
  printf("-----------------------------\n");
  printf("1 : Start Recording Trajectory\n");
  printf("2 : Stop Recording 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);
  file.open("/tmp/test.txt");
  while (ros::ok())
  {
     ros::spinOnce();
  }
  return 0;
}
cs


간단히 바뀐 부분을 설명해보면

#include <iostream>
#include <fstream>
#include <string.h>
using namespace std;

header와 namespace를 include하고

// for Writing File  
ofstream file;

좋은 방법은 아니지만, 단회성의 프로그램이므로
전역변수로 파일 입출력 변수를 선언해준다.


  file.open("/tmp/test.txt");

이후 main 문에서 파일 경로를 파라미터로 하여 open해주고

    // write File
    if(file.is_open()) {
       file << "j0:" << temp.joint_angle[0];
       file << "j1:" << temp.joint_angle[1];
       file << "j2:" << temp.joint_angle[2];
       file << "j3:" << temp.joint_angle[3];
       file << "j4:" << temp.joint_angle[4]; 
       file << "\n"
    }
    else {
       printf("fail");
    }
  }
  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)
  {
       file.close();
  }

publishCallback에서 위와 같은 방법으로 recording 중에는 file에 data를 입력하고,
recording을 stop 시에는 file 닫아 저장해준다.

5. Edit cmakeLists.txt


이제 위의 수정된 파일을 컴파일 할 수 있도록 CMakeLists.txt 파일을 수정해주어야 한다.

/home/catkin_ws/src/open_manipulator_applications/open_manipulator_master_slave

위 위치에 존재하는 CMakeLists.txt 파일 내부에

Build 부분에 다음과 같은 구문을 추가해준다.

1
2
3
4
5
6
7
# custum by suho, for recording trajectory and making these data to txt
add_executable(open_manipulator_recording_trajectory src/open_manipulator_recording_trajectory.cpp)
add_dependencies(open_manipulator_recording_trajectory ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(open_manipulator_recording_trajectory ${catkin_LIBRARIES} )
cs


그리고 Install 부분에 다음과 같은 부분을 추가해준다.


1
2
3
4
# custum by suho, for recording trajectory and making these data to txt
install(TARGETS open_manipulator_recording_trajectory
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
cs


그리고 다음 명령어를 통해 컴파일 해준다.

1
$ cd ~/catkin_ws && catkin_make
cs


6. Making Launch File


이제 Launch File을 만들어주자.

/home/catkin_ws/src/open_manipulator_applications/open_manipulator_master_slave/launch

위 경로에 있는 open_manipulator_master.launch 파일을 open_manipulator_recording_trajectory.launch 파일로 변경하여 준다.

그리고 open_manipulator_recording_trajectory.launch  파일의 내부 소스를 다음과 같이 수정해준다.


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
<launch>
  <arg name="robot_name"   default="open_manipulator"/>
  <arg name="usb_port"     default="/dev/ttyUSB0"/>
  <arg name="baud_rate"    default="1000000"/>
  <arg name="service_call_period"    default="0.010"/>
  <arg name="joint1_id"    default="11"/>
  <arg name="joint2_id"    default="12"/>
  <arg name="joint3_id"    default="13"/>
  <arg name="joint4_id"    default="14"/>
  <arg name="gripper_id"   default="15"/>
  <group ns="$(arg robot_name)">
    <node name="open_manipulator_recording_trajectory"
pkg="open_manipulator_master_slave"
type="open_manipulator_recording_trajectory"
output="screen"
args="$(arg usb_port) $(arg baud_rate)">
      <param name="service_call_period"       value="$(arg service_call_period)"/>
      <param name="joint1_id"                 value="$(arg joint1_id)"/>
      <param name="joint2_id"                 value="$(arg joint2_id)"/>
      <param name="joint3_id"                 value="$(arg joint3_id)"/>
      <param name="joint4_id"                 value="$(arg joint4_id)"/>
      <param name="gripper_id"                value="$(arg gripper_id)"/>
    </node>
  </group>
</launch>
cs

위 부분에서 joint id의 디폴트 값들은 1부터 5까지 였는데, 내 경우엔 11~15까지여서 저렇게 바꾸어 주었고, 또 다음 부분을 바꾸었다.

    <node name="open_manipulator_recording_trajectory" 
pkg="open_manipulator_master_slave" 
type="open_manipulator_recording_trajectory" 
output="screen" 
args="$(arg usb_port) $(arg baud_rate)">

node의 이름을 open_manipulator_recording_trajectory 로 바꾸었고,
특히 type을 open_manipulator_recording_trajectory로 바꿈으로써
CMakeList 파일에도 추가해준 open_manipulator_recording_trajectory node를 쓰겠다 언급한 것이다.

7. Execute Program


이제 프로그램을 실행시켜 보자.

아래 명령어를 사용한다.


1
2
3
4
5
6
# terminal_1
$ roscore

# terminal_2
$ roslaunch open_manipulator_master_slave open_manipulator_recording_trajectory.launch usb_port:=/dev/ttyUSB0
cs




위 창이 뜨면,
키보드에서 1번을 눌러 Manipulator를 조작하면 Trajectory가 Recording 된다.,
2번을 눌러 Recording을 stop해준다.


다음과 같이 data가 저장된 것을 확인할 수 있다.

8. Edit for Parsing text data


Subscribe 쪽에서 text 파일의 data를 해석하기 편하려면, 각 data별로 공백으로 나눠져 있는게 좋다.

다음 소스와 같이 수정한다.


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
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
/*******************************************************************************
* 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"
#include <iostream>
#include <fstream>
#include <string.h>
using namespace std;
// for Writing File  
ofstream file;
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();
}
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.0120.00.017),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.00.00.0595),                 // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.0240.00.128),                // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1240.00.0),                  // relative position
           math::convertRPYToRotationMatrix(0.00.00.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.1260.00.0),                  // relative position
          math::convertRPYToRotationMatrix(0.00.00.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();
}
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_ == 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);
    //cout << "joint 0 : "<<temp.joint_angle[0] << endl;
    //cout << "joint 1 : "<<temp.joint_angle[1] << endl;
    //cout << "joint 2 : "<<temp.joint_angle[2] << endl;
    //cout << "joint 3 : "<<temp.joint_angle[3] << endl;
    //cout << "tool position : "<< temp.tool_position << endl;
    // write File
    if(file.is_open()) {
       file << temp.joint_angle[0<< ' ';
       file << temp.joint_angle[1<< ' ';
       file << temp.joint_angle[2<< ' ';
       file << temp.joint_angle[3<< ' ';
       file << temp.tool_position; 
       file << "\n"
    }
    else {
       printf("fail");
    }
  }
  else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE)
  {
       file.close();
  }
}
void OpenManipulatorMaster::setModeState(char ch)
{
  if(ch == '1')
  {
    record_buffer_.clear();
    mode_state_ = START_RECORDING_TRAJECTORY_MODE;
  }
  else if(ch == '2')
  {
    mode_state_ = STOP_RECORDING_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_ == 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()));
  }
  printf("-----------------------------\n");
  printf("1 : Start Recording Trajectory\n");
  printf("2 : Stop Recording 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);
  file.open("/tmp/test.txt");
  while (ros::ok())
  {
     ros::spinOnce();
  }
  return 0;
}
cs




댓글 없음:

댓글 쓰기