gazebo_odometry_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 // MODULE
00023 #include "rotors_gazebo_plugins/gazebo_odometry_plugin.h"
00024 
00025 // SYSTEM
00026 #include <chrono>
00027 #include <iostream>
00028 
00029 // 3RD PARTY
00030 #include <opencv2/core/core.hpp>
00031 #include <opencv2/highgui/highgui.hpp>
00032 
00033 // USER
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   // Store the pointer to the model
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   // Initialise with default namespace (typically /gazebo/default/)
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   // Fill in covariance. We omit uniform noise here.
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   // Fill in covariance. We omit uniform noise here.
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   // Listen to the update event. This event is broadcast every
00230   // simulation iteration.
00231   updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00232       boost::bind(&GazeboOdometryPlugin::OnUpdate, this, _1));
00233 }
00234 
00235 // This gets called by the world update start event.
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   // C denotes child frame, P parent frame, and W world frame.
00247   // Further C_pose_W_P denotes pose of P wrt. W expressed in C.
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     // \prescript{}{C}{\dot{r}}_{PC} = -R_{CP}
00264     //       \cdot \prescript{}{P}{\omega}_{WP} \cross \prescript{}{P}{r}_{PC}
00265     //       + \prescript{}{C}{v}_{WC}
00266     //                                 - R_{CP} \cdot \prescript{}{P}{v}_{WP}
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     // \prescript{}{C}{\omega}_{PC} = \prescript{}{C}{\omega}_{WC}
00274     //       - R_{CP} \cdot \prescript{}{P}{\omega}_{WP}
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   // This flag could be set to false in the following code...
00283   bool publish_odometry = true;
00284 
00285   // First, determine whether we should publish a odometry.
00286   if (covariance_image_.data != NULL) {
00287     // We have an image.
00288 
00289     // Image is always centered around the origin:
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         // TODO: covariance scaling, according to the intensity values could be
00304         // implemented here.
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   // Is it time to publish the front element?
00354   if (gazebo_sequence_ == odometry_queue_.front().first) {
00355     // Copy the odometry message that is on the queue
00356     gz_geometry_msgs::Odometry odometry_msg(odometry_queue_.front().second);
00357 
00358     // Now that we have copied the first element from the queue, remove it.
00359     odometry_queue_.pop_front();
00360 
00361     // Calculate position distortions.
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     // Calculate attitude distortions.
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     // Calculate linear velocity distortions.
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     // Calculate angular velocity distortions.
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     // Publish all the topics, for which the topic name is specified.
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       // DEBUG
00482       odometry_pub_->Publish(odometry_msg);
00483     }
00484 
00485     //==============================================//
00486     //========= BROADCAST TRANSFORM MSG ============//
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   }  // if (gazebo_sequence_ == odometry_queue_.front().first) {
00511 
00512   ++gazebo_sequence_;
00513 }
00514 
00515 void GazeboOdometryPlugin::CreatePubsAndSubs() {
00516   // Create temporary "ConnectGazeboToRosTopic" publisher and message
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   // =============== POSE MSG SETUP ============= //
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   // == POSE WITH COVARIANCE STAMPED MSG SETUP == //
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   // ========= POSITION STAMPED MSG SETUP ======= //
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   // ============= ODOMETRY MSG SETUP =========== //
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   // ======== TRANSFORM STAMPED MSG SETUP ======= //
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   // ===== "BROADCAST TRANSFORM" MSG SETUP =====  //
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 }  // namespace gazebo


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43