Avoid publishing joint messages with duplicate stamps

openspur · Jan 9, 2024 · 3bb8a5d · 3bb8a5d
1 parent 32a36c1
commit 3bb8a5d
Showing 1 changed file with 10 additions and 1 deletion.
diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp
@@ -169,8 +169,10 @@ class YpspurRosNode
 
   int control_mode_;
 
+  bool avoid_publishing_duplicated_joints_;
   bool avoid_publishing_duplicated_odom_;
   bool publish_odom_tf_;
+  ros::Time previous_joints_stamp_;
   ros::Time previous_odom_stamp_;
 
   void cbControlMode(const ypspur_ros::ControlMode::ConstPtr& msg)
@@ -434,6 +436,7 @@ class YpspurRosNode
     , device_error_state_(0)
     , device_error_state_prev_(0)
     , device_error_state_time_(0)
+    , avoid_publishing_duplicated_joints_(true)
     , avoid_publishing_duplicated_odom_(true)
     , publish_odom_tf_(true)
   {
@@ -554,6 +557,7 @@ class YpspurRosNode
           nh_, "cmd_vel",
           pnh_, "cmd_vel", 1, &YpspurRosNode::cbCmdVel, this);
 
+      pnh_.param("avoid_publishing_duplicated_joints", avoid_publishing_duplicated_joints_, true);
       pnh_.param("avoid_publishing_duplicated_odom", avoid_publishing_duplicated_odom_, true);
       pnh_.param("publish_odom_tf", publish_odom_tf_, true);
     }
@@ -1027,7 +1031,12 @@ class YpspurRosNode
           }
           joint.header.stamp = ros::Time(t);
         }
-        pubs_["joint"].publish(joint);
+
+        if (!avoid_publishing_duplicated_joints_ || (joint.header.stamp > previous_joint_stamp_))
+        {
+          pubs_["joint"].publish(joint);
+        }
+
 
         for (unsigned int i = 0; i < joints_.size(); i++)
         {