Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: gz-sim8
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
380 changes: 380 additions & 0 deletions examples/worlds/drive_to_pose_controller.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,380 @@
<?xml version='1.0'?>
<!--
Gazebo drive-to-pose-controller demo

DriveToPoseController plugin is a simple proportional controller that is attached to a model
entity to move it by giving a pose in Gazebo's world coordinate system. This is not a
standalone plugin, and requires the DiffDrive and OdometryPublisher plugins respectively.

The plugin has the following 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

Sending commands to the robot to navigate it through the industrial warehouse

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

NOTE: Each time the robot reaches the desired pose, it will send a pose acknowledgement
on the topic /model/<model_name>/reached_pose.

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

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

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

This plugin is loosely based on the control schemes introduced in Chapter 4 (pp. 130-142) of
Robotics, Vision and Control by Corke, Jachimczyk and Pillat and it can be found at:
https://link.springer.com/chapter/10.1007/978-3-031-07262-8_4

-->
<sdf version="1.9">
<world name="drive_to_pose_controller">

<gui fullscreen="0">
<plugin filename="MinimalScene" name="3D View">
<gz-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</gz-gui>

<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>7 9 14 0 0.78 -2.35</camera_pose>
</plugin>

<!-- Plugins that add functionality to the scene -->
<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
<gz-gui>
<property key="state" type="string">floating</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="GzSceneManager" name="Scene Manager">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="InteractiveViewControl" name="Interactive view control">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="SelectEntities" name="Select Entities">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="CameraTracking" name="Camera Tracking">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>

<!-- World control -->
<plugin filename="WorldControl" name="World control">
<gz-gui>
<title>World control</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">72</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="left" target="left"/>
<line own="bottom" target="bottom"/>
</anchors>
</gz-gui>
<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>
<use_event>true</use_event>
</plugin>

<!-- Inspector -->
<plugin filename="ComponentInspector" name="Component inspector">
<gz-gui>
<property type="string" key="state">docked</property>
</gz-gui>
</plugin>

<!-- Entity tree -->
<plugin filename="EntityTree" name="Entity tree">
<gz-gui>
<property type="string" key="state">docked</property>
</gz-gui>
</plugin>
</gui>

<gravity>0 0 -9.8</gravity>
<physics default="0" name="default_physics" type="ode">
<max_step_size>0.01</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>100</real_time_update_rate>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
Comment on lines +151 to +154
Copy link
Contributor

@sauk2 sauk2 Jun 6, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When I ran the example, Gazebo launched successfully and the UI loaded but the simulation area remained blank. I found that removing this plugin block from the world file resolved the issue. Not sure why this happened or if it was just an isolated issue on my end, but thought it was worth mentioning.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe this is due to the behavior in Harmonic for default plugins mentioned here. If that is the case it should actually be OK to not have the plugin defined in both Ionic and Harmonic, can you give that a quick try if possible? Then I guess a solution could be to:

  • Open a PR to gz-sim9 that removes the Physics plugin.
  • Backport both that PR and this to Harmonic.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, removing the <plugin> tag should do it. I'd recommend removing the <gui> tag as well unless you have a specific reason for not using the default gui config.


<model name="aws_robomaker_warehouse_ShelfF_01_001">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfF_01</uri>
</include>
<pose>-5.795143 -0.956635 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_WallB_01_001">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_WallB_01</uri>
</include>
</model>
<model name="aws_robomaker_warehouse_ShelfE_01_001">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01</uri>
</include>
<pose>4.73156 0.57943 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_ShelfE_01_002">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01</uri>
</include>
<pose>4.73156 -4.827049 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_ShelfE_01_003">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01</uri>
</include>
<pose>4.73156 -8.6651 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_ShelfD_01_001">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01</uri>
</include>
<pose>4.73156 -1.242668 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_ShelfD_01_002">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01</uri>
</include>
<pose>4.73156 -3.038551 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_ShelfD_01_003">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01</uri>
</include>
<pose>4.73156 -6.750542 0 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_GroundB_01_001">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_GroundB_01</uri>
</include>
<pose>0.0 0.0 -0.090092 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_Lamp_01_005">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Lamp_01</uri>
</include>
<pose>0 0 -4 0 0 0</pose>
</model>

<model name="aws_robomaker_warehouse_Bucket_01_020">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01</uri>
</include>
<pose>0.433449 9.631706 0 0 0 -1.563161</pose>
</model>

<model name="aws_robomaker_warehouse_Bucket_01_021">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01</uri>
</include>
<pose>-1.8321 -6.3752 0 0 0 -1.563161</pose>
</model>

<model name="aws_robomaker_warehouse_Bucket_01_022">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01</uri>
</include>
<pose>0.433449 8.59 0 0 0 -1.563161</pose>
</model>

<model name='aws_robomaker_warehouse_ClutteringA_01_016'>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01</uri>
</include>
<pose>5.708138 8.616844 -0.017477 0 0 0</pose>
</model>

<model name='aws_robomaker_warehouse_ClutteringA_01_017'>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01</uri>
</include>
<pose>3.408638 8.616844 -0.017477 0 0 0</pose>
</model>

<model name='aws_robomaker_warehouse_ClutteringA_01_018'>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01</uri>
</include>
<pose>-1.491287 5.222435 -0.017477 0 0 -1.583185</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_027">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>3.324959 3.822449 -0.012064 0 0 1.563871</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_028">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>5.54171 3.816475 -0.015663 0 0 -1.583191</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_029">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>5.384239 6.137154 0 0 0 3.150000</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_030">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>3.236 6.137154 0 0 0 3.150000</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_031">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>-1.573677 2.301994 -0.015663 0 0 -3.133191</pose>
</model>

<model name="aws_robomaker_warehouse_ClutteringC_01_032">
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01</uri>
</include>
<pose>-1.2196 9.407 -0.015663 0 0 1.563871</pose>
</model>

<model name='aws_robomaker_warehouse_ClutteringD_01_005'>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringD_01</uri>
</include>
<pose>-1.634682 -7.811813 -0.319559 0 0 0</pose>
</model>

<model name='aws_robomaker_warehouse_TrashCanC_01_002'>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_TrashCanC_01</uri>
</include>
<pose>-1.592441 7.715420 0 0 0 0</pose>
</model>

<light name="Warehouse_CeilingLight_003" type="point">
<pose>0 0 8.5 0 0 0</pose>
<diffuse>0.5 0.5 0.5 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>80</range>
<constant>0.3</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<cast_shadows>1</cast_shadows>
<direction>0.1 0.1 -1</direction>
</light>

<model name="DeliveryBot">
<include>
<pose>1.78 9.31 0.1 0 0 -1.57</pose>
<uri>
https://fuel.gazebosim.org/1.0/OpenRobotics/models/DeliveryBot
</uri>

<!-- Differential Drive -->
<plugin
filename="gz-sim-diff-drive-system"
name="gz::sim::systems::DiffDrive">
<left_joint>joint_tire_left</left_joint>
<right_joint>joint_tire_right</right_joint>
<wheel_separation>0.52</wheel_separation>
<wheel_radius>0.06137</wheel_radius>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>1.0</max_linear_velocity>
<min_linear_velocity>-1.0</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

<!-- Odometry Publisher -->
<plugin
filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
</plugin>

<!-- Drive To Pose Controller -->
<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>
</include>
</model>

</world>
</sdf>
1 change: 1 addition & 0 deletions src/systems/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ add_subdirectory(contact)
add_subdirectory(camera_video_recorder)
add_subdirectory(detachable_joint)
add_subdirectory(diff_drive)
add_subdirectory(drive_to_pose_controller)
add_subdirectory(dvl)
# elevator system causes compile erros on windows
if (NOT WIN32)
Expand Down
6 changes: 6 additions & 0 deletions src/systems/drive_to_pose_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
gz_add_system(drive-to-pose-controller
SOURCES
DriveToPoseController.cc
PUBLIC_LINK_LIBS
gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
)
Loading
Loading