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)
     {