gazebo_ros_gps.cpp
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 "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   node_handle_->shutdown();
00055   delete node_handle_;
00056 }
00057 
00059 // Load the controller
00060 void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00061 {
00062   world = _model->GetWorld();
00063 
00064   // load parameters
00065   if (!_sdf->HasElement("robotNamespace"))
00066     namespace_.clear();
00067   else
00068     namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00069 
00070   if (!_sdf->HasElement("bodyName"))
00071   {
00072     link = _model->GetLink();
00073     link_name_ = link->GetName();
00074   }
00075   else {
00076     link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00077     link = _model->GetLink(link_name_);
00078   }
00079 
00080   if (!link)
00081   {
00082     ROS_FATAL("GazeboRosGps plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00083     return;
00084   }
00085 
00086   if (!_sdf->HasElement("frameId"))
00087     frame_id_ = "/world";
00088   else
00089     frame_id_ = _sdf->GetElement("frameId")->GetValueString();
00090 
00091   if (!_sdf->HasElement("topicName"))
00092     fix_topic_ = "fix";
00093   else
00094     fix_topic_ = _sdf->GetElement("topicName")->GetValueString();
00095 
00096   if (!_sdf->HasElement("velocityTopicName"))
00097     velocity_topic_ = "fix_velocity";
00098   else
00099     velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValueString();
00100 
00101   if (!_sdf->HasElement("referenceLatitude"))
00102     reference_latitude_ = DEFAULT_REFERENCE_LATITUDE;
00103   else
00104     reference_latitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble();
00105 
00106   if (!_sdf->HasElement("referenceLongitude"))
00107     reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE;
00108   else
00109     reference_longitude_ = _sdf->GetElement("referenceLongitude")->GetValueDouble();
00110 
00111   if (!_sdf->HasElement("referenceHeading"))
00112     reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
00113   else
00114     reference_heading_ = _sdf->GetElement("referenceLatitude")->GetValueDouble() * M_PI/180.0;
00115 
00116   if (!_sdf->HasElement("referenceAltitude"))
00117     reference_altitude_ = DEFAULT_REFERENCE_ALTITUDE;
00118   else
00119     reference_altitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble();
00120 
00121   if (!_sdf->HasElement("status"))
00122     status_ = sensor_msgs::NavSatStatus::STATUS_FIX;
00123   else
00124     status_ = _sdf->GetElement("status")->GetValueUInt();
00125 
00126   if (!_sdf->HasElement("service"))
00127     service_ = sensor_msgs::NavSatStatus::SERVICE_GPS;
00128   else
00129     service_ = _sdf->GetElement("service")->GetValueUInt();
00130 
00131   fix_.header.frame_id = frame_id_;
00132   fix_.status.status  = status_;
00133   fix_.status.service = service_;
00134   velocity_.header.frame_id = frame_id_;
00135 
00136   position_error_model_.Load(_sdf);
00137   velocity_error_model_.Load(_sdf, "velocity");
00138 
00139   // calculate earth radii
00140   double temp = 1.0 / (1.0 - excentrity2 * sin(reference_latitude_ * M_PI/180.0) * sin(reference_latitude_ * M_PI/180.0));
00141   double prime_vertical_radius = equatorial_radius * sqrt(temp);
00142   radius_north_ = prime_vertical_radius * (1 - excentrity2) * temp;
00143   radius_east_  = prime_vertical_radius * cos(reference_latitude_ * M_PI/180.0);
00144 
00145   // start ros node
00146   if (!ros::isInitialized())
00147   {
00148     int argc = 0;
00149     char** argv = NULL;
00150     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00151   }
00152 
00153   node_handle_ = new ros::NodeHandle(namespace_);
00154   fix_publisher_ = node_handle_->advertise<sensor_msgs::NavSatFix>(fix_topic_, 10);
00155   velocity_publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(velocity_topic_, 10);
00156 
00157   Reset();
00158 
00159   // connect Update function
00160   updateTimer.setUpdateRate(4.0);
00161   updateTimer.Load(world, _sdf);
00162   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosGps::Update, this));
00163 }
00164 
00165 void GazeboRosGps::Reset()
00166 {
00167   updateTimer.Reset();
00168   position_error_model_.reset();
00169   velocity_error_model_.reset();
00170 }
00171 
00173 // Update the controller
00174 void GazeboRosGps::Update()
00175 {
00176   common::Time sim_time = world->GetSimTime();
00177   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00178 
00179   math::Pose pose = link->GetWorldPose();
00180 
00181   gazebo::math::Vector3 velocity = velocity_error_model_(link->GetWorldLinearVel(), dt);
00182   position_error_model_.setCurrentDrift(position_error_model_.getCurrentDrift() + velocity_error_model_.getCurrentError() * dt);
00183   gazebo::math::Vector3 position = position_error_model_(pose.pos, dt);
00184 
00185   fix_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00186   velocity_.header.stamp = fix_.header.stamp;
00187 
00188   fix_.latitude  = reference_latitude_  + ( cos(reference_heading_) * position.x + sin(reference_heading_) * position.y) / radius_north_ * 180.0/M_PI;
00189   fix_.longitude = reference_longitude_ - (-sin(reference_heading_) * position.x + cos(reference_heading_) * position.y) / radius_east_  * 180.0/M_PI;
00190   fix_.altitude  = reference_altitude_  + position.z;
00191   fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00192   velocity_.vector.x =  cos(reference_heading_) * velocity.x + sin(reference_heading_) * velocity.y;
00193   velocity_.vector.y = -sin(reference_heading_) * velocity.x + cos(reference_heading_) * velocity.y;
00194   velocity_.vector.z = velocity.z;
00195 
00196   fix_publisher_.publish(fix_);
00197   velocity_publisher_.publish(velocity_);
00198 }
00199 
00200 // Register this plugin with the simulator
00201 GZ_REGISTER_MODEL_PLUGIN(GazeboRosGps)
00202 
00203 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30