From a83f8413e09d57b49d44b6cbfa992fa2af997481 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 27 Feb 2024 20:33:36 +0100 Subject: [PATCH] Fix uncaught exception in joint_state_controller Using sim time in Gazebo, the starting time is close to zero, such that time - ros::Duration(1/publish_rate_) is negative. This causes an exception to be thrown: "Time is out of dual 32-bit range". --- joint_state_controller/src/joint_state_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp index 9fb23b0a3..ae48b0b41 100644 --- a/joint_state_controller/src/joint_state_controller.cpp +++ b/joint_state_controller/src/joint_state_controller.cpp @@ -90,7 +90,11 @@ namespace joint_state_controller // 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 + try { + last_publish_time_ = time - ros::Duration(1.001/publish_rate_); //ensure publish on first cycle + } catch(std::runtime_error& ex) { // negative ros::Time is not allowed + last_publish_time_ = ros::Time::MIN(); + } pub_time_initialized_ = true; } }