[Proposal] Drive to point/configuration controller plugin for wheeled vehicles

[sauk2]

Desired behavior

Currently, Gazebo provides plugins for wheeled vehicles/robots (DiffDrive, AckermannSteering) that work using /cmd_vel and can be integrated with velocity-based controllers (VelocityControl) for precise movement. However, this may not always work for use cases requiring movement to a specific position on the global map. A position controller plugin for wheeled vehicles would address this gap, making it convenient to simulate point-to-point movement without the need to write external controllers or use other packages.

Alternatives considered

• Using existing velocity-based controllers
• Implementing controllers within ROS and sending commands over ros-gz-bridge

Implementation suggestion

Control problem

Note: The implementation follows the ideas outlined in Chapter 4 (pp. 130-141) of Corke et. al.[1]

The vehicle uses a bicycle model to describe its behaviour.

Figure 1: Bicycle model of a wheeled vehicle [1]

The following equations describe the control of a vehicle moving towards a goal $(x^{*}, y^{*})$ from its current position $(x, y)$

The linear error is minimised using the following equation,

$$v^{*} = K_v \sqrt{ (x^{*} - x)^2 + (y^{*} - y)^2 }$$

Angular error (in radians) is minimised using the following equations. The result should lie in the interval $[-\pi, \pi)$

$$\theta^{*} = atan2 \left( \frac{y^{*} - y}{x^{*} - x} \right)$$

$$\psi = K_h ( \theta^{*} - \theta )$$

Plugin structure

• The plugin is attached to a model and should be used with DiffDrive and OdometryPublisher plugins.
• It contains a callback to receive a position command and convert it to linear and angular velocities to be published on /cmd_vel (subscribed by DiffDrive)
• The SDF is used to define linear P-gain, angular P-gain and allowable linear deviation that will be extracted during plugin initialisation.
• The plugin implements the Configure function to subscribe to the position command topic and create a publisher for velocity messages.
• The PostUpdate callback contains the logic described by the equations. The heading and linear errors are calculated and the velocity is published to /cmd_vel.
• This is carried out by comparing the current position against the target position to calculate the linear and angular velocity commands.
• A zero velocity command is published once the vehicle reaches the commanded position or falls inside the linear deviation.

Additional context

Result

Implementation of a simple 'drive to point' controller tested on the model used in the DiffDrive example.

https://github.com/gazebosim/gz-sim/assets/75178156/83d0afc1-1a45-45d4-a9a7-c9b77e28fd53

References

[1] Corke, P., Jachimczyk, W., Pillat, R. (2023). Mobile Robot Vehicles. In: Robotics, Vision and Control. Springer Tracts in Advanced Robotics, vol 147. Springer, Cham. https://doi.org/10.1007/978-3-031-07262-8_4

[azeey]

@sauk2 are you interested in implementing this yourself?

[sauk2]

@sauk2 are you interested in implementing this yourself?

@azeey Yes, I can work on this implementation.

[azeey]

Thanks! I've assigned it to you.