gazebo_ros_gps.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
30 #include <gazebo/physics/physics.hh>
31 
32 // WGS84 constants
33 static const double equatorial_radius = 6378137.0;
34 static const double flattening = 1.0/298.257223563;
35 static const double excentrity2 = 2*flattening - flattening*flattening;
36 
37 // default reference position
38 static const double DEFAULT_REFERENCE_LATITUDE = 49.9;
39 static const double DEFAULT_REFERENCE_LONGITUDE = 8.9;
40 static const double DEFAULT_REFERENCE_HEADING = 0.0;
41 static const double DEFAULT_REFERENCE_ALTITUDE = 0.0;
42 
43 namespace gazebo {
44 
46 {
47 }
48 
50 // Destructor
52 {
54 
58 
60  delete node_handle_;
61 }
62 
64 // Load the controller
65 void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
66 {
67  world = _model->GetWorld();
68 
69  // load parameters
70  if (!_sdf->HasElement("robotNamespace"))
71  namespace_.clear();
72  else
73  namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
74 
75  if (!_sdf->HasElement("bodyName"))
76  {
77  link = _model->GetLink();
78  link_name_ = link->GetName();
79  }
80  else {
81  link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
82  link = _model->GetLink(link_name_);
83  }
84 
85  if (!link)
86  {
87  ROS_FATAL("GazeboRosGps plugin error: bodyName: %s does not exist\n", link_name_.c_str());
88  return;
89  }
90 
91  // default parameters
92  frame_id_ = "/world";
93  fix_topic_ = "fix";
94  velocity_topic_ = "fix_velocity";
95 
100 
101  fix_.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
102  fix_.status.service = 0;
103 
104  if (_sdf->HasElement("frameId"))
105  frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
106 
107  if (_sdf->HasElement("topicName"))
108  fix_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
109 
110  if (_sdf->HasElement("velocityTopicName"))
111  velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValue()->GetAsString();
112 
113  if (_sdf->HasElement("referenceLatitude"))
114  _sdf->GetElement("referenceLatitude")->GetValue()->Get(reference_latitude_);
115 
116  if (_sdf->HasElement("referenceLongitude"))
117  _sdf->GetElement("referenceLongitude")->GetValue()->Get(reference_longitude_);
118 
119  if (_sdf->HasElement("referenceHeading"))
120  if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
121  reference_heading_ *= M_PI/180.0;
122 
123  if (_sdf->HasElement("referenceAltitude"))
124  _sdf->GetElement("referenceAltitude")->GetValue()->Get(reference_altitude_);
125 
126  if (_sdf->HasElement("status")) {
127  int status = fix_.status.status;
128  if (_sdf->GetElement("status")->GetValue()->Get(status))
129  fix_.status.status = static_cast<sensor_msgs::NavSatStatus::_status_type>(status);
130  }
131 
132  if (_sdf->HasElement("service")) {
133  unsigned int service = fix_.status.service;
134  if (_sdf->GetElement("service")->GetValue()->Get(service))
135  fix_.status.service = static_cast<sensor_msgs::NavSatStatus::_service_type>(service);
136  }
137 
138  fix_.header.frame_id = frame_id_;
139  velocity_.header.frame_id = frame_id_;
140 
142  velocity_error_model_.Load(_sdf, "velocity");
143 
144  // calculate earth radii
145  double temp = 1.0 / (1.0 - excentrity2 * sin(reference_latitude_ * M_PI/180.0) * sin(reference_latitude_ * M_PI/180.0));
146  double prime_vertical_radius = equatorial_radius * sqrt(temp);
147  radius_north_ = prime_vertical_radius * (1 - excentrity2) * temp;
148  radius_east_ = prime_vertical_radius * cos(reference_latitude_ * M_PI/180.0);
149 
150  // Make sure the ROS node for Gazebo has already been initialized
151  if (!ros::isInitialized())
152  {
153  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
154  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
155  return;
156  }
157 
159  fix_publisher_ = node_handle_->advertise<sensor_msgs::NavSatFix>(fix_topic_, 10);
160  velocity_publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(velocity_topic_, 10);
161 
162  // setup dynamic_reconfigure servers
163  dynamic_reconfigure_server_position_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/position")));
164  dynamic_reconfigure_server_velocity_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/velocity")));
165  dynamic_reconfigure_server_status_.reset(new dynamic_reconfigure::Server<GNSSConfig>(ros::NodeHandle(*node_handle_, fix_topic_ + "/status")));
168  dynamic_reconfigure_server_status_->setCallback(boost::bind(&GazeboRosGps::dynamicReconfigureCallback, this, _1, _2));
169 
170  Reset();
171 
172  // connect Update function
174  updateTimer.Load(world, _sdf);
176 }
177 
179 {
180  updateTimer.Reset();
183 }
184 
186 {
187  using sensor_msgs::NavSatStatus;
188  if (level == 1) {
189  if (!config.STATUS_FIX) {
190  fix_.status.status = NavSatStatus::STATUS_NO_FIX;
191  } else {
192  fix_.status.status = (config.STATUS_SBAS_FIX ? NavSatStatus::STATUS_SBAS_FIX : 0) |
193  (config.STATUS_GBAS_FIX ? NavSatStatus::STATUS_GBAS_FIX : 0);
194  }
195  fix_.status.service = (config.SERVICE_GPS ? NavSatStatus::SERVICE_GPS : 0) |
196  (config.SERVICE_GLONASS ? NavSatStatus::SERVICE_GLONASS : 0) |
197  (config.SERVICE_COMPASS ? NavSatStatus::SERVICE_COMPASS : 0) |
198  (config.SERVICE_GALILEO ? NavSatStatus::SERVICE_GALILEO : 0);
199  } else {
200  config.STATUS_FIX = (fix_.status.status != NavSatStatus::STATUS_NO_FIX);
201  config.STATUS_SBAS_FIX = (fix_.status.status & NavSatStatus::STATUS_SBAS_FIX);
202  config.STATUS_GBAS_FIX = (fix_.status.status & NavSatStatus::STATUS_GBAS_FIX);
203  config.SERVICE_GPS = (fix_.status.service & NavSatStatus::SERVICE_GPS);
204  config.SERVICE_GLONASS = (fix_.status.service & NavSatStatus::SERVICE_GLONASS);
205  config.SERVICE_COMPASS = (fix_.status.service & NavSatStatus::SERVICE_COMPASS);
206  config.SERVICE_GALILEO = (fix_.status.service & NavSatStatus::SERVICE_GALILEO);
207  }
208 }
209 
211 // Update the controller
213 {
214 #if (GAZEBO_MAJOR_VERSION >= 8)
215  common::Time sim_time = world->SimTime();
216  double dt = updateTimer.getTimeSinceLastUpdate().Double();
217 
218  ignition::math::Pose3d pose = link->WorldPose();
219 
220  ignition::math::Vector3d velocity = velocity_error_model_(link->WorldLinearVel(), dt);
221  ignition::math::Vector3d position = position_error_model_(pose.Pos(), dt);
222 #else
223  common::Time sim_time = world->GetSimTime();
224  double dt = updateTimer.getTimeSinceLastUpdate().Double();
225 
226  math::Pose pose = link->GetWorldPose();
227 
228  gazebo::math::Vector3 velocity = velocity_error_model_(link->GetWorldLinearVel(), dt);
229  gazebo::math::Vector3 position = position_error_model_(pose.pos, dt);
230 #endif
231 
232  // An offset error in the velocity is integrated into the position error for the next timestep.
233  // Note: Usually GNSS receivers have almost no drift in the velocity signal.
235 
236  fix_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
237  velocity_.header.stamp = fix_.header.stamp;
238 
239 #if (GAZEBO_MAJOR_VERSION >= 8)
240  fix_.latitude = reference_latitude_ + ( cos(reference_heading_) * position.X() + sin(reference_heading_) * position.Y()) / radius_north_ * 180.0/M_PI;
241  fix_.longitude = reference_longitude_ - (-sin(reference_heading_) * position.X() + cos(reference_heading_) * position.Y()) / radius_east_ * 180.0/M_PI;
242  fix_.altitude = reference_altitude_ + position.Z();
243  velocity_.vector.x = cos(reference_heading_) * velocity.X() + sin(reference_heading_) * velocity.Y();
244  velocity_.vector.y = -sin(reference_heading_) * velocity.X() + cos(reference_heading_) * velocity.Y();
245  velocity_.vector.z = velocity.Z();
246 
247  fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
251 #else
252  fix_.latitude = reference_latitude_ + ( cos(reference_heading_) * position.x + sin(reference_heading_) * position.y) / radius_north_ * 180.0/M_PI;
253  fix_.longitude = reference_longitude_ - (-sin(reference_heading_) * position.x + cos(reference_heading_) * position.y) / radius_east_ * 180.0/M_PI;
254  fix_.altitude = reference_altitude_ + position.z;
255  velocity_.vector.x = cos(reference_heading_) * velocity.x + sin(reference_heading_) * velocity.y;
256  velocity_.vector.y = -sin(reference_heading_) * velocity.x + cos(reference_heading_) * velocity.y;
257  velocity_.vector.z = velocity.z;
258 
259  fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
263 #endif
264 
267 }
268 
269 // Register this plugin with the simulator
270 GZ_REGISTER_MODEL_PLUGIN(GazeboRosGps)
271 
272 } // namespace gazebo
static const double DEFAULT_REFERENCE_ALTITUDE
virtual const T & getCurrentDrift() const
Definition: sensor_model.h:61
#define ROS_FATAL(...)
static const double DEFAULT_REFERENCE_LONGITUDE
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
Definition: update_timer.h:52
UpdateTimer updateTimer
virtual void setCurrentDrift(const T &new_drift)
Definition: sensor_model.h:64
void publish(const boost::shared_ptr< M > &message) const
static const double flattening
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix=std::string())
Definition: sensor_model.h:101
void dynamicReconfigureCallback(GNSSConfig &config, uint32_t level)
static const double equatorial_radius
sensor_msgs::NavSatFix fix_
ROSCPP_DECL bool isInitialized()
virtual void Reset()
Definition: update_timer.h:175
physics::WorldPtr world
The parent World.
virtual event::ConnectionPtr Connect(const boost::function< void()> &_subscriber, bool connectToWorldUpdateBegin=true)
Definition: update_timer.h:85
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_velocity_
ros::Publisher fix_publisher_
boost::shared_ptr< dynamic_reconfigure::Server< GNSSConfig > > dynamic_reconfigure_server_status_
static const double DEFAULT_REFERENCE_HEADING
hector_gazebo_plugins::GNSSConfig GNSSConfig
#define ROS_FATAL_STREAM(args)
virtual void Disconnect(event::ConnectionPtr const &_c=event::ConnectionPtr())
Definition: update_timer.h:95
std::string velocity_topic_
virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
std::string link_name_
common::Time getTimeSinceLastUpdate() const
Definition: update_timer.h:132
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string fix_topic_
SensorModel3 velocity_error_model_
geometry_msgs::Vector3Stamped velocity_
static const double excentrity2
virtual void reset()
Definition: sensor_model.h:191
std::string namespace_
event::ConnectionPtr updateConnection
physics::LinkPtr link
The link referred to by this plugin.
static const double DEFAULT_REFERENCE_LATITUDE
ros::Publisher velocity_publisher_
void setUpdateRate(double rate)
Definition: update_timer.h:124
ros::NodeHandle * node_handle_
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_position_
SensorModel3 position_error_model_


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30