rotors_gazebo_plugins: gazebo_gps_plugin.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland
00003  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010 
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 // MODULE
00019 #include "rotors_gazebo_plugins/gazebo_gps_plugin.h"
00020 
00021 // 3RD PARTY
00022 #include "mav_msgs/default_topics.h"
00023 
00024 // USER
00025 #include "ConnectGazeboToRosTopic.pb.h"
00026 
00027 namespace gazebo {
00028 
00029 GazeboGpsPlugin::GazeboGpsPlugin()
00030     : SensorPlugin(),
00031       // node_handle_(0),
00032       random_generator_(random_device_()),
00033       pubs_and_subs_created_(false) {}
00034 
00035 GazeboGpsPlugin::~GazeboGpsPlugin() {
00036 }
00037 
00038 void GazeboGpsPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) {
00039   if (kPrintOnPluginLoad) {
00040     gzdbg << __FUNCTION__ << "() called." << std::endl;
00041   }
00042 
00043 // Store the pointer to the parent sensor.
00044   parent_sensor_ = std::dynamic_pointer_cast<sensors::GpsSensor>(_sensor);
00045   world_ = physics::get_world(parent_sensor_->WorldName());
00046 
00047   //==============================================//
00048   //========== READ IN PARAMS FROM SDF ===========//
00049   //==============================================//
00050 
00051   std::string link_name;
00052 
00053   if (_sdf->HasElement("robotNamespace"))
00054     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00055   else
00056     gzerr << "[gazebo_gps_plugin] Please specify a robotNamespace.\n";
00057 
00058   node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00059 
00060   // Initialise with default namespace (typically /gazebo/default/)
00061   node_handle_->Init();
00062 
00063   if (_sdf->HasElement("linkName"))
00064     link_name = _sdf->GetElement("linkName")->Get<std::string>();
00065   else
00066     gzerr << "[gazebo_gps_plugin] Please specify a linkName.\n";
00067 
00068   std::string frame_id = link_name;
00069 
00070   // Get the pointer to the link that holds the sensor.
00071   link_ =
00072       boost::dynamic_pointer_cast<physics::Link>(world_->EntityByName(link_name));
00073   if (link_ == NULL)
00074     gzerr << "[gazebo_gps_plugin] Couldn't find specified link \"" << link_name
00075           << "\"\n";
00076 
00077   // Retrieve the rest of the SDF parameters.
00078   double hor_pos_std_dev;
00079   double ver_pos_std_dev;
00080   double hor_vel_std_dev;
00081   double ver_vel_std_dev;
00082 
00083   getSdfParam<std::string>(_sdf, "gpsTopic", gps_topic_,
00084                            mav_msgs::default_topics::GPS);
00085 
00086   getSdfParam<std::string>(_sdf, "groundSpeedTopic", ground_speed_topic_,
00087                            mav_msgs::default_topics::GROUND_SPEED);
00088 
00089   getSdfParam<double>(_sdf, "horPosStdDev", hor_pos_std_dev,
00090                       kDefaultHorPosStdDev);
00091   getSdfParam<double>(_sdf, "verPosStdDev", ver_pos_std_dev,
00092                       kDefaultVerPosStdDev);
00093   getSdfParam<double>(_sdf, "horVelStdDev", hor_vel_std_dev,
00094                       kDefaultHorVelStdDev);
00095   getSdfParam<double>(_sdf, "verVelStdDev", ver_vel_std_dev,
00096                       kDefaultVerVelStdDev);
00097 
00098   // Connect to the sensor update event.
00099   this->updateConnection_ = this->parent_sensor_->ConnectUpdated(
00100       boost::bind(&GazeboGpsPlugin::OnUpdate, this));
00101 
00102   // Make sure the parent sensor is active.
00103   parent_sensor_->SetActive(true);
00104 
00105   // Initialize the normal distributions for ground speed.
00106   ground_speed_n_[0] = NormalDistribution(0, hor_vel_std_dev);
00107   ground_speed_n_[1] = NormalDistribution(0, hor_vel_std_dev);
00108   ground_speed_n_[2] = NormalDistribution(0, ver_vel_std_dev);
00109 
00110   // ============================================ //
00111   // ======= POPULATE STATIC PARTS OF MSGS ====== //
00112   // ============================================ //
00113 
00114   // Fill the static parts of the GPS message.
00115   gz_gps_message_.mutable_header()->set_frame_id(frame_id);
00116   gz_gps_message_.set_service(gz_sensor_msgs::NavSatFix::SERVICE_GPS);
00117   gz_gps_message_.set_status(gz_sensor_msgs::NavSatFix::STATUS_FIX);
00118   gz_gps_message_.set_position_covariance_type(
00119       gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN);
00120 
00121   for (int i = 0; i < 9; i++) {
00122     switch (i) {
00123       case 0:
00124         gz_gps_message_.add_position_covariance(hor_pos_std_dev *
00125                                                 hor_pos_std_dev);
00126         break;
00127       case 1:
00128       case 2:
00129       case 3:
00130         gz_gps_message_.add_position_covariance(0);
00131         break;
00132       case 4:
00133         gz_gps_message_.add_position_covariance(hor_pos_std_dev *
00134                                                 hor_pos_std_dev);
00135         break;
00136       case 5:
00137       case 6:
00138       case 7:
00139         gz_gps_message_.add_position_covariance(0);
00140         break;
00141       case 8:
00142         gz_gps_message_.add_position_covariance(ver_pos_std_dev *
00143                                                 ver_pos_std_dev);
00144         break;
00145     }
00146   }
00147 
00148   // Fill the static parts of the ground speed message.
00149   gz_ground_speed_message_.mutable_header()->set_frame_id(frame_id);
00150   gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_x(0.0);
00151   gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_y(0.0);
00152   gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_z(0.0);
00153 }
00154 
00155 void GazeboGpsPlugin::OnUpdate() {
00156   if (kPrintOnUpdates) {
00157     gzdbg << __FUNCTION__ << "() called." << std::endl;
00158   }
00159 
00160   if (!pubs_and_subs_created_) {
00161     CreatePubsAndSubs();
00162     pubs_and_subs_created_ = true;
00163   }
00164 
00165   // Get the time of the last measurement.
00166   common::Time current_time;
00167 
00168   // Get the linear velocity in the world frame.
00169   ignition::math::Vector3d W_ground_speed_W_L = link_->WorldLinearVel();
00170 
00171   // Apply noise to ground speed.
00172   W_ground_speed_W_L += ignition::math::Vector3d (ground_speed_n_[0](random_generator_),
00173                                       ground_speed_n_[1](random_generator_),
00174                                       ground_speed_n_[2](random_generator_));
00175 
00176   // Fill the GPS message.
00177   current_time = parent_sensor_->LastMeasurementTime();
00178 
00179   gz_gps_message_.set_latitude(parent_sensor_->Latitude().Degree());
00180   gz_gps_message_.set_longitude(parent_sensor_->Longitude().Degree());
00181   gz_gps_message_.set_altitude(parent_sensor_->Altitude());
00182 
00183   gz_gps_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
00184   gz_gps_message_.mutable_header()->mutable_stamp()->set_nsec(
00185       current_time.nsec);
00186 
00187   // Fill the ground speed message.
00188   gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_x(
00189       W_ground_speed_W_L.X());
00190   gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_y(
00191       W_ground_speed_W_L.Y());
00192   gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_z(
00193       W_ground_speed_W_L.Z());
00194   gz_ground_speed_message_.mutable_header()->mutable_stamp()->set_sec(
00195       current_time.sec);
00196   gz_ground_speed_message_.mutable_header()->mutable_stamp()->set_nsec(
00197       current_time.nsec);
00198 
00199   // Publish the GPS message.
00200   gz_gps_pub_->Publish(gz_gps_message_);
00201 
00202   // Publish the ground speed message.
00203   gz_ground_speed_pub_->Publish(gz_ground_speed_message_);
00204 }
00205 
00206 void GazeboGpsPlugin::CreatePubsAndSubs() {
00207   // Create temporary "ConnectGazeboToRosTopic" publisher and message
00208   gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
00209       node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00210           "~/" + kConnectGazeboToRosSubtopic, 1);
00211 
00212   gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00213 
00214   // ============================================ //
00215   // =========== NAV SAT FIX MSG SETUP ========== //
00216   // ============================================ //
00217   gz_gps_pub_ = node_handle_->Advertise<gz_sensor_msgs::NavSatFix>(
00218       "~/" + namespace_ + "/" + gps_topic_, 1);
00219 
00220   connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00221                                                    gps_topic_);
00222   connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + gps_topic_);
00223   connect_gazebo_to_ros_topic_msg.set_msgtype(
00224       gz_std_msgs::ConnectGazeboToRosTopic::NAV_SAT_FIX);
00225   connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00226                                            true);
00227 
00228   // ============================================ //
00229   // == GROUND SPEED (TWIST STAMPED) MSG SETUP == //
00230   // ============================================ //
00231   gz_ground_speed_pub_ =
00232       node_handle_->Advertise<gz_geometry_msgs::TwistStamped>(
00233           "~/" + namespace_ + "/" + ground_speed_topic_, 1);
00234 
00235   connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00236                                                    ground_speed_topic_);
00237   connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00238                                                 ground_speed_topic_);
00239   connect_gazebo_to_ros_topic_msg.set_msgtype(
00240       gz_std_msgs::ConnectGazeboToRosTopic::TWIST_STAMPED);
00241   connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg);
00242 }
00243 
00244 GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin);
00245 }