Skip to content

Commit

Permalink
Fix uncaught exception in joint_state_controller
Browse files Browse the repository at this point in the history
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".
  • Loading branch information
rhaschke committed Feb 27, 2024
1 parent 67b0e72 commit a83f841
Showing 1 changed file with 5 additions and 1 deletion.
6 changes: 5 additions & 1 deletion joint_state_controller/src/joint_state_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Expand Down

0 comments on commit a83f841

Please sign in to comment.