From a86878ebaa2e27f8ff0cf9d2a313788d3a6bb21c Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Wed, 14 Jun 2017 21:40:18 +0200
Subject: [PATCH] Testing robot_localization

---
 launch/robot_localization.launch | 15 ++++--------
 launch/wild_thumper.launch       | 39 +++++++++++++++++++++++---------
 2 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch
index a6e408d..d3da317 100644
--- a/launch/robot_localization.launch
+++ b/launch/robot_localization.launch
@@ -67,7 +67,6 @@
 		   parameters obviously have no default values, and must be specified. -->
 		<param name="odom0" value="odom"/>
 		<param name="imu0" value="imu"/>
-		<param name="odom1" value="gps"/>
 
 		<!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
 		   which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
@@ -76,10 +75,10 @@
 		   message types lack certain variables. For example, a TwistWithCovarianceStamped message has no pose information, so
 		   the first six values would be meaningless in that case. Each vector defaults to all false if unspecified, effectively
 		   making this parameter required for each sensor. -->
-		<rosparam param="odom0_config">[true, true, false, false, false, true, true, true, false, false, false, true, true, true, false]</rosparam>
-		<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, true, true, true, true, true, true]</rosparam>
-		<rosparam param="odom1_config">[true, true, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
-
+		<!-- x/y not included because of redundancy with velocities -->
+		<!-- vyaw not included in odom because too inaccurate -->
+		<rosparam param="odom0_config">[false, false, false, false, false, false, true, true, false, false, false, false, false, false, false]</rosparam>
+		<rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, false, false, false]</rosparam>
 
 		<!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
 		   measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation
@@ -97,9 +96,8 @@
 		   integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables,
 		   this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation
 		   variables have at least one source of absolute measurement. -->
-		<param name="odom0_differential" value="true"/>
+		<param name="odom0_differential" value="false"/>
 		<param name="imu0_differential" value="false"/>
-		<param name="odom1_differential" value="false"/>
 
 		<!-- When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all
 		   future measurements. While you can achieve the same effect with the differential paremeter, the key difference is
@@ -107,7 +105,6 @@
 		   you simply want your measurements to start at 0 for a given sensor, set this to true. -->
 		<param name="odom0_relative" value="false"/>
 		<param name="imu0_relative" value="false"/>
-		<param name="odom1_relative" value="true"/>
 
 		<!-- If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that
 		   is due to gravity for each IMU. If you don't want to, then set this to false. Defaults to false if unspecified. -->
@@ -118,7 +115,5 @@
 		<param name="print_diagnostics" value="true"/>
 
 		<remap from="odometry/filtered" to="odom_combined"/>
-
 	</node>
-
 </launch>
diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index 7fd4e59..9f2c287 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -20,17 +20,18 @@
 		<param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
 	</node>
 
-	<node pkg="wild_thumper" type="tinkerforge_imu2.py" name="tinkerforge_imu_brick2" output="screen"/>
+	<node pkg="wild_thumper" type="tinkerforge_imu2.py" name="tinkerforge_imu_brick2" output="screen">
+		<param name="uid" value="6DdNSn"/>
+	</node>
 
 	<node pkg="gpsd_client" type="gpsd_client"  name="gpsd_client" output="screen">
 		<param name="use_gps_time" value="false"/>
 	</node>
 
-	<node pkg="gps_common" type="utm_odometry_node" name="gps_conv" output="screen">
-		<remap from="odom" to="gps"/>
-		<param name="frame_id" value="odom"/>
-		<param name="child_frame_id" value="base_footprint"/>
-	</node>
+	<include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
+		<arg name="config_file" value="$(find wild_thumper)/config/cmd_vel_mux.yaml" />
+		<arg name="nodelet_manager_name" value="nodelet_manager" />
+	</include>
 
 	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
 		<remap from="imu_data" to="imu"/>
@@ -38,13 +39,29 @@
 		<remap from="robot_pose_ekf/odom_combined" to="odom_combined"/>
 		<param name="output_frame" value="odom"/>
 		<param name="freq" value="20.0"/>
-		<param name="vo_used" value="true"/>
+		<param name="vo_used" value="false"/>
 		<param name="debug" value="false"/>
 		<param name="sensor_timeout" value="2.0"/>
 	</node>
 
-	<include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
-		<arg name="config_file" value="$(find wild_thumper)/config/cmd_vel_mux.yaml" />
-		<arg name="nodelet_manager_name" value="nodelet_manager" />
-	</include>
+	<node pkg="gps_common" type="utm_odometry_node" name="gps_conv" output="screen">
+		<remap from="odom" to="gps"/>
+		<param name="frame_id" value="odom"/>
+		<param name="child_frame_id" value="base_footprint"/>
+	</node>
+
+	<!--
+	<include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/>
+	<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
+		<param name="magnetic_declination_radians" value="0.48"/>
+		<param name="yaw_offset" value="1.5708"/>
+		<param name="zero_altitude" value="true"/>
+		<param name="broadcast_utm_transform" value="true"/>
+
+		<remap from="/imu/data" to="imu"/>
+		<remap from="/gps/fix" to="fix" />
+		<remap from="/odometry/filtered" to="odom_combined"/>
+		<remap from="/odometry/gps" to="gps"/>
+	</node>
+	-->
 </launch>
-- 
2.39.5