Skip to content
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

Jump to bottom

New system plugin called DriveToPoseController #2679

Open
wants to merge 11 commits into
base: gz-sim9
Choose a base branch
from
+1,049 −0
Open

New system plugin called DriveToPoseController #2679

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
ed884dc
Added drive to pose controller
sauk2 Nov 11, 2024
c77c9c5
Added world for drive to pose controller and updated cmake
sauk2 Nov 11, 2024
c2d0d61
Added test for drive_to_pose_controller
sauk2 Nov 16, 2024
59d4056
Updated world name
sauk2 Nov 16, 2024
1892486
Added newline
sauk2 Nov 16, 2024
bfdb421
Added copyright
sauk2 Nov 16, 2024
894543b
Styling changes
sauk2 Nov 16, 2024
8478e28
Missing include for lint
sauk2 Nov 16, 2024
3f28d90
Fixed typo in copyright
sauk2 Nov 17, 2024
c00bbdd
Added warnings when correct set of plugins are not loaded
sauk2 Nov 21, 2024
c10acbd
Fixed styling errors
sauk2 Nov 21, 2024
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
384 changes: 384 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,384 @@
<?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 base 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>


<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