Publish additional information

iiwa_driver/CMakeLists.txt

@@ -31,6 +31,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 # Find catkin macros and libraries
 find_package(catkin REQUIRED COMPONENTS
   roscpp
+  message_generation
   rospy
   std_msgs
   geometry_msgs
@@ -39,14 +40,26 @@ find_package(catkin REQUIRED COMPONENTS
   controller_manager
   sensor_msgs
   urdf
+  realtime_tools
 )
 
 find_package(FRI REQUIRED COMPONENTS
     kuka_fri
 )
 
+# Message files
+add_message_files(
+  FILES
+  AdditionalOutputs.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
 # Needed for ros packages
-catkin_package(CATKIN_DEPENDS roscpp geometry_msgs tf std_msgs sensor_msgs hardware_interface controller_manager urdf)
+catkin_package(CATKIN_DEPENDS roscpp message_runtime geometry_msgs tf std_msgs sensor_msgs hardware_interface controller_manager urdf realtime_tools)
 
 add_executable(iiwa_driver src/iiwa.cpp src/iiwa_driver.cpp)
 
@@ -58,6 +71,8 @@ target_include_directories(iiwa_driver PUBLIC include ${catkin_INCLUDE_DIRS} ${F
 target_link_libraries(iiwa_driver ${catkin_LIBRARIES} ${FRI_LIBRARIES})
 add_dependencies(iiwa_driver ${catkin_EXPORTED_TARGETS})
 
+add_dependencies(iiwa_driver iiwa_driver_generate_messages_cpp)
+
 
 install(TARGETS iiwa_driver
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

iiwa_driver/include/iiwa_driver/iiwa.h

@@ -29,6 +29,11 @@
 #include <ros/ros.h>
 #include <std_msgs/Bool.h>
 
+#include <realtime_tools/realtime_publisher.h>
+
+#include <iiwa_driver/AdditionalOutputs.h>
+#include <std_msgs/Float64MultiArray.h>
+
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
@@ -91,6 +96,9 @@ namespace iiwa_ros {
         void _publish();
         void _on_fri_state_change(kuka::fri::ESessionState old_state, kuka::fri::ESessionState current_state) {}
 
+        // External torque publisher
+        realtime_tools::RealtimePublisher<iiwa_driver::AdditionalOutputs> _additional_pub;
+
         // Interfaces
         hardware_interface::JointStateInterface _joint_state_interface;
         hardware_interface::PositionJointInterface _position_joint_interface;

iiwa_driver/msg/AdditionalOutputs.msg

@@ -0,0 +1,4 @@
+Header header
+std_msgs/Float64MultiArray external_torques
+std_msgs/Float64MultiArray commanded_torques
+std_msgs/Float64MultiArray commanded_positions

iiwa_driver/package.xml

@@ -43,6 +43,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roscpp</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>controller_manager</build_depend>
@@ -51,9 +52,10 @@
   <build_depend>tf</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>urdf</build_depend>
-
+  <build_depend>realtime_tools</build_depend>
 
   <run_depend>roscpp</run_depend>
+  <run_depend>message_runtime</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
@@ -62,7 +64,7 @@
   <run_depend>tf</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>urdf</run_depend>
-
+  <run_depend>realtime_tools</run_depend>
 
   <export>
   </export>

iiwa_driver/src/iiwa.cpp

@@ -186,6 +186,23 @@ namespace iiwa_ros {
         registerInterface(&_position_joint_interface);
         registerInterface(&_effort_joint_interface);
         registerInterface(&_velocity_joint_interface);
+
+        _additional_pub.init(_nh, "additional_outputs", 20);
+        _additional_pub.msg_.external_torques.layout.dim.resize(1);
+        _additional_pub.msg_.external_torques.layout.data_offset = 0;
+        _additional_pub.msg_.external_torques.layout.dim[0].size = _num_joints;
+        _additional_pub.msg_.external_torques.layout.dim[0].stride = 0;
+        _additional_pub.msg_.external_torques.data.resize(_num_joints);
+        _additional_pub.msg_.commanded_torques.layout.dim.resize(1);
+        _additional_pub.msg_.commanded_torques.layout.data_offset = 0;
+        _additional_pub.msg_.commanded_torques.layout.dim[0].size = _num_joints;
+        _additional_pub.msg_.commanded_torques.layout.dim[0].stride = 0;
+        _additional_pub.msg_.commanded_torques.data.resize(_num_joints);
+        _additional_pub.msg_.commanded_positions.layout.dim.resize(1);
+        _additional_pub.msg_.commanded_positions.layout.data_offset = 0;
+        _additional_pub.msg_.commanded_positions.layout.dim[0].size = _num_joints;
+        _additional_pub.msg_.commanded_positions.layout.dim[0].stride = 0;
+        _additional_pub.msg_.commanded_positions.data.resize(_num_joints);
     }
 
     void Iiwa::_ctrl_loop()
@@ -201,6 +218,17 @@ namespace iiwa_ros {
             _controller_manager->update(ros::Time::now(), elapsed_time);
             _write(elapsed_time);
 
+            // publish additional outputs
+            if (_additional_pub.trylock()) {
+                _additional_pub.msg_.header.stamp = ros::Time::now();
+                for (unsigned i = 0; i < _num_joints; i++) {
+                    _additional_pub.msg_.external_torques.data[i] = _robot_state.getExternalTorque()[i];
+                    _additional_pub.msg_.commanded_torques.data[i] = _robot_state.getCommandedTorque()[i];
+                    _additional_pub.msg_.commanded_positions.data[i] = _robot_state.getCommandedJointPosition()[i];
+                }
+                _additional_pub.unlockAndPublish();
+            }
+
             _publish();
             rate.sleep();
         }