Merge pull request #48 from matthias-mayr/pr_realtime_publisher

Feature: Makes commanding_status_pub realtime compliant.
epfl-lasa · Nov 14, 2022 · 3d7b4eb · 3d7b4eb
2 parents 6baddce + 178f389
commit 3d7b4eb
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 17 deletions.
diff --git a/iiwa_driver/include/iiwa_driver/iiwa.h b/iiwa_driver/include/iiwa_driver/iiwa.h
@@ -33,6 +33,7 @@
 
 #include <iiwa_driver/AdditionalOutputs.h>
 #include <std_msgs/Float64MultiArray.h>
+#include <std_msgs/Bool.h>
 
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
@@ -96,8 +97,9 @@ namespace iiwa_ros {
         void _publish();
         void _on_fri_state_change(kuka::fri::ESessionState old_state, kuka::fri::ESessionState current_state) {}
 
-        // External torque publisher
+        // External torque and commanding status publishers
         realtime_tools::RealtimePublisher<iiwa_driver::AdditionalOutputs> _additional_pub;
+        realtime_tools::RealtimePublisher<std_msgs::Bool> _commanding_status_pub;
 
         // Interfaces
         hardware_interface::JointStateInterface _joint_state_interface;
@@ -142,7 +144,6 @@ namespace iiwa_ros {
         ros::NodeHandle _nh;
         std::string _robot_description;
         ros::Duration _control_period;
-        ros::Publisher _commanding_status_pub;
         double _control_freq;
         bool _initialized;
     };

diff --git a/iiwa_driver/src/iiwa.cpp b/iiwa_driver/src/iiwa.cpp
@@ -83,7 +83,6 @@ namespace iiwa_ros {
         _nh = nh;
         _load_params(); // load parameters
         _init(); // initialize
-        _commanding_status_pub = _nh.advertise<std_msgs::Bool>("commanding_status", 100);
         _controller_manager.reset(new controller_manager::ControllerManager(this, _nh));
 
         if (has_realtime_kernel()) {
@@ -225,6 +224,8 @@ namespace iiwa_ros {
         registerInterface(&_effort_joint_interface);
         registerInterface(&_velocity_joint_interface);
 
+        _commanding_status_pub.init(_nh, "commanding_status", 100);
+
         _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;
@@ -255,26 +256,28 @@ 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();
         }
     }
 
     void Iiwa::_publish()
     {
-        std_msgs::Bool msg;
-        msg.data = _commanding;
-        _commanding_status_pub.publish(msg);
+        // publish commanding status
+        if (_commanding_status_pub.trylock()) {
+            _commanding_status_pub.msg_.data = _commanding;
+            _commanding_status_pub.unlockAndPublish();
+        }
+
+        // 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();
+        }
     }
 
     void Iiwa::_load_params()