add flag controlling init of last_publish_time_

ros-controls · Oct 27, 2023 · bba197e · bba197e
1 parent 7df8be7
commit bba197e
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/joint_state_controller/include/joint_state_controller/joint_state_controller.h b/joint_state_controller/include/joint_state_controller/joint_state_controller.h
@@ -93,6 +93,7 @@ class JointStateController: public controller_interface::Controller<hardware_int
   ros::Time last_publish_time_;
   double publish_rate_;
   unsigned int num_hw_joints_; ///< Number of joints present in the JointStateInterface, excluding extra joints
+  bool pub_time_initialized_;
 
   void addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg);
 };

diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp
@@ -80,13 +80,18 @@ namespace joint_state_controller
     }
     addExtraJoints(controller_nh, realtime_pub_->msg_);
 
+    pub_time_initialized_=false;
+
     return true;
   }
 
   void JointStateController::starting(const ros::Time& time)
   {
-    // initialize time
-    last_publish_time_ = time;
+    // initialize time exactly once, to maintain publish rate through controller resets
+    if (!pub_time_initialized_)
+    {
+      last_publish_time_ = time - ros::Duration(1.001/publish_rate_); //ensure publish on first cycle
+    }
   }
 
   void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/)
@@ -132,7 +137,7 @@ namespace joint_state_controller
       ROS_ERROR("Extra joints specification is not an array. Ignoring.");
       return;
     }
-
+ 
     for(std::size_t i = 0; i < list.size(); ++i)
     {
       XmlRpc::XmlRpcValue& elem = list[i];