Skip to content

Commit c683915

Browse files
sunghowoosunghowoo
committed
Added completion condition for robot operation
1 parent 784eada commit c683915

File tree

2 files changed

+125
-37
lines changed

2 files changed

+125
-37
lines changed

‎.vscode/settings.json

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
{
2+
"files.associations": {
3+
"array": "cpp",
4+
"string_view": "cpp"
5+
}
6+
}

‎open_manipulator_controller/src/open_manipulator_controller.cpp

Lines changed: 119 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
/*******************************************************************************
1+
/*******************************************************************************
22
* Copyright 2018 ROBOTIS CO., LTD.
33
*
44
* Licensed under the Apache License, Version 2.0 (the "License");
@@ -58,32 +58,6 @@ OpenManipulatorController::~OpenManipulatorController()
5858

5959
void OpenManipulatorController::startTimerThread()
6060
{
61-
////////////////////////////////////////////////////////////////////
62-
/// Use this when you want to increase the priority of threads.
63-
////////////////////////////////////////////////////////////////////
64-
// pthread_attr_t attr_;
65-
// int error;
66-
// struct sched_param param;
67-
// pthread_attr_init(&attr_);
68-
69-
// error = pthread_attr_setschedpolicy(&attr_, SCHED_RR);
70-
// if (error != 0) log::error("pthread_attr_setschedpolicy error = ", (double)error);
71-
// error = pthread_attr_setinheritsched(&attr_, PTHREAD_EXPLICIT_SCHED);
72-
// if (error != 0) log::error("pthread_attr_setinheritsched error = ", (double)error);
73-
74-
// memset(&param, 0, sizeof(param));
75-
// param.sched_priority = 31; // RT
76-
// error = pthread_attr_setschedparam(&attr_, &param);
77-
// if (error != 0) log::error("pthread_attr_setschedparam error = ", (double)error);
78-
79-
// if ((error = pthread_create(&this->timer_thread_, &attr_, this->timerThread, this)) != 0)
80-
// {
81-
// log::error("Creating timer thread failed!!", (double)error);
82-
// exit(-1);
83-
// }
84-
// timer_thread_state_ = true;
85-
////////////////////////////////////////////////////////////////////
86-
8761
int error;
8862
if ((error = pthread_create(&this->timer_thread_, NULL, this->timerThread, this)) != 0)
8963
{
@@ -110,17 +84,14 @@ void *OpenManipulatorController::timerThread(void *param)
11084
controller->process(time);
11185

11286
clock_gettime(CLOCK_MONOTONIC, &curr_time);
113-
/////
11487
double delta_nsec = controller->getControlPeriod() - ((next_time.tv_sec - curr_time.tv_sec) + ((double)(next_time.tv_nsec - curr_time.tv_nsec)*0.000000001));
115-
//log::info("control time : ", controller->getControlPeriod() - delta_nsec);
88+
11689
if (delta_nsec > controller->getControlPeriod())
11790
{
118-
//log::warn("Over the control time : ", delta_nsec);
11991
next_time = curr_time;
12092
}
12193
else
12294
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL);
123-
/////
12495
}
12596
return 0;
12697
}
@@ -130,7 +101,7 @@ void *OpenManipulatorController::timerThread(void *param)
130101
********************************************************************************/
131102
void OpenManipulatorController::initPublisher()
132103
{
133-
// ros message publisher
104+
// ROS message publisher
134105
auto om_tools_name = open_manipulator_.getManipulator()->getAllToolComponentName();
135106

136107
for (auto const& name:om_tools_name)
@@ -159,9 +130,10 @@ void OpenManipulatorController::initPublisher()
159130
}
160131
}
161132
}
133+
162134
void OpenManipulatorController::initSubscriber()
163135
{
164-
// ros message subscriber
136+
// ROS message subscriber
165137
open_manipulator_option_sub_ = node_handle_.subscribe("option", 10, &OpenManipulatorController::openManipulatorOptionCallback, this);
166138
}
167139

@@ -202,6 +174,14 @@ void OpenManipulatorController::openManipulatorOptionCallback(const std_msgs::St
202174
bool OpenManipulatorController::goalJointSpacePathCallback(open_manipulator_msgs::SetJointPosition::Request &req,
203175
open_manipulator_msgs::SetJointPosition::Response &res)
204176
{
177+
// Ignore commands if the robot is moving
178+
if (open_manipulator_.getMovingState())
179+
{
180+
log::warn("Robot is moving. Ignoring command.");
181+
res.is_planned = false;
182+
return true;
183+
}
184+
205185
std::vector <double> target_angle;
206186

207187
for (int i = 0; i < req.joint_position.joint_name.size(); i ++)
@@ -218,6 +198,14 @@ bool OpenManipulatorController::goalJointSpacePathCallback(open_manipulator_msgs
218198
bool OpenManipulatorController::goalJointSpacePathToKinematicsPoseCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
219199
open_manipulator_msgs::SetKinematicsPose::Response &res)
220200
{
201+
// Ignore commands if the robot is moving
202+
if (open_manipulator_.getMovingState())
203+
{
204+
log::warn("Robot is moving. Ignoring command.");
205+
res.is_planned = false;
206+
return true;
207+
}
208+
221209
KinematicPose target_pose;
222210
target_pose.position[0] = req.kinematics_pose.pose.position.x;
223211
target_pose.position[1] = req.kinematics_pose.pose.position.y;
@@ -241,6 +229,14 @@ bool OpenManipulatorController::goalJointSpacePathToKinematicsPoseCallback(open_
241229
bool OpenManipulatorController::goalJointSpacePathToKinematicsPositionCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
242230
open_manipulator_msgs::SetKinematicsPose::Response &res)
243231
{
232+
// Ignore commands if the robot is moving
233+
if (open_manipulator_.getMovingState())
234+
{
235+
log::warn("Robot is moving. Ignoring command.");
236+
res.is_planned = false;
237+
return true;
238+
}
239+
244240
KinematicPose target_pose;
245241
target_pose.position[0] = req.kinematics_pose.pose.position.x;
246242
target_pose.position[1] = req.kinematics_pose.pose.position.y;
@@ -257,6 +253,14 @@ bool OpenManipulatorController::goalJointSpacePathToKinematicsPositionCallback(o
257253
bool OpenManipulatorController::goalJointSpacePathToKinematicsOrientationCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
258254
open_manipulator_msgs::SetKinematicsPose::Response &res)
259255
{
256+
// Ignore commands if the robot is moving
257+
if (open_manipulator_.getMovingState())
258+
{
259+
log::warn("Robot is moving. Ignoring command.");
260+
res.is_planned = false;
261+
return true;
262+
}
263+
260264
KinematicPose target_pose;
261265

262266
Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w,
@@ -270,12 +274,21 @@ bool OpenManipulatorController::goalJointSpacePathToKinematicsOrientationCallbac
270274
res.is_planned = false;
271275
else
272276
res.is_planned = true;
277+
273278
return true;
274279
}
275280

276281
bool OpenManipulatorController::goalTaskSpacePathCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
277282
open_manipulator_msgs::SetKinematicsPose::Response &res)
278283
{
284+
// Ignore commands if the robot is moving
285+
if (open_manipulator_.getMovingState())
286+
{
287+
log::warn("Robot is moving. Ignoring command.");
288+
res.is_planned = false;
289+
return true;
290+
}
291+
279292
KinematicPose target_pose;
280293
target_pose.position[0] = req.kinematics_pose.pose.position.x;
281294
target_pose.position[1] = req.kinematics_pose.pose.position.y;
@@ -298,6 +311,14 @@ bool OpenManipulatorController::goalTaskSpacePathCallback(open_manipulator_msgs:
298311
bool OpenManipulatorController::goalTaskSpacePathPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
299312
open_manipulator_msgs::SetKinematicsPose::Response &res)
300313
{
314+
// Ignore commands if the robot is moving
315+
if (open_manipulator_.getMovingState())
316+
{
317+
log::warn("Robot is moving. Ignoring command.");
318+
res.is_planned = false;
319+
return true;
320+
}
321+
301322
Eigen::Vector3d position;
302323
position[0] = req.kinematics_pose.pose.position.x;
303324
position[1] = req.kinematics_pose.pose.position.y;
@@ -314,6 +335,14 @@ bool OpenManipulatorController::goalTaskSpacePathPositionOnlyCallback(open_manip
314335
bool OpenManipulatorController::goalTaskSpacePathOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
315336
open_manipulator_msgs::SetKinematicsPose::Response &res)
316337
{
338+
// Ignore commands if the robot is moving
339+
if (open_manipulator_.getMovingState())
340+
{
341+
log::warn("Robot is moving. Ignoring command.");
342+
res.is_planned = false;
343+
return true;
344+
}
345+
317346
Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w,
318347
req.kinematics_pose.pose.orientation.x,
319348
req.kinematics_pose.pose.orientation.y,
@@ -332,6 +361,14 @@ bool OpenManipulatorController::goalTaskSpacePathOrientationOnlyCallback(open_ma
332361
bool OpenManipulatorController::goalJointSpacePathFromPresentCallback(open_manipulator_msgs::SetJointPosition::Request &req,
333362
open_manipulator_msgs::SetJointPosition::Response &res)
334363
{
364+
// Ignore commands if the robot is moving
365+
if (open_manipulator_.getMovingState())
366+
{
367+
log::warn("Robot is moving. Ignoring command.");
368+
res.is_planned = false;
369+
return true;
370+
}
371+
335372
std::vector <double> target_angle;
336373

337374
for (int i = 0; i < req.joint_position.joint_name.size(); i ++)
@@ -348,6 +385,14 @@ bool OpenManipulatorController::goalJointSpacePathFromPresentCallback(open_manip
348385
bool OpenManipulatorController::goalTaskSpacePathFromPresentCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
349386
open_manipulator_msgs::SetKinematicsPose::Response &res)
350387
{
388+
// Ignore commands if the robot is moving
389+
if (open_manipulator_.getMovingState())
390+
{
391+
log::warn("Robot is moving. Ignoring command.");
392+
res.is_planned = false;
393+
return true;
394+
}
395+
351396
KinematicPose target_pose;
352397
target_pose.position[0] = req.kinematics_pose.pose.position.x;
353398
target_pose.position[1] = req.kinematics_pose.pose.position.y;
@@ -371,6 +416,14 @@ bool OpenManipulatorController::goalTaskSpacePathFromPresentCallback(open_manipu
371416
bool OpenManipulatorController::goalTaskSpacePathFromPresentPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
372417
open_manipulator_msgs::SetKinematicsPose::Response &res)
373418
{
419+
// Ignore commands if the robot is moving
420+
if (open_manipulator_.getMovingState())
421+
{
422+
log::warn("Robot is moving. Ignoring command.");
423+
res.is_planned = false;
424+
return true;
425+
}
426+
374427
Eigen::Vector3d position;
375428
position[0] = req.kinematics_pose.pose.position.x;
376429
position[1] = req.kinematics_pose.pose.position.y;
@@ -387,6 +440,14 @@ bool OpenManipulatorController::goalTaskSpacePathFromPresentPositionOnlyCallback
387440
bool OpenManipulatorController::goalTaskSpacePathFromPresentOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req,
388441
open_manipulator_msgs::SetKinematicsPose::Response &res)
389442
{
443+
// Ignore commands if the robot is moving
444+
if (open_manipulator_.getMovingState())
445+
{
446+
log::warn("Robot is moving. Ignoring command.");
447+
res.is_planned = false;
448+
return true;
449+
}
450+
390451
Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w,
391452
req.kinematics_pose.pose.orientation.x,
392453
req.kinematics_pose.pose.orientation.y,
@@ -405,6 +466,14 @@ bool OpenManipulatorController::goalTaskSpacePathFromPresentOrientationOnlyCallb
405466
bool OpenManipulatorController::goalToolControlCallback(open_manipulator_msgs::SetJointPosition::Request &req,
406467
open_manipulator_msgs::SetJointPosition::Response &res)
407468
{
469+
// Ignore commands if the robot is moving
470+
if (open_manipulator_.getMovingState())
471+
{
472+
log::warn("Robot is moving. Ignoring command.");
473+
res.is_planned = false;
474+
return true;
475+
}
476+
408477
for (int i = 0; i < req.joint_position.joint_name.size(); i ++)
409478
{
410479
if (!open_manipulator_.makeToolTrajectory(req.joint_position.joint_name.at(i), req.joint_position.position.at(i)))
@@ -444,6 +513,14 @@ bool OpenManipulatorController::setActuatorStateCallback(open_manipulator_msgs::
444513
bool OpenManipulatorController::goalDrawingTrajectoryCallback(open_manipulator_msgs::SetDrawingTrajectory::Request &req,
445514
open_manipulator_msgs::SetDrawingTrajectory::Response &res)
446515
{
516+
// Ignore commands if the robot is moving
517+
if (open_manipulator_.getMovingState())
518+
{
519+
log::warn("Robot is moving. Ignoring command.");
520+
res.is_planned = false;
521+
return true;
522+
}
523+
447524
try
448525
{
449526
if (req.drawing_trajectory_name == "circle")
@@ -466,7 +543,7 @@ bool OpenManipulatorController::goalDrawingTrajectoryCallback(open_manipulator_m
466543
draw_line_arg.kinematic.position(1) = req.param[1]; // y axis (m)
467544
draw_line_arg.kinematic.position(2) = req.param[2]; // z axis (m)
468545
void *p_draw_line_arg = &draw_line_arg;
469-
546+
470547
if (!open_manipulator_.makeCustomTrajectory(CUSTOM_TRAJECTORY_LINE, req.end_effector_name, p_draw_line_arg, req.path_time))
471548
res.is_planned = false;
472549
else
@@ -626,14 +703,14 @@ void OpenManipulatorController::publishGazeboCommand()
626703
*****************************************************************************/
627704
int main(int argc, char **argv)
628705
{
629-
// init
706+
// Init ROS
630707
ros::init(argc, argv, "open_manipulator_controller");
631708
ros::NodeHandle node_handle("");
632709

633710
std::string usb_port = "/dev/ttyUSB0";
634711
std::string baud_rate = "1000000";
635712

636-
if (argc = 3)
713+
if (argc == 3)
637714
{
638715
usb_port = argv[1];
639716
baud_rate = argv[2];
@@ -647,9 +724,13 @@ int main(int argc, char **argv)
647724

648725
OpenManipulatorController om_controller(usb_port, baud_rate);
649726

650-
// update
727+
// Start the timer thread
651728
om_controller.startTimerThread();
729+
730+
// Set up a periodic publishing timer
652731
ros::Timer publish_timer = node_handle.createTimer(ros::Duration(om_controller.getControlPeriod()), &OpenManipulatorController::publishCallback, &om_controller);
732+
733+
// Run the loop
653734
ros::Rate loop_rate(100);
654735
while (ros::ok())
655736
{
@@ -659,3 +740,4 @@ int main(int argc, char **argv)
659740

660741
return 0;
661742
}
743+

0 commit comments

Comments
 (0)