made changes to get effort feedback in joint_states accourding to Uni…

…versalRobots#146 (comment). Got ERROR: Could not get fresh data package from robot
gtatiya · Apr 16, 2020 · dbfa638 · dbfa638
1 parent fcc4ae3
commit dbfa638
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 1 deletion.
diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h
@@ -242,7 +242,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::vector<std::string> joint_names_;
   int32_t robot_mode_;
   int32_t safety_mode_;
-
+  std::array<double, 6> output_double_register_;
+
   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_;

diff --git a/ur_robot_driver/resources/ros_control.urscript b/ur_robot_driver/resources/ros_control.urscript
@@ -93,8 +93,23 @@ thread speedThread():
   stopj(5.0)
 end
 
+thread torqueThread():
+  while True:
+    torques = get_joint_torques()
+    write_output_float_register(4, torques[0])
+    write_output_float_register(5, torques[1])
+    write_output_float_register(6, torques[2])
+    write_output_float_register(7, torques[3])
+    write_output_float_register(8, torques[4])
+    write_output_float_register(9, torques[5])
+    sync()
+  end
+end
+
 socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
+torque_thread = run torqueThread()
+
 control_mode = MODE_UNINITIALIZED
 thread_move = 0
 global keepalive = -2
@@ -132,5 +147,6 @@ end
 textmsg("Stopping communication and control")
 control_mode = MODE_STOPPED
 join thread_move
+kill torque_thread
 textmsg("All threads ended")
 socket_close("reverse_socket")
diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt
@@ -22,3 +22,9 @@ tool_output_current
 tool_temperature
 robot_mode
 safety_mode
+output_double_register_24
+output_double_register_25
+output_double_register_26
+output_double_register_27
+output_double_register_28
+output_double_register_29
diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp
@@ -413,6 +413,12 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
     readData(data_pkg, "tool_temperature", tool_temperature_);
     readData(data_pkg, "robot_mode", robot_mode_);
     readData(data_pkg, "safety_mode", safety_mode_);
+    readData(data_pkg, "output_double_register_24", output_double_register_[0]);
+    readData(data_pkg, "output_double_register_25", output_double_register_[1]);
+    readData(data_pkg, "output_double_register_26", output_double_register_[2]);
+    readData(data_pkg, "output_double_register_27", output_double_register_[3]);
+    readData(data_pkg, "output_double_register_28", output_double_register_[4]);
+    readData(data_pkg, "output_double_register_29", output_double_register_[5]);
     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_);
@@ -705,6 +711,12 @@ void HardwareInterface::publishToolData()
       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_->msg_.output_double_register_24 = output_double_register_[0];
+      tool_data_pub_->msg_.output_double_register_25 = output_double_register_[1];
+      tool_data_pub_->msg_.output_double_register_26 = output_double_register_[2];
+      tool_data_pub_->msg_.output_double_register_27 = output_double_register_[3];
+      tool_data_pub_->msg_.output_double_register_28 = output_double_register_[4];
+      tool_data_pub_->msg_.output_double_register_29 = output_double_register_[5];
       tool_data_pub_->unlockAndPublish();
     }
   }