00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "rotors_gazebo_plugins/gazebo_odometry_plugin.h"
00024
00025
00026 #include <chrono>
00027 #include <iostream>
00028
00029
00030 #include <opencv2/core/core.hpp>
00031 #include <opencv2/highgui/highgui.hpp>
00032
00033
00034 #include <rotors_gazebo_plugins/common.h>
00035 #include "ConnectGazeboToRosTopic.pb.h"
00036 #include "ConnectRosToGazeboTopic.pb.h"
00037 #include "PoseStamped.pb.h"
00038 #include "PoseWithCovarianceStamped.pb.h"
00039 #include "TransformStamped.pb.h"
00040 #include "TransformStampedWithFrameIds.pb.h"
00041 #include "Vector3dStamped.pb.h"
00042
00043 namespace gazebo {
00044
00045 GazeboOdometryPlugin::~GazeboOdometryPlugin() {
00046 }
00047
00048 void GazeboOdometryPlugin::Load(physics::ModelPtr _model,
00049 sdf::ElementPtr _sdf) {
00050 if (kPrintOnPluginLoad) {
00051 gzdbg << __FUNCTION__ << "() called." << std::endl;
00052 }
00053
00054
00055 model_ = _model;
00056 world_ = model_->GetWorld();
00057
00058 SdfVector3 noise_normal_position;
00059 SdfVector3 noise_normal_quaternion;
00060 SdfVector3 noise_normal_linear_velocity;
00061 SdfVector3 noise_normal_angular_velocity;
00062 SdfVector3 noise_uniform_position;
00063 SdfVector3 noise_uniform_quaternion;
00064 SdfVector3 noise_uniform_linear_velocity;
00065 SdfVector3 noise_uniform_angular_velocity;
00066 const SdfVector3 zeros3(0.0, 0.0, 0.0);
00067
00068 odometry_queue_.clear();
00069
00070 if (_sdf->HasElement("robotNamespace"))
00071 namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00072 else
00073 gzerr << "[gazebo_odometry_plugin] Please specify a robotNamespace.\n";
00074
00075 node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00076
00077
00078 node_handle_->Init();
00079
00080 if (_sdf->HasElement("linkName"))
00081 link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
00082 else
00083 gzerr << "[gazebo_odometry_plugin] Please specify a linkName.\n";
00084 link_ = model_->GetLink(link_name_);
00085 if (link_ == NULL)
00086 gzthrow("[gazebo_odometry_plugin] Couldn't find specified link \""
00087 << link_name_ << "\".");
00088
00089 if (_sdf->HasElement("covarianceImage")) {
00090 std::string image_name =
00091 _sdf->GetElement("covarianceImage")->Get<std::string>();
00092 covariance_image_ = cv::imread(image_name, CV_LOAD_IMAGE_GRAYSCALE);
00093 if (covariance_image_.data == NULL)
00094 gzerr << "loading covariance image " << image_name << " failed"
00095 << std::endl;
00096 else
00097 gzlog << "loading covariance image " << image_name << " successful"
00098 << std::endl;
00099 }
00100
00101 if (_sdf->HasElement("randomEngineSeed")) {
00102 random_generator_.seed(
00103 _sdf->GetElement("randomEngineSeed")->Get<unsigned int>());
00104 } else {
00105 random_generator_.seed(
00106 std::chrono::system_clock::now().time_since_epoch().count());
00107 }
00108 getSdfParam<std::string>(_sdf, "poseTopic", pose_pub_topic_, pose_pub_topic_);
00109 getSdfParam<std::string>(_sdf, "poseWithCovarianceTopic",
00110 pose_with_covariance_stamped_pub_topic_,
00111 pose_with_covariance_stamped_pub_topic_);
00112 getSdfParam<std::string>(_sdf, "positionTopic", position_stamped_pub_topic_,
00113 position_stamped_pub_topic_);
00114 getSdfParam<std::string>(_sdf, "transformTopic", transform_stamped_pub_topic_,
00115 transform_stamped_pub_topic_);
00116 getSdfParam<std::string>(_sdf, "odometryTopic", odometry_pub_topic_,
00117 odometry_pub_topic_);
00118 getSdfParam<std::string>(_sdf, "parentFrameId", parent_frame_id_,
00119 parent_frame_id_);
00120 getSdfParam<std::string>(_sdf, "childFrameId", child_frame_id_,
00121 child_frame_id_);
00122 getSdfParam<SdfVector3>(_sdf, "noiseNormalPosition", noise_normal_position,
00123 zeros3);
00124 getSdfParam<SdfVector3>(_sdf, "noiseNormalQuaternion",
00125 noise_normal_quaternion, zeros3);
00126 getSdfParam<SdfVector3>(_sdf, "noiseNormalLinearVelocity",
00127 noise_normal_linear_velocity, zeros3);
00128 getSdfParam<SdfVector3>(_sdf, "noiseNormalAngularVelocity",
00129 noise_normal_angular_velocity, zeros3);
00130 getSdfParam<SdfVector3>(_sdf, "noiseUniformPosition", noise_uniform_position,
00131 zeros3);
00132 getSdfParam<SdfVector3>(_sdf, "noiseUniformQuaternion",
00133 noise_uniform_quaternion, zeros3);
00134 getSdfParam<SdfVector3>(_sdf, "noiseUniformLinearVelocity",
00135 noise_uniform_linear_velocity, zeros3);
00136 getSdfParam<SdfVector3>(_sdf, "noiseUniformAngularVelocity",
00137 noise_uniform_angular_velocity, zeros3);
00138 getSdfParam<int>(_sdf, "measurementDelay", measurement_delay_,
00139 measurement_delay_);
00140 getSdfParam<int>(_sdf, "measurementDivisor", measurement_divisor_,
00141 measurement_divisor_);
00142 getSdfParam<double>(_sdf, "unknownDelay", unknown_delay_, unknown_delay_);
00143 getSdfParam<double>(_sdf, "covarianceImageScale", covariance_image_scale_,
00144 covariance_image_scale_);
00145
00146 parent_link_ = world_->EntityByName(parent_frame_id_);
00147 if (parent_link_ == NULL && parent_frame_id_ != kDefaultParentFrameId) {
00148 gzthrow("[gazebo_odometry_plugin] Couldn't find specified parent link \""
00149 << parent_frame_id_ << "\".");
00150 }
00151
00152 position_n_[0] = NormalDistribution(0, noise_normal_position.X());
00153 position_n_[1] = NormalDistribution(0, noise_normal_position.Y());
00154 position_n_[2] = NormalDistribution(0, noise_normal_position.Z());
00155
00156 attitude_n_[0] = NormalDistribution(0, noise_normal_quaternion.X());
00157 attitude_n_[1] = NormalDistribution(0, noise_normal_quaternion.Y());
00158 attitude_n_[2] = NormalDistribution(0, noise_normal_quaternion.Z());
00159
00160 linear_velocity_n_[0] =
00161 NormalDistribution(0, noise_normal_linear_velocity.X());
00162 linear_velocity_n_[1] =
00163 NormalDistribution(0, noise_normal_linear_velocity.Y());
00164 linear_velocity_n_[2] =
00165 NormalDistribution(0, noise_normal_linear_velocity.Z());
00166
00167 angular_velocity_n_[0] =
00168 NormalDistribution(0, noise_normal_angular_velocity.X());
00169 angular_velocity_n_[1] =
00170 NormalDistribution(0, noise_normal_angular_velocity.Y());
00171 angular_velocity_n_[2] =
00172 NormalDistribution(0, noise_normal_angular_velocity.Z());
00173
00174 position_u_[0] = UniformDistribution(-noise_uniform_position.X(),
00175 noise_uniform_position.X());
00176 position_u_[1] = UniformDistribution(-noise_uniform_position.Y(),
00177 noise_uniform_position.Y());
00178 position_u_[2] = UniformDistribution(-noise_uniform_position.Z(),
00179 noise_uniform_position.Z());
00180
00181 attitude_u_[0] = UniformDistribution(-noise_uniform_quaternion.X(),
00182 noise_uniform_quaternion.X());
00183 attitude_u_[1] = UniformDistribution(-noise_uniform_quaternion.Y(),
00184 noise_uniform_quaternion.Y());
00185 attitude_u_[2] = UniformDistribution(-noise_uniform_quaternion.Z(),
00186 noise_uniform_quaternion.Z());
00187
00188 linear_velocity_u_[0] = UniformDistribution(
00189 -noise_uniform_linear_velocity.X(), noise_uniform_linear_velocity.X());
00190 linear_velocity_u_[1] = UniformDistribution(
00191 -noise_uniform_linear_velocity.Y(), noise_uniform_linear_velocity.Y());
00192 linear_velocity_u_[2] = UniformDistribution(
00193 -noise_uniform_linear_velocity.Z(), noise_uniform_linear_velocity.Z());
00194
00195 angular_velocity_u_[0] = UniformDistribution(
00196 -noise_uniform_angular_velocity.X(), noise_uniform_angular_velocity.X());
00197 angular_velocity_u_[1] = UniformDistribution(
00198 -noise_uniform_angular_velocity.Y(), noise_uniform_angular_velocity.Y());
00199 angular_velocity_u_[2] = UniformDistribution(
00200 -noise_uniform_angular_velocity.Z(), noise_uniform_angular_velocity.Z());
00201
00202
00203 Eigen::Map<Eigen::Matrix<double, 6, 6> > pose_covariance(
00204 pose_covariance_matrix_.data());
00205 Eigen::Matrix<double, 6, 1> pose_covd;
00206
00207 pose_covd << noise_normal_position.X() * noise_normal_position.X(),
00208 noise_normal_position.Y() * noise_normal_position.Y(),
00209 noise_normal_position.Z() * noise_normal_position.Z(),
00210 noise_normal_quaternion.X() * noise_normal_quaternion.X(),
00211 noise_normal_quaternion.Y() * noise_normal_quaternion.Y(),
00212 noise_normal_quaternion.Z() * noise_normal_quaternion.Z();
00213 pose_covariance = pose_covd.asDiagonal();
00214
00215
00216 Eigen::Map<Eigen::Matrix<double, 6, 6> > twist_covariance(
00217 twist_covariance_matrix_.data());
00218 Eigen::Matrix<double, 6, 1> twist_covd;
00219
00220 twist_covd << noise_normal_linear_velocity.X() *
00221 noise_normal_linear_velocity.X(),
00222 noise_normal_linear_velocity.Y() * noise_normal_linear_velocity.Y(),
00223 noise_normal_linear_velocity.Z() * noise_normal_linear_velocity.Z(),
00224 noise_normal_angular_velocity.X() * noise_normal_angular_velocity.X(),
00225 noise_normal_angular_velocity.Y() * noise_normal_angular_velocity.Y(),
00226 noise_normal_angular_velocity.Z() * noise_normal_angular_velocity.Z();
00227 twist_covariance = twist_covd.asDiagonal();
00228
00229
00230
00231 updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00232 boost::bind(&GazeboOdometryPlugin::OnUpdate, this, _1));
00233 }
00234
00235
00236 void GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) {
00237 if (kPrintOnUpdates) {
00238 gzdbg << __FUNCTION__ << "() called." << std::endl;
00239 }
00240
00241 if (!pubs_and_subs_created_) {
00242 CreatePubsAndSubs();
00243 pubs_and_subs_created_ = true;
00244 }
00245
00246
00247
00248 ignition::math::Pose3d W_pose_W_C = link_->WorldCoGPose();
00249 ignition::math::Vector3d C_linear_velocity_W_C = link_->RelativeLinearVel();
00250 ignition::math::Vector3d C_angular_velocity_W_C = link_->RelativeAngularVel();
00251
00252 ignition::math::Vector3d gazebo_linear_velocity = C_linear_velocity_W_C;
00253 ignition::math::Vector3d gazebo_angular_velocity = C_angular_velocity_W_C;
00254 ignition::math::Pose3d gazebo_pose = W_pose_W_C;
00255
00256 if (parent_frame_id_ != kDefaultParentFrameId) {
00257 ignition::math::Pose3d W_pose_W_P = parent_link_->WorldPose();
00258 ignition::math::Vector3d P_linear_velocity_W_P = parent_link_->RelativeLinearVel();
00259 ignition::math::Vector3d P_angular_velocity_W_P =
00260 parent_link_->RelativeAngularVel();
00261 ignition::math::Pose3d C_pose_P_C_ = W_pose_W_C - W_pose_W_P;
00262 ignition::math::Vector3d C_linear_velocity_P_C;
00263
00264
00265
00266
00267 C_linear_velocity_P_C =
00268 -C_pose_P_C_.Rot().Inverse() *
00269 P_angular_velocity_W_P.Cross(C_pose_P_C_.Pos()) +
00270 C_linear_velocity_W_C -
00271 C_pose_P_C_.Rot().Inverse() * P_linear_velocity_W_P;
00272
00273
00274
00275 gazebo_angular_velocity =
00276 C_angular_velocity_W_C -
00277 C_pose_P_C_.Rot().Inverse() * P_angular_velocity_W_P;
00278 gazebo_linear_velocity = C_linear_velocity_P_C;
00279 gazebo_pose = C_pose_P_C_;
00280 }
00281
00282
00283 bool publish_odometry = true;
00284
00285
00286 if (covariance_image_.data != NULL) {
00287
00288
00289
00290 int width = covariance_image_.cols;
00291 int height = covariance_image_.rows;
00292 int x = static_cast<int>(
00293 std::floor(gazebo_pose.Pos().X() / covariance_image_scale_)) +
00294 width / 2;
00295 int y = static_cast<int>(
00296 std::floor(gazebo_pose.Pos().Y() / covariance_image_scale_)) +
00297 height / 2;
00298
00299 if (x >= 0 && x < width && y >= 0 && y < height) {
00300 uint8_t pixel_value = covariance_image_.at<uint8_t>(y, x);
00301 if (pixel_value == 0) {
00302 publish_odometry = false;
00303
00304
00305 }
00306 }
00307 }
00308
00309 if (gazebo_sequence_ % measurement_divisor_ == 0) {
00310 gz_geometry_msgs::Odometry odometry;
00311 odometry.mutable_header()->set_frame_id(parent_frame_id_);
00312 odometry.mutable_header()->mutable_stamp()->set_sec(
00313 (world_->SimTime()).sec + static_cast<int32_t>(unknown_delay_));
00314 odometry.mutable_header()->mutable_stamp()->set_nsec(
00315 (world_->SimTime()).nsec + static_cast<int32_t>(unknown_delay_));
00316 odometry.set_child_frame_id(child_frame_id_);
00317
00318 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_x(
00319 gazebo_pose.Pos().X());
00320 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_y(
00321 gazebo_pose.Pos().Y());
00322 odometry.mutable_pose()->mutable_pose()->mutable_position()->set_z(
00323 gazebo_pose.Pos().Z());
00324
00325 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_x(
00326 gazebo_pose.Rot().X());
00327 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_y(
00328 gazebo_pose.Rot().Y());
00329 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_z(
00330 gazebo_pose.Rot().Z());
00331 odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_w(
00332 gazebo_pose.Rot().W());
00333
00334 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_x(
00335 gazebo_linear_velocity.X());
00336 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_y(
00337 gazebo_linear_velocity.Y());
00338 odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_z(
00339 gazebo_linear_velocity.Z());
00340
00341 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_x(
00342 gazebo_angular_velocity.X());
00343 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_y(
00344 gazebo_angular_velocity.Y());
00345 odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_z(
00346 gazebo_angular_velocity.Z());
00347
00348 if (publish_odometry)
00349 odometry_queue_.push_back(
00350 std::make_pair(gazebo_sequence_ + measurement_delay_, odometry));
00351 }
00352
00353
00354 if (gazebo_sequence_ == odometry_queue_.front().first) {
00355
00356 gz_geometry_msgs::Odometry odometry_msg(odometry_queue_.front().second);
00357
00358
00359 odometry_queue_.pop_front();
00360
00361
00362 Eigen::Vector3d pos_n;
00363 pos_n << position_n_[0](random_generator_) +
00364 position_u_[0](random_generator_),
00365 position_n_[1](random_generator_) + position_u_[1](random_generator_),
00366 position_n_[2](random_generator_) + position_u_[2](random_generator_);
00367
00368 gazebo::msgs::Vector3d* p =
00369 odometry_msg.mutable_pose()->mutable_pose()->mutable_position();
00370 p->set_x(p->x() + pos_n[0]);
00371 p->set_y(p->y() + pos_n[1]);
00372 p->set_z(p->z() + pos_n[2]);
00373
00374
00375 Eigen::Vector3d theta;
00376 theta << attitude_n_[0](random_generator_) +
00377 attitude_u_[0](random_generator_),
00378 attitude_n_[1](random_generator_) + attitude_u_[1](random_generator_),
00379 attitude_n_[2](random_generator_) + attitude_u_[2](random_generator_);
00380 Eigen::Quaterniond q_n = QuaternionFromSmallAngle(theta);
00381 q_n.normalize();
00382
00383 gazebo::msgs::Quaternion* q_W_L =
00384 odometry_msg.mutable_pose()->mutable_pose()->mutable_orientation();
00385
00386 Eigen::Quaterniond _q_W_L(q_W_L->w(), q_W_L->x(), q_W_L->y(), q_W_L->z());
00387 _q_W_L = _q_W_L * q_n;
00388 q_W_L->set_w(_q_W_L.w());
00389 q_W_L->set_x(_q_W_L.x());
00390 q_W_L->set_y(_q_W_L.y());
00391 q_W_L->set_z(_q_W_L.z());
00392
00393
00394 Eigen::Vector3d linear_velocity_n;
00395 linear_velocity_n << linear_velocity_n_[0](random_generator_) +
00396 linear_velocity_u_[0](random_generator_),
00397 linear_velocity_n_[1](random_generator_) +
00398 linear_velocity_u_[1](random_generator_),
00399 linear_velocity_n_[2](random_generator_) +
00400 linear_velocity_u_[2](random_generator_);
00401
00402 gazebo::msgs::Vector3d* linear_velocity =
00403 odometry_msg.mutable_twist()->mutable_twist()->mutable_linear();
00404
00405 linear_velocity->set_x(linear_velocity->x() + linear_velocity_n[0]);
00406 linear_velocity->set_y(linear_velocity->y() + linear_velocity_n[1]);
00407 linear_velocity->set_z(linear_velocity->z() + linear_velocity_n[2]);
00408
00409
00410 Eigen::Vector3d angular_velocity_n;
00411 angular_velocity_n << angular_velocity_n_[0](random_generator_) +
00412 angular_velocity_u_[0](random_generator_),
00413 angular_velocity_n_[1](random_generator_) +
00414 angular_velocity_u_[1](random_generator_),
00415 angular_velocity_n_[2](random_generator_) +
00416 angular_velocity_u_[2](random_generator_);
00417
00418 gazebo::msgs::Vector3d* angular_velocity =
00419 odometry_msg.mutable_twist()->mutable_twist()->mutable_angular();
00420
00421 angular_velocity->set_x(angular_velocity->x() + angular_velocity_n[0]);
00422 angular_velocity->set_y(angular_velocity->y() + angular_velocity_n[1]);
00423 angular_velocity->set_z(angular_velocity->z() + angular_velocity_n[2]);
00424
00425 odometry_msg.mutable_pose()->mutable_covariance()->Clear();
00426 for (int i = 0; i < pose_covariance_matrix_.size(); i++) {
00427 odometry_msg.mutable_pose()->mutable_covariance()->Add(
00428 pose_covariance_matrix_[i]);
00429 }
00430
00431 odometry_msg.mutable_twist()->mutable_covariance()->Clear();
00432 for (int i = 0; i < twist_covariance_matrix_.size(); i++) {
00433 odometry_msg.mutable_twist()->mutable_covariance()->Add(
00434 twist_covariance_matrix_[i]);
00435 }
00436
00437
00438 if (pose_pub_->HasConnections()) {
00439 pose_pub_->Publish(odometry_msg.pose().pose());
00440 }
00441
00442 if (pose_with_covariance_stamped_pub_->HasConnections()) {
00443 gz_geometry_msgs::PoseWithCovarianceStamped
00444 pose_with_covariance_stamped_msg;
00445
00446 pose_with_covariance_stamped_msg.mutable_header()->CopyFrom(
00447 odometry_msg.header());
00448 pose_with_covariance_stamped_msg.mutable_pose_with_covariance()->CopyFrom(
00449 odometry_msg.pose());
00450
00451 pose_with_covariance_stamped_pub_->Publish(
00452 pose_with_covariance_stamped_msg);
00453 }
00454
00455 if (position_stamped_pub_->HasConnections()) {
00456 gz_geometry_msgs::Vector3dStamped position_stamped_msg;
00457 position_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
00458 position_stamped_msg.mutable_position()->CopyFrom(
00459 odometry_msg.pose().pose().position());
00460
00461 position_stamped_pub_->Publish(position_stamped_msg);
00462 }
00463
00464 if (transform_stamped_pub_->HasConnections()) {
00465 gz_geometry_msgs::TransformStamped transform_stamped_msg;
00466
00467 transform_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
00468 transform_stamped_msg.mutable_transform()->mutable_translation()->set_x(
00469 p->x());
00470 transform_stamped_msg.mutable_transform()->mutable_translation()->set_y(
00471 p->y());
00472 transform_stamped_msg.mutable_transform()->mutable_translation()->set_z(
00473 p->z());
00474 transform_stamped_msg.mutable_transform()->mutable_rotation()->CopyFrom(
00475 *q_W_L);
00476
00477 transform_stamped_pub_->Publish(transform_stamped_msg);
00478 }
00479
00480 if (odometry_pub_->HasConnections()) {
00481
00482 odometry_pub_->Publish(odometry_msg);
00483 }
00484
00485
00486
00487
00488
00489 gz_geometry_msgs::TransformStampedWithFrameIds
00490 transform_stamped_with_frame_ids_msg;
00491 transform_stamped_with_frame_ids_msg.mutable_header()->CopyFrom(
00492 odometry_msg.header());
00493 transform_stamped_with_frame_ids_msg.mutable_transform()
00494 ->mutable_translation()
00495 ->set_x(p->x());
00496 transform_stamped_with_frame_ids_msg.mutable_transform()
00497 ->mutable_translation()
00498 ->set_y(p->y());
00499 transform_stamped_with_frame_ids_msg.mutable_transform()
00500 ->mutable_translation()
00501 ->set_z(p->z());
00502 transform_stamped_with_frame_ids_msg.mutable_transform()
00503 ->mutable_rotation()
00504 ->CopyFrom(*q_W_L);
00505 transform_stamped_with_frame_ids_msg.set_parent_frame_id(parent_frame_id_);
00506 transform_stamped_with_frame_ids_msg.set_child_frame_id(child_frame_id_);
00507
00508 broadcast_transform_pub_->Publish(transform_stamped_with_frame_ids_msg);
00509
00510 }
00511
00512 ++gazebo_sequence_;
00513 }
00514
00515 void GazeboOdometryPlugin::CreatePubsAndSubs() {
00516
00517 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
00518 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00519 "~/" + kConnectGazeboToRosSubtopic, 1);
00520
00521 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00522
00523
00524
00525
00526
00527 pose_pub_ = node_handle_->Advertise<gazebo::msgs::Pose>(
00528 "~/" + namespace_ + "/" + pose_pub_topic_, 1);
00529
00530 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00531 pose_pub_topic_);
00532 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00533 pose_pub_topic_);
00534 connect_gazebo_to_ros_topic_msg.set_msgtype(
00535 gz_std_msgs::ConnectGazeboToRosTopic::POSE);
00536 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00537 true);
00538
00539
00540
00541
00542
00543 pose_with_covariance_stamped_pub_ =
00544 node_handle_->Advertise<gz_geometry_msgs::PoseWithCovarianceStamped>(
00545 "~/" + namespace_ + "/" + pose_with_covariance_stamped_pub_topic_, 1);
00546
00547 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
00548 "~/" + namespace_ + "/" + pose_with_covariance_stamped_pub_topic_);
00549 connect_gazebo_to_ros_topic_msg.set_ros_topic(
00550 namespace_ + "/" + pose_with_covariance_stamped_pub_topic_);
00551 connect_gazebo_to_ros_topic_msg.set_msgtype(
00552 gz_std_msgs::ConnectGazeboToRosTopic::POSE_WITH_COVARIANCE_STAMPED);
00553 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00554 true);
00555
00556
00557
00558
00559
00560 position_stamped_pub_ =
00561 node_handle_->Advertise<gz_geometry_msgs::Vector3dStamped>(
00562 "~/" + namespace_ + "/" + position_stamped_pub_topic_, 1);
00563
00564 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00565 position_stamped_pub_topic_);
00566 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00567 position_stamped_pub_topic_);
00568 connect_gazebo_to_ros_topic_msg.set_msgtype(
00569 gz_std_msgs::ConnectGazeboToRosTopic::VECTOR_3D_STAMPED);
00570 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00571 true);
00572
00573
00574
00575
00576
00577 odometry_pub_ = node_handle_->Advertise<gz_geometry_msgs::Odometry>(
00578 "~/" + namespace_ + "/" + odometry_pub_topic_, 1);
00579
00580 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00581 odometry_pub_topic_);
00582 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00583 odometry_pub_topic_);
00584 connect_gazebo_to_ros_topic_msg.set_msgtype(
00585 gz_std_msgs::ConnectGazeboToRosTopic::ODOMETRY);
00586 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00587 true);
00588
00589
00590
00591
00592
00593 transform_stamped_pub_ =
00594 node_handle_->Advertise<gz_geometry_msgs::TransformStamped>(
00595 "~/" + namespace_ + "/" + transform_stamped_pub_topic_, 1);
00596
00597 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
00598 "~/" + namespace_ + "/" + transform_stamped_pub_topic_);
00599 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00600 transform_stamped_pub_topic_);
00601 connect_gazebo_to_ros_topic_msg.set_msgtype(
00602 gz_std_msgs::ConnectGazeboToRosTopic::TRANSFORM_STAMPED);
00603 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00604 true);
00605
00606
00607
00608
00609
00610 broadcast_transform_pub_ =
00611 node_handle_->Advertise<gz_geometry_msgs::TransformStampedWithFrameIds>(
00612 "~/" + kBroadcastTransformSubtopic, 1);
00613 }
00614
00615 GZ_REGISTER_MODEL_PLUGIN(GazeboOdometryPlugin);
00616
00617 }