Added new parameters to set the odometry covariance.

cfg/AnaOdom.cfg

@@ -46,5 +46,9 @@ gen.add("wheel_radius",            double_t,  0,                               "
 gen.add("wheel_distance",          double_t,  0,                               "Disatance between wheels",       0.4,      0.1,  10.0)
 gen.add("publish_tf",              bool_t,    0,                               "Whether to publish the TF transform",True)
 gen.add("base_link_frame",         str_t,     0,                               "Base link frame id",             "")
-
+gen.add("x_position_cov",          double_t,  0,                               "X position covariance",          0.1, 0.0, 1.0)
+gen.add("y_position_cov",          double_t,  0,                               "Y position covariance",          0.1, 0.0, 1.0)
+gen.add("heading_cov",             double_t,  0,                               "Heading covariance",             0.1, 0.0, 1.0)
+gen.add("linear_speed_cov",        double_t,  0,                               "Linear speed covariance",        0.1, 0.0, 1.0)
+gen.add("angular_speed_cov",       double_t,  0,                               "Angular speed covariance",       0.1, 0.0, 1.0)
 exit(gen.generate(PACKAGE, "AnaOdomAlgorithm", "AnaOdom"))

config/params.yaml

@@ -4,5 +4,10 @@ prev_gearbox_ratio: 65.5
 transmission_ratio: 1.333
 wheel_radius: 0.11
 wheel_distance: 0.4
-publish_tf: True
+publish_tf: False
 base_link_frame: ana/base_footprint
+x_position_cov: 0.1
+y_position_cov: 0.1
+heading_cov: 0.1
+linear_speed_cov: 0.1
+angular_speed_cov: 0.1

src/ana_odom_alg_node.cpp

@@ -139,12 +139,18 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
   odom_Odometry_msg_.pose.pose.position.y=y;
   odom_Odometry_msg_.pose.pose.position.z=0.0;
   odom_Odometry_msg_.pose.pose.orientation=tf::createQuaternionMsgFromYaw(theta);
+  odom_Odometry_msg_.pose.covariance[0]=this->config_.x_position_cov;
+  odom_Odometry_msg_.pose.covariance[7]=this->config_.y_position_cov;
+  odom_Odometry_msg_.pose.covariance[35]=this->config_.heading_cov;
   odom_Odometry_msg_.twist.twist.linear.x=v_trans;
   odom_Odometry_msg_.twist.twist.linear.y=0.0;
   odom_Odometry_msg_.twist.twist.linear.z=0.0;
   odom_Odometry_msg_.twist.twist.angular.x=0.0;
   odom_Odometry_msg_.twist.twist.angular.y=0.0;
   odom_Odometry_msg_.twist.twist.angular.z=v_rot;
+  odom_Odometry_msg_.twist.covariance[0]=this->config_.linear_speed_cov;
+  odom_Odometry_msg_.twist.covariance[35]=this->config_.angular_speed_cov;
+
   odom_publisher_.publish(odom_Odometry_msg_);
 
   prev_left_enc=msg->left_encoder;