-
Notifications
You must be signed in to change notification settings - Fork 2
/
ekf.launch
126 lines (103 loc) · 5.68 KB
/
ekf.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
<?xml version="1.0"?>
<launch>
<env name="ROSCONSOLE_FORMAT" value="[${thread}] [${node}/${function}:${line}]: ${message}"/>
<!-- export ROS_MASTER_URI='http://rc-car.local:11311' -->
<!-- STATIC_TRANSFORM_PUBLISHER arg = x y z yaw pitch roll frame_id child_frame_id-->
#### 3.141592653
#### 1.570796327
#### args="0.255 0 0.180 3.14159 3.14159 0 robot_base imu"
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_imu" args="0.255 0 0.180 3.14159 3.14159 0 robot_base imu" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_gps" args="-0.050 -0.076 0.750 0 0 0 robot_base gps" />
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_gps0" args="0.050 -0.076 0.200 0 0 0 robot_base gps0" /> -->
<node pkg="tf2_ros" type="static_transform_publisher" name="rbase_to_base_link" args="0 0 0 0 0 0 robot_base base_link" />
<!--node pkg="tf2_ros" type="static_transform_publisher" name="swri_transform" args="0 0 0 0 0 0 world map 100" -->
<!-- NAVSAT_TRANSFORM_NODE -->
<node pkg="robot_localization" type="navsat_transform_node"
name="navsat_transform_node" respawn="true" output="screen">
<!--rosparam command="load" file="$(find robot_localization)/params/navsat_transform_template.yaml" -->
<!-- http://www.magnetic-declination.com/-->
<!-- https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml-->
###This parameter apperars not to affect heading
<param name="magnetic_declination_radians" value="0.0"/> <!-- Indianapolis -4*49'= -0.0840666923 Toronto -0.18238691 -->
<param name="zero_altitude" value="true" />
<param name="broadcast_utm_transform" value="true" />
<param name="publish_filtered_gps" value="true" />
<!-- wait 5 secs before calculating the transform from GPS coordinates to robot’s world frame. -->
<!--<param name="delay" value="5"/> -->
<!-- Makefsure wait_for_datum is false to use sensors for
robot’s current pose estimate in its world frame,
an earth-referenced heading,
and a geographic coordinate -->
<!--param name="wait_for_datum" value="false"-->
<!-- If IMU reads 0 facing magnetic north -->
<!-- using below config so that imu reads 0 when facing east -->
###This parameter appears not to affect heading
<param name="yaw_offset" value="0.0"/>
<param name="broadcast_utm_transform" value="true" />
<!-- OUTPUT TOPICS -->
<remap from="/odometry/gps" to="/odometry/filtered_gps"/>
<remap from="/gps/filtered" to="/gps/filtered"/>
<!-- INPUT TOPICS -->
<remap from="/gps/fix" to="/gps/fix"/>
<remap from="imu/data/" to="/imu/data_fixed"/>
<remap from="/odometry/filtered" to="/odometry/filtered_global"/>
</node>
<!-- GLOBAL_EKF_LOCALIZATION -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true" respawn="true">
<remap from="/odometry/filtered" to="/odometry/filtered_global"/>
<param name="frequency" value="5"/>
<param name="world_frame" value="map"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="robot_base"/>
<param name="two_d_mode" value="true"/>
<!-- IMU -->
<param name="imu0" value="/imu/data_fixed"/>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
<!-- true, true, true, -->
false, false, false,
false, false, false]</rosparam>
<!-- WHEEL ODOMETRY -->
<param name="odom0" value="/wheel_encoder/odom"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]</rosparam> -->
<!-- GPS ODOMETRY -->
<param name="odom1" value="/odometry/filtered_gps"/>
<rosparam param="odom1_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
true, false, false]</rosparam>
<param name="odom1_differential" value="false"/>
</node>
<!-- LOCAL_EKF_LOCALIZATION -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true" respawn="true">
<remap from="/odometry/filtered" to="/odometry/filtered_local"/>
<param name="frequency" value="5"/>
<param name="world_frame" value="odom"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="robot_base"/>
<param name="two_d_mode" value="true"/>
<!-- IMU -->
<param name="imu0" value="/imu/data_fixed"/>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="imu0_nodelay" value="true"/>
<!-- WHEEL ODOMETRY -->
<param name="odom0" value="/wheel_encoder/odom"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]</rosparam> -->
</node>
</launch>