-
Notifications
You must be signed in to change notification settings - Fork 320
/
robot_launch.launch
103 lines (95 loc) · 4.97 KB
/
robot_launch.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
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="kinova_robotType" default="j2n6s300"/>
<arg name="kinova_robotName" default="$(arg kinova_robotType)"/>
<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="use_trajectory_controller" default="true"/>
<arg name="is7dof" default="false"/>
<arg name="rqt" default="false"/>
<arg name="has2finger" default="false"/>
<arg name="description_name" default="robot_description"/> <!-- allows remapping of robot_description into a namespace -->
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find kinova_gazebo)/worlds/jaco.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)"/>
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find kinova_description)/urdf/$(arg kinova_robotType)_standalone.xacro'" />
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<!-- For the 6DOF -->
<group unless="$(arg is7dof)">
<!-- For 3 fingers -->
<node unless="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model $(arg kinova_robotName) -param robot_description
-J $(arg kinova_robotType)_joint_1 0.0
-J $(arg kinova_robotType)_joint_2 2.9
-J $(arg kinova_robotType)_joint_3 1.3
-J $(arg kinova_robotType)_joint_4 -2.07
-J $(arg kinova_robotType)_joint_5 1.4
-J $(arg kinova_robotType)_joint_6 0.0
-J $(arg kinova_robotType)_joint_finger_1 1.0
-J $(arg kinova_robotType)_joint_finger_2 1.0
-J $(arg kinova_robotType)_joint_finger_3 1.0" />
<!-- For 2 fingers -->
<node if="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model $(arg kinova_robotName) -param robot_description
-J $(arg kinova_robotType)_joint_1 0.0
-J $(arg kinova_robotType)_joint_2 2.9
-J $(arg kinova_robotType)_joint_3 1.3
-J $(arg kinova_robotType)_joint_4 -2.07
-J $(arg kinova_robotType)_joint_5 1.4
-J $(arg kinova_robotType)_joint_6 0.0
-J $(arg kinova_robotType)_joint_finger_1 1.0
-J $(arg kinova_robotType)_joint_finger_2 1.0" />
</group>
<!-- For the 7DOF -->
<group if="$(arg is7dof)">
<!-- For 3 fingers -->
<node unless="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model $(arg kinova_robotName) -param robot_description
-J $(arg kinova_robotType)_joint_1 0.0
-J $(arg kinova_robotType)_joint_2 2.9
-J $(arg kinova_robotType)_joint_3 0.0
-J $(arg kinova_robotType)_joint_4 1.3
-J $(arg kinova_robotType)_joint_5 -2.07
-J $(arg kinova_robotType)_joint_6 1.4
-J $(arg kinova_robotType)_joint_7 0.0
-J $(arg kinova_robotType)_joint_finger_1 1.0
-J $(arg kinova_robotType)_joint_finger_2 1.0
-J $(arg kinova_robotType)_joint_finger_3 1.0" />
<!-- For 2 fingers -->
<node if="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model $(arg kinova_robotName) -param robot_description
-J $(arg kinova_robotType)_joint_1 0.0
-J $(arg kinova_robotType)_joint_2 2.9
-J $(arg kinova_robotType)_joint_3 0.0
-J $(arg kinova_robotType)_joint_4 1.3
-J $(arg kinova_robotType)_joint_5 -2.07
-J $(arg kinova_robotType)_joint_6 1.4
-J $(arg kinova_robotType)_joint_7 0.0
-J $(arg kinova_robotType)_joint_finger_1 1.0
-J $(arg kinova_robotType)_joint_finger_2 1.0" />
</group>
<!-- ros_control launch file -->
<include file="$(find kinova_control)/launch/kinova_control.launch">
<arg name="kinova_robotName" value="$(arg kinova_robotName)"/>
<arg name="kinova_robotType" value="$(arg kinova_robotType)"/>
<arg name="use_trajectory_controller" value="$(arg use_trajectory_controller)"/>
<arg name="is7dof" value="$(arg is7dof)"/>
<arg name="has2finger" value="$(arg has2finger)"/>
<arg name="description_name" value="$(arg description_name)"/>
</include>
<!-- rqt launch file -->
<include if="$(arg rqt)" file="$(find kinova_control)/launch/kinova_rqt.launch">
<arg name="kinova_robotType" value="$(arg kinova_robotType)"/>
</include>
</launch>