-
Notifications
You must be signed in to change notification settings - Fork 155
/
Copy pathrmf_sim.launch
80 lines (70 loc) · 4.89 KB
/
rmf_sim.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
<launch>
<!-- All settings -->
<arg name="robot_name" default="rmf_obelix"/>
<arg name="gazebo_gui_en" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="rviz_en" default="true" />
<arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->
<param name="use_sim_time" value="$(arg use_sim_time)"/>
<!-- Config files -->
<arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
<arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
<arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
<arg name="map_config_file" default="$(arg voxblox_config_file)"/>
<!-- <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_cave_01.world"/> -->
<arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>
<!-- Static TF -->
<node pkg="tf" type="static_transform_publisher" name="tf_53" args="0 0 0 0 0 0 world navigation 100" />
<node pkg="tf" type="static_transform_publisher" name="tf_51" args="0.15 0 0.0 0.0 0.261 0.0 $(arg robot_name)/base_link $(arg robot_name)/vi_sensor/base_link 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_1" args="0 0 0 0 0 0 $(arg robot_name)/vi_sensor/base_link fcu 1" />
<node pkg="tf" type="static_transform_publisher" name="tf_2" args="0.015 0.055 0.0065 -1.57 0.1 -1.57 fcu $(arg robot_name)/vi_sensor/camera_depth_optical_center_link 1" />
<node pkg="tf" type="static_transform_publisher" name="tf_3" args="0.015 0.055 0.0065 -1.57 0.1 -1.57 fcu $(arg robot_name)/vi_sensor/camera_left_link 1" />
<node pkg="tf" type="static_transform_publisher" name="tf_4" args="0.015 -0.055 0.0065 -1.57 0.1 -1.57 fcu $(arg robot_name)/vi_sensor/camera_right_link 1" />
<node pkg="tf" type="static_transform_publisher" name="tf_5" args="0 0 0.05 0.0 0.0 0.0 $(arg robot_name)/base_link $(arg robot_name)/$(arg robot_name)/velodyne 10" />
<!-- ROS Gazebo -->
<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find planner_gazebo_sim)/models:$(find subt_cave_sim)/models"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_file)" />
<arg name="gui" value="$(arg gazebo_gui_en)" />
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="paused" value="false" />
<arg name="verbose" value="false"/>
</include>
<group ns="$(arg robot_name)">
<node name="img_throttler" type="throttle" pkg="topic_tools" args="messages vi_sensor/camera_depth/depth/points 5 vi_sensor/camera_depth/depth/points_throttled" />
<node name="odo_throttler" type="throttle" pkg="topic_tools" args="messages ground_truth/odometry 100 ground_truth/odometry_throttled" />
<!-- MAV launch -->
<include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
<arg name="mav_name" value="$(arg robot_name)" />
<arg name="model" value="$(find rotors_description)/urdf/rmf_obelix.gazebo" />
<arg name="enable_ground_truth" value="true" />
<arg name="x" value="0.0"/>
<arg name="y" value="0.0"/>
<arg name="z" value="0.5"/>
</include>
<!-- Position controller -->
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
<rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg robot_name).yaml" />
<rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg robot_name).yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
</group>
<arg name="odometry_topic" default="$(arg robot_name)/ground_truth/odometry_throttled"/>
<node name="pose_throttler" type="throttle" pkg="topic_tools" args="messages $(arg robot_name)/ground_truth/pose_with_covariance 10 /msf_core/pose" />
<!-- Graph based planning -->
<node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
<remap from="odometry" to="$(arg odometry_topic)" />
<remap from="/pointcloud" to="/$(arg robot_name)/velodyne_points" />
<rosparam command="load" file="$(arg gbplanner_config_file)" />
<rosparam command="load" file="$(arg map_config_file)" />
</node>
<!-- Planner and Control Interface -->
<node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">
<remap from="command/trajectory" to="$(arg robot_name)/command/trajectory" />
<remap from="planner_server" to="gbplanner" />
<remap from="planner_homing_server" to="gbplanner/homing" />
<remap from="odometry" to="$(arg odometry_topic)"/>
<rosparam command="load" file="$(arg pci_file)" />
</node>
<node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>
</launch>