Break deadlock

ros-controls · May 12, 2023 · eac9765 · eac9765
1 parent 6d17547
commit eac9765
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -177,7 +177,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // Timeout to consider commands old
   std::chrono::milliseconds cmd_timeout_{500};
   // save the timestamp when the last msg was received
-  rclcpp::Time last_msg_received_;
+  rclcpp::Time last_msg_received_{0};
 
   // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
   bool subscriber_is_active_ = false;

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -177,7 +177,7 @@ controller_interface::return_type JointTrajectoryController::update(
     // is timeout configured?
     if (params_.cmd_timeout > 0.0)
     {
-      if (time - last_msg_received_ > cmd_timeout_)
+      if (last_msg_received_.seconds() > 0 && (time - last_msg_received_ > cmd_timeout_))
       {
         RCLCPP_WARN(get_node()->get_logger(), "Aborted due to timeout");
         set_hold_position();
@@ -1410,6 +1410,9 @@ void JointTrajectoryController::set_hold_position()
 
   auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(empty_msg);
   add_new_trajectory_msg(traj_msg);
+
+  // otherwise we will end up in a loop after timeout
+  last_msg_received_ = rclcpp::Time(0, 0, last_msg_received_.get_clock_type());
 }
 
 bool JointTrajectoryController::contains_interface_type(