-
Notifications
You must be signed in to change notification settings - Fork 0
/
people_tracker_robot.launch
151 lines (138 loc) · 8.15 KB
/
people_tracker_robot.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
149
150
151
<launch>
<!-- Global paramters -->
<arg name="load_params_from_file" default="true" />
<arg name="gp_queue_size" default="5" />
<arg name="vo_queue_size" default="5" />
<arg name="ubd_queue_size" default="5" />
<arg name="pt_queue_size" default="10" />
<arg name="ptu_state" default="/ptu/state" />
<arg name="camera_namespace" default="/xtion" />
<arg name="rgb_image" default="/rgb/image_rect_color" />
<arg name="depth_image" default="/depth/image_rect_meters" />
<arg name="mono_image" default="/rgb/image_mono" />
<arg name="camera_info_rgb" default="/rgb/camera_info" />
<arg name="camera_info_depth" default="/depth/camera_info" />
<arg name="odom" default="/odom" />
<arg name="ground_plane" default="/ground_plane" />
<arg name="upper_body_detections" default="/upper_body_detector/detections" />
<arg name="upper_body_bb_centres" default="/upper_body_detector/bounding_box_centres" />
<arg name="upper_body_markers" default="/upper_body_detector/marker_array" />
<arg name="upper_body_image" default="/upper_body_detector/image" />
<arg name="visual_odometry" default="/visual_odometry/motion_matrix" />
<arg name="mdl_people_array" default="/mdl_people_tracker/people_array" />
<arg name="mdl_people_markers" default="/mdl_people_tracker/marker_array" />
<arg name="mdl_people_poses" default="/mdl_people_tracker/pose_array" />
<arg name="mdl_people_image" default="/mdl_people_tracker/image" />
<arg name="tf_target_frame" default="/map" />
<arg name="bayes_people_param_file" default="$(find bayes_people_tracker)/config/detectors.yaml" />
<arg name="bayes_people_positions" default="/people_tracker/positions" />
<arg name="bayes_people_pose" default="/people_tracker/pose" />
<arg name="bayes_people_pose_array" default="/people_tracker/pose_array" />
<arg name="bayes_people_people" default="/people_tracker/people" />
<arg name="bayes_people_marker" default="/people_tracker/marker_array" />
<arg name="scan" default="/scan_merged" />
<arg name="with_mdl_tracker" default="false"/>
<arg name="log" default="false" />
<arg name="manager_topic" default="" />
<arg name="with_map_filter" default="false"/>
<arg name="machine" default="localhost" />
<arg name="user" default="" />
<machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true"/>
<!-- Ground Plane - fixed ->
<include file="$(find ground_plane_estimation)/launch/ground_plane_fixed.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="load_params_from_file" value="$(arg load_params_from_file)"/>
<arg name="ptu_state" value="$(arg ptu_state)"/>
<arg name="ground_plane" value="$(arg ground_plane)"/>
</include> -->
<!-- Upper Body Detector ->
<include file="$(find upper_body_detector)/launch/upper_body_detector.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="load_params_from_file" value="$(arg load_params_from_file)"/>
<arg name="queue_size" value="$(arg ubd_queue_size)"/>
<arg name="camera_namespace" value="$(arg camera_namespace)"/>
<arg name="rgb_image" value="$(arg rgb_image)"/>
<arg name="depth_image" value="$(arg depth_image)"/>
<arg name="camera_info_depth" value="$(arg camera_info_depth)"/>
<arg name="upper_body_detections" value="$(arg upper_body_detections)"/>
<arg name="upper_body_bb_centres" value="$(arg upper_body_bb_centres)"/>
<arg name="upper_body_markers" value="$(arg upper_body_markers)"/>
<arg name="upper_body_image" value="$(arg upper_body_image)"/>
<arg name="ground_plane" value="$(arg ground_plane)"/>
</include> -->
<group if="$(arg with_mdl_tracker)">
<!-- Odometry -->
<include file="$(find odometry_to_motion_matrix)/launch/odom2visual.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="odom" value="$(arg odom)"/>
<arg name="motion_parameters" value="$(arg visual_odometry)"/>
</include>
<!-- Pedestrian Tracking -->
<include file="$(find mdl_people_tracker)/launch/mdl_people_tracker.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="load_params_from_file" value="$(arg load_params_from_file)"/>
<arg name="queue_size" value="$(arg pt_queue_size)"/>
<arg name="camera_namespace" value="$(arg camera_namespace)"/>
<arg name="rgb_image" value="$(arg rgb_image)"/>
<arg name="camera_info_rgb" value="$(arg camera_info_rgb)"/>
<!-- <arg name="ground_plane" value="$(arg ground_plane)"/> -->
<arg name="upper_body_detections" value="$(arg upper_body_detections)"/>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="people_array" value="$(arg mdl_people_array)"/>
<arg name="people_image" value="$(arg mdl_people_image)"/>
<arg name="people_markers" value="$(arg mdl_people_markers)"/>
<arg name="people_poses" value="$(arg mdl_people_poses)"/>
<arg name="target_frame" value="$(arg tf_target_frame)"/>
</include>
</group>
<!-- People Tracker -->
<include file="$(find bayes_people_tracker)/launch/people_tracker.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="param_file" value="$(arg bayes_people_param_file)"/>
<arg name="target_frame" value="$(arg tf_target_frame)"/>
<arg name="positions" value="$(arg bayes_people_positions)"/>
<arg name="pose" value="$(arg bayes_people_pose)"/>
<arg name="pose_array" value="$(arg bayes_people_pose_array)"/>
<arg name="people" value="$(arg bayes_people_people)"/>
<arg name="marker" value="$(arg bayes_people_marker)"/>
</include>
<!-- To PoseArray -->
<include file="$(find detector_msg_to_pose_array)/launch/to_pose_array.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
</include>
<!-- Leg Detector -->
<group if="$(arg with_map_filter)">
<node pkg="laser_filters" type="scan_to_scan_filter_chain" respawn="true" name="laser_filter">
<!--rosparam command="load" file="$(find map_laser)/filters.yaml" /-->
</node>
<!--node pkg="map_laser" type="filter.py" name="map_laser_filter" respawn="true" output="screen"/-->
<node pkg="leg_detector" type="leg_detector" name="leg_detector" args="scan:=/base_scan_filter $(find leg_detector)/config/bigDataSet_Tree.yaml" respawn="true" output="screen">
<param name="fixed_frame" type="string" value="odom" />
</node>
</group>
<group unless="$(arg with_map_filter)">
<node pkg="leg_detector" type="leg_detector" name="leg_detector" args="scan:=$(arg scan) $(find leg_detector)/config/bigDataSet_Tree.yaml" respawn="true" output="screen">
<param name="fixed_frame" type="string" value="odom" />
<param name="connection_threshold" type="double" value="0.1" />
<param name="leg_reliability_limit" type="double" value="0.7" />
<param name="no_observation_timeout" type="double" value="1.4" />
<param name="max_second_leg_age" type="double" value="0" />
<param name="max_track_jump" type="double" value="0.8" />
<param name="max_meas_jump" type="double" value="0.8" />
<param name="leg_pair_separation" type="double" value="0.5" />
</node>
</group>
<!-- Logging -->
<include file="$(find bayes_people_tracker_logging)/launch/logging.launch">
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="log" value="$(arg log)"/>
<arg name="manager_topic" value="$(arg manager_topic)"/>
</include>
</launch>