various improvements and fixes for use_ros_control=true (#6)

* Find matching hardware_interface using the required type

The name of the controller was used in order to find and start
the matching hardware interface.
In consequence this meant that one could only define one controller
for each hardware interface.

Now, the controller's required type of hardware interface is used
to find and start the matching hardware interface.

* separate read & update in controller

consume is defined as read+update, but update
does not include read in ros_control terminology.

* Handle latency in pipeline loop

The controllers need to update at a rate of *at least* 125Hz,
but the wait_dequeue_timed call could in theory slow the loop down to 62.5Hz.
The old ur_modern_driver worked around this problem by sending goals
at 4*125Hz.

This patch exploits the onTimeout method of a consumer to update with
the specified frequency of the control loop, even if no new state message
arrived after the previous command.

* publish wrench w.r.t. tcp frame

The messages had an empty frame_id before and could not be displayed in RViz

* support ros_control in indigo

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 @@ private:
       {
         if (!queue_.try_enqueue(std::move(p)))
         {
-          LOG_ERROR("Pipeline producer owerflowed!");
+          LOG_ERROR("Pipeline producer overflowed!");
         }
       }
 
@@ -141,27 +141,18 @@ private:
   {
     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 @@ public:
     pThread_.join();
     cThread_.join();
   }
-};
+};

include/ur_modern_driver/ros/controller.h

@@ -49,13 +49,13 @@ private:
   }
 
   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 @@ public:
   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 @@ public:
 
   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 @@ public:
   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);
   }