From 2ee98f31c1f2527cdb4e190e37d9f6ad9e246859 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Mon, 12 Oct 2015 20:46:16 +0200
Subject: [PATCH] ekf: gps fixes

---
 launch/robot_localization.launch | 6 +++---
 launch/wild_thumper.launch       | 7 +++----
 scripts/wt_node.py               | 4 ++--
 3 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch
index 683c021..a6e408d 100644
--- a/launch/robot_localization.launch
+++ b/launch/robot_localization.launch
@@ -78,7 +78,7 @@
 		   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, true, true, true, false, false, false, true, true, true]</rosparam>
+		<rosparam param="odom1_config">[true, true, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
 
 
 		<!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
@@ -99,7 +99,7 @@
 		   variables have at least one source of absolute measurement. -->
 		<param name="odom0_differential" value="true"/>
 		<param name="imu0_differential" value="false"/>
-		<param name="odom1_differential" value="true"/>
+		<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 +107,7 @@
 		   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="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. -->
diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index 567d95a..0529577 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -36,15 +36,14 @@
 
 	<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"/>
-		<remap from="vo" to="gps"/>
 		<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="debug" value="true"/>
+		<param name="vo_used" value="false"/>
+		<param name="gps_used" value="true"/>
+		<param name="debug" value="false"/>
 		<param name="sensor_timeout" value="2.0"/>
 	</node>
-	<!-- <include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/> -->
 
 	<include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
 		<arg name="config_file" value="$(find wild_thumper)/cfg/cmd_vel_mux.yaml" />
diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 4064fb8..5056107 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -65,10 +65,10 @@ class MoveBase:
 		(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
 		if pitch > 30*pi/180:
 			val = (100.0/65)*abs(pitch)*180/pi
-			self.set_motor_handicap(0, int(val))
+			self.set_motor_handicap(int(val), 0)
 		elif pitch < -30*pi/180:
 			val = (100.0/65)*abs(pitch)*180/pi
-			self.set_motor_handicap(int(val), 0)
+			self.set_motor_handicap(0, int(val))
 		else:
 			self.set_motor_handicap(0, 0)
 
-- 
2.39.2