/
gazebo.launch
80 lines (61 loc) · 3.64 KB
/
gazebo.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>
<arg name="paused" default="true"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="rviz" default="false"/>
<arg name="gpu_lidar" default="false"/>
<arg name="visualize_lidar" default="false"/>
<arg name="world" default="worlds/empty.world"/>
<arg name="dex_wrist" default="false"/>
<param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
<arg name="publish_upright_img" default="false" doc="whether to pub rotated upright color image" />
<arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
<arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="verbose" value="true"/>
</include>
<param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)"/>
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/>
<rosparam command="load"
file="$(find stretch_gazebo)/config/joints.yaml"
ns="stretch_joint_state_controller" />
<rosparam command="load"
file="$(find stretch_gazebo)/config/drive_config.yaml"
ns="stretch_diff_drive_controller" />
<rosparam command="load"
file="$(find stretch_gazebo)/config/arm.yaml"/>
<rosparam command="load"
file="$(find stretch_gazebo)/config/head.yaml" />
<rosparam command="load"
file="$(find stretch_gazebo)/config/gripper.yaml" />
<rosparam command="load"
file="$(find stretch_gazebo)/config/dex_wrist.yaml" if="$(arg dex_wrist)"/>
<node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"
unless="$(arg dex_wrist)"/>
<node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller"
if="$(arg dex_wrist)"/>
<node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
<!-- UPRIGHT ROTATED CAMERA VIEW -->
<node name="upright_rotater" pkg="image_rotate" type="image_rotate" if="$(arg publish_upright_img)">
<remap from="image" to="/camera/color/image_raw" />
<remap from="rotated/image" to="/camera/color/upright_image_raw" />
<param name="target_frame_id" type="str" value="" />
<param name="target_x" type="double" value="-1.5708" />
</node>
</launch>