Ask Your Question
0

Gazebo change Covariance Matrix

asked 2020-06-03 09:08:46 -0500

MarkusHHN gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-06-05 06:47:50 -0500

Weasfas gravatar image

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.
edit flag offensive delete link more

Comments

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

MarkusHHN gravatar image MarkusHHN  ( 2020-06-06 09:35:43 -0500 )edit
1

all right it worked! Thank you

MarkusHHN gravatar image MarkusHHN  ( 2020-06-07 03:07:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-06-03 09:08:46 -0500

Seen: 182 times

Last updated: Jun 05 '20