added Publisher for tool data

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -39,6 +39,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <ur_msgs/IOStates.h>
+#include <ur_msgs/ToolDataMsg.h>
 
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
@@ -98,6 +99,7 @@ protected:
   void publishPose();
 
   void publishIOData();
+  void publishToolData();
 
   bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
@@ -130,6 +132,12 @@ protected:
   std::array<double, 2> standard_analog_input_;
   std::array<double, 2> standard_analog_output_;
   std::bitset<4> analog_io_types_;
+  uint32_t tool_mode_;
+  std::bitset<2> tool_analog_input_types_;
+  std::array<double, 2> tool_analog_input_;
+  int32_t tool_output_voltage_;
+  double tool_output_current_;
+  double tool_temperature_;
   tf2::Vector3 tcp_force_;
   tf2::Vector3 tcp_torque_;
   geometry_msgs::TransformStamped tcp_transform_;
@@ -140,6 +148,7 @@ protected:
 
   std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::IOStates>> io_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
 
   uint32_t runtime_state_;
   bool position_controller_running_;

ur_rtde_driver/resources/rtde_recipe.txt

@@ -13,3 +13,10 @@ standard_analog_input1
 standard_analog_output0
 standard_analog_output1
 analog_io_types
+tool_mode
+tool_analog_input_types
+tool_analog_input0
+tool_analog_input1
+tool_output_voltage
+tool_output_current
+tool_temperature

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -208,6 +208,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   {
     io_pub_->msg_.analog_out_states[i].pin = i;
   }
+  tool_data_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>(robot_hw_nh, "tool_data", 1));
 
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
 
@@ -256,11 +257,19 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
     readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]);
     readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]);
     readData(data_pkg, "standard_analog_output1", standard_analog_output_[1]);
+    readData(data_pkg, "tool_mode", tool_mode_);
+    readData(data_pkg, "tool_analog_input0", tool_analog_input_[0]);
+    readData(data_pkg, "tool_analog_input1", tool_analog_input_[1]);
+    readData(data_pkg, "tool_output_voltage", tool_output_voltage_);
+    readData(data_pkg, "tool_output_current", tool_output_current_);
+    readData(data_pkg, "tool_temperature", tool_temperature_);
     readBitsetData<uint64_t>(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_);
     readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
     readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
+    readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
 
     publishIOData();
+    publishToolData();
 
     // Transform fts measurements to tool frame
     extractToolPose(time);
@@ -479,12 +488,30 @@ void HardwareInterface::publishIOData()
         io_pub_->msg_.analog_out_states[i].state = standard_analog_output_[i];
       }
       // TODO: Handle analog domain
-      // TODO: Handle tool IO
       io_pub_->unlockAndPublish();
     }
   }
 }
 
+void HardwareInterface::publishToolData()
+{
+  if (tool_data_pub_)
+  {
+    if (tool_data_pub_->trylock())
+    {
+      tool_data_pub_->msg_.tool_mode = tool_mode_;
+      tool_data_pub_->msg_.analog_input_range2 = tool_analog_input_types_[0];
+      tool_data_pub_->msg_.analog_input_range3 = tool_analog_input_types_[1];
+      tool_data_pub_->msg_.analog_input2 = tool_analog_input_[0];
+      tool_data_pub_->msg_.analog_input2 = tool_analog_input_[1];
+      tool_data_pub_->msg_.tool_output_voltage = tool_output_voltage_;
+      tool_data_pub_->msg_.tool_current = tool_output_current_;
+      tool_data_pub_->msg_.tool_temperature = tool_temperature_;
+      tool_data_pub_->unlockAndPublish();
+    }
+  }
+}
+
 bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
 {
   if (isRobotProgramRunning())