GPSROSPlugin.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
17 
18 namespace gazebo {
19 
22 { }
23 
26 { }
27 
29 void GPSROSPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
30 {
31  gzmsg << "GPSROSPlugin - Loading base sensor plugin" << std::endl;
32  ROSBaseSensorPlugin::Load(_parent, _sdf);
33 
34  gzmsg << "GPSROSPlugin - Converting GPS sensor pointer" << std::endl;
35  this->gazeboGPSSensor =
36  std::dynamic_pointer_cast<sensors::GpsSensor>(_parent);
37 
38  gzmsg << "GPSROSPlugin - Initialize sensor topic publisher" << std::endl;
39  this->rosSensorOutputPub = this->rosNode->advertise<sensor_msgs::NavSatFix>(
40  this->sensorOutputTopic, 10);
41 
42  // Set the frame ID
43  this->gpsMessage.header.frame_id = this->robotNamespace + "/gps_link";
44  // TODO: Get the position covariance from the GPS sensor
45  this->gpsMessage.position_covariance_type =
46  sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
47 
48  double horizontalPosStdDev = 0.0;
49  GetSDFParam(_sdf, "horizontal_pos_std_dev", horizontalPosStdDev, 0.0);
50 
51  double verticalPosStdDev = 0.0;
52  GetSDFParam(_sdf, "vertical_pos_std_dev", verticalPosStdDev, 0.0);
53 
54  this->gpsMessage.position_covariance[0] = horizontalPosStdDev * horizontalPosStdDev;
55  this->gpsMessage.position_covariance[4] = horizontalPosStdDev * horizontalPosStdDev;
56  this->gpsMessage.position_covariance[8] = verticalPosStdDev * verticalPosStdDev;
57 
58  // TODO: Configurable status setup
59  this->gpsMessage.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
60  this->gpsMessage.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
61 
62  // Connect to the sensor update event.
63  this->updateConnection = this->gazeboGPSSensor->ConnectUpdated(
64  boost::bind(&GPSROSPlugin::OnUpdateGPS, this));
65 }
66 
69 {
70  // Publish sensor state
71  this->PublishState();
72  common::Time currentTime = this->gazeboGPSSensor->LastMeasurementTime();
73 
74  this->gpsMessage.header.stamp.sec = currentTime.sec;
75  this->gpsMessage.header.stamp.nsec = currentTime.nsec;
76 
77  // Copy the output of Gazebo's GPS sensor into a NavSatFix message
78  this->gpsMessage.latitude = -this->gazeboGPSSensor->Latitude().Degree();
79  this->gpsMessage.longitude = -this->gazeboGPSSensor->Longitude().Degree();
80  this->gpsMessage.altitude = this->gazeboGPSSensor->Altitude();
81 
83 
84  return true;
85 }
86 
88 GZ_REGISTER_SENSOR_PLUGIN(GPSROSPlugin)
89 }
GPSROSPlugin()
Class constructor.
Definition: GPSROSPlugin.cc:21
virtual void Load(sensors::SensorPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
void publish(const boost::shared_ptr< M > &message) const
bool GetSDFParam(sdf::ElementPtr sdf, const std::string &name, T &param, const T &default_value, const bool &verbose=false)
Obtains a parameter from sdf.
Definition: Common.hh:47
event::ConnectionPtr updateConnection
Pointer to the update event connection.
bool OnUpdateGPS()
Update GPS ROS message.
Definition: GPSROSPlugin.cc:68
virtual ~GPSROSPlugin()
Class destructor.
Definition: GPSROSPlugin.cc:25
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
ros::Publisher rosSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
sensors::GpsSensorPtr gazeboGPSSensor
Pointer to the parent sensor.
Definition: GPSROSPlugin.hh:43
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load module and read parameters from SDF.
Definition: GPSROSPlugin.cc:29
std::string robotNamespace
Robot namespace.
sensor_msgs::NavSatFix gpsMessage
Output GPS ROS message.
Definition: GPSROSPlugin.hh:46
void PublishState()
Publish the current state of the plugin.


uuv_sensor_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Mon Jul 1 2019 19:39:20