how to create noisy odom topic in ros C++

edit

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;

  }

Hi,

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: https://bitbucket.org/DataspeedInc/ve...

GPS Plugin (search for 'noise'): https://github.com/tu-darmstadt-ros-p...

Maybe thats a starting point.

Regards