diff --git a/.gitignore b/.gitignore index 25867e4f..5e9f4b02 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ build # Version control generated files *.orig +.vscode \ No newline at end of file diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh new file mode 100644 index 00000000..be685f3b --- /dev/null +++ b/include/ignition/sensors/GpsSensor.hh @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_SENSORS_GPS_HH_ +#define IGNITION_SENSORS_GPS_HH_ + +#include + +#include + +#include +#include + +#include +#include + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class GpsPrivate; + + /// \brief Gps Sensor Class + /// + /// An gps sensor that reports vertical position and velocity + /// readings over ign transport + class IGNITION_SENSORS_GPS_VISIBLE GpsSensor : public Sensor + { + /// \brief constructor + public: GpsSensor(); + + /// \brief destructor + public: virtual ~GpsSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + + + /// \brief Set the latitude of the GPS + /// \param[in] _latitude Latitude of GPS in degrees + public: void SetLatitude(double _latitude); + + /// \brief Get the latitude of the GPS, wraped between +/- 180 + /// \return Latitude in degrees. + public: double Latitude() const; + + + /// \brief Set the longitude of the GPS + /// \param[in] _longitude Longitude of GPS in degrees + public: void SetLongitude(double _longitude); + + /// \brief Get the longitude of the GPS, wraped between +/- 180 + /// \return Longitude in degrees. + public: double Longitude() const; + + + /// \brief Set the altitude of the GPS + /// \param[in] _altitude altitude of GPS in meters + public: void SetAltitude(double _altitude); + + /// \brief GPS altitude is the GEOMETRIC altitude above sea level + /// \return Altitude in meters + public: double Altitude() const; + + + /// \brief Set the velocity of the GPS + /// \param[in] _vel vector3 of GPS in meters per second. + public: void SetVelocity(ignition::math::Vector3d &_vel); + + /// \brief Get the velocity of the GPS sensor + /// \return velocity vector3 in meters per second + public: ignition::math::Vector3d Velocity() const; + + + /// \brief Easy short hand for setting the lat/long of the gps. + /// \param[in] _latitude in degrees + /// \param[in] _longitude in degrees + public: void SetPosition(double _latitude, double _longitude, + double _altitude = 0.0); + + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 479e671b..2736eb70 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,6 +149,22 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, + /// \brief Noise streams for the Gps position sensor + /// \sa Gps + GPS_HORIZONTAL_POSITION_NOISE = 15, + + /// \brief Noise streams for the Gps position sensor + /// \sa Gps + GPS_VERTICAL_POSITION_NOISE = 16, + + /// \brief Noise streams for the Gps velocity sensor + /// \sa Gps + GPS_HORIZONTAL_VELOCITY_NOISE = 17, + + /// \brief Noise streams for the Gps velocity sensor + /// \sa Gps + GPS_VERTICAL_VELOCITY_NOISE = 18, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 97a38024..77306c6f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -114,6 +114,9 @@ ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) +set(gps_sources GpsSensor.cc) +ign_add_component(gps SOURCES ${gps_sources} GET_TARGET_NAME gps_target) + set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) diff --git a/src/GpsSensor.cc b/src/GpsSensor.cc new file mode 100644 index 00000000..8220b4b6 --- /dev/null +++ b/src/GpsSensor.cc @@ -0,0 +1,275 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include "ignition/sensors/GpsSensor.hh" +#include "ignition/sensors/Noise.hh" +#include "ignition/sensors/SensorFactory.hh" +#include "ignition/sensors/SensorTypes.hh" + +using namespace ignition; +using namespace sensors; + +/// \brief Private data for Gps +class ignition::sensors::GpsPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish gps messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief latitude angle + public: ignition::math::Angle latitude; + + /// \brief longitude angle + public: ignition::math::Angle longitude; + + /// \brief altitude + public: double altitude = 0.0; + + /// \brief velocity + public: ignition::math::Vector3d velocity; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +GpsSensor::GpsSensor() + : dataPtr(new GpsPrivate()) +{ +} + +////////////////////////////////////////////////// +GpsSensor::~GpsSensor() +{ +} + +////////////////////////////////////////////////// +bool GpsSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool GpsSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::GPS) + { + ignerr << "Attempting to a load an GPS sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.GpsSensor() == nullptr) + { + ignerr << "Attempting to a load an GPS sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/gps"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise(this->Topic()); + + if (!this->dataPtr->pub) + { + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + // Load the noise parameters + + if (_sdf.GpsSensor()->HorizontalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->HorizontalPositionNoise()); + } + if (_sdf.GpsSensor()->VerticalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_VERTICAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->VerticalPositionNoise()); + } + if (_sdf.GpsSensor()->HorizontalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_HORIZONTAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->HorizontalVelocityNoise()); + } + if (_sdf.GpsSensor()->VerticalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_VERTICAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->VerticalVelocityNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool GpsSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool GpsSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool GpsSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("GpsSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::GPS msg; + + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + + // Apply gps horizontal position noise + if (this->dataPtr->noises.find(GPS_HORIZONTAL_POSITION_NOISE) != + this->dataPtr->noises.end()) + { + this->SetLatitude( + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE]->Apply( + this->Latitude())); + + this->SetLongitude( + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE]->Apply( + this->Longitude())); + } + + if (this->dataPtr->noises.find(GPS_VERTICAL_POSITION_NOISE) != + this->dataPtr->noises.end()) + { + this->SetAltitude( + this->dataPtr->noises[GPS_VERTICAL_POSITION_NOISE]->Apply( + this->Altitude())); + } + + // taken from ImuSensor.cc - convenience method + auto applyNoise = [&](SensorNoiseType noiseType, double &value) + { + if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()){ + value = this->dataPtr->noises[noiseType]->Apply(value); + } + }; + + applyNoise(GPS_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.X()); + applyNoise(GPS_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.Y()); + applyNoise(GPS_VERTICAL_VELOCITY_NOISE, this->dataPtr->velocity.Z()); + + // normalise so that it is within +/- 180 + this->dataPtr->latitude.Normalize(); + this->dataPtr->longitude.Normalize(); + + + msg.set_latitude_deg(this->dataPtr->latitude.Degree()); + msg.set_longitude_deg(this->dataPtr->longitude.Degree()); + msg.set_altitude(this->dataPtr->altitude); + msg.set_velocity_east(this->dataPtr->velocity.X()); + msg.set_velocity_north(this->dataPtr->velocity.X()); + msg.set_velocity_up(this->dataPtr->velocity.X()); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +void GpsSensor::SetLatitude(double _latitude) +{ + this->dataPtr->latitude.Degree(_latitude); +} + +double GpsSensor::Latitude() const +{ + return this->dataPtr->latitude.Degree(); +} + +void GpsSensor::SetAltitude(double _altitude) +{ + this->dataPtr->altitude = _altitude; +} + +double GpsSensor::Altitude() const +{ + return this->dataPtr->altitude; +} + +void GpsSensor::SetLongitude(double _longitude) +{ + this->dataPtr->longitude.Degree(_longitude); +} + +double GpsSensor::Longitude() const +{ + return this->dataPtr->longitude.Degree(); +} + +void GpsSensor::SetVelocity(ignition::math::Vector3d &_vel) +{ + this->dataPtr->velocity = _vel; +} + +ignition::math::Vector3d GpsSensor::Velocity() const +{ + return this->dataPtr->velocity; +} + +void GpsSensor::SetPosition(double _latitude, double _longitude, + double _altitude) +{ + this->SetLongitude(_longitude); + this->SetLatitude(_latitude); + this->SetAltitude(_altitude); +} + +IGN_SENSORS_REGISTER_SENSOR(GpsSensor)