Squashed commits:
* Avoid updating ros_control controllers on pipeline timeout
* Revert "Avoid updating ros_control controllers on pipeline timeout"
* Do not update controller_manager until RTPackets have been received
* Mark handles as initialized after actually doing it rather than just before
* Improve readability
Low Bandwidth Trajectory Follower has now much less comments
and debugging information - which means that it will parse on UR robot
in around 400 ms instead of 700 ms. It also has an adjustment loop
in case the robot does not reach any of the trajectory points specified.
It will catch-up at every trajectory point sent by MoveIt and will try
to get as close as possible to the desired pointi (it will check if all
joints are within MAX_JOINT_DIFFERENCE from the desired positions)
Safe Trajectory Follower implements different approach for controlling
the robot. Rather than calculate the interpolation steps in the driver
and send the small interpolated steps over the network to the URScript
program with 500Hz frequency, the coarser MoveIt trajectory is sent
(with few Hz) and the interpolation steps are calculated by the
URScript.
The algorithm for time progress has also built-in protection against
any delays induced by load on the driver, network or URControl - it
will never "catch-up" dangerously when such delay are introduced,
It will rather pause and wait for the next small interpolation step
instructions and re-start the move slower - never skipping any
interpolated steps.
Those changes make Safe Trajectory Follower much more resilient to
network communication problems and removes any superficial requirements
for the network setup, kernel latency and no-load-requirement for the
driver's PC - making it much more suitable for research, development
and quick iteration loops. It works reliably even over WiFi.
* Re-added UR script - for custom UR Script execution
* Restarting the driver when robot closes the connection on script error.
The pipelines work in the way that if the connection is
is closed by the control PC, it will not be re-established. This
happens for example if you use the URScript topic and upload
script that does not compile. The robot will then close the
connection, the pipeline will close and any subsequent
uploads will fail and noone realises there is a problem.
While we could re-establish the connection, I think much better
solution is to shutdown the driver in such case. This is much more
resilient behaviour as it will clean up any inconsistent driver state.
We can utilise "respawn" feature of ROS launch and restart such
driver automatically (launch files are updated as part of that change).
On top of "production" stability, it allows for much nicer development
workflow - you can use URScript topic for development of new scripts
and have the driver restart every time you make mistake.
Without it, any mistake requires restarting the driver manually.
* Find matching hardware_interface using the required type
The name of the controller was used in order to find and start
the matching hardware interface.
In consequence this meant that one could only define one controller
for each hardware interface.
Now, the controller's required type of hardware interface is used
to find and start the matching hardware interface.
* separate read & update in controller
consume is defined as read+update, but update
does not include read in ros_control terminology.
* Handle latency in pipeline loop
The controllers need to update at a rate of *at least* 125Hz,
but the wait_dequeue_timed call could in theory slow the loop down to 62.5Hz.
The old ur_modern_driver worked around this problem by sending goals
at 4*125Hz.
This patch exploits the onTimeout method of a consumer to update with
the specified frequency of the control loop, even if no new state message
arrived after the previous command.
* publish wrench w.r.t. tcp frame
The messages had an empty frame_id before and could not be displayed in RViz
* support ros_control in indigo
Maintains default behavior of indigo that no controller activation is required.
Enabling required activation can be done by passing 'require_activation' as Always/OnStartup
to the ur_common.launch or by modifying corresponding launch files.
