-
Notifications
You must be signed in to change notification settings - Fork 0
/
gazebo_turtlebot.launch
executable file
·141 lines (112 loc) · 7.16 KB
/
gazebo_turtlebot.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
<launch>
<arg name = "laser_type" default="laser"/> <!-- others can be "vlp16", "laser"-->
<!-- ############################ TSRB WORLD BEGIN ####################### -->
<!-- <arg name="world_file" default="$(find closedloop_nav_slam)/configs/world/fourth_floor_stixel.world"/>
<arg name="map_file" default="$(find closedloop_nav_slam)/configs/map/tsrb/fourth_floor_gazebo.yaml"/>
<arg name = "start_x" default="-40"/>
<arg name = "start_y" default="14"/>
<arg name = "start_z" default="0"/>
<arg name = "start_yaw" default="0.0"/> -->
<!-- 34, -6, 0 (pi/2 = 1.5707963) -->
<!-- ############################ TSRB WORLD END ####################### -->
<!-- ############################ CLASSROOM WORLD BEGIN ####################### -->
<!-- <arg name="world_file" default="$(find closedloop_nav_slam)/configs/world/classroom_textured_collid.world"/>
<arg name="map_file" default="$(find closedloop_nav_slam)/configs/map/classroom/classroom_textured_20231206.yaml"/>
<arg name = "start_x" default="9"/>
<arg name = "start_y" default="-5"/>
<arg name = "start_z" default="0"/>
<arg name = "start_yaw" default="3.14159265359"/> -->
<!-- ############################ CLASSROOM WORLD END ####################### -->
<!-- ############################ AMAZON WORLD BEGIN ######################## -->
<!-- <arg name="world_file" default="$(find aws_robomaker_bookstore_world)/worlds/bookstore.world"/> -->
<arg name="world_file" default="$(find aws_robomaker_hospital_world)/worlds/hospital.world"/>
<!-- <arg name="world_file" default="$(find aws_robomaker_small_house_world)/worlds/small_house.world"/> -->
<!-- <arg name="world_file" default="$(find aws_robomaker_small_warehouse_world)/worlds/no_roof_small_warehouse.world"/> -->
<arg name="map_file" default="$(find closedloop_nav_slam)/configs/map/aws/aws_hospital.yaml"/>
<arg name = "start_x" default="0"/>
<arg name = "start_y" default="10"/>
<arg name = "start_z" default="0"/>
<!-- <arg name = "start_z" default="0.034"/> -->
<arg name = "start_yaw" default="0"/>
<!-- ############################ AMAZON WORLD END ######################## -->
<arg name="gui" default="false"/>
<arg name="mode" default="localization"/>
<!--BEGIN turtlebot_gazebo turtlebot_world.launch -->
<!--BEGIN gazebo_ros empty_world.launch-->
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="extra_gazebo_args" default=""/>
<arg name="recording" default="false"/>
<!-- Note that 'headless' is currently non-functional. See gazebo_ros_pkgs issue #491 (-r arg does not disable
rendering, but instead enables recording). The arg definition has been left here to prevent breaking downstream
launch files, but it does nothing. -->
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="physics" default="ode"/>
<arg name="verbose" default="true"/>
<arg name="respawn_gazebo" default="false"/>
<arg name="use_clock_frequency" default="false"/>
<arg name="pub_clock_frequency" default="100"/>
<!-- set use_sim_time flag -->
<param name="/use_sim_time" value="true" />
<!-- set command arguments -->
<arg unless="$(arg paused)" name="command_arg1" value=""/>
<arg if="$(arg paused)" name="command_arg1" value="-u"/>
<arg unless="$(arg recording)" name="command_arg2" value=""/>
<arg if="$(arg recording)" name="command_arg2" value="-r"/>
<arg unless="$(arg verbose)" name="command_arg3" value=""/>
<arg if="$(arg verbose)" name="command_arg3" value="--verbose"/>
<arg unless="$(arg debug)" name="script_type" value="gzserver"/>
<arg if="$(arg debug)" name="script_type" value="debug"/>
<!-- start gazebo server-->
<group if="$(arg use_clock_frequency)">
<param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
</group>
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="screen"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_file)" required="true">
</node>
<!-- start gazebo client -->
<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
</group>
<!--END gazebo_ros empty_world.launch-->
<!--BEGIN turtlebot_gazebo kobuki.launch.xml-->
<arg name="urdf_file" default="$(find xacro)/xacro '$(find closedloop_nav_slam)/configs/urdf/gazebo/kobuki_hexagons_fisheye_stereo_mpu_6000_$(arg laser_type).urdf.xacro'" />
<param name="robot_description" command="$(arg urdf_file)" />
<!-- Gazebo model spawner -->
<node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
args="-x $(arg start_x) -y $(arg start_y) -z $(arg start_z) -Y $(arg start_yaw) -unpause -urdf -param robot_description -model mobile_base"/>
<!-- Velocity muxer -->
<node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager" required="true"/>
<node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager" required="true">
<param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
<remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
</node>
<!-- Bumper/cliff to pointcloud (not working, as it needs sensors/core messages) -->
<include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
<!--END turtlebot_gazebo kobuki.launch.xml-->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" required="true">
<param name="publish_frequency" type="double" value="30.0" />
<!-- <param name="tf_prefix" value="groundtruth"/> -->
</node>
<group if="$(eval arg('mode') == 'localization')">
<!--END turtlebot_gazebo turtlebot_world.launch -->
<node name="map" pkg="map_server" type="map_server" args="$(arg map_file)" required="true">
<param name="frame_id" value="map"/>
</node>
<!-- map to slam_map tf -->
<node pkg="tf" type="static_transform_publisher" name="nav_map_to_slam_map_publisher"
args="$(arg start_x) $(arg start_y) 0 $(arg start_yaw) 0 0 map slam_map 50" >
</node>
</group>
<node name="ground_truth_odometry_publisher" pkg="closedloop_nav_slam" type="ground_truth_odometry_publisher.py" output="screen">
</node>
<!-- downsample the imu message in gazebo; no need in real turtlebot; there might be warning on msf_core: imu message drop: simply ignore that! -->
<node pkg="topic_tools" type="throttle" name="imu_downsample" args=" messages /mobile_base/sensors/imu_data 100 /imu0">
</node>
<!-- downsample the odom message to be logged in rosbag; no need in real turtlebot; -->
<node pkg="topic_tools" type="throttle" name="odom_downsample" args=" messages /odom 50 /odom_sparse">
</node>
</launch>