30 #include <opencv2/core/core.hpp> 31 #include <opencv2/highgui/highgui.hpp> 35 #include "ConnectGazeboToRosTopic.pb.h" 36 #include "ConnectRosToGazeboTopic.pb.h" 37 #include "PoseStamped.pb.h" 38 #include "PoseWithCovarianceStamped.pb.h" 39 #include "TransformStamped.pb.h" 40 #include "TransformStampedWithFrameIds.pb.h" 41 #include "Vector3dStamped.pb.h" 49 sdf::ElementPtr _sdf) {
51 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
70 if (_sdf->HasElement(
"robotNamespace"))
71 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
73 gzerr <<
"[gazebo_odometry_plugin] Please specify a robotNamespace.\n";
75 node_handle_ = gazebo::transport::NodePtr(
new transport::Node());
80 if (_sdf->HasElement(
"linkName"))
81 link_name_ = _sdf->GetElement(
"linkName")->Get<std::string>();
83 gzerr <<
"[gazebo_odometry_plugin] Please specify a linkName.\n";
86 gzthrow(
"[gazebo_odometry_plugin] Couldn't find specified link \"" 89 if (_sdf->HasElement(
"covarianceImage")) {
90 std::string image_name =
91 _sdf->GetElement(
"covarianceImage")->Get<std::string>();
93 if (covariance_image_.data == NULL)
94 gzerr <<
"loading covariance image " << image_name <<
" failed" 97 gzlog <<
"loading covariance image " << image_name <<
" successful" 101 if (_sdf->HasElement(
"randomEngineSeed")) {
103 _sdf->GetElement(
"randomEngineSeed")->Get<
unsigned int>());
109 getSdfParam<std::string>(_sdf,
"poseWithCovarianceTopic",
122 getSdfParam<SdfVector3>(_sdf,
"noiseNormalPosition", noise_normal_position,
124 getSdfParam<SdfVector3>(_sdf,
"noiseNormalQuaternion",
125 noise_normal_quaternion, zeros3);
126 getSdfParam<SdfVector3>(_sdf,
"noiseNormalLinearVelocity",
127 noise_normal_linear_velocity, zeros3);
128 getSdfParam<SdfVector3>(_sdf,
"noiseNormalAngularVelocity",
129 noise_normal_angular_velocity, zeros3);
130 getSdfParam<SdfVector3>(_sdf,
"noiseUniformPosition", noise_uniform_position,
132 getSdfParam<SdfVector3>(_sdf,
"noiseUniformQuaternion",
133 noise_uniform_quaternion, zeros3);
134 getSdfParam<SdfVector3>(_sdf,
"noiseUniformLinearVelocity",
135 noise_uniform_linear_velocity, zeros3);
136 getSdfParam<SdfVector3>(_sdf,
"noiseUniformAngularVelocity",
137 noise_uniform_angular_velocity, zeros3);
148 gzthrow(
"[gazebo_odometry_plugin] Couldn't find specified parent link \"" 149 << parent_frame_id_ <<
"\".");
162 linear_velocity_n_[1] =
164 linear_velocity_n_[2] =
169 angular_velocity_n_[1] =
171 angular_velocity_n_[2] =
175 noise_uniform_position.X());
177 noise_uniform_position.Y());
179 noise_uniform_position.Z());
182 noise_uniform_quaternion.X());
184 noise_uniform_quaternion.Y());
186 noise_uniform_quaternion.Z());
189 -noise_uniform_linear_velocity.X(), noise_uniform_linear_velocity.X());
191 -noise_uniform_linear_velocity.Y(), noise_uniform_linear_velocity.Y());
193 -noise_uniform_linear_velocity.Z(), noise_uniform_linear_velocity.Z());
196 -noise_uniform_angular_velocity.X(), noise_uniform_angular_velocity.X());
198 -noise_uniform_angular_velocity.Y(), noise_uniform_angular_velocity.Y());
200 -noise_uniform_angular_velocity.Z(), noise_uniform_angular_velocity.Z());
203 Eigen::Map<Eigen::Matrix<double, 6, 6> > pose_covariance(
205 Eigen::Matrix<double, 6, 1> pose_covd;
207 pose_covd << noise_normal_position.X() * noise_normal_position.X(),
208 noise_normal_position.Y() * noise_normal_position.Y(),
209 noise_normal_position.Z() * noise_normal_position.Z(),
210 noise_normal_quaternion.X() * noise_normal_quaternion.X(),
211 noise_normal_quaternion.Y() * noise_normal_quaternion.Y(),
212 noise_normal_quaternion.Z() * noise_normal_quaternion.Z();
213 pose_covariance = pose_covd.asDiagonal();
216 Eigen::Map<Eigen::Matrix<double, 6, 6> > twist_covariance(
218 Eigen::Matrix<double, 6, 1> twist_covd;
220 twist_covd << noise_normal_linear_velocity.X() *
221 noise_normal_linear_velocity.X(),
222 noise_normal_linear_velocity.Y() * noise_normal_linear_velocity.Y(),
223 noise_normal_linear_velocity.Z() * noise_normal_linear_velocity.Z(),
224 noise_normal_angular_velocity.X() * noise_normal_angular_velocity.X(),
225 noise_normal_angular_velocity.Y() * noise_normal_angular_velocity.Y(),
226 noise_normal_angular_velocity.Z() * noise_normal_angular_velocity.Z();
227 twist_covariance = twist_covd.asDiagonal();
238 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
248 ignition::math::Pose3d W_pose_W_C =
link_->WorldCoGPose();
249 ignition::math::Vector3d C_linear_velocity_W_C =
link_->RelativeLinearVel();
250 ignition::math::Vector3d C_angular_velocity_W_C =
link_->RelativeAngularVel();
252 ignition::math::Vector3d gazebo_linear_velocity = C_linear_velocity_W_C;
253 ignition::math::Vector3d gazebo_angular_velocity = C_angular_velocity_W_C;
254 ignition::math::Pose3d gazebo_pose = W_pose_W_C;
257 ignition::math::Pose3d W_pose_W_P =
parent_link_->WorldPose();
258 ignition::math::Vector3d P_linear_velocity_W_P =
parent_link_->RelativeLinearVel();
259 ignition::math::Vector3d P_angular_velocity_W_P =
261 ignition::math::Pose3d C_pose_P_C_ = W_pose_W_C - W_pose_W_P;
262 ignition::math::Vector3d C_linear_velocity_P_C;
267 C_linear_velocity_P_C =
268 -C_pose_P_C_.Rot().Inverse() *
269 P_angular_velocity_W_P.Cross(C_pose_P_C_.Pos()) +
270 C_linear_velocity_W_C -
271 C_pose_P_C_.Rot().Inverse() * P_linear_velocity_W_P;
275 gazebo_angular_velocity =
276 C_angular_velocity_W_C -
277 C_pose_P_C_.Rot().Inverse() * P_angular_velocity_W_P;
278 gazebo_linear_velocity = C_linear_velocity_P_C;
279 gazebo_pose = C_pose_P_C_;
283 bool publish_odometry =
true;
292 int x =
static_cast<int>(
295 int y =
static_cast<int>(
299 if (x >= 0 && x < width && y >= 0 && y < height) {
301 if (pixel_value == 0) {
302 publish_odometry =
false;
310 gz_geometry_msgs::Odometry odometry;
312 odometry.mutable_header()->mutable_stamp()->set_sec(
314 odometry.mutable_header()->mutable_stamp()->set_nsec(
318 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_x(
319 gazebo_pose.Pos().X());
320 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_y(
321 gazebo_pose.Pos().Y());
322 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_z(
323 gazebo_pose.Pos().Z());
325 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_x(
326 gazebo_pose.Rot().X());
327 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_y(
328 gazebo_pose.Rot().Y());
329 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_z(
330 gazebo_pose.Rot().Z());
331 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_w(
332 gazebo_pose.Rot().W());
334 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_x(
335 gazebo_linear_velocity.X());
336 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_y(
337 gazebo_linear_velocity.Y());
338 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_z(
339 gazebo_linear_velocity.Z());
341 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_x(
342 gazebo_angular_velocity.X());
343 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_y(
344 gazebo_angular_velocity.Y());
345 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_z(
346 gazebo_angular_velocity.Z());
348 if (publish_odometry)
356 gz_geometry_msgs::Odometry odometry_msg(
odometry_queue_.front().second);
362 Eigen::Vector3d pos_n;
368 gazebo::msgs::Vector3d* p =
369 odometry_msg.mutable_pose()->mutable_pose()->mutable_position();
370 p->set_x(p->x() + pos_n[0]);
371 p->set_y(p->y() + pos_n[1]);
372 p->set_z(p->z() + pos_n[2]);
375 Eigen::Vector3d theta;
383 gazebo::msgs::Quaternion* q_W_L =
384 odometry_msg.mutable_pose()->mutable_pose()->mutable_orientation();
386 Eigen::Quaterniond _q_W_L(q_W_L->w(), q_W_L->x(), q_W_L->y(), q_W_L->z());
387 _q_W_L = _q_W_L * q_n;
388 q_W_L->set_w(_q_W_L.w());
389 q_W_L->set_x(_q_W_L.x());
390 q_W_L->set_y(_q_W_L.y());
391 q_W_L->set_z(_q_W_L.z());
394 Eigen::Vector3d linear_velocity_n;
402 gazebo::msgs::Vector3d* linear_velocity =
403 odometry_msg.mutable_twist()->mutable_twist()->mutable_linear();
405 linear_velocity->set_x(linear_velocity->x() + linear_velocity_n[0]);
406 linear_velocity->set_y(linear_velocity->y() + linear_velocity_n[1]);
407 linear_velocity->set_z(linear_velocity->z() + linear_velocity_n[2]);
410 Eigen::Vector3d angular_velocity_n;
418 gazebo::msgs::Vector3d* angular_velocity =
419 odometry_msg.mutable_twist()->mutable_twist()->mutable_angular();
421 angular_velocity->set_x(angular_velocity->x() + angular_velocity_n[0]);
422 angular_velocity->set_y(angular_velocity->y() + angular_velocity_n[1]);
423 angular_velocity->set_z(angular_velocity->z() + angular_velocity_n[2]);
425 odometry_msg.mutable_pose()->mutable_covariance()->Clear();
427 odometry_msg.mutable_pose()->mutable_covariance()->Add(
431 odometry_msg.mutable_twist()->mutable_covariance()->Clear();
433 odometry_msg.mutable_twist()->mutable_covariance()->Add(
439 pose_pub_->Publish(odometry_msg.pose().pose());
443 gz_geometry_msgs::PoseWithCovarianceStamped
444 pose_with_covariance_stamped_msg;
446 pose_with_covariance_stamped_msg.mutable_header()->CopyFrom(
447 odometry_msg.header());
448 pose_with_covariance_stamped_msg.mutable_pose_with_covariance()->CopyFrom(
449 odometry_msg.pose());
452 pose_with_covariance_stamped_msg);
456 gz_geometry_msgs::Vector3dStamped position_stamped_msg;
457 position_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
458 position_stamped_msg.mutable_position()->CopyFrom(
459 odometry_msg.pose().pose().position());
465 gz_geometry_msgs::TransformStamped transform_stamped_msg;
467 transform_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
468 transform_stamped_msg.mutable_transform()->mutable_translation()->set_x(
470 transform_stamped_msg.mutable_transform()->mutable_translation()->set_y(
472 transform_stamped_msg.mutable_transform()->mutable_translation()->set_z(
474 transform_stamped_msg.mutable_transform()->mutable_rotation()->CopyFrom(
489 gz_geometry_msgs::TransformStampedWithFrameIds
490 transform_stamped_with_frame_ids_msg;
491 transform_stamped_with_frame_ids_msg.mutable_header()->CopyFrom(
492 odometry_msg.header());
493 transform_stamped_with_frame_ids_msg.mutable_transform()
494 ->mutable_translation()
496 transform_stamped_with_frame_ids_msg.mutable_transform()
497 ->mutable_translation()
499 transform_stamped_with_frame_ids_msg.mutable_transform()
500 ->mutable_translation()
502 transform_stamped_with_frame_ids_msg.mutable_transform()
506 transform_stamped_with_frame_ids_msg.set_child_frame_id(
child_frame_id_);
517 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
518 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
521 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
530 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
532 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
534 connect_gazebo_to_ros_topic_msg.set_msgtype(
535 gz_std_msgs::ConnectGazeboToRosTopic::POSE);
536 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
544 node_handle_->Advertise<gz_geometry_msgs::PoseWithCovarianceStamped>(
547 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
548 "~/" +
namespace_ +
"/" + pose_with_covariance_stamped_pub_topic_);
549 connect_gazebo_to_ros_topic_msg.set_ros_topic(
550 namespace_ +
"/" + pose_with_covariance_stamped_pub_topic_);
551 connect_gazebo_to_ros_topic_msg.set_msgtype(
552 gz_std_msgs::ConnectGazeboToRosTopic::POSE_WITH_COVARIANCE_STAMPED);
553 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
561 node_handle_->Advertise<gz_geometry_msgs::Vector3dStamped>(
564 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
565 position_stamped_pub_topic_);
566 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
567 position_stamped_pub_topic_);
568 connect_gazebo_to_ros_topic_msg.set_msgtype(
569 gz_std_msgs::ConnectGazeboToRosTopic::VECTOR_3D_STAMPED);
570 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
580 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
581 odometry_pub_topic_);
582 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
583 odometry_pub_topic_);
584 connect_gazebo_to_ros_topic_msg.set_msgtype(
585 gz_std_msgs::ConnectGazeboToRosTopic::ODOMETRY);
586 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
594 node_handle_->Advertise<gz_geometry_msgs::TransformStamped>(
597 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
598 "~/" +
namespace_ +
"/" + transform_stamped_pub_topic_);
599 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
600 transform_stamped_pub_topic_);
601 connect_gazebo_to_ros_topic_msg.set_msgtype(
602 gz_std_msgs::ConnectGazeboToRosTopic::TRANSFORM_STAMPED);
603 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
611 node_handle_->Advertise<gz_geometry_msgs::TransformStampedWithFrameIds>(
physics::EntityPtr parent_link_
double covariance_image_scale_
UniformDistribution angular_velocity_u_[3]
NormalDistribution attitude_n_[3]
gazebo::transport::PublisherPtr position_stamped_pub_
gazebo::transport::PublisherPtr transform_stamped_pub_
gazebo::transport::PublisherPtr odometry_pub_
UniformDistribution position_u_[3]
std::string transform_stamped_pub_topic_
gazebo::transport::PublisherPtr pose_pub_
UniformDistribution linear_velocity_u_[3]
std::string pose_with_covariance_stamped_pub_topic_
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
std::string pose_pub_topic_
NormalDistribution linear_velocity_n_[3]
NormalDistribution position_n_[3]
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
cv::Mat covariance_image_
static const bool kPrintOnPluginLoad
std::string parent_frame_id_
std::string position_stamped_pub_topic_
Eigen::Quaternion< typename Derived::Scalar > QuaternionFromSmallAngle(const Eigen::MatrixBase< Derived > &theta)
Computes a quaternion from the 3-element small angle approximation theta.
std::string child_frame_id_
CovarianceMatrix twist_covariance_matrix_
OdometryQueue odometry_queue_
gazebo::transport::PublisherPtr pose_with_covariance_stamped_pub_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
NormalDistribution angular_velocity_n_[3]
static const bool kPrintOnUpdates
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
UniformDistribution attitude_u_[3]
std::normal_distribution NormalDistribution
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
double time_since_epoch()
static const std::string kConnectGazeboToRosSubtopic
CovarianceMatrix pose_covariance_matrix_
std::mt19937 random_generator_
void OnUpdate(const common::UpdateInfo &)
static const std::string kBroadcastTransformSubtopic
Special-case topic for ROS interface plugin to listen to (if present) and broadcast transforms to the...
gazebo::transport::NodePtr node_handle_
std::string odometry_pub_topic_
static const std::string kDefaultParentFrameId
std::uniform_real_distribution UniformDistribution
gazebo::transport::PublisherPtr broadcast_transform_pub_
Special-case publisher to publish stamped transforms with frame IDs. The ROS interface plugin (if pre...