Fixed minor issues

include/ur_modern_driver/ros/converter.h

@@ -1 +0,0 @@
-#pragma once

include/ur_modern_driver/ros/hardware_interface.h

@@ -48,10 +48,11 @@ public:
 class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
 {
   std::array<double, 6> tcp_;
+
 public:
-  WrenchInterface() 
+  WrenchInterface()
   {
-    registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin()+3));
+    registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
   }
 
   void update(RTShared& packet)
@@ -65,7 +66,7 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V
 private:
   URCommander& commander_;
   std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
-  double max_vel_change_;
+  double max_vel_change_ = 0.12 / 125;
 
 public:
   VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector<std::string>& joint_names)
@@ -88,9 +89,11 @@ public:
       prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
     }
 
-    //times 125???
+    // times 125???
     commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125);
   }
+
+  typedef hardware_interface::VelocityJointInterface parent_type;
 };
 
 static const std::string POSITION_PROGRAM = R"(
@@ -165,4 +168,6 @@ public:
   virtual void write()
   {
   }
+
+  typedef hardware_interface::PositionJointInterface parent_type;
 };

include/ur_modern_driver/ros/robot_hardware.h

@@ -1,6 +1,7 @@
 #pragma once
 #include <controller_manager/controller_manager.h>
 #include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
@@ -16,9 +17,16 @@ private:
   WrenchInterface wrench_interface_;
   PositionInterface position_interface_;
   VelocityInterface velocity_interface_;
-  hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
+
   HardwareInterface* active_interface_;
-  std::vector<void*> available_interfaces_;
+  std::map<std::string, HardwareInterface*> available_interfaces_;
+
+  template <typename T>
+  void registerHardwareInterface(T* interface)
+  {
+    registerInterface<typename T::parent_type>(interface);
+    available_interfaces_[hardware_interface::internal::demangledTypeName<typename T::parent_type>()] = interface;
+  }
 
 public:
   RobotHardware(URCommander& commander, std::vector<std::string>& joint_names)
@@ -26,18 +34,17 @@ public:
     , wrench_interface_()
     , position_interface_(commander, joint_interface_, joint_names)
     , velocity_interface_(commander, joint_interface_, joint_names)
-    , available_interfaces_{&position_interface_, &velocity_interface_}
   {
     registerInterface<hardware_interface::JointStateInterface>(&joint_interface_);
     registerInterface<hardware_interface::ForceTorqueSensorInterface>(&wrench_interface_);
-    registerInterface<hardware_interface::PositionJointInterface>(&position_interface_);
-    registerInterface<hardware_interface::VelocityJointInterface>(&velocity_interface_);
+
+    registerHardwareInterface(&position_interface_);
+    registerHardwareInterface(&velocity_interface_);
   }
 
-  //bool canSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list) const;
+  // bool canSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list) const;
   void doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list);
 
-
   /// \brief Read the state from the robot hardware.
   virtual void read(RTShared& packet);
 

include/ur_modern_driver/ros/rt_publisher.h

@@ -31,6 +31,7 @@ private:
   std::vector<std::string> joint_names_;
   std::string base_frame_;
   std::string tool_frame_;
+  bool temp_only_;
 
   bool publishJoints(RTShared& packet, Time& t);
   bool publishWrench(RTShared& packet, Time& t);
@@ -41,13 +42,14 @@ private:
   bool publish(RTShared& packet);
 
 public:
-  RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
+  RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false)
     : joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
     , wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
     , tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
     , joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
     , base_frame_(base_frame)
     , tool_frame_(tool_frame)
+    , temp_only_(temp_only)
   {
     for (auto const& j : JOINTS)
     {

src/ros/robot_hardware.cpp

@@ -16,7 +16,7 @@ bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
     if(it == interfaces_.end() || it->second != active_interface_)
       return false;
   }
-  
+
   for (auto const& ci : start_list)
   {
     auto it = interfaces_.find(ci.hardware_interface);
@@ -29,43 +29,39 @@ bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
 }
 */
 
-void RobotHardware::doSwitch(const std::list<ControllerInfo>& start_list,
-                             const std::list<ControllerInfo>& stop_list)
+void RobotHardware::doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list)
 {
-  if(active_interface_ != nullptr && stop_list.size() > 0)
+  if (active_interface_ != nullptr && stop_list.size() > 0)
   {
+    LOG_INFO("Stopping active interface");
     active_interface_->stop();
     active_interface_ = nullptr;
   }
 
-  for(auto const& ci : start_list)
+  for (auto const& ci : start_list)
   {
-    auto it = interfaces_.find(ci.hardware_interface);
-    if(it == interfaces_.end())
-      continue;
-    
-    //we can only switch to one of the available interfaces
-    if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end())
+    auto ait = available_interfaces_.find(ci.hardware_interface);
+
+    if (ait == available_interfaces_.end())
       continue;
 
-    auto new_interface = static_cast<HardwareInterface*>(it->second);
+    auto new_interface = ait->second;
 
-    if(new_interface == nullptr)
-      continue;
-    
     LOG_INFO("Starting %s", ci.hardware_interface.c_str());
     active_interface_ = new_interface;
     new_interface->start();
-    
+
     return;
   }
+
+  LOG_WARN("Failed to start interface!");
 }
 
 void RobotHardware::write()
 {
-  if(active_interface_ == nullptr)
+  if (active_interface_ == nullptr)
     return;
-  
+
   active_interface_->write();
 }
 

src/ros/rt_publisher.cpp

@@ -91,8 +91,14 @@ bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
 bool RTPublisher::publish(RTShared& packet)
 {
   Time time = Time::now();
-  return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
-         publishTransform(packet, time) && publishTemperature(packet, time);
+  bool res = true;
+  if (!temp_only_)
+  {
+    res = publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
+          publishTransform(packet, time);
+  }
+
+  return res && publishTemperature(packet, time);
 }
 
 bool RTPublisher::consume(RTState_V1_6__7& state)

src/ros_main.cpp

@@ -6,10 +6,10 @@
 
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
-#include "ur_modern_driver/ros/rt_publisher.h"
-#include "ur_modern_driver/ros/mb_publisher.h"
 #include "ur_modern_driver/ros/io_service.h"
+#include "ur_modern_driver/ros/mb_publisher.h"
 #include "ur_modern_driver/ros/ros_controller.h"
+#include "ur_modern_driver/ros/rt_publisher.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
 #include "ur_modern_driver/ur/messages.h"
@@ -58,7 +58,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::get(JOINT_NAMES_PARAM, args.joint_names);  
+  ros::param::get(JOINT_NAMES_PARAM, args.joint_names);
   return true;
 }
 
@@ -75,32 +75,29 @@ int main(int argc, char** argv)
   }
 
   URFactory factory(args.host);
-  //RT packets
+  // RT packets
   auto rt_parser = factory.getRTParser();
   URStream rt_stream(args.host, UR_RT_PORT);
   URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
-  RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame);
-  EventCounter rt_ec;
-
+  RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
   URCommander rt_commander(rt_stream);
-  vector<IConsumer<RTPacket>*> rt_vec;
+  vector<IConsumer<RTPacket>*> rt_vec{ &rt_pub };
 
-  if(args.use_ros_control)
+  if (args.use_ros_control)
   {
+    LOG_INFO("ROS control enabled");
     rt_vec.push_back(new ROSController(rt_commander, args.joint_names));
   }
 
-  //rt_vec.push_back(&rt_pub);
-
   MultiConsumer<RTPacket> rt_cons(rt_vec);
   Pipeline<RTPacket> rt_pl(rt_prod, rt_cons);
 
-  //Message packets
+  // Message packets
   auto state_parser = factory.getStateParser();
   URStream state_stream(args.host, UR_SECONDARY_PORT);
   URProducer<StatePacket> state_prod(state_stream, *state_parser);
   MBPublisher state_pub;
-  vector<IConsumer<StatePacket>*> state_vec{&state_pub};
+  vector<IConsumer<StatePacket>*> state_vec{ &state_pub };
   MultiConsumer<StatePacket> state_cons(state_vec);
   Pipeline<StatePacket> state_pl(state_prod, state_cons);