Add prefix to wrench hw interface (UniversalRobots#217)

Use a parameter to set the wrench name

This name will be picked up by the `force_torque_sensor_controller` in order to name the respective topic.

Co-authored-by: carebare47 <[email protected]>

Author: carebare47

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -58,6 +58,10 @@ Controllers that are initally loaded, but not started.
 
 tf_prefix used for the robot.
 
+##### wrench_frame_id (default: "wrench")
+
+Parameter to set the id of the wrench frame, required if using multiple robots
+
 ##### tool_baud_rate (default: "115200")
 
 Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -69,6 +69,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } };
   joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
   std::string script_filename;
+  std::string wrench_frame_id;
   std::string output_recipe_filename;
   std::string input_recipe_filename;
 
@@ -89,6 +90,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // to publish correct frame names for frames reported by the robot directly.
   robot_hw_nh.param<std::string>("tf_prefix", tf_prefix_, "");
 
+  // Optional parameter to change the id of the wrench frame
+  robot_hw_nh.param<std::string>("wrench_frame_id", wrench_frame_id, "wrench");
+
   // Path to the urscript code that will be sent to the robot.
   if (!robot_hw_nh.getParam("script_file", script_filename))
   {
@@ -309,7 +313,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
       ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
 
   fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle(
-      "wrench", tf_prefix_ + "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3));
+      wrench_frame_id, tf_prefix_ + "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3));
 
   robot_status_interface_.registerHandle(industrial_robot_status_interface::IndustrialRobotStatusHandle(
       "industrial_robot_status_handle", robot_status_resource_));