ekf: gps fixes

diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch
index 683c02108c786d76c4476dfae03686b70adb539c..a6e408dfaf8144cd14b4346c218b81fa805457f9 100644 (file)
--- 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 567d95a3d1bcb9784c24bcf4aadeef89c6bda0fb..05295776e3535c134cf41dd36d67e0048b8b9214 100644 (file)
--- 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 4064fb89a364ec91e9aaa7c9c5da2c8fe5607ade..5056107206766930273850efc4dc6a8ce579a222 100755 (executable)
--- 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)