publish wrench w.r.t. tcp frame

The messages had an empty frame_id before and could not be displayed in RViz
Zagitta · Dec 7, 2017 · 0276987 · 0276987
1 parent eb92f43
commit 0276987
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 8 deletions.
diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h
@@ -55,7 +55,7 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons
 
 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()
   {
   }

diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/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;
-};
+};
diff --git a/src/ros/controller.cpp b/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)
 {

diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp
@@ -19,9 +19,9 @@ void JointInterface::update(RTShared &packet)
 
 
 const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface";
-WrenchInterface::WrenchInterface()
+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)

diff --git a/src/ros_main.cpp b/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);
   }