-
Notifications
You must be signed in to change notification settings - Fork 0
/
tracker.launch
148 lines (125 loc) · 6.72 KB
/
tracker.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
142
143
144
145
146
147
148
<!--
Launch file that launches the brain_LOP tracker, locate_sphero, and all
background ROS nodes used by the Locobot
-->
<launch>
<arg name="robot_name" default="locobot"/>
<arg name="robot_model" default="locobot_wx200" />
<arg name="base_type" default="$(optenv INTERBOTIX_XSLOCOBOT_BASE_TYPE create3)"/> <!-- 'kobuki', 'create3' -->
<arg name="external_urdf_loc" default=""/>
<arg name="use_lidar" default="true"/>
<arg name="show_lidar" default="$(arg use_lidar)"/>
<arg name="use_camera" default="true"/>
<arg name="dof" default="5"/>
<arg name="use_moveit_rviz" default="true"/>
<arg name="rtabmap_args" default=""/>
<arg name="localization" default="true"/>
<arg name="mode_configs" default="$(find interbotix_xslocobot_moveit)/config/modes_all.yaml"/>
<arg name="use_actual" default="true"/>
<arg name="moveit_interface_gui" default="false"/>
<arg name="use_cpp_interface" default="true"/>
<arg name="use_python_interface" default="true"/>
<arg name="camera_tilt_angle" default="0.7"/>
<arg name="use_rviz" default="true"/>
<arg name="load_configs" default="true"/>
<!-- driver -->
<include file="$(find interbotix_xslocobot_control)/launch/xslocobot_control.launch">
<arg name="robot_model" value="$(arg robot_model)"/>
<arg name="robot_name" value="$(arg robot_name)"/>
<arg name="use_lidar" value="true"/>
<arg name="show_lidar" value="true"/>
<arg name="external_urdf_loc" value="$(arg external_urdf_loc)"/>
<arg name="base_type" value="$(arg base_type)"/>
<arg name="use_camera" value="true"/>
<arg name="filters" value="pointcloud"/>
<arg name="align_depth" value="true"/>
<arg name="load_configs" value="$(arg load_configs)"/>
<arg name="use_base" value="true"/>
<arg name="use_dock" value="true"/>
<arg name="use_rviz" value="$(arg use_rviz)"/>
</include>
<!-- perception -->
<include file="$(find interbotix_xslocobot_perception)/launch/xslocobot_perception.launch">
<arg name="robot_model" value="$(arg robot_model)"/>
<arg name="robot_name" value="$(arg robot_name)"/>
<arg name="use_rviz" value="false"/>
<arg name="launch_driver" value="false"/>
<arg name="enable_pipeline" value="true"/>
<arg name ="use_pointcloud_tuner_gui" value="false"/>
</include>
<!-- navigation -->
<include file="$(find interbotix_xslocobot_nav)/launch/xslocobot_nav.launch">
<arg name="robot_model" value="$(arg robot_model)" />
<arg name="use_lidar" value="$(arg use_lidar)" />
<arg name="rtabmap_args" value="$(arg rtabmap_args)" />
<arg name="localization" value="$(arg localization)"/>
<arg name="camera_tilt_angle" value="$(arg camera_tilt_angle)"/>
<arg name="launch_driver" value="false"/>
</include>
<!-- interbotix move it cpp/python interface -->
<group if="$(arg use_cpp_interface)">
<node
name="moveit_interface"
pkg="interbotix_moveit_interface"
type="moveit_interface"
respawn="false"
output="screen"
ns="$(arg robot_name)"/>
<node if="$(arg moveit_interface_gui)"
name="moveit_interface_gui"
pkg="interbotix_moveit_interface"
type="moveit_interface_gui"
output="screen"
ns="$(arg robot_name)"/>
</group>
<node if="$(arg use_python_interface)"
name="moveit_python_interface"
pkg="interbotix_moveit_interface"
type="moveit_python_interface"
respawn="false"
output="screen"
ns="$(arg robot_name)">
<rosparam command="load" file="$(find interbotix_xslocobot_moveit_interface)/config/$(arg robot_model).yaml"/>
<param name="robot_model" value="$(arg robot_model)"/>
<remap from="/attached_collision_object" to="/$(arg robot_name)/attached_collision_object"/>
<remap from="/collision_object" to="/$(arg robot_name)/collision_object"/>
</node>
<include file="$(find interbotix_xslocobot_moveit)/launch/move_group.launch" ns="$(arg robot_name)">
<arg name="robot_model" value="$(arg robot_model)"/>
<arg name="robot_name" value="$(arg robot_name)"/>
<arg name="show_lidar" value="$(arg show_lidar)"/>
<arg name="dof" value="$(arg dof)"/>
<arg name="use_camera" value="$(arg use_camera)"/>
<arg name="publish_monitored_planning_scene" value="true" />
</include>
<rosparam file="$(find interbotix_xslocobot_ros_control)/config/$(arg dof)dof_controllers.yaml" command="load" ns="$(arg robot_name)"/>
<rosparam file="$(find interbotix_xslocobot_ros_control)/config/hardware.yaml" command="load" ns="$(arg robot_name)"/>
<rosparam file="$(find locobot_learning)/config/real_nav_goals.yaml" command="load"/>
<param name="/mobile_base/motion_control/reflexes_enabled" type="bool" value="false" />
<node
name="controller_spawner"
pkg="controller_manager"
type="controller_manager"
respawn="false"
output="screen"
ns="$(arg robot_name)"
args="spawn arm_controller gripper_controller"/>
<node
name="xs_hardware_interface"
pkg="interbotix_xs_ros_control"
type="xs_hardware_interface"
output="screen"
ns="$(arg robot_name)">
</node>
<!-- custom nodes needed to track Sphero -->
<node
name="brain"
pkg="SpheroHunter"
type="brain_LOP.py">
</node>
<node
name="locate"
pkg="SpheroHunter"
type="locate_sphero.py">
</node>
</launch>