Skip to content

Commit 48a65b7

Browse files
azazdeazazazdeaz
committed
Move depot world into the local package and add observer cam
1 parent d8572a3 commit 48a65b7

File tree

5 files changed

+170
-56
lines changed

5 files changed

+170
-56
lines changed

‎src/sam_bot_nav2_gz/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ find_package(rclpy REQUIRED)
2525
# find_package(<dependency> REQUIRED)
2626

2727
install(
28-
DIRECTORY scripts launch world models
28+
DIRECTORY scripts launch worlds models
2929
DESTINATION share/${PROJECT_NAME}
3030
)
3131

‎src/sam_bot_nav2_gz/launch/waypoint_follower_example_launch.py‎

Lines changed: 89 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -18,129 +18,163 @@
1818

1919
from ament_index_python.packages import get_package_share_directory
2020
from launch import LaunchDescription
21-
from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
22-
IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, EmitEvent)
21+
from launch.actions import (
22+
AppendEnvironmentVariable,
23+
DeclareLaunchArgument,
24+
ExecuteProcess,
25+
IncludeLaunchDescription,
26+
OpaqueFunction,
27+
RegisterEventHandler,
28+
EmitEvent,
29+
)
2330
from launch.conditions import IfCondition
2431
from launch.event_handlers import OnProcessExit, OnShutdown
2532
from launch.events import Shutdown
2633
from launch.launch_description_sources import PythonLaunchDescriptionSource
2734
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
2835
from launch_ros.actions import Node
36+
from launch.substitutions import PathJoinSubstitution
2937

3038

3139
def generate_launch_description():
32-
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
40+
# Launch configuration variables
41+
world_file_name = LaunchConfiguration("world_file")
42+
use_rviz = LaunchConfiguration("use_rviz")
43+
headless = LaunchConfiguration("headless")
44+
45+
nav2_bringup_dir = get_package_share_directory("nav2_bringup")
3346
sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
34-
desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
47+
desc_dir = get_package_share_directory("nav2_minimal_tb4_description")
48+
test_pkg_dir = get_package_share_directory('sam_bot_nav2_gz')
3549

36-
robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro')
37-
world = os.path.join(sim_dir, 'worlds', 'depot.sdf')
38-
map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml')
50+
robot_sdf = os.path.join(desc_dir, "urdf", "standard", "turtlebot4.urdf.xacro")
51+
world = PathJoinSubstitution([test_pkg_dir, "worlds", world_file_name])
52+
map_yaml_file = os.path.join(nav2_bringup_dir, "maps", "depot.yaml")
3953

40-
# Launch configuration variables
41-
use_rviz = LaunchConfiguration('use_rviz')
42-
headless = LaunchConfiguration('headless')
4354

4455
# Declare the launch arguments
4556
declare_use_rviz_cmd = DeclareLaunchArgument(
46-
'use_rviz', default_value='True', description='Whether to start RVIZ'
57+
"use_rviz", default_value="True", description="Whether to start RVIZ"
4758
)
4859

4960
declare_simulator_cmd = DeclareLaunchArgument(
50-
'headless', default_value='False', description='Whether to execute gzclient)'
61+
"headless", default_value="False", description="Whether to execute gzclient)"
62+
)
63+
64+
declare_world_file_cmd = DeclareLaunchArgument(
65+
name="world_file",
66+
default_value="depot.sdf",
67+
description="Name of the world file to load",
5168
)
5269

5370
# start the simulation
54-
world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
71+
world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf")
72+
print(
73+
f"Temporary world file created at {world_sdf}. This will be removed on shutdown."
74+
)
5575
world_sdf_xacro = ExecuteProcess(
56-
cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
76+
cmd=["xacro", "-o", world_sdf, ["headless:=", headless], world]
77+
)
5778
start_gazebo_server_cmd = IncludeLaunchDescription(
5879
PythonLaunchDescriptionSource(
59-
os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
60-
'gz_sim.launch.py')),
61-
launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
80+
os.path.join(
81+
get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"
82+
)
83+
),
84+
launch_arguments={"gz_args": ["-r -s ", world_sdf]}.items(),
85+
)
6286

63-
remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
64-
on_shutdown=[
65-
OpaqueFunction(function=lambda _: os.remove(world_sdf))
66-
]))
87+
remove_temp_sdf_file = RegisterEventHandler(
88+
event_handler=OnShutdown(
89+
on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))]
90+
)
91+
)
6792

6893
set_env_vars_resources = AppendEnvironmentVariable(
69-
'GZ_SIM_RESOURCE_PATH',
70-
os.path.join(sim_dir, 'worlds'))
94+
"GZ_SIM_RESOURCE_PATH", os.path.join(sim_dir, "worlds")
95+
)
7196
start_gazebo_client_cmd = IncludeLaunchDescription(
7297
PythonLaunchDescriptionSource(
73-
os.path.join(get_package_share_directory('ros_gz_sim'),
74-
'launch',
75-
'gz_sim.launch.py')
98+
os.path.join(
99+
get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"
100+
)
76101
),
77-
condition=IfCondition(PythonExpression(
78-
['not ', headless])),
79-
launch_arguments={'gz_args': ['-v4 -g ']}.items(),
102+
condition=IfCondition(PythonExpression(["not ", headless])),
103+
launch_arguments={"gz_args": ["-v4 -g "]}.items(),
80104
)
81105

82106
spawn_robot_cmd = IncludeLaunchDescription(
83107
PythonLaunchDescriptionSource(
84-
os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')),
85-
launch_arguments={'use_sim_time': 'True',
86-
'robot_sdf': robot_sdf,
87-
'x_pose': '-8.0',
88-
'y_pose': '0.0',
89-
'z_pose': '0.0',
90-
'roll': '0.0',
91-
'pitch': '0.0',
92-
'yaw': '0.0'}.items())
108+
os.path.join(sim_dir, "launch", "spawn_tb4.launch.py")
109+
),
110+
launch_arguments={
111+
"use_sim_time": "True",
112+
"robot_sdf": robot_sdf,
113+
"x_pose": "-8.0",
114+
"y_pose": "0.0",
115+
"z_pose": "0.0",
116+
"roll": "0.0",
117+
"pitch": "0.0",
118+
"yaw": "0.0",
119+
}.items(),
120+
)
93121

94122
start_robot_state_publisher_cmd = Node(
95-
package='robot_state_publisher',
96-
executable='robot_state_publisher',
97-
name='robot_state_publisher',
98-
output='screen',
123+
package="robot_state_publisher",
124+
executable="robot_state_publisher",
125+
name="robot_state_publisher",
126+
output="screen",
99127
parameters=[
100-
{'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])}
101-
]
128+
{
129+
"use_sim_time": True,
130+
"robot_description": Command(["xacro", " ", robot_sdf]),
131+
}
132+
],
102133
)
103134

104135
# start the visualization
105136
rviz_cmd = IncludeLaunchDescription(
106137
PythonLaunchDescriptionSource(
107-
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')
138+
os.path.join(nav2_bringup_dir, "launch", "rviz_launch.py")
108139
),
109140
condition=IfCondition(use_rviz),
110-
launch_arguments={'namespace': ''}.items(),
141+
launch_arguments={"namespace": ""}.items(),
111142
)
112143

113144
# start navigation
114145
bringup_cmd = IncludeLaunchDescription(
115146
PythonLaunchDescriptionSource(
116-
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
147+
os.path.join(nav2_bringup_dir, "launch", "bringup_launch.py")
117148
),
118-
launch_arguments={'map': map_yaml_file}.items(),
149+
launch_arguments={"map": map_yaml_file}.items(),
119150
)
120151

121152
# start the demo autonomy task
122153
demo_cmd = Node(
123-
package='sam_bot_nav2_gz',
124-
executable='example_waypoint_follower.py',
154+
package="sam_bot_nav2_gz",
155+
executable="example_waypoint_follower.py",
125156
emulate_tty=True,
126-
output='screen',
157+
output="screen",
127158
)
128-
159+
129160
# Register an event handler to shutdown everything when the demo node exits
130161
shutdown_on_demo_exit = RegisterEventHandler(
131162
OnProcessExit(
132163
target_action=demo_cmd,
133-
on_exit=[EmitEvent(event=Shutdown(reason='Waypoint follower demo completed'))]
164+
on_exit=[
165+
EmitEvent(event=Shutdown(reason="Waypoint follower demo completed"))
166+
],
134167
)
135168
)
136169

137170
set_env_vars_resources2 = AppendEnvironmentVariable(
138-
'GZ_SIM_RESOURCE_PATH',
139-
str(Path(os.path.join(desc_dir)).parent.resolve()))
171+
"GZ_SIM_RESOURCE_PATH", str(Path(os.path.join(desc_dir)).parent.resolve())
172+
)
140173

141174
ld = LaunchDescription()
142175
ld.add_action(declare_use_rviz_cmd)
143176
ld.add_action(declare_simulator_cmd)
177+
ld.add_action(declare_world_file_cmd)
144178
ld.add_action(world_sdf_xacro)
145179
ld.add_action(remove_temp_sdf_file)
146180
ld.add_action(set_env_vars_resources)

src/sam_bot_nav2_gz/world/bookstore.sdf renamed to src/sam_bot_nav2_gz/worlds/bookstore.sdf

File renamed without changes.

‎src/sam_bot_nav2_gz/worlds/depot.sdf‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
<?xml version="1.0"?>
2+
<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:experimental="http://sdformat.org/schemas/experimental">
3+
<xacro:arg name="headless" default="true"/>
4+
<world name="depot">
5+
<scene>
6+
<grid>false</grid>
7+
</scene>
8+
<physics name="1ms" type="ode">
9+
<max_step_size>0.003</max_step_size>
10+
<real_time_update_rate>1000.0</real_time_update_rate>
11+
<real_time_factor>1.0</real_time_factor>
12+
</physics>
13+
14+
<plugin
15+
filename="gz-sim-physics-system"
16+
name="gz::sim::systems::Physics">
17+
</plugin>
18+
<plugin
19+
filename="gz-sim-user-commands-system"
20+
name="gz::sim::systems::UserCommands">
21+
</plugin>
22+
<xacro:unless value="$(arg headless)">
23+
<plugin
24+
filename="gz-sim-scene-broadcaster-system"
25+
name="gz::sim::systems::SceneBroadcaster">
26+
</plugin>
27+
</xacro:unless>
28+
<plugin
29+
filename="gz-sim-sensors-system"
30+
name="gz::sim::systems::Sensors">
31+
<render_engine>ogre2</render_engine>
32+
</plugin>
33+
<plugin
34+
filename="gz-sim-imu-system"
35+
name="gz::sim::systems::Imu">
36+
</plugin>
37+
38+
39+
<!-- A camera sensor that looks straight down on the map. -->
40+
<model name="sky_cam">
41+
<static>true</static>
42+
<link name="camera_link">
43+
<pose>0 0 10 1.57 1.57 0</pose>
44+
<sensor name="sky_cam" type="camera">
45+
<camera>
46+
<horizontal_fov>1.047</horizontal_fov>
47+
<image>
48+
<width>1024</width>
49+
<height>768</height>
50+
</image>
51+
<clip>
52+
<near>0.1</near>
53+
<far>100</far>
54+
</clip>
55+
</camera>
56+
<always_on>1</always_on>
57+
<update_rate>1</update_rate>
58+
<visualize>true</visualize>
59+
<topic>sky_cam</topic>
60+
</sensor>
61+
</link>
62+
</model>
63+
64+
<include>
65+
<uri>
66+
https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depot
67+
</uri>
68+
69+
<!-- Remove annoying things that get in the way of visualizing the robot -->
70+
<experimental:params>
71+
<visual element_id="Fan1::visual" action="remove"/>
72+
<visual element_id="Fan2::visual" action="remove"/>
73+
<visual element_id="main::ROOF_visual" action="remove"/>
74+
<visual element_id="main::FANS_visual" action="remove"/>
75+
</experimental:params>
76+
77+
</include>
78+
79+
</world>
80+
</sdf>

src/sam_bot_nav2_gz/world/empty.sdf renamed to src/sam_bot_nav2_gz/worlds/empty.sdf

File renamed without changes.

0 commit comments

Comments
 (0)