00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <rotors_gazebo_plugins/gazebo_ros_interface_plugin.h>
00019
00020
00021 #include <stdio.h>
00022 #include <chrono>
00023 #include <cmath>
00024 #include <iostream>
00025
00026
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
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
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066 gz_node_handle_ = transport::NodePtr(new transport::Node());
00067
00068 gz_node_handle_->Init();
00069
00070
00071
00072 ros_node_handle_ = new ros::NodeHandle();
00073
00074
00075
00076 this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00077 boost::bind(&GazeboRosInterfacePlugin::OnUpdate, this, _1));
00078
00079
00080
00081
00082
00083 gz_connect_gazebo_to_ros_topic_sub_ = gz_node_handle_->Subscribe(
00084 "~/" + kConnectGazeboToRosSubtopic,
00085 &GazeboRosInterfacePlugin::GzConnectGazeboToRosTopicMsgCallback, this);
00086
00087
00088
00089
00090
00091 gz_connect_ros_to_gazebo_topic_sub_ = gz_node_handle_->Subscribe(
00092 "~/" + kConnectRosToGazeboSubtopic,
00093 &GazeboRosInterfacePlugin::GzConnectRosToGazeboTopicMsgCallback, this);
00094
00095
00096
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
00106
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
00144 static std::map<std::string, ConnectHelperStorage<GazeboMsgT> > callback_map;
00145
00146
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
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
00161 gazebo::transport::SubscriberPtr subscriberPtr;
00162 subscriberPtr = gz_node_handle->Subscribe(
00163 gazeboTopicName, &ConnectHelperStorage<GazeboMsgT>::callback,
00164 &callback_entry.first->second);
00165
00166
00167
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 "";
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
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
00301
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
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
00320
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
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
00340
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
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
00358
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
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
00393
00394
00395 void GazeboRosInterfacePlugin::GzActuatorsMsgCallback(
00396 GzActuatorsMsgPtr& gz_actuators_msg, ros::Publisher ros_publisher) {
00397
00398
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
00410 ros_publisher.publish(ros_actuators_msg_);
00411 }
00412
00413 void GazeboRosInterfacePlugin::GzFloat32MsgCallback(
00414 GzFloat32MsgPtr& gz_float_32_msg, ros::Publisher ros_publisher) {
00415
00416 ros_float_32_msg_.data = gz_float_32_msg->data();
00417
00418
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
00426
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
00437 ros_publisher.publish(ros_fluid_pressure_msg_);
00438 }
00439
00440 void GazeboRosInterfacePlugin::GzImuMsgCallback(GzImuPtr& gz_imu_msg,
00441 ros::Publisher ros_publisher) {
00442
00443
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
00453
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
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
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
00522
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
00535
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
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
00555
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
00635
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
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
00654
00655
00656
00657
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
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
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
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
00732
00733 ConvertHeaderGzToRos(gz_pose_with_covariance_stamped_msg->header(),
00734 &ros_pose_with_covariance_stamped_msg_.header);
00735
00736
00737
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
00777
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
00802
00803 ConvertHeaderGzToRos(gz_transform_stamped_msg->header(),
00804 &ros_transform_stamped_msg_.header);
00805
00806
00807
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
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
00835
00836 ConvertHeaderGzToRos(gz_twist_stamped_msg->header(),
00837 &ros_twist_stamped_msg_.header);
00838
00839
00840
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
00865
00866 ConvertHeaderGzToRos(gz_vector_3d_stamped_msg->header(),
00867 &ros_position_stamped_msg_.header);
00868
00869
00870
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
00885
00886 ConvertHeaderGzToRos(gz_wind_speed_msg->header(),
00887 &ros_wind_speed_msg_.header);
00888
00889
00890
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
00906
00907 ConvertHeaderGzToRos(gz_wrench_stamped_msg->header(),
00908 &ros_wrench_stamped_msg_.header);
00909
00910
00911
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
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
00935
00936
00937 void GazeboRosInterfacePlugin::RosActuatorsMsgCallback(
00938 const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
00939 gazebo::transport::PublisherPtr gz_publisher_ptr) {
00940
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
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
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
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
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
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
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
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 }