-
Notifications
You must be signed in to change notification settings - Fork 526
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Joint state controller throws exception when start time is less than update rate #628
Comments
This bug in the joint_state_controller was introduced by commit bba197e made on 2023-May-29.
This is bad advice. In a simulation, the current time does not necessarily advance at the same rate as this clock. |
@01binary Thank you for taking a look at this. It's unclear to me what commit bba197e was attempting to accomplish (since there's no discussion, and the commit message doesn't say.) In your patch, I don't see that it's useful to check for |
Hello, let's address your questions.
The commit you are referring to is linked to the following pull request: There is not much discussion in pull request comments, however that pull request in turn was trying to fix the following bug: This does have a detailed description and a discussion thread. The author asserts that the The author goes on to say that when an E-stop condition occurs and the last publish time is reset to the current time, then the rate-limiting logic in the update method will cause the controller to be never updated again, because
If
I built & tested my change, loading joint state controller for my robot (unfortunately I could not do this for Panda arm because it defines its own joint state controller and doesn't use this one). I confirmed the As for theory, you'll notice
The ros::Time class stores time like this:
The
Here's the
The implementation looks like this:
Since the underlying representation is just "seconds" and "nanoseconds", first it calculates the second by taking a floor of the double value and converting to a signed 64-bit integer. That will be Next it subtracts the seconds (which, let's say gets something like Finally it does this bit of magic:
You'll notice that In other words, the joint state controller constructor will cause the
So here's the proof that my changes work, besides executing them and verifying it all works when running:
I know this is long, but you asked for it! :) I need help with unit tests to complete this PR. The failing test is called publishOk. I don't have experience with ROS tests yet and the logic in that test is very messy. The gist of what it's trying to do is verify the controller publishes a specific number of times within a second that should reflect the publish rate in Hz, and that the delta of each update is equal to the publish rate. No idea how to fix yet, but will probably figure this out within a couple of days - help welcome. |
Had a busy few weeks, sorry. So the failing test, as I mentioned is called
I get the following output:
|
This was addressed by #630 which was merged. |
The joint state controller in
joint_state_controller.cpp
makes an assumption in itsJointStateController::starting(const ros::Time& time)
method that the time value passed to this function is always greater than its publish interval, which is set in Hz.If the
time
passed into this function is less than0.02
seconds for a50
Hz publish rate, for example, this will result inlast_publish_time
being negative after the subtraction, which would throw astd::runtime_error
exception causing the joint state controller to die:The exception is thrown because the time in ROS is represented as an unsigned value, which would cause it wrap to the maximum possible value of the number type if a negative value was set.
Proposed fix
If we calculate the publish interval...
...and compare to
time
before subtracting...Then we can set the
last_publish_time
to0
if the interval is larger thantime
, or set to their difference as before if thetime
is greater thaninterval
. I see that Dave Coleman also recommends setting last update time to this in ROS Controllers Boilerplate:I don't know what this means yet, will investigate if I should do this here instead of simply setting to
0
.Testing
I will submit a pull request for this issue, and test the joint state controller with Panda arm used in MoveIt tutorials to ensure everything is working as expected, and test results are not polluted by my own robot configuration which could be broken.
Hopefully setting last publish time to zero will not break anything.
You may have unit tests as well, I will see if I can add one.
The text was updated successfully, but these errors were encountered: