-
Notifications
You must be signed in to change notification settings - Fork 6
/
control_manager.launch
184 lines (160 loc) · 11 KB
/
control_manager.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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
<launch>
<!-- args corresponding to environment variables -->
<arg name="RUN_TYPE" default="$(optenv RUN_TYPE)" />
<arg name="PROFILER" default="$(optenv PROFILER false)" />
<arg name="UAV_NAME" default="$(optenv UAV_NAME)" />
<arg name="UAV_MASS" default="$(optenv UAV_MASS)" />
<arg name="BODY_DISTURBANCE_X" default="$(optenv INITIAL_DISTURBANCE_X 0.0)" />
<arg name="BODY_DISTURBANCE_Y" default="$(optenv INITIAL_DISTURBANCE_Y 0.0)" />
<arg name="LOGGER_DEBUG" default="$(optenv LOGGER_DEBUG false)" />
<arg name="OLD_PX4_FW" default="$(optenv OLD_PX4_FW false)" />
<!-- other args -->
<arg name="debug" default="false" />
<arg name="profile" default="true" />
<arg name="body_frame" default="fcu" />
<arg name="standalone" default="true" />
<arg name="bond" default="$(optenv BOND true)" />
<arg name="nodelet_manager_name" default="$(optenv NODELET_MANAGER)" />
<arg name="custom_config" default="" />
<arg name="world_config" default="" />
<arg name="platform_config" default="" />
<arg name="network_config" default="" />
<env name="ROSCONSOLE_CONFIG_FILE" if="$(eval arg('LOGGER_DEBUG'))" value="$(find mrs_uav_managers)/config/debug_verbosity.yaml" />
<arg name="g" default="9.81" />
<arg if="$(arg debug)" name="launch_prefix" value="debug_roslaunch" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg bond)" name="bond_suffix" value="" />
<arg unless="$(arg bond)" name="bond_suffix" value="--no-bond" />
<arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="standalone" />
<arg unless="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="load" />
<arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="" />
<arg unless="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="$(arg nodelet_manager_name)" />
<group ns="$(arg UAV_NAME)">
<!-- Control manager -->
<node pkg="nodelet" type="nodelet" name="control_manager" args="$(arg nodelet) mrs_uav_managers/ControlManager $(arg nodelet_manager) $(arg bond_suffix)" output="screen" launch-prefix="$(arg launch_prefix)">
<!-- Load the default param files -->
<param name="private_config" value="$(find mrs_uav_managers)/config/private/control_manager.yaml" />
<param name="public_config" value="$(find mrs_uav_managers)/config/public/control_manager.yaml" />
<param name="private_trackers" value="$(find mrs_uav_managers)/config/private/trackers.yaml" />
<param name="private_controllers" value="$(find mrs_uav_managers)/config/private/controllers.yaml" />
<param name="public_controllers" value="$(find mrs_uav_managers)/config/public/controllers.yaml" />
<param name="enable_profiler" type="bool" value="$(arg PROFILER)" />
<param name="uav_name" type="string" value="$(arg UAV_NAME)" />
<param name="body_frame" type="string" value="$(arg body_frame)" />
<param name="g" value="$(arg g)" />
<param name="run_type" type="string" value="$(arg RUN_TYPE)" />
<param if="$(eval arg('RUN_TYPE') == 'realworld')" name="uav_mass" value="$(arg UAV_MASS)" />
<param name="body_disturbance_x" value="$(arg BODY_DISTURBANCE_X)" />
<param name="body_disturbance_y" value="$(arg BODY_DISTURBANCE_Y)" />
<param if="$(eval arg('platform_config') == '')" name="platform_config" value="" />
<param if="$(eval not arg('platform_config') == '' and arg('platform_config')[0] == '/')" name="platform_config" value="$(arg platform_config)" />
<param if="$(eval not arg('platform_config') == '' and not arg('platform_config')[0] == '/')" name="platform_config" value="$(env PWD)/$(arg platform_config)" />
<param if="$(eval arg('custom_config') == '')" name="custom_config" value="" />
<param if="$(eval not arg('custom_config') == '' and arg('custom_config')[0] == '/')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval not arg('custom_config') == '' and not arg('custom_config')[0] == '/')" name="custom_config" value="$(env PWD)/$(arg custom_config)" />
<param if="$(eval arg('world_config') == '')" name="world_config" value="" />
<param if="$(eval not arg('world_config') == '' and arg('world_config')[0] == '/')" name="world_config" value="$(arg world_config)" />
<param if="$(eval not arg('world_config') == '' and not arg('world_config')[0] == '/')" name="world_config" value="$(env PWD)/$(arg world_config)" />
<param if="$(eval arg('network_config') == '')" name="network_config" value="" />
<param if="$(eval not arg('network_config') == '' and arg('network_config')[0] == '/')" name="network_config" value="$(arg network_config)" />
<param if="$(eval not arg('network_config') == '' and not arg('network_config')[0] == '/')" name="network_config" value="$(env PWD)/$(arg network_config)" />
<!-- Trackers' configs -->
<!-- Subscribers -->
<remap from="~odometry_in" to="estimation_manager/odom_main" />
<remap from="~uav_state_in" to="estimation_manager/uav_state" />
<remap from="~odometry_innovation_in" to="estimation_manager/innovation" />
<remap from="~local_odometry_in" to="hw_api/odometry" />
<remap from="~gnss_in" to="hw_api/gnss" />
<remap from="~max_z_in" to="estimation_manager/max_flight_z_agl" />
<remap from="~joystick_in" to="joy" />
<remap from="~bumper_sectors_in" to="bumper/obstacle_sectors" />
<remap from="~hw_api_capabilities_in" to="hw_api/capabilities" />
<remap from="~hw_api_status_in" to="hw_api/status" />
<remap from="~hw_api_rc_in" to="hw_api/rc_channels" />
<!-- Subscribers and Services -->
<remap from="~reference_in" to="~reference" />
<remap from="~velocity_reference_in" to="~velocity_reference" />
<remap from="~trajectory_reference_in" to="~trajectory_reference" />
<remap from="~goto_in" to="~goto" />
<remap from="~goto_fcu_in" to="~goto_fcu" />
<remap from="~goto_relative_in" to="~goto_relative" />
<remap from="~goto_altitude_in" to="~goto_altitude" />
<remap from="~set_heading_in" to="~set_heading" />
<remap from="~set_heading_relative_in" to="~set_heading_relative" />
<remap from="~transform_reference_in" to="~transform_reference" />
<remap from="~transform_reference_array_in" to="~transform_reference_array" />
<remap from="~transform_pose_in" to="~transform_pose" />
<remap from="~transform_vector3_in" to="~transform_vector3" />
<remap from="~validate_reference_in" to="~validate_reference" />
<remap from="~validate_reference_2d_in" to="~validate_reference_2d" />
<remap from="~validate_reference_array_in" to="~validate_reference_array" />
<remap from="~set_min_z_in" to="~set_min_z" />
<!-- Publishers -->
<remap from="~hw_api_actuator_cmd_out" to="hw_api/actuator_cmd" />
<remap from="~hw_api_control_group_cmd_out" to="hw_api/control_group_cmd" />
<remap from="~hw_api_attitude_rate_cmd_out" to="hw_api/attitude_rate_cmd" />
<remap from="~hw_api_attitude_cmd_out" to="hw_api/attitude_cmd" />
<remap from="~hw_api_acceleration_hdg_rate_cmd_out" to="hw_api/acceleration_hdg_rate_cmd" />
<remap from="~hw_api_acceleration_hdg_cmd_out" to="hw_api/acceleration_hdg_cmd" />
<remap from="~hw_api_velocity_hdg_rate_cmd_out" to="hw_api/velocity_hdg_rate_cmd" />
<remap from="~hw_api_velocity_hdg_cmd_out" to="hw_api/velocity_hdg_cmd" />
<remap from="~hw_api_position_cmd_out" to="hw_api/position_cmd" />
<remap from="~control_reference_out" to="~control_reference" />
<remap from="~tracker_cmd_out" to="~tracker_cmd" />
<remap from="~estimator_input_out" to="~estimator_input" />
<remap from="~control_error_out" to="~control_error" />
<remap from="~tilt_error_out" to="~tilt_error" />
<remap from="~thrust_force_out" to="~thrust_force" />
<remap from="~throttle_out" to="~throttle" />
<remap from="~thrust_out" to="~thrust" />
<remap from="~offboard_on_out" to="~offboard_on" />
<remap from="~mass_estimate_out" to="~mass_estimate" />
<remap from="~mass_nominal_out" to="~mass_nominal" />
<remap from="~safety_area_markers_out" to="~safety_area_markers" />
<remap from="~safety_area_coordinates_markers_out" to="~safety_area_coordinates_markers" />
<remap from="~disturbances_markers_out" to="~disturbances_markers" />
<remap from="~diagnostics_out" to="~diagnostics" />
<remap from="~bumper_status_out" to="~bumper_status" />
<remap from="~current_constraints_out" to="~current_constraints" />
<remap from="~heading_out" to="~heading" />
<remap from="~speed_out" to="~speed" />
<remap from="~trajectory_original/poses_out" to="~trajectory_original/poses" />
<remap from="~trajectory_original/markers_out" to="~trajectory_original/markers" />
<remap from="~controller_diagnostics_out" to="~controller_diagnostics" />
<remap from="~profiler" to="profiler" />
<!-- Services -->
<remap from="~switch_tracker_in" to="~switch_tracker" />
<remap from="~switch_controller_in" to="~switch_controller" />
<remap from="~tracker_reset_static_in" to="~tracker_reset_static" />
<remap from="~hover_in" to="~hover" />
<remap from="~ehover_in" to="~ehover" />
<remap from="~start_trajectory_tracking_in" to="~start_trajectory_tracking" />
<remap from="~stop_trajectory_tracking_in" to="~stop_trajectory_tracking" />
<remap from="~resume_trajectory_tracking_in" to="~resume_trajectory_tracking" />
<remap from="~goto_trajectory_start_in" to="~goto_trajectory_start" />
<remap from="~toggle_output_in" to="~toggle_output" />
<remap from="~emergency_reference_in" to="~emergency_reference" />
<remap from="~enable_callbacks_in" to="~enable_callbacks" />
<remap from="~set_gains_out" to="gain_manager/set_gains" />
<remap from="~set_constraints_in" to="~set_constraints" />
<remap from="~use_joystick_in" to="~use_joystick" />
<remap from="~hw_api_arming_out" to="hw_api/arming" />
<remap from="~arm_in" to="~arm" />
<remap from="~eland_in" to="~eland" />
<remap from="~failsafe_in" to="~failsafe" />
<remap from="~failsafe_escalating_in" to="~failsafe_escalating" />
<remap from="~eland_out" to="~landoff_tracker/eland" />
<remap from="~land_out" to="~landoff_tracker/land" />
<remap from="~pirouette_in" to="~pirouette" />
<remap from="~bumper_in" to="~bumper" />
<remap from="~bumper_repulsion_in" to="~bumper_repulsion" />
<remap from="~bumper_set_params_in" to="~bumper_set_params" />
<remap from="~get_min_z_in" to="~get_min_z" />
<remap from="~set_odometry_callbacks_out" to="estimation_manager/toggle_service_callbacks" />
<remap from="~ungrip_out" to="gripper/ungrip" />
<remap from="~use_safety_area_in" to="~use_safety_area" />
<remap from="~parachute_out" to="parachute_driver/fire_parachute" />
<remap from="~parachute_in" to="~parachute" />
</node>
</group>
</launch>