From 80e344d167337a5d0153021b6bb03afa68405cbf Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 9 Oct 2015 10:47:29 +0200 Subject: [PATCH] Fixed an issue where canceling a trajectory wouldn't stop the robot. --- src/ur_driver.cpp | 125 +++++++++++++++++++++-------------------- src/ur_ros_wrapper.cpp | 51 ++++++++++++----- 2 files changed, 101 insertions(+), 75 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c02ffdf..acd5703 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -13,15 +13,17 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, - double max_time_step, double min_payload, double max_payload) : + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload) : REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) { + min_payload), maximum_payload_(max_payload), servoj_time_( + servoj_time) { char buffer[256]; struct sockaddr_in serv_addr; int n, flag; //char *ip_addr; - + executing_traj_ = false; rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); new_sockfd_ = -1; @@ -63,77 +65,76 @@ std::vector UrDriver::interp_cubic(double t, double T, } return positions; } - /* -void UrDriver::addTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - // DEPRECATED - printf("!! addTraj is deprecated !!\n"); - std::vector timestamps; - std::vector > positions; - std::string command_string = "def traj():\n"; +/* + void UrDriver::addTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities) { + // DEPRECATED + printf("!! addTraj is deprecated !!\n"); + std::vector timestamps; + std::vector > positions; + std::string command_string = "def traj():\n"; - for (unsigned int i = 1; i < inp_timestamps.size(); i++) { - timestamps.push_back(inp_timestamps[i - 1]); - double dt = inp_timestamps[i] - inp_timestamps[i - 1]; - unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); - double step_size = dt / steps; - for (unsigned int j = 1; j < steps; j++) { - timestamps.push_back(inp_timestamps[i - 1] + step_size * j); - } - } - // //make sure we come to a smooth stop - // while (timestamps.back() < inp_timestamps.back()) { - // timestamps.push_back(timestamps.back() + 0.008); - // } - // timestamps.pop_back(); + for (unsigned int i = 1; i < inp_timestamps.size(); i++) { + timestamps.push_back(inp_timestamps[i - 1]); + double dt = inp_timestamps[i] - inp_timestamps[i - 1]; + unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); + double step_size = dt / steps; + for (unsigned int j = 1; j < steps; j++) { + timestamps.push_back(inp_timestamps[i - 1] + step_size * j); + } + } + // //make sure we come to a smooth stop + // while (timestamps.back() < inp_timestamps.back()) { + // timestamps.push_back(timestamps.back() + 0.008); + // } + // timestamps.pop_back(); - unsigned int j = 0; - for (unsigned int i = 0; i < timestamps.size(); i++) { - while (inp_timestamps[j] <= timestamps[i]) { - j += 1; - } - positions.push_back( - UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], - inp_positions[j - 1], inp_positions[j], - inp_velocities[j - 1], inp_velocities[j])); - } + unsigned int j = 0; + for (unsigned int i = 0; i < timestamps.size(); i++) { + while (inp_timestamps[j] <= timestamps[i]) { + j += 1; + } + positions.push_back( + UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], + inp_positions[j - 1], inp_positions[j], + inp_velocities[j - 1], inp_velocities[j])); + } - timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); - positions.push_back(inp_positions[inp_positions.size() - 1]); - /// This is actually faster than using a stringstream :-o - for (unsigned int i = 1; i < timestamps.size(); i++) { - char buf[128]; - sprintf(buf, - "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", - positions[i][0], positions[i][1], positions[i][2], - positions[i][3], positions[i][4], positions[i][5], - timestamps[i] - timestamps[i - 1]); - command_string += buf; - } - command_string += "end\n"; - //printf("%s", command_string.c_str()); - rt_interface_->addCommandToQueue(command_string); + timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); + positions.push_back(inp_positions[inp_positions.size() - 1]); + /// This is actually faster than using a stringstream :-o + for (unsigned int i = 1; i < timestamps.size(); i++) { + char buf[128]; + sprintf(buf, + "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", + positions[i][0], positions[i][1], positions[i][2], + positions[i][3], positions[i][4], positions[i][5], + timestamps[i] - timestamps[i - 1]); + command_string += buf; + } + command_string += "end\n"; + //printf("%s", command_string.c_str()); + rt_interface_->addCommandToQueue(command_string); -} -*/ + } + */ void UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; std::vector positions; unsigned int j; executing_traj_ = true; UrDriver::uploadProg(); - t0 = std::chrono::high_resolution_clock::now(); t = t0; j = 0; while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) and executing_traj_) { + >= std::chrono::duration_cast>(t - t0).count()) + and executing_traj_) { while (inp_timestamps[j] <= std::chrono::duration_cast>( t - t0).count() && j < inp_timestamps.size() - 1) { @@ -144,19 +145,19 @@ void UrDriver::doTraj(std::vector inp_timestamps, t - t0).count() - inp_timestamps[j - 1], inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.))); + std::this_thread::sleep_for( + std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() UrDriver::closeServo(positions); } -void UrDriver::servoj(std::vector positions, - int keepalive, double time) { +void UrDriver::servoj(std::vector positions, int keepalive, + double time) { unsigned int bytes_written; int tmp; unsigned char buf[32]; diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 92c1e1b..494e6c1 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -148,6 +148,7 @@ public: "The control thread for this driver has been started"); } else { //start actionserver + goal_handle_ = NULL; as_.start(); //subscribe to the data topic of interest @@ -180,8 +181,13 @@ private: actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); + actionlib::ActionServer::Goal goal = + *gh.getGoal(); //make a copy that we can modify + if (goal_handle_ != NULL) { + RosWrapper::cancelCB(*goal_handle_); + } goal_handle_ = &gh; - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); //make a copy that we can modify + if (!validateJointNames()) { std::string outp_joint_names = ""; for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); @@ -195,7 +201,6 @@ private: gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } - if (!has_positions()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without positions"; @@ -226,8 +231,15 @@ private: } reorder_traj_joints(goal.trajectory); + std::vector timestamps; std::vector > positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); + } for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { timestamps.push_back( goal.trajectory.points[i].time_from_start.toSec()); @@ -235,10 +247,14 @@ private: velocities.push_back(goal.trajectory.points[i].velocities); } - gh.setAccepted(); + + goal_handle_->setAccepted(); robot_.doTraj(timestamps, positions, velocities); - result_.error_code = result_.SUCCESSFUL; - gh.setSucceeded(result_); + if (goal_handle_ != NULL) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_->setSucceeded(result_); + goal_handle_ = NULL; + } } void cancelCB( @@ -246,9 +262,13 @@ private: control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_cancel"); // set the action state to preempted - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Goal cancelled by client"; + if (goal_handle_ != NULL) { + print_info("Stopping previous traj"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Goal cancelled by client"; + goal_handle_ = NULL; + } gh.setCanceled(result_); } @@ -285,7 +305,8 @@ private: bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); if (goal.trajectory.joint_names.size() != actual_joint_names.size()) return false; @@ -336,7 +357,8 @@ private: } bool has_velocities() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) @@ -346,7 +368,8 @@ private: } bool has_positions() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); if (goal.trajectory.points.size() == 0) return false; for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { @@ -358,7 +381,8 @@ private: } bool has_limited_velocities() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) { @@ -371,7 +395,8 @@ private: } bool traj_is_finite() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) {