various improvements and fixes for use_ros_control=true

CMakeLists.txt

@@ -137,6 +137,11 @@ add_compile_options(-Wall)
 add_compile_options(-Wextra)
 add_compile_options(-Wno-unused-parameter)
 
+# support indigo's ros_control - This can be removed upon EOL indigo
+if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0")
+    add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
+endif()
+
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 # include_directories(include)

include/ur_modern_driver/pipeline.h

@@ -126,7 +126,7 @@ class Pipeline
       {
         if (!queue_.try_enqueue(std::move(p)))
         {
-          LOG_ERROR("Pipeline producer owerflowed!");
+          LOG_ERROR("Pipeline producer overflowed!");
         }
       }
 
@@ -141,27 +141,18 @@ class Pipeline
   {
     consumer_.setupConsumer();
     unique_ptr<T> product;
-    Time last_pkg = Clock::now();
-    Time last_warn = last_pkg;
     while (running_)
     {
-      // 16000us timeout was chosen because we should
-      // roughly recieve messages at 125hz which is every
-      // 8ms so double it for some error margin
-      if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16)))
+      // timeout was chosen because we should receive messages
+      // at roughly 125hz (every 8ms) and have to update
+      // the controllers (i.e. the consumer) with *at least* 125Hz
+      // So we update the consumer more frequently via onTimeout
+      if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(8)))
       {
-        Time now = Clock::now();
-        auto pkg_diff = now - last_pkg;
-        auto warn_diff = now - last_warn;
-        if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
-        {
-          last_warn = now;
-          consumer_.onTimeout();
-        }
+        consumer_.onTimeout();
         continue;
       }
 
-      last_pkg = Clock::now();
       if (!consumer_.consume(std::move(product)))
         break;
     }
@@ -201,4 +192,4 @@ class Pipeline
     pThread_.join();
     cThread_.join();
   }
-};
+};

include/ur_modern_driver/ros/controller.h

@@ -49,13 +49,13 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons
   }
 
   void read(RTShared& state);
-  bool update(RTShared& state);
+  bool update();
   bool write();
   void reset();
 
 public:
   ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
-                double max_vel_change);
+                double max_vel_change, std::string tcp_link);
   virtual ~ROSController()
   {
   }
@@ -66,20 +66,26 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons
   virtual void setupConsumer();
   virtual bool consume(RTState_V1_6__7& state)
   {
-    return update(state);
+    read(state);
+    return update();
   }
   virtual bool consume(RTState_V1_8& state)
   {
-    return update(state);
+    read(state);
+    return update();
   }
   virtual bool consume(RTState_V3_0__1& state)
   {
-    return update(state);
+    read(state);
+    return update();
   }
   virtual bool consume(RTState_V3_2__3& state)
   {
-    return update(state);
+    read(state);
+    return update();
   }
 
+  virtual void onTimeout();
+
   virtual void onRobotStateChange(RobotState state);
-};
+};

include/ur_modern_driver/ros/hardware_interface.h

@@ -43,7 +43,7 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
   std::array<double, 6> tcp_;
 
 public:
-  WrenchInterface();
+  WrenchInterface(std::string tcp_link);
   void update(RTShared &packet);
   typedef hardware_interface::ForceTorqueSensorInterface parent_type;
   static const std::string INTERFACE_NAME;
@@ -80,4 +80,4 @@ class PositionInterface : public HardwareInterface, public hardware_interface::P
 
   typedef hardware_interface::PositionJointInterface parent_type;
   static const std::string INTERFACE_NAME;
-};
+};

src/ros/controller.cpp

@@ -1,10 +1,10 @@
 #include "ur_modern_driver/ros/controller.h"
 
 ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
-                             std::vector<std::string>& joint_names, double max_vel_change)
+                             std::vector<std::string>& joint_names, double max_vel_change, std::string tcp_link)
   : controller_(this, nh_)
   , joint_interface_(joint_names)
-  , wrench_interface_()
+  , wrench_interface_(tcp_link)
   , position_interface_(follower, joint_interface_, joint_names)
   , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
 {
@@ -33,7 +33,16 @@ void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>
 
   for (auto const& ci : start_list)
   {
-    auto ait = available_interfaces_.find(ci.name);
+    std::string requested_interface("");
+
+#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
+    requested_interface = ci.hardware_interface;
+#else
+    if (!ci.claimed_resources.empty())
+      requested_interface = ci.claimed_resources[0].hardware_interface;
+#endif
+
+    auto ait = available_interfaces_.find(requested_interface);
 
     if (ait == available_interfaces_.end())
       continue;
@@ -74,13 +83,12 @@ void ROSController::read(RTShared& packet)
   wrench_interface_.update(packet);
 }
 
-bool ROSController::update(RTShared& state)
+bool ROSController::update()
 {
   auto time = ros::Time::now();
   auto diff = time - lastUpdate_;
   lastUpdate_ = time;
 
-  read(state);
   controller_.update(time, diff, !service_enabled_);
 
   // emergency stop and such should not kill the pipeline
@@ -101,6 +109,11 @@ bool ROSController::update(RTShared& state)
   return write();
 }
 
+void ROSController::onTimeout()
+{
+  update();
+}
+
 void ROSController::onRobotStateChange(RobotState state)
 {
   bool next = (state == RobotState::Running);
@@ -109,4 +122,4 @@ void ROSController::onRobotStateChange(RobotState state)
 
   service_enabled_ = next;
   service_cooldown_ = 125;
-}
+}

src/ros/hardware_interface.cpp

@@ -1,7 +1,7 @@
 #include "ur_modern_driver/ros/hardware_interface.h"
 #include "ur_modern_driver/log.h"
 
-const std::string JointInterface::INTERFACE_NAME = "joint_state_controller";
+const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface";
 JointInterface::JointInterface(std::vector<std::string> &joint_names)
 {
   for (size_t i = 0; i < 6; i++)
@@ -18,10 +18,10 @@ void JointInterface::update(RTShared &packet)
 }
 
 
-const std::string WrenchInterface::INTERFACE_NAME = "force_torque_sensor_controller";
-WrenchInterface::WrenchInterface()
+const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface";
+WrenchInterface::WrenchInterface(std::string tcp_link)
 {
-  registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
+  registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3));
 }
 
 void WrenchInterface::update(RTShared &packet)
@@ -30,7 +30,7 @@ void WrenchInterface::update(RTShared &packet)
 }
 
 
-const std::string VelocityInterface::INTERFACE_NAME = "vel_based_pos_traj_controller";
+const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface";
 VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
                                      std::vector<std::string> &joint_names, double max_vel_change)
   : commander_(commander), max_vel_change_(max_vel_change)
@@ -62,7 +62,7 @@ void VelocityInterface::reset()
   }
 }
 
-const std::string PositionInterface::INTERFACE_NAME = "pos_based_pos_traj_controller";
+const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface";
 PositionInterface::PositionInterface(TrajectoryFollower &follower,
                                      hardware_interface::JointStateInterface &js_interface,
                                      std::vector<std::string> &joint_names)
@@ -87,4 +87,4 @@ void PositionInterface::start()
 void PositionInterface::stop()
 {
   follower_.stop();
-}
+}

src/ros_main.cpp

@@ -28,6 +28,7 @@ static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
 static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
+static const std::string TCP_LINK_ARG("~tcp_link");
 static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
 
 static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
@@ -43,6 +44,7 @@ struct ProgArgs
   std::string prefix;
   std::string base_frame;
   std::string tool_frame;
+  std::string tcp_link;
   std::vector<std::string> joint_names;
   double max_acceleration;
   double max_velocity;
@@ -65,6 +67,7 @@ bool parse_args(ProgArgs &args)
   ros::param::param(PREFIX_ARG, args.prefix, std::string());
   ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
   ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
+  ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "ee_link");
   ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS);
   return true;
 }
@@ -109,7 +112,7 @@ int main(int argc, char **argv)
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change);
+    controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
     rt_vec.push_back(controller);
     services.push_back(controller);
   }