-
Notifications
You must be signed in to change notification settings - Fork 9
/
freyja_controller.launch
99 lines (77 loc) · 4.01 KB
/
freyja_controller.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
<launch>
<!-- vim: set ft=xml noet : -->
<!-- Common launch file for Freyja.
Supports both AscTec and Pixhawk-based vehicles. Can
also do velocity-only control (ignores position refs).
~ aj // Nimbus Lab.
-->
<!-- allowed autopilots: "asctec" or "arducopter" -->
<arg name="autopilot" default="arducopter" />
<arg name="vicon_topic" />
<!-- use "device:baudrate" for apm, "device" for asctec -->
<arg name="comm_port" default="/dev/ttyUSB0:57600"/>
<arg name="total_mass" default="0.95"/>
<arg name="thrust_scaler" default="200.0"/>
<!-- launch a handler for discrete waypoints -->
<arg name="use_waypoint_handler" default="false" />
<!-- advanced settings -->
<arg name="use_velctrl_only" default="false" />
<arg name="enable_flatness_ff" default="false" />
<arg name="bias_compensation" default="auto" /> <!--auto, on, off-->
<!-- Used for examples -->
<arg name="use_examples" default="false"/>
<arg name="example_number" default="0" />
<arg name="start_rosbag" default="false" />
<!-- state source can be: "apm", "asctec", "vicon" or "onboard_camera" -->
<!-- implemented filters: "median", "gauss" and "lwma" -->
<node name="state_manager" pkg="state_manager" type="state_manager_node">
<param name="state_source" type="string" value="vicon" />
<param name="filter_type" type="string" value="median" />
<param name="vicon_topic" type="string" value="$(arg vicon_topic)" />
</node>
<!-- control node (see velctrl flag above) -->
<node name="lqg_controller" pkg="lqg_control" type="lqg_control_node"
output="screen" if="$(eval not use_velctrl_only)">
<param name="controller_rate" type="int" value="45" />
<param name="total_mass" type="double" value="$(arg total_mass)"/>
<param name="use_stricter_gains" type="bool" value="false" />
<param name="estimator_rate" type="int" value="50" />
<param name="bias_compensation" type="string" value="$(arg bias_compensation)" />
<param name="mass_estimation" type="bool" value="true" />
<param name="mass_correction" type="bool" value="false" />
<param name="enable_flatness_ff" type="bool" value="$(arg enable_flatness_ff)" />
</node>
<node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
if="$(eval use_velctrl_only)">
<param name="controller_rate" type="int" value="30" />
<param name="total_mass" type="double" value="$(arg total_mass)" />
</node>
<!-- autopilot communication node -->
<node name="asctec_handler" pkg="asctec_handler" type="asctec_handler_node"
if="$(eval autopilot=='asctec')">
<param name="serial_port" value="/dev/ttyUSB0" />
</node>
<!-- apm/px4 needs a handler for translation, plus a mavros node -->
<group if="$(eval autopilot=='arducopter')">
<node name="apm_handler" pkg="apm_handler" type="apm_handler_node">
<param name="thrust_scaler" type="double" value="$(arg thrust_scaler)" />
</node>
<include file="$(find freyja_configfiles)/apm_nimbus.launch">
<arg name="fcu_url" value="$(arg comm_port)"/>
</include>
</group>
<!-- handler for discrete waypoints -->
<node name="waypoint_manager" pkg="waypoint_manager" type="waypoint_manager_node"
output="screen" if="$(eval use_waypoint_handler and not use_examples)">
<param name="init_pn" type="double" value="0.0" />
<param name="init_pe" type="double" value="0.0" />
<param name="init_pd" type="double" value="-0.5" />
<param name="init_yaw" type="double" value="0.0" />
</node>
<!-- examples -->
<include file="$(find freyja_examples)/launch/example.launch" if="$(eval use_examples)" >
<arg name="example_number" value="$(arg example_number)"/>
</include>
<!-- record stuff -->
<node name="recorder" pkg="rosbag" type="record" args="-a" if="$(eval start_rosbag)"/>
</launch>