From 6441cb5abef8811d7cd68069e1f943b866decac0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 27 Mar 2016 19:35:10 -0700 Subject: [PATCH] Added default covariance values for odometry message as well as use_imu_heading parameter --- .../src/ros_arduino_python/base_controller.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py index 2ae923d..599d3ff 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -28,6 +28,7 @@ from math import sin, cos, pi from geometry_msgs.msg import Quaternion, Twist, Pose from nav_msgs.msg import Odometry from tf.broadcaster import TransformBroadcaster +from sys import float_info """ Class to receive Twist commands and publish Odometry data """ class BaseController: @@ -38,6 +39,7 @@ class BaseController: self.timeout = rospy.get_param("~base_controller_timeout", 1.0) self.odom_linear_scale_correction = rospy.get_param("~odom_linear_scale_correction", 1.0) self.odom_angular_scale_correction = rospy.get_param("~odom_angular_scale_correction", 1.0) + self.use_imu_heading = rospy.get_param("~use_imu_heading", False) self.stopped = False pid_params = dict() @@ -184,6 +186,22 @@ class BaseController: odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth + + """ + Covariance values taken from Kobuki node odometry.hpp at: + https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp + + Pose covariance (required by robot_pose_ekf) TODO: publish realistic values + Odometry yaw covariance must be much bigger than the covariance provided + by the imu, as the later takes much better measures + """ + odom.pose.covariance[0] = 0.1 + odom.pose.covariance[7] = 0.1 + odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2 + + odom.pose.covariance[14] = float_info.max # set a non-zero covariance on unused + odom.pose.covariance[21] = float_info.max # dimensions (z, pitch and roll); this + odom.pose.covariance[28] = float_info.max # is a requirement of robot_pose_ekf self.odomPub.publish(odom)