New system plugin called `DriveToPoseController` #2679

sauk2 · 2024-11-17T09:39:55Z

🎉 New feature

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,

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

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}}]"

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}}]"

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}}]"

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.

Signed-off-by: Saurabh Kamat <[email protected]>

src/systems/drive_to_pose_controller/DriveToPoseController.cc

test/integration/drive_to_pose_controller_system.cc

arjo129

Thanks for the contribution. Here is some feedback.

src/systems/drive_to_pose_controller/DriveToPoseController.cc

arjo129 · 2024-11-18T11:59:00Z

src/systems/drive_to_pose_controller/DriveToPoseController.cc

+    topicNamespace + "/cmd_pose", &DriveToPoseControllerPrivate::OnCmdPose,
+    this->dataPtr.get());
+
+  this->dataPtr->node.Subscribe(


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).

@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?

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.

@sauk2 can you create an issue to add this functionality in follow up PRs.

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.

Yes, we currently have the possibility to load plugins with the LoadSdfPlugins event (

gz-sim/include/gz/sim/Events.hh

Line 61 in 13bf5be

using LoadSdfPlugins = common::EventT<void(Entity, sdf::Plugins),

) but I don't think we have an API for introspecting what plugins are currently available.

Ideally, we would create a new component that holds the list of plugins attached to an entity. Or, maybe better would be to create entities for each plugin and use the ParentEntity component to point to the entity they're attached to. The latter would be my preference.

Created a new issue (#2773) for this discussion.

Signed-off-by: Saurabh Kamat <[email protected]>

ahcorde

Some minor changes, otherwise look good to me

src/systems/drive_to_pose_controller/DriveToPoseController.hh

test/integration/drive_to_pose_controller_system.cc

examples/worlds/drive_to_pose_controller.sdf

…tests Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 · 2024-12-17T13:36:41Z

Some minor changes, otherwise look good to me

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

Signed-off-by: Saurabh Kamat <[email protected]>

azeey · 2025-01-07T21:55:42Z

@ahcorde , @arjo129 mind taking another look?

azeey · 2025-01-08T16:53:21Z

Tests are failing on windows because DART is missing in our windows CI.
Test Result (4 failures / +4)

Can you disable these tests on windows?

Signed-off-by: Saurabh Kamat <[email protected]>

j-rivero · 2025-02-12T17:06:53Z

Can you disable these tests on windows?

Time has passed we back to have DART for Windows. I have reverted the commit in a local branch here and launched a new Windows PR test:

j-rivero · 2025-02-13T11:39:24Z

Time has passed we back to have DART for Windows. I have reverted the commit in a local branch here and launched a new Windows PR test:

Passed !

109: (2025-02-12 18:50:33.733) [info] [SceneBroadcaster.cc:711] Publishing dynamic pose messages on [/world/drive_to_pose_controller/dynamic_pose/info]
109: (2025-02-12 18:50:34.269) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (0)
109: (2025-02-12 18:50:34.269) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (4)
109: (2025-02-12 18:50:34.269) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (1)
109: (2025-02-12 18:50:34.269) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (3)
109: (2025-02-12 18:50:34.269) [debug] [SimulationRunner.cc:569] Exiting postupdate worker thread (2)
109: [       OK ] DriveToPoseControllerTest.XYCoordinateYawPublish (759 ms)
109: [----------] 5 tests from DriveToPoseControllerTest (5687 ms total)
109: 
109: [----------] Global test environment tear-down
109: [==========] 5 tests from 1 test suite ran. (5687 ms total)
109: [  PASSED  ] 5 tests.
109/252 Test #109: INTEGRATION_drive_to_pose_controller_system ............   Passed    5.79 sec

@sauk2 could you revert a0901ee ?

Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 · 2025-02-13T12:58:42Z

@sauk2 could you revert a0901ee ?

@j-rivero Thank you for the update! I have reverted that commit

j-rivero

Confirmed that it works as expected ! Congrats @sauk2

We need to find some ways to give this a bit of publicity. Some ideas:

Post in Gazebo community forum. You can reuse mainly what you wrote for this PR.
Write a new PR for adding a tutorial in https://gazebosim.org/api/sim/8/tutorials.html

sauk2 · 2025-02-24T13:58:11Z

Confirmed that it works as expected ! Congrats @sauk2

We need to find some ways to give this a bit of publicity. Some ideas:

Post in Gazebo community forum. You can reuse mainly what you wrote for this PR.

Write a new PR for adding a tutorial in https://gazebosim.org/api/sim/8/tutorials.html

Thanks Jose, for testing it out. I will work on contributing a tutorial soon!

elmeripk · 2025-06-05T11:49:41Z

Will this be ported to Harmonic at some point? This would provide a nice alternative to nav2 in simple use cases/demos.

j-rivero · 2025-06-05T16:43:48Z

Will this be ported to Harmonic at some point? This would provide a nice alternative to nav2 in simple use cases/demos.

@Mergifyio backport gz-sim8

It was a really nice contribution indeed!

New system plugin to gz-sim called DriveToPoseController: 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. --------- Signed-off-by: Saurabh Kamat <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>

j-rivero · 2025-06-06T11:33:14Z

@Mergifyio backport gz-sim8

Stupid bot.

@elmeripk could you please check if #2939 works for you on Harmonic?

sauk2 added 8 commits November 12, 2024 02:49

Added drive to pose controller

ed884dc

Signed-off-by: Saurabh Kamat <[email protected]>

Added world for drive to pose controller and updated cmake

c77c9c5

Signed-off-by: Saurabh Kamat <[email protected]>

Added test for drive_to_pose_controller

c2d0d61

Signed-off-by: Saurabh Kamat <[email protected]>

Updated world name

59d4056

Signed-off-by: Saurabh Kamat <[email protected]>

Added newline

1892486

Signed-off-by: Saurabh Kamat <[email protected]>

Added copyright

bfdb421

Signed-off-by: Saurabh Kamat <[email protected]>

Styling changes

894543b

Signed-off-by: Saurabh Kamat <[email protected]>

Missing include for lint

8478e28

Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 requested a review from mjcarroll as a code owner November 17, 2024 09:39

github-actions bot added 🏛️ ionic Gazebo Ionic 🪵 jetty Gazebo Jetty labels Nov 17, 2024

Fixed typo in copyright

3f28d90

Signed-off-by: Saurabh Kamat <[email protected]>

ahcorde requested changes Nov 18, 2024

View reviewed changes

arjo129 reviewed Nov 18, 2024

View reviewed changes

Added warnings when correct set of plugins are not loaded

c00bbdd

Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 requested review from ahcorde and arjo129 November 21, 2024 11:29

Fixed styling errors

c10acbd

Signed-off-by: Saurabh Kamat <[email protected]>

ahcorde requested changes Dec 17, 2024

View reviewed changes

Removed unnecessary line breaks in example and fixed folder paths in …

410a510

…tests Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 requested a review from ahcorde December 17, 2024 13:37

Fixed end of line error in pre-commit

55b8709

Signed-off-by: Saurabh Kamat <[email protected]>

ahcorde approved these changes Jan 8, 2025

View reviewed changes

Merge branch 'gz-sim9' into drive-to-pose-controller

9e7ae26

sauk2 and others added 3 commits January 9, 2025 01:33

Disabled failing tests on windows

a0901ee

Signed-off-by: Saurabh Kamat <[email protected]>

Merge branch 'gz-sim9' into drive-to-pose-controller

cbdaf93

Merge branch 'gz-sim9' into drive-to-pose-controller

1a1ee35

Adding back windows test for drive_to_pose_controller

a78bc97

Signed-off-by: Saurabh Kamat <[email protected]>

sauk2 mentioned this pull request Feb 13, 2025

Automatically load required plugins for DriveToPoseController #2773

Open

sauk2 requested a review from azeey February 13, 2025 14:19

j-rivero approved these changes Feb 19, 2025

View reviewed changes

j-rivero merged commit 54830b9 into gazebosim:gz-sim9 Feb 19, 2025
11 checks passed

sauk2 deleted the drive-to-pose-controller branch February 24, 2025 13:56

j-rivero mentioned this pull request Jun 6, 2025

New system plugin called DriveToPoseController (backport #2679) #2939

Open

New system plugin called DriveToPoseController #2679

New system plugin called DriveToPoseController #2679

Uh oh!

Conversation

sauk2 commented Nov 17, 2024

🎉 New feature

Summary

Test it

Checklist

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

arjo129 left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

arjo129 Nov 18, 2024

Choose a reason for hiding this comment

Uh oh!

sauk2 Nov 21, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

arjo129 Nov 21, 2024

Choose a reason for hiding this comment

Uh oh!

azeey Feb 5, 2025

Choose a reason for hiding this comment

Uh oh!

sauk2 Feb 13, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ahcorde left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sauk2 commented Dec 17, 2024

Uh oh!

azeey commented Jan 7, 2025

Uh oh!

azeey commented Jan 8, 2025

Uh oh!

j-rivero commented Feb 12, 2025

Uh oh!

j-rivero commented Feb 13, 2025

Uh oh!

sauk2 commented Feb 13, 2025

Uh oh!

j-rivero left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

sauk2 commented Feb 24, 2025

Uh oh!

elmeripk commented Jun 5, 2025

Uh oh!

j-rivero commented Jun 5, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

j-rivero commented Jun 6, 2025

Uh oh!

New system plugin called `DriveToPoseController` #2679

New system plugin called `DriveToPoseController` #2679

arjo129 left a comment •

edited

Loading

sauk2 Nov 21, 2024 •

edited

Loading

sauk2 Feb 13, 2025 •

edited

Loading

j-rivero commented Jun 5, 2025 •

edited

Loading