From ff8b5522a200fcbb3d8cc3124db03a2e7c15116a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 17 Sep 2015 10:10:45 +0200 Subject: [PATCH] Added sanity check to setSpeed input --- README.md | 2 +- src/ur_ros_wrapper.cpp | 22 ++++++++++++---------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 941dfdc..7e6ad23 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ Besides this, the driver subscribes to two new topics: */joint\_speed : takes messages of type trajectory\_msgs/JointTrajectory, parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. -This driver is written in c++, which should make it possible to integrate it with ros_control. If you fell like undertaking this task, please dive right in. I don't have the posibility to do this. +This driver is written in c++, which should make it possible to integrate it with ros_control. If you feel like undertaking this task, please dive right in. I don't have the posibility to do this. No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running. diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index b0b5a1e..7d7f207 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -88,12 +88,10 @@ public: double min_payload = 0.; double max_payload = 1.; if (ros::param::get("~min_payload", min_payload)) { - ROS_DEBUG("Min payload set to: %f [kg]", - min_payload); + ROS_DEBUG("Min payload set to: %f [kg]", min_payload); } if (ros::param::get("~max_payload", max_payload)) { - ROS_DEBUG("Max payload set to: %f [kg]", - max_payload); + ROS_DEBUG("Max payload set to: %f [kg]", max_payload); } robot_.setMinPayload(min_payload); robot_.setMaxPayload(max_payload); @@ -317,12 +315,16 @@ private: } void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { - double acc = *std::max_element(msg->points[0].accelerations.begin(), - msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], - msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], - msg->points[0].velocities[5], acc); + if (msg->points[0].velocities.size() == 6) { + double acc = 100; + if (msg->points[0].accelerations.size() > 0) + acc = *std::max_element(msg->points[0].accelerations.begin(), + msg->points[0].accelerations.end()); + robot_.setSpeed(msg->points[0].velocities[0], + msg->points[0].velocities[1], msg->points[0].velocities[2], + msg->points[0].velocities[3], msg->points[0].velocities[4], + msg->points[0].velocities[5], acc); + } } void urscriptInterface(const std_msgs::String::ConstPtr& msg) {