gazebo_ros_interface_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 // MODULE
00018 #include <rotors_gazebo_plugins/gazebo_ros_interface_plugin.h>
00019 
00020 // SYSTEM
00021 #include <stdio.h>
00022 #include <chrono>
00023 #include <cmath>
00024 #include <iostream>
00025 
00026 // 3RD PARTY
00027 #include <std_msgs/Header.h>
00028 #include <boost/bind.hpp>
00029 
00030 namespace gazebo {
00031 
00032 GazeboRosInterfacePlugin::GazeboRosInterfacePlugin()
00033     : WorldPlugin(), gz_node_handle_(0), ros_node_handle_(0) {}
00034 
00035 GazeboRosInterfacePlugin::~GazeboRosInterfacePlugin() {
00036 
00037   // Shutdown and delete ROS node handle
00038   if (ros_node_handle_) {
00039     ros_node_handle_->shutdown();
00040     delete ros_node_handle_;
00041   }
00042 }
00043 
00044 void GazeboRosInterfacePlugin::Load(physics::WorldPtr _world,
00045                                     sdf::ElementPtr _sdf) {
00046   if (kPrintOnPluginLoad) {
00047     gzdbg << __FUNCTION__ << "() called." << std::endl;
00048   }
00049 
00051   world_ = _world;
00052 
00053   // namespace_.clear();
00054 
00055   //==============================================//
00056   //========== READ IN PARAMS FROM SDF ===========//
00057   //==============================================//
00058 
00059   /*if (_sdf->HasElement("robotNamespace"))
00060     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00061   else
00062     gzerr << "Please specify a robotNamespace.\n";
00063   gzdbg << "namespace_ = \"" << namespace_ << "\"." << std::endl;*/
00064 
00065   // Get Gazebo node handle
00066   gz_node_handle_ = transport::NodePtr(new transport::Node());
00067   // gz_node_handle_->Init(namespace_);
00068   gz_node_handle_->Init();
00069 
00070   // Get ROS node handle
00071   // ros_node_handle_ = new ros::NodeHandle(namespace_);
00072   ros_node_handle_ = new ros::NodeHandle();
00073 
00074   // Listen to the update event. This event is broadcast every
00075   // simulation iteration.
00076   this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00077       boost::bind(&GazeboRosInterfacePlugin::OnUpdate, this, _1));
00078 
00079   // ============================================ //
00080   // === CONNECT GAZEBO TO ROS MESSAGES SETUP === //
00081   // ============================================ //
00082 
00083   gz_connect_gazebo_to_ros_topic_sub_ = gz_node_handle_->Subscribe(
00084       "~/" + kConnectGazeboToRosSubtopic,
00085       &GazeboRosInterfacePlugin::GzConnectGazeboToRosTopicMsgCallback, this);
00086 
00087   // ============================================ //
00088   // === CONNECT ROS TO GAZEBO MESSAGES SETUP === //
00089   // ============================================ //
00090 
00091   gz_connect_ros_to_gazebo_topic_sub_ = gz_node_handle_->Subscribe(
00092       "~/" + kConnectRosToGazeboSubtopic,
00093       &GazeboRosInterfacePlugin::GzConnectRosToGazeboTopicMsgCallback, this);
00094 
00095   // ============================================ //
00096   // ===== BROADCAST TRANSFORM MESSAGE SETUP ==== //
00097   // ============================================ //
00098 
00099   gz_broadcast_transform_sub_ = gz_node_handle_->Subscribe(
00100       "~/" + kBroadcastTransformSubtopic,
00101       &GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback, this);
00102 }
00103 
00104 void GazeboRosInterfacePlugin::OnUpdate(const common::UpdateInfo& _info) {
00105   // Do nothing
00106   // This plugins actions are all executed through message callbacks.
00107 }
00108 
00114 template <typename GazeboMsgT>
00115 struct ConnectHelperStorage {
00117   GazeboRosInterfacePlugin* ptr;
00118 
00121   void (GazeboRosInterfacePlugin::*fp)(
00122       const boost::shared_ptr<GazeboMsgT const>&, ros::Publisher ros_publisher);
00123 
00125   ros::Publisher ros_publisher;
00126 
00131   void callback(const boost::shared_ptr<GazeboMsgT const>& msg_ptr) {
00132     (ptr->*fp)(msg_ptr, ros_publisher);
00133   }
00134 };
00135 
00136 template <typename GazeboMsgT, typename RosMsgT>
00137 void GazeboRosInterfacePlugin::ConnectHelper(
00138     void (GazeboRosInterfacePlugin::*fp)(
00139         const boost::shared_ptr<GazeboMsgT const>&, ros::Publisher),
00140     GazeboRosInterfacePlugin* ptr, std::string gazeboNamespace,
00141     std::string gazeboTopicName, std::string rosTopicName,
00142     transport::NodePtr gz_node_handle) {
00143   // One map will be created for each Gazebo message type
00144   static std::map<std::string, ConnectHelperStorage<GazeboMsgT> > callback_map;
00145 
00146   // Create ROS publisher
00147   ros::Publisher ros_publisher =
00148       ros_node_handle_->advertise<RosMsgT>(rosTopicName, 1);
00149 
00150   auto callback_entry = callback_map.emplace(
00151       gazeboTopicName,
00152       ConnectHelperStorage<GazeboMsgT>{ptr, fp, ros_publisher});
00153 
00154   // Check if element was already present
00155   if (!callback_entry.second)
00156     gzerr << "Tried to add element to map but the gazebo topic name was "
00157              "already present in map."
00158           << std::endl;
00159 
00160   // Create subscriber
00161   gazebo::transport::SubscriberPtr subscriberPtr;
00162   subscriberPtr = gz_node_handle->Subscribe(
00163       gazeboTopicName, &ConnectHelperStorage<GazeboMsgT>::callback,
00164       &callback_entry.first->second);
00165 
00166   // Save a reference to the subscriber pointer so subscriber
00167   // won't be deleted.
00168   subscriberPtrs_.push_back(subscriberPtr);
00169 }
00170 
00171 void GazeboRosInterfacePlugin::GzConnectGazeboToRosTopicMsgCallback(
00172     GzConnectGazeboToRosTopicMsgPtr& gz_connect_gazebo_to_ros_topic_msg) {
00173   if (kPrintOnMsgCallback) {
00174     gzdbg << __FUNCTION__ << "() called." << std::endl;
00175   }
00176 
00177   const std::string gazeboNamespace =
00178       "";  // gz_connect_gazebo_to_ros_topic_msg->gazebo_namespace();
00179   const std::string gazeboTopicName =
00180       gz_connect_gazebo_to_ros_topic_msg->gazebo_topic();
00181   const std::string rosTopicName =
00182       gz_connect_gazebo_to_ros_topic_msg->ros_topic();
00183 
00184   gzdbg << "Connecting Gazebo topic \"" << gazeboTopicName
00185         << "\" to ROS topic \"" << rosTopicName << "\"." << std::endl;
00186 
00187   switch (gz_connect_gazebo_to_ros_topic_msg->msgtype()) {
00188     case gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS:
00189       ConnectHelper<gz_sensor_msgs::Actuators, mav_msgs::Actuators>(
00190           &GazeboRosInterfacePlugin::GzActuatorsMsgCallback, this,
00191           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00192       break;
00193     case gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32:
00194       ConnectHelper<gz_std_msgs::Float32, std_msgs::Float32>(
00195           &GazeboRosInterfacePlugin::GzFloat32MsgCallback, this,
00196           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00197       break;
00198     case gz_std_msgs::ConnectGazeboToRosTopic::FLUID_PRESSURE:
00199       ConnectHelper<gz_sensor_msgs::FluidPressure, sensor_msgs::FluidPressure>(
00200           &GazeboRosInterfacePlugin::GzFluidPressureMsgCallback, this,
00201           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00202       break;
00203     case gz_std_msgs::ConnectGazeboToRosTopic::IMU:
00204       ConnectHelper<gz_sensor_msgs::Imu, sensor_msgs::Imu>(
00205           &GazeboRosInterfacePlugin::GzImuMsgCallback, this, gazeboNamespace,
00206           gazeboTopicName, rosTopicName, gz_node_handle_);
00207       break;
00208     case gz_std_msgs::ConnectGazeboToRosTopic::JOINT_STATE:
00209       ConnectHelper<gz_sensor_msgs::JointState, sensor_msgs::JointState>(
00210           &GazeboRosInterfacePlugin::GzJointStateMsgCallback, this,
00211           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00212       break;
00213     case gz_std_msgs::ConnectGazeboToRosTopic::MAGNETIC_FIELD:
00214       ConnectHelper<gz_sensor_msgs::MagneticField, sensor_msgs::MagneticField>(
00215           &GazeboRosInterfacePlugin::GzMagneticFieldMsgCallback, this,
00216           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00217       break;
00218     case gz_std_msgs::ConnectGazeboToRosTopic::NAV_SAT_FIX:
00219       ConnectHelper<gz_sensor_msgs::NavSatFix, sensor_msgs::NavSatFix>(
00220           &GazeboRosInterfacePlugin::GzNavSatFixCallback, this, gazeboNamespace,
00221           gazeboTopicName, rosTopicName, gz_node_handle_);
00222       break;
00223     case gz_std_msgs::ConnectGazeboToRosTopic::POSE:
00224       ConnectHelper<gazebo::msgs::Pose, geometry_msgs::Pose>(
00225           &GazeboRosInterfacePlugin::GzPoseMsgCallback, this, gazeboNamespace,
00226           gazeboTopicName, rosTopicName, gz_node_handle_);
00227       break;
00228     case gz_std_msgs::ConnectGazeboToRosTopic::POSE_WITH_COVARIANCE_STAMPED:
00229       ConnectHelper<gz_geometry_msgs::PoseWithCovarianceStamped,
00230                     geometry_msgs::PoseWithCovarianceStamped>(
00231           &GazeboRosInterfacePlugin::GzPoseWithCovarianceStampedMsgCallback,
00232           this, gazeboNamespace, gazeboTopicName, rosTopicName,
00233           gz_node_handle_);
00234       break;
00235     case gz_std_msgs::ConnectGazeboToRosTopic::ODOMETRY:
00236       ConnectHelper<gz_geometry_msgs::Odometry, nav_msgs::Odometry>(
00237           &GazeboRosInterfacePlugin::GzOdometryMsgCallback, this,
00238           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00239       break;
00240     case gz_std_msgs::ConnectGazeboToRosTopic::TRANSFORM_STAMPED:
00241       ConnectHelper<gz_geometry_msgs::TransformStamped,
00242                     geometry_msgs::TransformStamped>(
00243           &GazeboRosInterfacePlugin::GzTransformStampedMsgCallback, this,
00244           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00245       break;
00246     case gz_std_msgs::ConnectGazeboToRosTopic::TWIST_STAMPED:
00247       ConnectHelper<gz_geometry_msgs::TwistStamped,
00248                     geometry_msgs::TwistStamped>(
00249           &GazeboRosInterfacePlugin::GzTwistStampedMsgCallback, this,
00250           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00251       break;
00252     case gz_std_msgs::ConnectGazeboToRosTopic::VECTOR_3D_STAMPED:
00253       ConnectHelper<gz_geometry_msgs::Vector3dStamped,
00254                     geometry_msgs::PointStamped>(
00255           &GazeboRosInterfacePlugin::GzVector3dStampedMsgCallback, this,
00256           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00257       break;
00258     case gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED:
00259       ConnectHelper<gz_mav_msgs::WindSpeed,
00260                     rotors_comm::WindSpeed>(
00261           &GazeboRosInterfacePlugin::GzWindSpeedMsgCallback, this,
00262           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00263       break;
00264     case gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED:
00265       ConnectHelper<gz_geometry_msgs::WrenchStamped,
00266                     geometry_msgs::WrenchStamped>(
00267           &GazeboRosInterfacePlugin::GzWrenchStampedMsgCallback, this,
00268           gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_);
00269       break;
00270     default:
00271       gzthrow("ConnectGazeboToRosTopic message type with enum val = "
00272               << gz_connect_gazebo_to_ros_topic_msg->msgtype()
00273               << " is not supported by GazeboRosInterfacePlugin.");
00274   }
00275 
00276   gzdbg << __FUNCTION__ << "() finished." << std::endl;
00277 }
00278 
00279 void GazeboRosInterfacePlugin::GzConnectRosToGazeboTopicMsgCallback(
00280     GzConnectRosToGazeboTopicMsgPtr& gz_connect_ros_to_gazebo_topic_msg) {
00281   if (kPrintOnMsgCallback) {
00282     gzdbg << __FUNCTION__ << "() called." << std::endl;
00283   }
00284 
00285   static std::vector<ros::Subscriber> ros_subscribers;
00286 
00287   switch (gz_connect_ros_to_gazebo_topic_msg->msgtype()) {
00288     case gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS: {
00289       gazebo::transport::PublisherPtr gz_publisher_ptr =
00290           gz_node_handle_->Advertise<gz_sensor_msgs::Actuators>(
00291               gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1);
00292 
00293       // Create ROS subscriber.
00294       ros::Subscriber ros_subscriber =
00295           ros_node_handle_->subscribe<mav_msgs::Actuators>(
00296               gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1,
00297               boost::bind(&GazeboRosInterfacePlugin::RosActuatorsMsgCallback,
00298                           this, _1, gz_publisher_ptr));
00299 
00300       // Save reference to the ROS subscriber so callback will continue to be
00301       // called.
00302       ros_subscribers.push_back(ros_subscriber);
00303 
00304       break;
00305     }
00306     case gz_std_msgs::ConnectRosToGazeboTopic::COMMAND_MOTOR_SPEED: {
00307       gazebo::transport::PublisherPtr gz_publisher_ptr =
00308           gz_node_handle_->Advertise<gz_mav_msgs::CommandMotorSpeed>(
00309               gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1);
00310 
00311       // Create ROS subscriber.
00312       ros::Subscriber ros_subscriber =
00313           ros_node_handle_->subscribe<mav_msgs::Actuators>(
00314               gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1,
00315               boost::bind(
00316                   &GazeboRosInterfacePlugin::RosCommandMotorSpeedMsgCallback,
00317                   this, _1, gz_publisher_ptr));
00318 
00319       // Save reference to the ROS subscriber so callback will continue to be
00320       // called.
00321       ros_subscribers.push_back(ros_subscriber);
00322 
00323       break;
00324     }
00325     case gz_std_msgs::ConnectRosToGazeboTopic::ROLL_PITCH_YAWRATE_THRUST: {
00326       gazebo::transport::PublisherPtr gz_publisher_ptr =
00327           gz_node_handle_->Advertise<gz_mav_msgs::RollPitchYawrateThrust>(
00328               gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1);
00329 
00330       // Create ROS subscriber.
00331       ros::Subscriber ros_subscriber =
00332           ros_node_handle_->subscribe<mav_msgs::RollPitchYawrateThrust>(
00333               gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1,
00334               boost::bind(
00335                   &GazeboRosInterfacePlugin::
00336                       RosRollPitchYawrateThrustMsgCallback,
00337                   this, _1, gz_publisher_ptr));
00338 
00339       // Save reference to the ROS subscriber so callback will continue to be
00340       // called.
00341       ros_subscribers.push_back(ros_subscriber);
00342 
00343       break;
00344     }
00345     case gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED: {
00346       gazebo::transport::PublisherPtr gz_publisher_ptr =
00347           gz_node_handle_->Advertise<gz_mav_msgs::WindSpeed>(
00348               gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1);
00349 
00350       // Create ROS subscriber.
00351       ros::Subscriber ros_subscriber =
00352           ros_node_handle_->subscribe<rotors_comm::WindSpeed>(
00353               gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1,
00354               boost::bind(&GazeboRosInterfacePlugin::RosWindSpeedMsgCallback,
00355                           this, _1, gz_publisher_ptr));
00356 
00357       // Save reference to the ROS subscriber so callback will continue to be
00358       // called.
00359       ros_subscribers.push_back(ros_subscriber);
00360 
00361       break;
00362     }
00363     default: {
00364       gzthrow("ConnectRosToGazeboTopic message type with enum val = "
00365               << gz_connect_ros_to_gazebo_topic_msg->msgtype()
00366               << " is not supported by GazeboRosInterfacePlugin.");
00367     }
00368   }
00369 }
00370 
00371 //===========================================================================//
00372 //==================== HELPER METHODS FOR MSG CONVERSION ====================//
00373 //===========================================================================//
00374 
00375 void GazeboRosInterfacePlugin::ConvertHeaderGzToRos(
00376     const gz_std_msgs::Header& gz_header,
00377     std_msgs::Header_<std::allocator<void> >* ros_header) {
00378   ros_header->stamp.sec = gz_header.stamp().sec();
00379   ros_header->stamp.nsec = gz_header.stamp().nsec();
00380   ros_header->frame_id = gz_header.frame_id();
00381 }
00382 
00383 void GazeboRosInterfacePlugin::ConvertHeaderRosToGz(
00384     const std_msgs::Header_<std::allocator<void> >& ros_header,
00385     gz_std_msgs::Header* gz_header) {
00386   gz_header->mutable_stamp()->set_sec(ros_header.stamp.sec);
00387   gz_header->mutable_stamp()->set_nsec(ros_header.stamp.nsec);
00388   gz_header->set_frame_id(ros_header.frame_id);
00389 }
00390 
00391 //===========================================================================//
00392 //================ GAZEBO -> ROS MSG CALLBACKS/CONVERTERS ===================//
00393 //===========================================================================//
00394 
00395 void GazeboRosInterfacePlugin::GzActuatorsMsgCallback(
00396     GzActuatorsMsgPtr& gz_actuators_msg, ros::Publisher ros_publisher) {
00397   // We need to convert the Acutuators message from a Gazebo message to a
00398   // ROS message and then publish it to the ROS framework
00399 
00400   ConvertHeaderGzToRos(gz_actuators_msg->header(), &ros_actuators_msg_.header);
00401 
00402   ros_actuators_msg_.angular_velocities.resize(
00403       gz_actuators_msg->angular_velocities_size());
00404   for (int i = 0; i < gz_actuators_msg->angular_velocities_size(); i++) {
00405     ros_actuators_msg_.angular_velocities[i] =
00406         gz_actuators_msg->angular_velocities(i);
00407   }
00408 
00409   // Publish to ROS.
00410   ros_publisher.publish(ros_actuators_msg_);
00411 }
00412 
00413 void GazeboRosInterfacePlugin::GzFloat32MsgCallback(
00414     GzFloat32MsgPtr& gz_float_32_msg, ros::Publisher ros_publisher) {
00415   // Convert Gazebo message to ROS message
00416   ros_float_32_msg_.data = gz_float_32_msg->data();
00417 
00418   // Publish to ROS
00419   ros_publisher.publish(ros_float_32_msg_);
00420 }
00421 
00422 void GazeboRosInterfacePlugin::GzFluidPressureMsgCallback(
00423     GzFluidPressureMsgPtr &gz_fluid_pressure_msg,
00424     ros::Publisher ros_publisher) {
00425   // We need to convert from a Gazebo message to a ROS message,
00426   // and then forward the FluidPressure message onto ROS.
00427 
00428   ConvertHeaderGzToRos(gz_fluid_pressure_msg->header(),
00429                        &ros_fluid_pressure_msg_.header);
00430 
00431   ros_fluid_pressure_msg_.fluid_pressure =
00432       gz_fluid_pressure_msg->fluid_pressure();
00433 
00434   ros_fluid_pressure_msg_.variance = gz_fluid_pressure_msg->variance();
00435 
00436   // Publish to ROS.
00437   ros_publisher.publish(ros_fluid_pressure_msg_);
00438 }
00439 
00440 void GazeboRosInterfacePlugin::GzImuMsgCallback(GzImuPtr& gz_imu_msg,
00441                                                 ros::Publisher ros_publisher) {
00442   // We need to convert from a Gazebo message to a ROS message,
00443   // and then forward the IMU message onto ROS
00444 
00445   ConvertHeaderGzToRos(gz_imu_msg->header(), &ros_imu_msg_.header);
00446 
00447   ros_imu_msg_.orientation.x = gz_imu_msg->orientation().x();
00448   ros_imu_msg_.orientation.y = gz_imu_msg->orientation().y();
00449   ros_imu_msg_.orientation.z = gz_imu_msg->orientation().z();
00450   ros_imu_msg_.orientation.w = gz_imu_msg->orientation().w();
00451 
00452   // Orientation covariance should have 9 elements, and both the Gazebo and ROS
00453   // arrays should be the same size!
00454   GZ_ASSERT(gz_imu_msg->orientation_covariance_size() == 9,
00455             "The Gazebo IMU message does not have 9 orientation covariance "
00456             "elements.");
00457   GZ_ASSERT(
00458       ros_imu_msg_.orientation_covariance.size() == 9,
00459       "The ROS IMU message does not have 9 orientation covariance elements.");
00460   for (int i = 0; i < gz_imu_msg->orientation_covariance_size(); i++) {
00461     ros_imu_msg_.orientation_covariance[i] =
00462         gz_imu_msg->orientation_covariance(i);
00463   }
00464 
00465   ros_imu_msg_.angular_velocity.x = gz_imu_msg->angular_velocity().x();
00466   ros_imu_msg_.angular_velocity.y = gz_imu_msg->angular_velocity().y();
00467   ros_imu_msg_.angular_velocity.z = gz_imu_msg->angular_velocity().z();
00468 
00469   GZ_ASSERT(gz_imu_msg->angular_velocity_covariance_size() == 9,
00470             "The Gazebo IMU message does not have 9 angular velocity "
00471             "covariance elements.");
00472   GZ_ASSERT(ros_imu_msg_.angular_velocity_covariance.size() == 9,
00473             "The ROS IMU message does not have 9 angular velocity covariance "
00474             "elements.");
00475   for (int i = 0; i < gz_imu_msg->angular_velocity_covariance_size(); i++) {
00476     ros_imu_msg_.angular_velocity_covariance[i] =
00477         gz_imu_msg->angular_velocity_covariance(i);
00478   }
00479 
00480   ros_imu_msg_.linear_acceleration.x = gz_imu_msg->linear_acceleration().x();
00481   ros_imu_msg_.linear_acceleration.y = gz_imu_msg->linear_acceleration().y();
00482   ros_imu_msg_.linear_acceleration.z = gz_imu_msg->linear_acceleration().z();
00483 
00484   GZ_ASSERT(gz_imu_msg->linear_acceleration_covariance_size() == 9,
00485             "The Gazebo IMU message does not have 9 linear acceleration "
00486             "covariance elements.");
00487   GZ_ASSERT(ros_imu_msg_.linear_acceleration_covariance.size() == 9,
00488             "The ROS IMU message does not have 9 linear acceleration "
00489             "covariance elements.");
00490   for (int i = 0; i < gz_imu_msg->linear_acceleration_covariance_size(); i++) {
00491     ros_imu_msg_.linear_acceleration_covariance[i] =
00492         gz_imu_msg->linear_acceleration_covariance(i);
00493   }
00494 
00495   // Publish to ROS.
00496   ros_publisher.publish(ros_imu_msg_);
00497 }
00498 
00499 void GazeboRosInterfacePlugin::GzJointStateMsgCallback(
00500     GzJointStateMsgPtr& gz_joint_state_msg, ros::Publisher ros_publisher) {
00501   ConvertHeaderGzToRos(gz_joint_state_msg->header(),
00502                        &ros_joint_state_msg_.header);
00503 
00504   ros_joint_state_msg_.name.resize(gz_joint_state_msg->name_size());
00505   for (int i = 0; i < gz_joint_state_msg->name_size(); i++) {
00506     ros_joint_state_msg_.name[i] = gz_joint_state_msg->name(i);
00507   }
00508 
00509   ros_joint_state_msg_.position.resize(gz_joint_state_msg->position_size());
00510   for (int i = 0; i < gz_joint_state_msg->position_size(); i++) {
00511     ros_joint_state_msg_.position[i] = gz_joint_state_msg->position(i);
00512   }
00513 
00514   // Publish to ROS.
00515   ros_publisher.publish(ros_joint_state_msg_);
00516 }
00517 
00518 void GazeboRosInterfacePlugin::GzMagneticFieldMsgCallback(
00519     GzMagneticFieldMsgPtr& gz_magnetic_field_msg,
00520     ros::Publisher ros_publisher) {
00521   // We need to convert from a Gazebo message to a ROS message,
00522   // and then forward the MagneticField message onto ROS
00523 
00524   ConvertHeaderGzToRos(gz_magnetic_field_msg->header(),
00525                        &ros_magnetic_field_msg_.header);
00526 
00527   ros_magnetic_field_msg_.magnetic_field.x =
00528       gz_magnetic_field_msg->magnetic_field().x();
00529   ros_magnetic_field_msg_.magnetic_field.y =
00530       gz_magnetic_field_msg->magnetic_field().y();
00531   ros_magnetic_field_msg_.magnetic_field.z =
00532       gz_magnetic_field_msg->magnetic_field().z();
00533 
00534   // Position covariance should have 9 elements, and both the Gazebo and ROS
00535   // arrays should be the same size!
00536   GZ_ASSERT(gz_magnetic_field_msg->magnetic_field_covariance_size() == 9,
00537             "The Gazebo MagneticField message does not have 9 magnetic field "
00538             "covariance elements.");
00539   GZ_ASSERT(ros_magnetic_field_msg_.magnetic_field_covariance.size() == 9,
00540             "The ROS MagneticField message does not have 9 magnetic field "
00541             "covariance elements.");
00542   for (int i = 0; i < gz_magnetic_field_msg->magnetic_field_covariance_size();
00543        i++) {
00544     ros_magnetic_field_msg_.magnetic_field_covariance[i] =
00545         gz_magnetic_field_msg->magnetic_field_covariance(i);
00546   }
00547 
00548   // Publish to ROS.
00549   ros_publisher.publish(ros_magnetic_field_msg_);
00550 }
00551 
00552 void GazeboRosInterfacePlugin::GzNavSatFixCallback(
00553     GzNavSatFixPtr& gz_nav_sat_fix_msg, ros::Publisher ros_publisher) {
00554   // We need to convert from a Gazebo message to a ROS message, and then forward
00555   // the NavSatFix message to ROS.
00556 
00557   ConvertHeaderGzToRos(gz_nav_sat_fix_msg->header(),
00558                        &ros_nav_sat_fix_msg_.header);
00559 
00560   switch (gz_nav_sat_fix_msg->service()) {
00561     case gz_sensor_msgs::NavSatFix::SERVICE_GPS:
00562       ros_nav_sat_fix_msg_.status.service =
00563           sensor_msgs::NavSatStatus::SERVICE_GPS;
00564       break;
00565     case gz_sensor_msgs::NavSatFix::SERVICE_GLONASS:
00566       ros_nav_sat_fix_msg_.status.service =
00567           sensor_msgs::NavSatStatus::SERVICE_GLONASS;
00568       break;
00569     case gz_sensor_msgs::NavSatFix::SERVICE_COMPASS:
00570       ros_nav_sat_fix_msg_.status.service =
00571           sensor_msgs::NavSatStatus::SERVICE_COMPASS;
00572       break;
00573     case gz_sensor_msgs::NavSatFix::SERVICE_GALILEO:
00574       ros_nav_sat_fix_msg_.status.service =
00575           sensor_msgs::NavSatStatus::SERVICE_GALILEO;
00576       break;
00577     default:
00578       gzthrow(
00579           "Specific value of enum type gz_sensor_msgs::NavSatFix::Service is "
00580           "not yet supported.");
00581   }
00582 
00583   switch (gz_nav_sat_fix_msg->status()) {
00584     case gz_sensor_msgs::NavSatFix::STATUS_NO_FIX:
00585       ros_nav_sat_fix_msg_.status.status =
00586           sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00587       break;
00588     case gz_sensor_msgs::NavSatFix::STATUS_FIX:
00589       ros_nav_sat_fix_msg_.status.status =
00590           sensor_msgs::NavSatStatus::STATUS_FIX;
00591       break;
00592     case gz_sensor_msgs::NavSatFix::STATUS_SBAS_FIX:
00593       ros_nav_sat_fix_msg_.status.status =
00594           sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
00595       break;
00596     case gz_sensor_msgs::NavSatFix::STATUS_GBAS_FIX:
00597       ros_nav_sat_fix_msg_.status.status =
00598           sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
00599       break;
00600     default:
00601       gzthrow(
00602           "Specific value of enum type gz_sensor_msgs::NavSatFix::Status is "
00603           "not yet supported.");
00604   }
00605 
00606   ros_nav_sat_fix_msg_.latitude = gz_nav_sat_fix_msg->latitude();
00607   ros_nav_sat_fix_msg_.longitude = gz_nav_sat_fix_msg->longitude();
00608   ros_nav_sat_fix_msg_.altitude = gz_nav_sat_fix_msg->altitude();
00609 
00610   switch (gz_nav_sat_fix_msg->position_covariance_type()) {
00611     case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN:
00612       ros_nav_sat_fix_msg_.position_covariance_type =
00613           sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00614       break;
00615     case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED:
00616       ros_nav_sat_fix_msg_.position_covariance_type =
00617           sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
00618       break;
00619     case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN:
00620       ros_nav_sat_fix_msg_.position_covariance_type =
00621           sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00622       break;
00623     case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN:
00624       ros_nav_sat_fix_msg_.position_covariance_type =
00625           sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
00626       break;
00627     default:
00628       gzthrow(
00629           "Specific value of enum type "
00630           "gz_sensor_msgs::NavSatFix::PositionCovarianceType is not yet "
00631           "supported.");
00632   }
00633 
00634   // Position covariance should have 9 elements, and both the Gazebo and ROS
00635   // arrays should be the same size!
00636   GZ_ASSERT(gz_nav_sat_fix_msg->position_covariance_size() == 9,
00637             "The Gazebo NavSatFix message does not have 9 position covariance "
00638             "elements.");
00639   GZ_ASSERT(ros_nav_sat_fix_msg_.position_covariance.size() == 9,
00640             "The ROS NavSatFix message does not have 9 position covariance "
00641             "elements.");
00642   for (int i = 0; i < gz_nav_sat_fix_msg->position_covariance_size(); i++) {
00643     ros_nav_sat_fix_msg_.position_covariance[i] =
00644         gz_nav_sat_fix_msg->position_covariance(i);
00645   }
00646 
00647   // Publish to ROS.
00648   ros_publisher.publish(ros_nav_sat_fix_msg_);
00649 }
00650 
00651 void GazeboRosInterfacePlugin::GzOdometryMsgCallback(
00652     GzOdometryMsgPtr& gz_odometry_msg, ros::Publisher ros_publisher) {
00653   // We need to convert from a Gazebo message to a ROS message, and then forward
00654   // the Odometry message to ROS.
00655 
00656   // ============================================ //
00657   // =================== HEADER ================= //
00658   // ============================================ //
00659   ConvertHeaderGzToRos(gz_odometry_msg->header(), &ros_odometry_msg_.header);
00660 
00661   ros_odometry_msg_.child_frame_id = gz_odometry_msg->child_frame_id();
00662 
00663   // ============================================ //
00664   // ===================== POSE ================= //
00665   // ============================================ //
00666   ros_odometry_msg_.pose.pose.position.x =
00667       gz_odometry_msg->pose().pose().position().x();
00668   ros_odometry_msg_.pose.pose.position.y =
00669       gz_odometry_msg->pose().pose().position().y();
00670   ros_odometry_msg_.pose.pose.position.z =
00671       gz_odometry_msg->pose().pose().position().z();
00672 
00673   ros_odometry_msg_.pose.pose.orientation.w =
00674       gz_odometry_msg->pose().pose().orientation().w();
00675   ros_odometry_msg_.pose.pose.orientation.x =
00676       gz_odometry_msg->pose().pose().orientation().x();
00677   ros_odometry_msg_.pose.pose.orientation.y =
00678       gz_odometry_msg->pose().pose().orientation().y();
00679   ros_odometry_msg_.pose.pose.orientation.z =
00680       gz_odometry_msg->pose().pose().orientation().z();
00681 
00682   for (int i = 0; i < gz_odometry_msg->pose().covariance_size(); i++) {
00683     ros_odometry_msg_.pose.covariance[i] =
00684         gz_odometry_msg->pose().covariance(i);
00685   }
00686 
00687   // ============================================ //
00688   // ===================== TWIST ================ //
00689   // ============================================ //
00690   ros_odometry_msg_.twist.twist.linear.x =
00691       gz_odometry_msg->twist().twist().linear().x();
00692   ros_odometry_msg_.twist.twist.linear.y =
00693       gz_odometry_msg->twist().twist().linear().y();
00694   ros_odometry_msg_.twist.twist.linear.z =
00695       gz_odometry_msg->twist().twist().linear().z();
00696 
00697   ros_odometry_msg_.twist.twist.angular.x =
00698       gz_odometry_msg->twist().twist().angular().x();
00699   ros_odometry_msg_.twist.twist.angular.y =
00700       gz_odometry_msg->twist().twist().angular().y();
00701   ros_odometry_msg_.twist.twist.angular.z =
00702       gz_odometry_msg->twist().twist().angular().z();
00703 
00704   for (int i = 0; i < gz_odometry_msg->twist().covariance_size(); i++) {
00705     ros_odometry_msg_.twist.covariance[i] =
00706         gz_odometry_msg->twist().covariance(i);
00707   }
00708 
00709   // Publish to ROS framework.
00710   ros_publisher.publish(ros_odometry_msg_);
00711 }
00712 
00713 void GazeboRosInterfacePlugin::GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg,
00714                                                  ros::Publisher ros_publisher) {
00715   ros_pose_msg_.position.x = gz_pose_msg->position().x();
00716   ros_pose_msg_.position.y = gz_pose_msg->position().y();
00717   ros_pose_msg_.position.z = gz_pose_msg->position().z();
00718 
00719   ros_pose_msg_.orientation.w = gz_pose_msg->orientation().w();
00720   ros_pose_msg_.orientation.x = gz_pose_msg->orientation().x();
00721   ros_pose_msg_.orientation.y = gz_pose_msg->orientation().y();
00722   ros_pose_msg_.orientation.z = gz_pose_msg->orientation().z();
00723 
00724   ros_publisher.publish(ros_pose_msg_);
00725 }
00726 
00727 void GazeboRosInterfacePlugin::GzPoseWithCovarianceStampedMsgCallback(
00728     GzPoseWithCovarianceStampedMsgPtr& gz_pose_with_covariance_stamped_msg,
00729     ros::Publisher ros_publisher) {
00730   // ============================================ //
00731   // =================== HEADER ================= //
00732   // ============================================ //
00733   ConvertHeaderGzToRos(gz_pose_with_covariance_stamped_msg->header(),
00734                        &ros_pose_with_covariance_stamped_msg_.header);
00735 
00736   // ============================================ //
00737   // === POSE (both position and orientation) === //
00738   // ============================================ //
00739   ros_pose_with_covariance_stamped_msg_.pose.pose.position.x =
00740       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00741           .pose()
00742           .position()
00743           .x();
00744   ros_pose_with_covariance_stamped_msg_.pose.pose.position.y =
00745       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00746           .pose()
00747           .position()
00748           .y();
00749   ros_pose_with_covariance_stamped_msg_.pose.pose.position.z =
00750       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00751           .pose()
00752           .position()
00753           .z();
00754 
00755   ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.w =
00756       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00757           .pose()
00758           .orientation()
00759           .w();
00760   ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.x =
00761       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00762           .pose()
00763           .orientation()
00764           .x();
00765   ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.y =
00766       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00767           .pose()
00768           .orientation()
00769           .y();
00770   ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.z =
00771       gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00772           .pose()
00773           .orientation()
00774           .z();
00775 
00776   // Covariance should have 36 elements, and both the Gazebo and ROS
00777   // arrays should be the same size!
00778   GZ_ASSERT(gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00779                     .covariance_size() == 36,
00780             "The Gazebo PoseWithCovarianceStamped message does not have 9 "
00781             "position covariance elements.");
00782   GZ_ASSERT(ros_pose_with_covariance_stamped_msg_.pose.covariance.size() == 36,
00783             "The ROS PoseWithCovarianceStamped message does not have 9 "
00784             "position covariance elements.");
00785   for (int i = 0;
00786        i < gz_pose_with_covariance_stamped_msg->pose_with_covariance()
00787                .covariance_size();
00788        i++) {
00789     ros_pose_with_covariance_stamped_msg_.pose.covariance[i] =
00790         gz_pose_with_covariance_stamped_msg->pose_with_covariance().covariance(
00791             i);
00792   }
00793 
00794   ros_publisher.publish(ros_pose_with_covariance_stamped_msg_);
00795 }
00796 
00797 void GazeboRosInterfacePlugin::GzTransformStampedMsgCallback(
00798     GzTransformStampedMsgPtr& gz_transform_stamped_msg,
00799     ros::Publisher ros_publisher) {
00800   // ============================================ //
00801   // =================== HEADER ================= //
00802   // ============================================ //
00803   ConvertHeaderGzToRos(gz_transform_stamped_msg->header(),
00804                        &ros_transform_stamped_msg_.header);
00805 
00806   // ============================================ //
00807   // =========== TRANSFORM, TRANSLATION ========= //
00808   // ============================================ //
00809   ros_transform_stamped_msg_.transform.translation.x =
00810       gz_transform_stamped_msg->transform().translation().x();
00811   ros_transform_stamped_msg_.transform.translation.y =
00812       gz_transform_stamped_msg->transform().translation().y();
00813   ros_transform_stamped_msg_.transform.translation.z =
00814       gz_transform_stamped_msg->transform().translation().z();
00815 
00816   // ============================================ //
00817   // ============ TRANSFORM, ROTATION =========== //
00818   // ============================================ //
00819   ros_transform_stamped_msg_.transform.rotation.w =
00820       gz_transform_stamped_msg->transform().rotation().w();
00821   ros_transform_stamped_msg_.transform.rotation.x =
00822       gz_transform_stamped_msg->transform().rotation().x();
00823   ros_transform_stamped_msg_.transform.rotation.y =
00824       gz_transform_stamped_msg->transform().rotation().y();
00825   ros_transform_stamped_msg_.transform.rotation.z =
00826       gz_transform_stamped_msg->transform().rotation().z();
00827 
00828   ros_publisher.publish(ros_transform_stamped_msg_);
00829 }
00830 
00831 void GazeboRosInterfacePlugin::GzTwistStampedMsgCallback(
00832     GzTwistStampedMsgPtr& gz_twist_stamped_msg, ros::Publisher ros_publisher) {
00833   // ============================================ //
00834   // =================== HEADER ================= //
00835   // ============================================ //
00836   ConvertHeaderGzToRos(gz_twist_stamped_msg->header(),
00837                        &ros_twist_stamped_msg_.header);
00838 
00839   // ============================================ //
00840   // =================== TWIST ================== //
00841   // ============================================ //
00842 
00843   ros_twist_stamped_msg_.twist.linear.x =
00844       gz_twist_stamped_msg->twist().linear().x();
00845   ros_twist_stamped_msg_.twist.linear.y =
00846       gz_twist_stamped_msg->twist().linear().y();
00847   ros_twist_stamped_msg_.twist.linear.z =
00848       gz_twist_stamped_msg->twist().linear().z();
00849 
00850   ros_twist_stamped_msg_.twist.angular.x =
00851       gz_twist_stamped_msg->twist().angular().x();
00852   ros_twist_stamped_msg_.twist.angular.y =
00853       gz_twist_stamped_msg->twist().angular().y();
00854   ros_twist_stamped_msg_.twist.angular.z =
00855       gz_twist_stamped_msg->twist().angular().z();
00856 
00857   ros_publisher.publish(ros_twist_stamped_msg_);
00858 }
00859 
00860 void GazeboRosInterfacePlugin::GzVector3dStampedMsgCallback(
00861     GzVector3dStampedMsgPtr& gz_vector_3d_stamped_msg,
00862     ros::Publisher ros_publisher) {
00863   // ============================================ //
00864   // =================== HEADER ================= //
00865   // ============================================ //
00866   ConvertHeaderGzToRos(gz_vector_3d_stamped_msg->header(),
00867                        &ros_position_stamped_msg_.header);
00868 
00869   // ============================================ //
00870   // ================== POSITION ================ //
00871   // ============================================ //
00872 
00873   ros_position_stamped_msg_.point.x = gz_vector_3d_stamped_msg->position().x();
00874   ros_position_stamped_msg_.point.y = gz_vector_3d_stamped_msg->position().y();
00875   ros_position_stamped_msg_.point.z = gz_vector_3d_stamped_msg->position().z();
00876 
00877   ros_publisher.publish(ros_position_stamped_msg_);
00878 }
00879 
00880 void GazeboRosInterfacePlugin::GzWindSpeedMsgCallback(
00881     GzWindSpeedMsgPtr& gz_wind_speed_msg,
00882     ros::Publisher ros_publisher) {
00883   // ============================================ //
00884   // =================== HEADER ================= //
00885   // ============================================ //
00886   ConvertHeaderGzToRos(gz_wind_speed_msg->header(),
00887                        &ros_wind_speed_msg_.header);
00888 
00889   // ============================================ //
00890   // ================== VELOCITY ================ //
00891   // ============================================ //
00892   ros_wind_speed_msg_.velocity.x =
00893       gz_wind_speed_msg->velocity().x();
00894   ros_wind_speed_msg_.velocity.y =
00895       gz_wind_speed_msg->velocity().y();
00896   ros_wind_speed_msg_.velocity.z =
00897       gz_wind_speed_msg->velocity().z();
00898   ros_publisher.publish(ros_wind_speed_msg_);
00899 }
00900 
00901 void GazeboRosInterfacePlugin::GzWrenchStampedMsgCallback(
00902     GzWrenchStampedMsgPtr& gz_wrench_stamped_msg,
00903     ros::Publisher ros_publisher) {
00904   // ============================================ //
00905   // =================== HEADER ================= //
00906   // ============================================ //
00907   ConvertHeaderGzToRos(gz_wrench_stamped_msg->header(),
00908                        &ros_wrench_stamped_msg_.header);
00909 
00910   // ============================================ //
00911   // =================== FORCE ================== //
00912   // ============================================ //
00913   ros_wrench_stamped_msg_.wrench.force.x =
00914       gz_wrench_stamped_msg->wrench().force().x();
00915   ros_wrench_stamped_msg_.wrench.force.y =
00916       gz_wrench_stamped_msg->wrench().force().y();
00917   ros_wrench_stamped_msg_.wrench.force.z =
00918       gz_wrench_stamped_msg->wrench().force().z();
00919 
00920   // ============================================ //
00921   // ==================== TORQUE ================ //
00922   // ============================================ //
00923   ros_wrench_stamped_msg_.wrench.torque.x =
00924       gz_wrench_stamped_msg->wrench().torque().x();
00925   ros_wrench_stamped_msg_.wrench.torque.y =
00926       gz_wrench_stamped_msg->wrench().torque().y();
00927   ros_wrench_stamped_msg_.wrench.torque.z =
00928       gz_wrench_stamped_msg->wrench().torque().z();
00929 
00930   ros_publisher.publish(ros_wrench_stamped_msg_);
00931 }
00932 
00933 //===========================================================================//
00934 //================ ROS -> GAZEBO MSG CALLBACKS/CONVERTERS ===================//
00935 //===========================================================================//
00936 
00937 void GazeboRosInterfacePlugin::RosActuatorsMsgCallback(
00938     const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
00939     gazebo::transport::PublisherPtr gz_publisher_ptr) {
00940   // Convert ROS message to Gazebo message
00941 
00942   gz_sensor_msgs::Actuators gz_actuators_msg;
00943 
00944   ConvertHeaderRosToGz(ros_actuators_msg_ptr->header,
00945                        gz_actuators_msg.mutable_header());
00946 
00947   for (int i = 0; i < ros_actuators_msg_ptr->angles.size(); i++) {
00948     gz_actuators_msg.add_angles(
00949         ros_actuators_msg_ptr->angles[i]);
00950   }
00951 
00952   for (int i = 0; i < ros_actuators_msg_ptr->angular_velocities.size(); i++) {
00953     gz_actuators_msg.add_angular_velocities(
00954         ros_actuators_msg_ptr->angular_velocities[i]);
00955   }
00956 
00957   for (int i = 0; i < ros_actuators_msg_ptr->normalized.size(); i++) {
00958     gz_actuators_msg.add_normalized(
00959         ros_actuators_msg_ptr->normalized[i]);
00960   }
00961 
00962   // Publish to Gazebo
00963   gz_publisher_ptr->Publish(gz_actuators_msg);
00964 }
00965 
00966 void GazeboRosInterfacePlugin::RosCommandMotorSpeedMsgCallback(
00967     const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
00968     gazebo::transport::PublisherPtr gz_publisher_ptr) {
00969   // Convert ROS message to Gazebo message
00970 
00971   gz_mav_msgs::CommandMotorSpeed gz_command_motor_speed_msg;
00972 
00973   for (int i = 0; i < ros_actuators_msg_ptr->angular_velocities.size(); i++) {
00974     gz_command_motor_speed_msg.add_motor_speed(
00975         ros_actuators_msg_ptr->angular_velocities[i]);
00976   }
00977 
00978   // Publish to Gazebo
00979   gz_publisher_ptr->Publish(gz_command_motor_speed_msg);
00980 }
00981 
00982 void GazeboRosInterfacePlugin::RosRollPitchYawrateThrustMsgCallback(
00983     const mav_msgs::RollPitchYawrateThrustConstPtr&
00984         ros_roll_pitch_yawrate_thrust_msg_ptr,
00985     gazebo::transport::PublisherPtr gz_publisher_ptr) {
00986   // Convert ROS message to Gazebo message
00987 
00988   gz_mav_msgs::RollPitchYawrateThrust gz_roll_pitch_yawrate_thrust_msg;
00989 
00990   ConvertHeaderRosToGz(ros_roll_pitch_yawrate_thrust_msg_ptr->header,
00991                        gz_roll_pitch_yawrate_thrust_msg.mutable_header());
00992 
00993   gz_roll_pitch_yawrate_thrust_msg.set_roll(
00994       ros_roll_pitch_yawrate_thrust_msg_ptr->roll);
00995   gz_roll_pitch_yawrate_thrust_msg.set_pitch(
00996       ros_roll_pitch_yawrate_thrust_msg_ptr->pitch);
00997   gz_roll_pitch_yawrate_thrust_msg.set_yaw_rate(
00998       ros_roll_pitch_yawrate_thrust_msg_ptr->yaw_rate);
00999 
01000   gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_x(
01001       ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.x);
01002   gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_y(
01003       ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.y);
01004   gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_z(
01005       ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.z);
01006 
01007   // Publish to Gazebo
01008   gz_publisher_ptr->Publish(gz_roll_pitch_yawrate_thrust_msg);
01009 }
01010 
01011 void GazeboRosInterfacePlugin::RosWindSpeedMsgCallback(
01012     const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr,
01013     gazebo::transport::PublisherPtr gz_publisher_ptr) {
01014   // Convert ROS message to Gazebo message
01015 
01016   gz_mav_msgs::WindSpeed gz_wind_speed_msg;
01017 
01018   ConvertHeaderRosToGz(ros_wind_speed_msg_ptr->header,
01019                        gz_wind_speed_msg.mutable_header());
01020 
01021   gz_wind_speed_msg.mutable_velocity()->set_x(
01022       ros_wind_speed_msg_ptr->velocity.x);
01023   gz_wind_speed_msg.mutable_velocity()->set_y(
01024       ros_wind_speed_msg_ptr->velocity.y);
01025   gz_wind_speed_msg.mutable_velocity()->set_z(
01026       ros_wind_speed_msg_ptr->velocity.z);
01027 
01028   // Publish to Gazebo
01029   gz_publisher_ptr->Publish(gz_wind_speed_msg);
01030 }
01031 
01032 void GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback(
01033     GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg) {
01034   ros::Time stamp;
01035   stamp.sec = broadcast_transform_msg->header().stamp().sec();
01036   stamp.nsec = broadcast_transform_msg->header().stamp().nsec();
01037 
01038   tf::Quaternion tf_q_W_L(broadcast_transform_msg->transform().rotation().x(),
01039                           broadcast_transform_msg->transform().rotation().y(),
01040                           broadcast_transform_msg->transform().rotation().z(),
01041                           broadcast_transform_msg->transform().rotation().w());
01042 
01043   tf::Vector3 tf_p(broadcast_transform_msg->transform().translation().x(),
01044                    broadcast_transform_msg->transform().translation().y(),
01045                    broadcast_transform_msg->transform().translation().z());
01046 
01047   tf_ = tf::Transform(tf_q_W_L, tf_p);
01048   transform_broadcaster_.sendTransform(tf::StampedTransform(
01049       tf_, stamp, broadcast_transform_msg->parent_frame_id(),
01050       broadcast_transform_msg->child_frame_id()));
01051 }
01052 
01053 GZ_REGISTER_WORLD_PLUGIN(GazeboRosInterfacePlugin);
01054 
01055 }  // 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