odometry tuning

diff --git a/cfg/razor.yaml b/cfg/razor.yaml
index 65d6d448d7e899f5358db03bbdcea1d045048a39..264e6b6b94646892be65f13ca949de6e2848e9a1 100644 (file)
--- a/cfg/razor.yaml
+++ b/cfg/razor.yaml
@@ -23,8 +23,8 @@ magn_z_max: 596
 
 # extended calibration
 calibration_magn_use_extended: true
-magn_ellipsoid_center: [164.907, 212.010, 16.4033]
-magn_ellipsoid_transform: [[0.977989, 0.00268088, 0.0115993], [0.00268088, 0.974660, 0.0151645], [0.0115993, 0.0151645, 0.982901]]
+magn_ellipsoid_center: [192.863, 93.3924, -413.187]
+magn_ellipsoid_transform: [[0.969535, 0.00462330, 0.0198726], [0.00462330, 0.964774, 0.0291415], [0.0198726, 0.0291415, 0.957085]]
 
 # AHRS to robot calibration
 imu_yaw_calibration: 0.0

diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index cc2db13288403c4ff10ddad91d71f42e89644983..fbd17ec6eac99f9e0b229cc08ef2b0ea5bb73692 100644 (file)
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -8,12 +8,14 @@
                <rosparam command="load" file="$(find wild_thumper)/cfg/analyzers.yaml" />
        </node>
 
+       <node pkg="wild_thumper" type="move_base.py" name="move_base" output="screen" respawn="true">
+               <param name="enable_odom_tf" value="false" />
+       </node>
+
        <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
                <rosparam file="$(find wild_thumper)/cfg/razor.yaml" command="load"/>
        </node>
 
-       <node pkg="wild_thumper" type="move_base.py" name="move_base" output="screen" respawn="true" />
-
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
                <remap from="imu_data" to="imu"/>
                <param name="output_frame" value="odom"/>

diff --git a/scripts/forward_tty.sh b/scripts/forward_tty.sh
new file mode 100755 (executable)
index 0000000..daf6040
--- /dev/null
+++ b/scripts/forward_tty.sh
@@ -0,0 +1,3 @@
+#!/bin/sh
+
+socat /dev/ttyUSB0,b57600 TCP4-LISTEN:10001,reuseaddr

diff --git a/scripts/move_base.py b/scripts/move_base.py
index c4da5d02fa239c586ca2ecc499693aea7a58bd38..09f823b576dd790073b949f52c784ed85ad5dc3f 100755 (executable)
--- a/scripts/move_base.py
+++ b/scripts/move_base.py
@@ -18,7 +18,11 @@ class MoveBase:
                rospy.init_node('wild_thumper_move_base')
                rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
                rospy.Subscriber("imu", Imu, self.imuReceived)
-               #self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
+               enable_odom_tf = rospy.get_param("~enable_odom_tf", True)
+               if enable_odom_tf:
+                       self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
+               else:
+                       self.tf_broadcaster = None
                self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
                self.pub_diag = rospy.Publisher("diagnostics", DiagnosticArray, queue_size=16)
                self.set_speed(0, 0)
@@ -119,7 +123,8 @@ class MoveBase:
                odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
 
                # first, we'll publish the transform over tf
-               #self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_footprint", "odom")
+               if self.tf_broadcaster is not None:
+                       self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_footprint", "odom")
 
                # next, we'll publish the odometry message over ROS
                odom = Odometry()
@@ -136,9 +141,9 @@ class MoveBase:
                odom.pose.pose.orientation.w = odom_quat[3]
                odom.pose.covariance[0] = 1e-3 # x
                odom.pose.covariance[7] = 1e-3 # y
-               odom.pose.covariance[14] = 1e-3 # z
-               odom.pose.covariance[21] = 0.1 # rotation about X axis
-               odom.pose.covariance[28] = 0.1 # rotation about Y axis
+               odom.pose.covariance[14] = 1e6 # z
+               odom.pose.covariance[21] = 1e6 # rotation about X axis
+               odom.pose.covariance[28] = 1e6 # rotation about Y axis
                odom.pose.covariance[35] = 0.1 # rotation about Z axis
 
                # set the velocity
@@ -148,9 +153,9 @@ class MoveBase:
                odom.twist.twist.angular.z = speed_rot
                odom.twist.covariance[0] = 1e-3 # x
                odom.twist.covariance[7] = 1e-3 # y
-               odom.twist.covariance[14] = 1e-3 # z
-               odom.twist.covariance[21] = 0.1 # rotation about X axis
-               odom.twist.covariance[28] = 0.1 # rotation about Y axis
+               odom.twist.covariance[14] = 1e6 # z
+               odom.twist.covariance[21] = 1e6 # rotation about X axis
+               odom.twist.covariance[28] = 1e6 # rotation about Y axis
                odom.twist.covariance[35] = 0.1 # rotation about Z axis
 
                # publish the message