Gazebo change Covariance Matrix

edit

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