Gazebo change Covariance Matrix

Question

Hello,

i would like to gave my simulation a bad covariance matrix on my odometry. I use the gazebo-simulation with the plugin "libgazebo_ros_planar_move.so", which you can see on the code below.

     <gazebo>
        <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
            <commandTopic>cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <odometryRate>20</odometryRate>
            <robotBaseFrame>base_link</robotBaseFrame>
        </plugin>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
    </gazebo>

I tried to change the covariance matrix to publish on the odometry topic like this (code snippet from my node):

    # create an odometry msgs
    odom = Odometry()

    odom.header.stamp = rospy.Time.now()
    odom.header.frame_id = frame_id
    odom.child_frame_id = child_frame_id

    odom.pose.pose = data.pose.pose

    #s et calculate Covariance Matrix in message
    odom.pose.covariance[0] = 0.0
    odom.pose.covariance[7] = 0.0
    odom.pose.covariance[14] = 0.0
    odom.pose.covariance[21] = 0.0
    odom.pose.covariance[28] = 0.0
    odom.pose.covariance[35] = 0.0

    #print odom

    odom.twist.twist = data.twist.twist

    # publish the message 
    odom_pub.publish(odom)

But when I use rostopic echo than it seems that both publisher (gazebo and my node) publish at the same time. My publisher set all values of the covariance matrix to 0, but Gazebo changes them back to the ideal values. How can I fix it?

Regards, Markus

---

Originally posted by MarkusHHN on ROS Answers with karma: 54 on 2020-06-03

Post score: 0

Answer 1

Hi @MarkusHHN,

Unfortunately, the covariance implementation in that plugin is hard coded, so there is no way to change that.

As I see, you have here two possible solutions:

1. Naive approach: You generate a node that subscribes to the odometry from that plugin and change the message covariance values then publish in a new topic and in your control nodes read from this new odometry. This is not a good approach in my honest opinion, but hey, it is there, just for you to know.
2. Implement your own plugin: That will allow you to have control over the covariance values. The libgazebo_ros_planar_move is a model plugin, you can implement your own model plugin which can be based on the implementation of the libgazebo_ros_planar_move. This way you will not only have control over the covarriance values but you will be able to implement things like gaussian error, etc.

---

Originally posted by Weasfas with karma: 1695 on 2020-06-05

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by MarkusHHN on 2020-06-06:
Thank you for your answer! I think i use the second idea, because it make more sense, when I change some parameters on my own odometry

Comment by MarkusHHN on 2020-06-07:
all right it worked! Thank you