hector_gazebo_plugins: gazebo_ros

Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_gazebo_plugins/gazebo_ros_gps.h>
00030 #include <gazebo/physics/physics.hh>
00031 
00032 // WGS84 constants
00033 static const double equatorial_radius = 6378137.0;
00034 static const double flattening = 1.0/298.257223563;
00035 static const double excentrity2 = 2*flattening - flattening*flattening;
00036 
00037 // default reference position
00038 static const double DEFAULT_REFERENCE_LATITUDE  = 49.9;
00039 static const double DEFAULT_REFERENCE_LONGITUDE = 8.9;
00040 static const double DEFAULT_REFERENCE_HEADING   = 0.0;
00041 static const double DEFAULT_REFERENCE_ALTITUDE  = 0.0;
00042 
00043 namespace gazebo {
00044 
00045 GazeboRosGps::GazeboRosGps()
00046 {
00047 }
00048 
00050 // Destructor
00051 GazeboRosGps::~GazeboRosGps()
00052 {
00053   updateTimer.Disconnect(updateConnection);
00054 
00055   dynamic_reconfigure_server_position_.reset();
00056   dynamic_reconfigure_server_velocity_.reset();
00057   dynamic_reconfigure_server_status_.reset();
00058 
00059   node_handle_->shutdown();
00060   delete node_handle_;
00061 }
00062 
00064 // Load the controller
00065 void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00066 {
00067   world = _model->GetWorld();
00068 
00069   // load parameters
00070   if (!_sdf->HasElement("robotNamespace"))
00071     namespace_.clear();
00072   else
00073     namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
00074 
00075   if (!_sdf->HasElement("bodyName"))
00076   {
00077     link = _model->GetLink();
00078     link_name_ = link->GetName();
00079   }
00080   else {
00081     link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
00082     link = _model->GetLink(link_name_);
00083   }
00084 
00085   if (!link)
00086   {
00087     ROS_FATAL("GazeboRosGps plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00088     return;
00089   }
00090 
00091   // default parameters
00092   frame_id_ = "/world";
00093   fix_topic_ = "fix";
00094   velocity_topic_ = "fix_velocity";
00095 
00096   reference_latitude_  = DEFAULT_REFERENCE_LATITUDE;
00097   reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE;
00098   reference_heading_   = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
00099   reference_altitude_  = DEFAULT_REFERENCE_ALTITUDE;
00100 
00101   fix_.status.status  = sensor_msgs::NavSatStatus::STATUS_FIX;
00102   fix_.status.service = sensor_msgs::NavSatStatus::STATUS_FIX;
00103 
00104   if (_sdf->HasElement("frameId"))
00105     frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
00106 
00107   if (_sdf->HasElement("topicName"))
00108     fix_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
00109 
00110   if (_sdf->HasElement("velocityTopicName"))
00111     velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValue()->GetAsString();
00112 
00113   if (_sdf->HasElement("referenceLatitude"))
00114     _sdf->GetElement("referenceLatitude")->GetValue()->Get(reference_latitude_);
00115 
00116   if (_sdf->HasElement("referenceLongitude"))
00117     _sdf->GetElement("referenceLongitude")->GetValue()->Get(reference_longitude_);
00118 
00119   if (_sdf->HasElement("referenceHeading"))
00120     if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
00121       reference_heading_ *= M_PI/180.0;
00122 
00123   if (_sdf->HasElement("referenceAltitude"))
00124     _sdf->GetElement("referenceAltitude")->GetValue()->Get(reference_altitude_);
00125 
00126   if (_sdf->HasElement("status"))
00127     _sdf->GetElement("status")->GetValue()->Get(fix_.status.status);
00128 
00129   if (_sdf->HasElement("service"))
00130     _sdf->GetElement("service")->GetValue()->Get(fix_.status.service);
00131 
00132   fix_.header.frame_id = frame_id_;
00133   velocity_.header.frame_id = frame_id_;
00134 
00135   position_error_model_.Load(_sdf);
00136   velocity_error_model_.Load(_sdf, "velocity");
00137 
00138   // calculate earth radii
00139   double temp = 1.0 / (1.0 - excentrity2 * sin(reference_latitude_ * M_PI/180.0) * sin(reference_latitude_ * M_PI/180.0));
00140   double prime_vertical_radius = equatorial_radius * sqrt(temp);
00141   radius_north_ = prime_vertical_radius * (1 - excentrity2) * temp;
00142   radius_east_  = prime_vertical_radius * cos(reference_latitude_ * M_PI/180.0);
00143 
00144   // Make sure the ROS node for Gazebo has already been initialized
00145   if (!ros::isInitialized())
00146   {
00147     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00148       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00149     return;
00150   }
00151 
00152   node_handle_ = new ros::NodeHandle(namespace_);
00153   fix_publisher_ = node_handle_->advertise<sensor_msgs::NavSatFix>(fix_topic_, 10);
00154   velocity_publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(velocity_topic_, 10);
00155 
00156   // setup dynamic_reconfigure servers
00157   dynamic_reconfigure_server_position_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/position")));
00158   dynamic_reconfigure_server_velocity_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/velocity")));
00159   dynamic_reconfigure_server_status_.reset(new dynamic_reconfigure::Server<GNSSConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/status")));
00160   dynamic_reconfigure_server_position_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &position_error_model_, _1, _2));
00161   dynamic_reconfigure_server_velocity_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &velocity_error_model_, _1, _2));
00162   dynamic_reconfigure_server_status_->setCallback(boost::bind(&GazeboRosGps::dynamicReconfigureCallback, this, _1, _2));
00163 
00164   Reset();
00165 
00166   // connect Update function
00167   updateTimer.setUpdateRate(4.0);
00168   updateTimer.Load(world, _sdf);
00169   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosGps::Update, this));
00170 }
00171 
00172 void GazeboRosGps::Reset()
00173 {
00174   updateTimer.Reset();
00175   position_error_model_.reset();
00176   velocity_error_model_.reset();
00177 }
00178 
00179 void GazeboRosGps::dynamicReconfigureCallback(GazeboRosGps::GNSSConfig &config, uint32_t level)
00180 {
00181   using sensor_msgs::NavSatStatus;
00182 
00183   if (level == 1) {
00184     if (!config.STATUS_FIX) {
00185       fix_.status.status = NavSatStatus::STATUS_NO_FIX;
00186     } else {
00187       fix_.status.status = (config.STATUS_SBAS_FIX ? NavSatStatus::STATUS_SBAS_FIX : 0) |
00188                            (config.STATUS_GBAS_FIX ? NavSatStatus::STATUS_GBAS_FIX : 0);
00189     }
00190     fix_.status.service = (config.SERVICE_GPS     ? NavSatStatus::SERVICE_GPS : 0) |
00191                           (config.SERVICE_GLONASS ? NavSatStatus::SERVICE_GLONASS : 0) |
00192                           (config.SERVICE_COMPASS ? NavSatStatus::SERVICE_COMPASS : 0) |
00193                           (config.SERVICE_GALILEO ? NavSatStatus::SERVICE_GALILEO : 0);
00194   } else {
00195     config.STATUS_FIX      = (fix_.status.status != NavSatStatus::STATUS_NO_FIX);
00196     config.STATUS_SBAS_FIX = (fix_.status.status & NavSatStatus::STATUS_SBAS_FIX);
00197     config.STATUS_GBAS_FIX = (fix_.status.status & NavSatStatus::STATUS_GBAS_FIX);
00198     config.SERVICE_GPS     = (fix_.status.service & NavSatStatus::SERVICE_GPS);
00199     config.SERVICE_GLONASS = (fix_.status.service & NavSatStatus::SERVICE_GLONASS);
00200     config.SERVICE_COMPASS = (fix_.status.service & NavSatStatus::SERVICE_COMPASS);
00201     config.SERVICE_GALILEO = (fix_.status.service & NavSatStatus::SERVICE_GALILEO);
00202   }
00203 }
00204 
00206 // Update the controller
00207 void GazeboRosGps::Update()
00208 {
00209   common::Time sim_time = world->GetSimTime();
00210   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00211 
00212   math::Pose pose = link->GetWorldPose();
00213 
00214   gazebo::math::Vector3 velocity = velocity_error_model_(link->GetWorldLinearVel(), dt);
00215   gazebo::math::Vector3 position = position_error_model_(pose.pos, dt);
00216 
00217   // An offset error in the velocity is integrated into the position error for the next timestep.
00218   // Note: Usually GNSS receivers have almost no drift in the velocity signal.
00219   position_error_model_.setCurrentDrift(position_error_model_.getCurrentDrift() + dt * velocity_error_model_.getCurrentDrift());
00220 
00221   fix_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00222   velocity_.header.stamp = fix_.header.stamp;
00223 
00224   fix_.latitude  = reference_latitude_  + ( cos(reference_heading_) * position.x + sin(reference_heading_) * position.y) / radius_north_ * 180.0/M_PI;
00225   fix_.longitude = reference_longitude_ - (-sin(reference_heading_) * position.x + cos(reference_heading_) * position.y) / radius_east_  * 180.0/M_PI;
00226   fix_.altitude  = reference_altitude_  + position.z;
00227   velocity_.vector.x =  cos(reference_heading_) * velocity.x + sin(reference_heading_) * velocity.y;
00228   velocity_.vector.y = -sin(reference_heading_) * velocity.x + cos(reference_heading_) * velocity.y;
00229   velocity_.vector.z = velocity.z;
00230 
00231   fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00232   fix_.position_covariance[0] = position_error_model_.drift.x*position_error_model_.drift.x + position_error_model_.gaussian_noise.x*position_error_model_.gaussian_noise.x;
00233   fix_.position_covariance[4] = position_error_model_.drift.y*position_error_model_.drift.y + position_error_model_.gaussian_noise.y*position_error_model_.gaussian_noise.y;
00234   fix_.position_covariance[8] = position_error_model_.drift.z*position_error_model_.drift.z + position_error_model_.gaussian_noise.z*position_error_model_.gaussian_noise.z;
00235 
00236   fix_publisher_.publish(fix_);
00237   velocity_publisher_.publish(velocity_);
00238 }
00239 
00240 // Register this plugin with the simulator
00241 GZ_REGISTER_MODEL_PLUGIN(GazeboRosGps)
00242 
00243 } // namespace gazebo