From b72aac3c17ecf746202c3739c7b4f718d1f6f285 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Sat, 25 Jun 2016 02:33:54 +0200 Subject: [PATCH 1/4] ur_ros_wrapper: introduce a bit of thread safety into goal handling --- src/ur_ros_wrapper.cpp | 43 +++++++++++++++++++++++++++++++++++------- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ab74534b..1740d9fe 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -63,9 +63,7 @@ class RosWrapper { ros::NodeHandle nh_; actionlib::ActionServer as_; actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; @@ -82,6 +80,12 @@ class RosWrapper { boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + std::mutex goal_mutex_; + bool has_goal_; + std::thread traj_thread_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + public: RosWrapper(std::string host, int reverse_port) : as_(nh_, "follow_joint_trajectory", @@ -227,6 +231,9 @@ class RosWrapper { std::vector > velocities) { robot_.doTraj(timestamps, positions, velocities); + + std::unique_lock lock(goal_mutex_); + if (has_goal_) { result_.error_code = result_.SUCCESSFUL; goal_handle_.setSucceeded(result_); @@ -283,11 +290,22 @@ class RosWrapper { actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify + + std::unique_lock lock(goal_mutex_); if (has_goal_) { print_warning( "Received new goal while still executing previous trajectory. Canceling previous trajectory"); has_goal_ = false; - robot_.stopTraj(); + + { + lock.unlock(); + + robot_.stopTraj(); + traj_thread_.join(); + + lock.lock(); + } + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Received another trajectory"; goal_handle_.setAborted(result_, result_.error_string); @@ -373,21 +391,32 @@ class RosWrapper { goal_handle_.setAccepted(); has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities).detach(); + traj_thread_ = std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities); } void cancelCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted + + std::unique_lock lock(goal_mutex_); + print_info("on_cancel"); if (has_goal_) { if (gh == goal_handle_) { - robot_.stopTraj(); + has_goal_ = false; + + { + lock.unlock(); + robot_.stopTraj(); + traj_thread_.join(); + lock.lock(); + } has_goal_ = false; } } + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Goal cancelled by client"; gh.setCanceled(result_); From c52eb64b78d58b55e984f5d9aec7384b3e7dda76 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Sat, 25 Jun 2016 13:29:44 +0200 Subject: [PATCH 2/4] ur_ros_wrapper: more thread safety --- src/ur_driver.cpp | 6 ++++- src/ur_ros_wrapper.cpp | 60 ++++++++++++++++++++---------------------- 2 files changed, 34 insertions(+), 32 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 22081151..3f580718 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -112,7 +112,11 @@ bool UrDriver::doTraj(std::vector inp_timestamps, executing_traj_ = false; //Signal robot to stop driverProg() UrDriver::closeServo(positions); - return true; + + if(inp_timestamps[inp_timestamps.size() - 1] < std::chrono::duration_cast>(t - t0).count()) + return true; + else + return false; } void UrDriver::servoj(std::vector positions, int keepalive) { diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 1740d9fe..d514d3a4 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -80,6 +80,9 @@ class RosWrapper { boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + // Thread semantics: + // cancelCB and goalCB are both executed with locked action server mutex. + std::mutex goal_mutex_; bool has_goal_; std::thread traj_thread_; @@ -226,20 +229,22 @@ class RosWrapper { } private: - void trajThread(std::vector timestamps, + void trajThread( + std::vector timestamps, std::vector > positions, - std::vector > velocities) { - - robot_.doTraj(timestamps, positions, velocities); - - std::unique_lock lock(goal_mutex_); + std::vector > velocities) + { + bool finished = robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { + if (finished) + { + std::unique_lock lock(goal_mutex_); result_.error_code = result_.SUCCESSFUL; goal_handle_.setSucceeded(result_); has_goal_ = false; } } + void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { @@ -291,26 +296,26 @@ class RosWrapper { actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify - std::unique_lock lock(goal_mutex_); - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - - { - lock.unlock(); - + { + std::unique_lock lock(goal_mutex_); + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; robot_.stopTraj(); - traj_thread_.join(); - lock.lock(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); } - - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); } + + if(traj_thread_.joinable()) + traj_thread_.join(); + + std::unique_lock lock(goal_mutex_); + goal_handle_ = gh; if (!validateJointNames()) { std::string outp_joint_names = ""; @@ -406,14 +411,7 @@ class RosWrapper { if (has_goal_) { if (gh == goal_handle_) { has_goal_ = false; - - { - lock.unlock(); - robot_.stopTraj(); - traj_thread_.join(); - lock.lock(); - } - has_goal_ = false; + robot_.stopTraj(); } } From 2212d9be01aca3be17b9d74b886bb186bf5b8515 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Thu, 30 Jun 2016 15:10:42 +0200 Subject: [PATCH 3/4] even more thread safety (ur_realtime_comm) --- include/ur_modern_driver/ur_realtime_communication.h | 1 + src/ur_realtime_communication.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 9d6b4454..cdec94e9 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -60,6 +60,7 @@ class UrRealtimeCommunication { public: bool connected_; RobotStateRT* robot_state_; + std::mutex mutex_; UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12); diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe2..b38239de 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -85,6 +85,8 @@ void UrRealtimeCommunication::halt() { } void UrRealtimeCommunication::addCommandToQueue(std::string inp) { + std::unique_lock lock(mutex_); + int bytes_written; if (inp.back() != '\n') { inp.append("\n"); From 94b1ebfe7dd25fd663df7b6eeff27f536034ac66 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Thu, 30 Jun 2016 15:15:26 +0200 Subject: [PATCH 4/4] ur_ros_wrapper: protect against simultaneous action callbacks --- src/ur_ros_wrapper.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index d514d3a4..b89ef86b 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -81,7 +81,10 @@ class RosWrapper { boost::shared_ptr controller_manager_; // Thread semantics: - // cancelCB and goalCB are both executed with locked action server mutex. + // * cancelCB and goalCB can be called simultaneously by the action server. + // We prevent that by locking the as_mutex_. + // * The has_goal_ variable is protected by the goal_mutex_, as it is + // accessed by trajThread() and the callbacks. std::mutex goal_mutex_; bool has_goal_; @@ -89,6 +92,8 @@ class RosWrapper { control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; + std::mutex as_mutex_; + public: RosWrapper(std::string host, int reverse_port) : as_(nh_, "follow_joint_trajectory", @@ -248,6 +253,9 @@ class RosWrapper { void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { + + std::unique_lock asLock(as_mutex_); + std::string buf; print_info("on_goal"); if (!robot_.sec_interface_->robot_state_->isReady()) { @@ -405,6 +413,7 @@ class RosWrapper { control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted + std::unique_lock asLock(as_mutex_); std::unique_lock lock(goal_mutex_); print_info("on_cancel");