New system plugin called DriveToPoseController

[sauk2]

🎉 New feature

Closes #2379

Summary

This PR adds a new system plugin to gz-sim called DriveToPoseController. This is not a standalone plugin and requires other systems plugins (DiffDrive and OdometryPublisher) to function.

This controller receives a target pose in the Gazebo coordinate system on /cmd_pose topic and calculates and publishes a command velocity on /cmd_vel topic to move the robot towards the target. This plugin contains separate linear and angular proportional controllers with adjustable gains. The user can also specify an allowable deviation from the goal pose in the form of linear and angular deviations. Upon reaching the goal pose, the controller publishes an acknowledgement on /reached_pose topic to indicate the end of the navigation operation.

The plugin needs to be attached to a model entity as in the SDF as follows,

    <model>
        <!-- Model information --> 
 
        <!-- Required to define DiffDrive and OdometryPublisher -->

        <!-- DriveToPoseController --> 
        <plugin
          filename="gz-sim-drive-to-pose-controller-system"
          name="gz::sim::systems::DriveToPoseController">
          <linear_p_gain>1.0</linear_p_gain>
          <angular_p_gain>2.0</angular_p_gain>
          <linear_deviation>0.1</linear_deviation>
          <angular_deviation>0.05</angular_deviation>
        </plugin>
    </model>

It has the following parameter tags,

• <linear_p_gain> - (Optional) Proportional gain for the linear velocity controller. Default: 1.0
• <angular_p_gain> - (Optional) Proportional gain for the angular velocity controller. Default: 2.0
• <linear_deviation> - (Optional) Allowable linear deviation (in meters) from the desired coordinate. Default: 0.1
• <angular_deviation> - (Optional) Allowable angular deviation (in radians) from the desired orientation. Default: 0.05

The following example warehouse world can be found at src/gz-sim/examples/worlds/drive_to_pose_controller.sdf with an AMR spawned at its start position.

Test it

The integration test for this plugin can be run using the following command,

  colcon test --event-handlers console_direct+ --merge-install \
    --packages-select gz-sim9 --ctest-args -R drive_to_pose_controller

Use the following steps to test the functionality in the warehouse world by sending the robot to four different poses,

1. Launch the following world,

  gz sim drive_to_pose_controller.sdf

NOTE: The world opens in a paused state so make sure to unpause before sending the pose commands

2. Send the robot to the first pose in the warehouse, [x, y, orientation] = [1.78, -9.4, -3.14]

  gz topic -t "/model/DeliveryBot/cmd_pose" -m gz.msgs.Pose_V \
    -p "pose:[{position: {x: 1.78, y: -9.4, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: -0.99, w: 0.0}}]"

3. Send the robot to the second pose, [x, y, orientation] = [-3.75, -9.4, 1.57]

  gz topic -t "/model/DeliveryBot/cmd_pose" -m gz.msgs.Pose_V \
    -p "pose:[{position: {x: -3.75, y: -9.4, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.707, w: 0.707}}]"

4. Send the robot to its third pose, [x, y, orientation] = [-3.75, 9.18, 3.14]

  gz topic -t "/model/DeliveryBot/cmd_pose" -m gz.msgs.Pose_V \
    -p "pose:[{position: {x: -3.75, y: 9.18, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.99, w: 0.0}}]"

5. Send the robot to its final pose, [x, y, orientation] = [-5.9, 9.18, 3.14]

  gz topic -t "/model/DeliveryBot/cmd_pose" -m gz.msgs.Pose_V \
    -p "pose:[{position: {x: -5.9, y: 9.18, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.99, w: 0.0}}]"

The entire operation is shown in the following video,

drive_to_pose_controller_test.mp4

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[ahcorde]

maybe we should add here some gzdbg or gzwarm to show which are the values of the gains

[sauk2]

Sounds good. I am now printing all the parameter values through gzdbg

[ahcorde]

where is this used? and if this is used somewhere, you should include <chrono>

[sauk2]

Thanks for all the feedback! I have added the necessary includes

This is used in the following line where I am using the ns operator

server.SetUpdatePeriod(0ns);

[arjo129]

Thanks for the contribution. Here is some feedback.

[arjo129]

Same thing for subscribers, we can detect if there are publishers. If no publishers inform the user that we need to load the correct set of plugins. (We could also automatically load them if that is something you are willing to look into).

[sauk2]

@arjo129 Thanks for all the feedback! It makes perfect sense to warn users if no publishers or subscribers are found. I have made the changes to check for the availability of necessary publishers and subscribers and warn users to load the required plugins.

However, I agree it would be a good idea to load the plugins ourselves if they are found to be missing, and I would be happy to implement this. If you have any references for this that would be great help. Also, do you want this change to be a part of this PR?

[arjo129]

Of the top of my head there is a service to load plugins:

gz-sim/src/gui/plugins/environment_loader/EnvironmentLoader.cc

Line 167 in af92e29

msgs::EntityPlugin_V req;

Since the system is running inside the server it may be worth exposing an API for doing this via the ECS instead of relying on gz-transport for later.

[ahcorde]

Some minor changes, otherwise look good to me

[sauk2]

Some minor changes, otherwise look good to me

Thank you for the feedback! I have made the requested changes.