controller_manager_->update uses a new timepoint

[ThomasTimm]

In the control loop in https://github.com/davetcoleman/ros_control_boilerplate/blob/jade-devel/src/generic_hw_control_loop.cpp#L69 a timestamp is fetched in line 72 with clock_gettime(), but when you call the update function in line 92, a new timestamp is used.

The read-function in-between on line 89 is not realtime safe, so you risk an arbitrary big difference between these two timestamps.
Would it not be more correct to reuse the current_time timestamp in the call to update, especially if the duration of the read-call is varying?

[davetcoleman]

yea, that's a really good point!

[davetcoleman]

this compiles but could you check if it fixes the issue? thanks!

[ThomasTimm]

Intuitively I would think that it works, and it is also how I've done it in my ur_modern_driver ( https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_ros_wrapper.cpp#L573 )

But apparently, and that is why I've raised this issue, that solution gave someone problems ( ros-industrial-attic/ur_modern_driver#44 ). I don't know what system @nikhilkalige uses, but maybe the implicit conversion from the long current_time_.nsec to uint32_t ros_current_time.nsec is the culprint.
I'll try and ask him if an explicit typecast like ros::Time ros_current_time((uint32_t)current_time_.tv_sec, (uint32_t)current_time_.tv_nsec); also works in his case. Then, that would be the way to go.

An alternative solution would be to use ros::Time::now() instead of clock_gettime(CLOCK_MONOTONIC, &current_time_);, but that relies on CLOCK_REALTIME instead of CLOCK_MONOTONIC. Apparently, which to use in ROS is a hornets nest that I would rather not kick to hard ( ros/roscpp_core#33 )

[davetcoleman]

Interesting. Let me know - when I'm back on hardware (and not theory/simulations) I can test this PR myself but I'm not sure when that will be.

[gavanderhoorn]

Using CLOCK_REALTIME for this has bitten users of ros_control in the past, see Undefined behaviour at startup, shutdown fi. For ΔT you really don't want a clock that can change due to updates to the RTC.

[ThomasTimm]

So, I think @miguelprada figured out why someone has issues with the solution I'm using in the ur_modern_driver and which you also suggested.

CLOCK_MONOTONIC and CLOCK_REALTIME does not reference the same point in time, so when the absolute value acquired with CLOCK_MONOTONIC is converted to a ros::Time value, that timestamp is way off.

I'm not sure I understand exactly how the two timing values for the controller->update() call is used (which is partly why I raised this issue), but I am guessing they don't need to be based off of the same time scale.
The time value is used later in the control chain, so that needs to be based on CLOCK_REALTIME ( ros::Time::now() )
The elapsed_time_, or the period, needs to be based on CLOCK_MONOTONIC as per what @gavanderhoorn is saying.

The solution is thus pretty simple; Call clock_gettime(CLOCK_MONOTONIC, &current_time); and then in the next line set the ros time with ros_current_time = ros::Time::now(); This is not perfect, but I don't see any better way of doing it, and it does away with any problems of a non-deterministic call to read.
Also, I would assume that the duration of the read-call should be reflected in the period value used in the update-call, so wouldn't the ideal solution be:

// Input 
hardware_interface_->read(); 

// Control 
clock_gettime(CLOCK_MONOTONIC, &current_time); 
elapsed_time = ros::Duration( current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)   / BILLION); 
controller_manager_->update( ros::Time::now(), elapsed_time); 
last_time = current_time; 

// Output 
hardware_interface_->write(); 

[davetcoleman]

I've written some hardware_interface drivers before that also use the elapsed_time variable within the read() command, so many only feedback would be to move the time code above the read() line.

[davetcoleman]

I've updated the PR to simply save the ros::time for use in the update loop...

#5

[ThomasTimm]

I didn't think about the ´read´ command needing the elapsed_time. Then I guess that PR should solve this issue.