ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

how to create noisy odom topic in ros C++

asked 2021-08-12 06:34:07 -0500

rcbot gravatar image

updated 2021-08-12 06:35:06 -0500

Hi, I am trying to simulate robot localization in ros c++. For that, I want to add noise to the doom topic available from the gazebo diff drive plugin. I am just wondering how can I add this noise or how to create a noisy odom topic to emulate real-world odometry.

Thank you

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2021-08-12 11:05:06 -0500

Gazebo publishes the odom data on the topic /husky_velocity_controller/odom. You can use that to get access to the odometry data.

Also. if you look inside the control.yaml file inside the husky_gazebo package, you can find the pose_covariance_diagonal parameter. It corresponds to the variances in [x, y, z, roll, pitch,yaw] in the odometry data.

You can then add a gaussian noise by using something like this -

void ParticleFilter::add_gaussian_noise(double &point_, double variance_, double mean = 0){

    double sigma = sqrt(variance_);

    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> d{mean,sigma};

    double noise = d(gen);

    point_ = point_ + noise;

    //return noise;


  void ParticleFilter::add_gaussian_noise(tf2::Quaternion &q_t, vector<double> angular_cov, double mean = 0 ){

    vector<double> pose_v(3);

    for(int i = 0; i < 3; i++){

      double sigma = sqrt(angular_cov[i]);
      std::random_device rd{};
      std::mt19937 gen{rd()};
      std::normal_distribution<> d{mean,sigma};

      pose_v[i] = d(gen);


    tf2::Quaternion quat_tf;;
    quat_tf.setRPY(pose_v[0],pose_v[1], pose_v[2]);

    q_t = q_t + quat_tf;

edit flag offensive delete link more


Hi thanks for the information. But, I am not using the husky robot. I am using my own simple differential drive robot with a gazebo differential drive plugin. I want to apply robot localization for this robot. But, instead of using perfect odometry from the plugin. I want to add a bit of noise to that topic to use it in robot localization.

rcbot gravatar image rcbot  ( 2021-08-12 20:47:57 -0500 )edit

answered 2021-08-12 07:30:55 -0500

Webadone gravatar image


my first idea would be to look at other plugins that are incorporate noise in their sources so like gps simulation or lidar simulation. Did a quick search and found that:

Velodyne Lidar plugin:

GPS Plugin (search for 'noise'):

Maybe thats a starting point.


edit flag offensive delete link more

Question Tools



Asked: 2021-08-12 06:34:07 -0500

Seen: 449 times

Last updated: Aug 12 '21