1818#include " OdometryPublisher.hh"
1919
2020#include < ignition/msgs/odometry.pb.h>
21+ #include < ignition/msgs/odometry_with_covariance.pb.h>
2122
2223#include < limits>
2324#include < string>
@@ -76,6 +77,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
7677 // / \brief Diff drive odometry message publisher.
7778 public: transport::Node::Publisher odomPub;
7879
80+ // / \brief Diff drive odometry with covariance message publisher.
81+ public: transport::Node::Publisher odomCovPub;
82+
7983 // / \brief Rolling mean accumulators for the linear velocity
8084 public: std::tuple<math::RollingMean, math::RollingMean, math::RollingMean>
8185 linearMean;
@@ -95,6 +99,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
9599
96100 // / \brief Allow specifying constant xyz and rpy offsets
97101 public: ignition::math::Pose3d offset = {0 , 0 , 0 , 0 , 0 , 0 };
102+
103+ // / \brief Gaussian noise
104+ public: double gaussianNoise = 0.0 ;
98105};
99106
100107// ////////////////////////////////////////////////
@@ -158,6 +165,11 @@ void OdometryPublisher::Configure(const Entity &_entity,
158165 " rpy_offset"
159166 }
160167
168+ if (_sdf->HasElement (" gaussian_noise"
169+ {
170+ this ->dataPtr ->gaussianNoise = _sdf->Get <double >(" gaussian_noise"
171+ }
172+
161173 this ->dataPtr ->robotBaseFrame = this ->dataPtr ->model .Name (_ecm)
162174 + " /" " base_footprint"
163175 if (!_sdf->HasElement (" robot_base_frame"
@@ -199,17 +211,38 @@ void OdometryPublisher::Configure(const Entity &_entity,
199211 // Setup odometry
200212 std::string odomTopic{" /model/" this ->dataPtr ->model .Name (_ecm) +
201213 " /odometry"
214+ std::string odomCovTopic{" /model/" this ->dataPtr ->model .Name (_ecm) +
215+ " /odometry_with_covariance"
216+
202217 if (_sdf->HasElement (" odom_topic"
203218 odomTopic = _sdf->Get <std::string>(" odom_topic"
219+ if (_sdf->HasElement (" odom_covariance_topic"
220+ odomCovTopic = _sdf->Get <std::string>(" odom_covariance_topic"
221+
204222 std::string odomTopicValid {transport::TopicUtils::AsValidTopic (odomTopic)};
205223 if (odomTopicValid.empty ())
206224 {
207225 ignerr << " Failed to generate odom topic ["
208226 << odomTopic << " ]"
209- return ;
210227 }
211- this ->dataPtr ->odomPub = this ->dataPtr ->node .Advertise <msgs::Odometry>(
212- odomTopicValid);
228+ else
229+ {
230+ this ->dataPtr ->odomPub = this ->dataPtr ->node .Advertise <msgs::Odometry>(
231+ odomTopicValid);
232+ }
233+
234+ std::string odomCovTopicValid {
235+ transport::TopicUtils::AsValidTopic (odomCovTopic)};
236+ if (odomCovTopicValid.empty ())
237+ {
238+ ignerr << " Failed to generate odom topic ["
239+ << odomCovTopic << " ]"
240+ }
241+ else
242+ {
243+ this ->dataPtr ->odomCovPub = this ->dataPtr ->node .Advertise <
244+ msgs::OdometryWithCovariance>(odomCovTopicValid);
245+ }
213246}
214247
215248// ////////////////////////////////////////////////
@@ -303,9 +336,18 @@ void OdometryPublisherPrivate::UpdateOdometry(
303336 std::get<0 >(this ->linearMean ).Push (linearVelocityX);
304337 std::get<1 >(this ->linearMean ).Push (linearVelocityY);
305338 msg.mutable_twist ()->mutable_linear ()->set_x (
306- std::get<0 >(this ->linearMean ).Mean ());
339+ std::get<0 >(this ->linearMean ).Mean () +
340+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
307341 msg.mutable_twist ()->mutable_linear ()->set_y (
308- std::get<1 >(this ->linearMean ).Mean ());
342+ std::get<1 >(this ->linearMean ).Mean () +
343+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
344+ msg.mutable_twist ()->mutable_linear ()->set_z (
345+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
346+
347+ msg.mutable_twist ()->mutable_angular ()->set_x (
348+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
349+ msg.mutable_twist ()->mutable_angular ()->set_y (
350+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
309351 }
310352 // Get velocities and roll/pitch rates assuming 3D
311353 else if (this ->dimensions == 3 )
@@ -334,21 +376,27 @@ void OdometryPublisherPrivate::UpdateOdometry(
334376 std::get<0 >(this ->angularMean ).Push (rollDiff / dt.count ());
335377 std::get<1 >(this ->angularMean ).Push (pitchDiff / dt.count ());
336378 msg.mutable_twist ()->mutable_linear ()->set_x (
337- std::get<0 >(this ->linearMean ).Mean ());
379+ std::get<0 >(this ->linearMean ).Mean () +
380+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
338381 msg.mutable_twist ()->mutable_linear ()->set_y (
339- std::get<1 >(this ->linearMean ).Mean ());
382+ std::get<1 >(this ->linearMean ).Mean () +
383+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
340384 msg.mutable_twist ()->mutable_linear ()->set_z (
341- std::get<2 >(this ->linearMean ).Mean ());
385+ std::get<2 >(this ->linearMean ).Mean () +
386+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
342387 msg.mutable_twist ()->mutable_angular ()->set_x (
343- std::get<0 >(this ->angularMean ).Mean ());
388+ std::get<0 >(this ->angularMean ).Mean () +
389+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
344390 msg.mutable_twist ()->mutable_angular ()->set_y (
345- std::get<1 >(this ->angularMean ).Mean ());
391+ std::get<1 >(this ->angularMean ).Mean () +
392+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
346393 }
347394
348395 // Set yaw rate
349396 std::get<2 >(this ->angularMean ).Push (yawDiff / dt.count ());
350397 msg.mutable_twist ()->mutable_angular ()->set_z (
351- std::get<2 >(this ->angularMean ).Mean ());
398+ std::get<2 >(this ->angularMean ).Mean () +
399+ ignition::math::Rand::DblNormal (0 , this ->gaussianNoise ));
352400
353401 // Set the time stamp in the header.
354402 msg.mutable_header ()->mutable_stamp ()->CopyFrom (
@@ -373,7 +421,73 @@ void OdometryPublisherPrivate::UpdateOdometry(
373421 return ;
374422 }
375423 this ->lastOdomPubTime = _info.simTime ;
376- this ->odomPub .Publish (msg);
424+ if (this ->odomPub .Valid ())
425+ {
426+ this ->odomPub .Publish (msg);
427+ }
428+
429+ // Generate odometry with covariance message and publish it.
430+ msgs::OdometryWithCovariance msgCovariance;
431+
432+ // Set the time stamp in the header.
433+ msgCovariance.mutable_header ()->mutable_stamp ()->CopyFrom (
434+ convert<msgs::Time>(_info.simTime ));
435+
436+ // Set the frame ids.
437+ frame = msgCovariance.mutable_header ()->add_data ();
438+ frame->set_key (" frame_id"
439+ frame->add_value (odomFrame);
440+ childFrame = msg.mutable_header ()->add_data ();
441+ childFrame->set_key (" child_frame_id"
442+ childFrame->add_value (robotBaseFrame);
443+
444+ // Copy position from odometry msg.
445+ msgCovariance.mutable_pose_with_covariance ()->
446+ mutable_pose ()->mutable_position ()->set_x (msg.pose ().position ().x ());
447+ msgCovariance.mutable_pose_with_covariance ()->
448+ mutable_pose ()->mutable_position ()->set_y (msg.pose ().position ().y ());
449+ msgCovariance.mutable_pose_with_covariance ()->
450+ mutable_pose ()->mutable_position ()->set_z (msg.pose ().position ().z ());
451+
452+ // Copy twist from odometry msg.
453+ msgCovariance.mutable_twist_with_covariance ()->
454+ mutable_twist ()->mutable_angular ()->set_x (msg.twist ().angular ().x ());
455+ msgCovariance.mutable_twist_with_covariance ()->
456+ mutable_twist ()->mutable_angular ()->set_y (msg.twist ().angular ().y ());
457+ msgCovariance.mutable_twist_with_covariance ()->
458+ mutable_twist ()->mutable_angular ()->set_z (msg.twist ().angular ().z ());
459+
460+ msgCovariance.mutable_twist_with_covariance ()->
461+ mutable_twist ()->mutable_linear ()->set_x (msg.twist ().linear ().x ());
462+ msgCovariance.mutable_twist_with_covariance ()->
463+ mutable_twist ()->mutable_linear ()->set_y (msg.twist ().linear ().y ());
464+ msgCovariance.mutable_twist_with_covariance ()->
465+ mutable_twist ()->mutable_linear ()->set_z (msg.twist ().linear ().z ());
466+
467+ // Populate the covariance matrix.
468+ // Should the matrix me populated for pose as well ?
469+ auto gn2 = this ->gaussianNoise * this ->gaussianNoise ;
470+ for (int i = 0 ; i < 36 ; i++)
471+ {
472+ if (i % 7 == 0 )
473+ {
474+ msgCovariance.mutable_pose_with_covariance ()->
475+ mutable_covariance ()->add_data (gn2);
476+ msgCovariance.mutable_twist_with_covariance ()->
477+ mutable_covariance ()->add_data (gn2);
478+ }
479+ else
480+ {
481+ msgCovariance.mutable_pose_with_covariance ()->
482+ mutable_covariance ()->add_data (0 );
483+ msgCovariance.mutable_twist_with_covariance ()->
484+ mutable_covariance ()->add_data (0 );
485+ }
486+ }
487+ if (this ->odomCovPub .Valid ())
488+ {
489+ this ->odomCovPub .Publish (msgCovariance);
490+ }
377491}
378492
379493IGNITION_ADD_PLUGIN (OdometryPublisher,
0 commit comments