From 4015d876d227c98612a1ccde11e88d6178931ab3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 4 Jan 2022 16:36:36 -0800 Subject: [PATCH] Add NavSat (GPS) sensor (#177) Signed-off-by: Dre Westcook Signed-off-by: Louise Poubel Signed-off-by: Ashton Larkin Co-authored-by: Dre Westcook Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- CMakeLists.txt | 2 +- include/ignition/sensors/NavSatSensor.hh | 128 +++++++++ include/ignition/sensors/SensorTypes.hh | 16 ++ src/CMakeLists.txt | 3 + src/NavSatSensor.cc | 262 ++++++++++++++++++ test/integration/CMakeLists.txt | 2 + test/integration/navsat.cc | 339 +++++++++++++++++++++++ 7 files changed, 751 insertions(+), 1 deletion(-) create mode 100644 include/ignition/sensors/NavSatSensor.hh create mode 100644 src/NavSatSensor.cc create mode 100644 test/integration/navsat.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e1e9c1b..e31c56fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ endif() #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs8 REQUIRED) +ign_find_package(ignition-msgs8 REQUIRED VERSION 8.2) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh new file mode 100644 index 00000000..0c6dfc9a --- /dev/null +++ b/include/ignition/sensors/NavSatSensor.hh @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2021 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_NAVSAT_HH_ +#define IGNITION_SENSORS_NAVSAT_HH_ + +#include + +#include +#include + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/navsat/Export.hh" + +#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 NavSatPrivate; + + /// \brief NavSat Sensor Class + /// + /// A sensor that reports position and velocity readings over + /// Ignition Transport using spherical coordinates (latitude / longitude). + /// + /// By default, it publishes `ignition::msgs::NavSat` messages on the + /// `/.../navsat` topic. + /// + /// This sensor assumes the world is using the East-North-Up (ENU) frame. + class IGNITION_SENSORS_NAVSAT_VISIBLE NavSatSensor : public Sensor + { + /// \brief Constructor + public: NavSatSensor(); + + /// \brief Destructor + public: virtual ~NavSatSensor(); + + /// \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 Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Set the latitude of the NavSat + /// \param[in] _latitude Latitude of NavSat + public: void SetLatitude(const math::Angle &_latitude); + + /// \brief Get the latitude of the NavSat, wrapped between +/- 180 + /// degrees. + /// \return Latitude angle. + public: const math::Angle &Latitude() const; + + /// \brief Set the longitude of the NavSat + /// \param[in] _longitude Longitude of NavSat + public: void SetLongitude(const math::Angle &_longitude); + + /// \brief Get the longitude of the NavSat, wrapped between +/- 180 + /// degrees. + /// \return Longitude angle. + public: const math::Angle &Longitude() const; + + /// \brief Set the altitude of the NavSat + /// \param[in] _altitude altitude of NavSat in meters + public: void SetAltitude(double _altitude); + + /// \brief Get NavSat altitude above sea level + /// \return Altitude in meters + public: double Altitude() const; + + /// \brief Set the velocity of the NavSat in ENU world frame. + /// \param[in] _vel NavSat in meters per second. + public: void SetVelocity(const math::Vector3d &_vel); + + /// \brief Get the velocity of the NavSat sensor in the ENU world frame. + /// \return Velocity in meters per second + public: const math::Vector3d &Velocity() const; + + /// \brief Easy short hand for setting the position of the sensor. + /// \param[in] _latitude Latitude angle. + /// \param[in] _longitude Longitude angle. + /// \param[in] _altitude Altitude in meters; defaults to zero. + public: void SetPosition(const math::Angle &_latitude, + const math::Angle &_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 875ccb07..63345045 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -173,6 +173,22 @@ namespace ignition /// \sa ForceTorqueSensor TORQUE_Z_NOISE_N_M = 20, + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_POSITION_NOISE = 21, + + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_VERTICAL_POSITION_NOISE = 22, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_VELOCITY_NOISE = 23, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_VERTICAL_VELOCITY_NOISE = 24, + /// \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 e6408671..53c2782c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -120,6 +120,9 @@ ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimet set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) +set(navsat_sources NavSatSensor.cc) +ign_add_component(navsat SOURCES ${navsat_sources} GET_TARGET_NAME navsat_target) + set(rgbd_camera_sources RgbdCameraSensor.cc) ign_add_component(rgbd_camera SOURCES ${rgbd_camera_sources} GET_TARGET_NAME rgbd_camera_target) target_compile_definitions(${rgbd_camera_target} PUBLIC RgbdCameraSensor_EXPORTS) diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc new file mode 100644 index 00000000..d9bf65d0 --- /dev/null +++ b/src/NavSatSensor.cc @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2021 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/NavSatSensor.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 NavSat +class ignition::sensors::NavSatPrivate +{ + /// \brief Node to create publisher + public: transport::Node node; + + /// \brief To publish NavSat messages. + public: transport::Node::Publisher pub; + + /// \brief True if Load() has been called and was successful + public: bool loaded = false; + + /// \brief Latitude angle + public: math::Angle latitude; + + /// \brief Longitude angle + public: math::Angle longitude; + + /// \brief Altitude + public: double altitude = 0.0; + + /// \brief Velocity in ENU frame. + public: math::Vector3d velocity; + + /// \brief Noise added to sensor data + public: std::unordered_map noises; +}; + +////////////////////////////////////////////////// +NavSatSensor::NavSatSensor() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +NavSatSensor::~NavSatSensor() = default; + +////////////////////////////////////////////////// +bool NavSatSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool NavSatSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::NAVSAT) + { + ignerr << "Attempting to a load an NAVSAT sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.NavSatSensor() == nullptr) + { + ignerr << "Attempting to a load an NAVSAT sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/navsat"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise(this->Topic()); + + if (!this->dataPtr->pub) + { + ignerr << "Unable to create publisher on topic [" << this->Topic() + << "]." << std::endl; + return false; + } + + // Load the noise parameters + if (_sdf.NavSatSensor()->HorizontalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_HORIZONTAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->HorizontalPositionNoise()); + } + if (_sdf.NavSatSensor()->VerticalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_VERTICAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->VerticalPositionNoise()); + } + if (_sdf.NavSatSensor()->HorizontalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_HORIZONTAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->HorizontalVelocityNoise()); + } + if (_sdf.NavSatSensor()->VerticalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_VERTICAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->VerticalVelocityNoise()); + } + + this->dataPtr->loaded = true; + return true; +} + +////////////////////////////////////////////////// +bool NavSatSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("NavSatSensor::Update"); + if (!this->dataPtr->loaded) + { + ignerr << "Not loaded, update ignored.\n"; + return false; + } + + msgs::NavSat msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + msg.set_frame_id(this->Name()); + + // Apply noise + auto iter = this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE); + if (iter != this->dataPtr->noises.end()) + { + this->SetLatitude(IGN_DTOR(iter->second->Apply(this->Latitude().Degree()))); + this->SetLongitude(IGN_DTOR(iter->second->Apply( + this->Longitude().Degree()))); + } + iter = this->dataPtr->noises.find(NAVSAT_VERTICAL_POSITION_NOISE); + if (iter != this->dataPtr->noises.end()) + { + this->SetAltitude(iter->second->Apply(this->Altitude())); + } + iter = this->dataPtr->noises.find(NAVSAT_HORIZONTAL_VELOCITY_NOISE); + if (iter != this->dataPtr->noises.end()) + { + this->dataPtr->velocity.X(iter->second->Apply(this->dataPtr->velocity.X())); + this->dataPtr->velocity.Y(iter->second->Apply(this->dataPtr->velocity.Y())); + } + iter = this->dataPtr->noises.find(NAVSAT_VERTICAL_VELOCITY_NOISE); + if (iter != this->dataPtr->noises.end()) + { + this->dataPtr->velocity.Z(iter->second->Apply(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.Y()); + msg.set_velocity_up(this->dataPtr->velocity.Z()); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetLatitude(const math::Angle &_latitude) +{ + this->dataPtr->latitude = _latitude; +} + +////////////////////////////////////////////////// +const math::Angle &NavSatSensor::Latitude() const +{ + return this->dataPtr->latitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetAltitude(double _altitude) +{ + this->dataPtr->altitude = _altitude; +} + +////////////////////////////////////////////////// +double NavSatSensor::Altitude() const +{ + return this->dataPtr->altitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetLongitude(const math::Angle &_longitude) +{ + this->dataPtr->longitude = _longitude; +} + +////////////////////////////////////////////////// +const math::Angle &NavSatSensor::Longitude() const +{ + return this->dataPtr->longitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetVelocity(const math::Vector3d &_vel) +{ + this->dataPtr->velocity = _vel; +} + +////////////////////////////////////////////////// +const math::Vector3d &NavSatSensor::Velocity() const +{ + return this->dataPtr->velocity; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetPosition(const math::Angle &_latitude, + const math::Angle &_longitude, double _altitude) +{ + this->SetLatitude(_latitude); + this->SetLongitude(_longitude); + this->SetAltitude(_altitude); +} + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 31e8acee..cfaeccc9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(tests force_torque.cc logical_camera.cc magnetometer.cc + navsat.cc imu.cc ) @@ -55,5 +56,6 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu ${PROJECT_LIBRARY_TARGET_NAME}-force_torque + ${PROJECT_LIBRARY_TARGET_NAME}-navsat ) diff --git a/test/integration/navsat.cc b/test/integration/navsat.cc new file mode 100644 index 00000000..46261bc4 --- /dev/null +++ b/test/integration/navsat.cc @@ -0,0 +1,339 @@ +/* + * Copyright (C) 2021 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 + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; +using namespace sensors; +using namespace std::chrono_literals; + +/// \brief Helper function to create a navsat sdf element +sdf::ElementPtr NavSatToSdf(const std::string &_name, + const math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Helper function to create a navsat sdf element with noise +sdf::ElementPtr NavSatToSdfWithNoise(const std::string &_name, + const math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize, double _mean, double _stddev, double _bias) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Test navsat sensor +class NavSatSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, CreateNavSat) +{ + // Create SDF describing a navsat sensor + const std::string name = "TestNavSat"; + const std::string topic = "/ignition/sensors/test/navsat"; + const std::string topicNoise = "/ignition/sensors/test/navsat_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + math::Pose3d sensorPose(math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity); + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize); + + auto navsatSdfNoise = NavSatToSdfWithNoise(name, sensorPose, updateRate, + topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + SensorFactory sf; + auto sensor = sf.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); + + auto sensorNoise = sf.CreateSensor(navsatSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + EXPECT_EQ(name, sensorNoise->Name()); + EXPECT_EQ(topicNoise, sensorNoise->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, SensorReadings) +{ + // Create SDF describing a navsat sensor + const std::string name{"TestNavSat"}; + const std::string topic{"/ignition/sensors/test/navsat"}; + const std::string topicNoise{"/ignition/sensors/test/navsat_noise"}; + const double updateRate{30.0}; + const bool alwaysOn{true}; + const bool visualize{true}; + + // Create sensor SDF + math::Pose3d sensorPose(math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity); + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsatSdfNoise = NavSatToSdfWithNoise(name, sensorPose, updateRate, + topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + SensorFactory sf; + auto sensor = sf.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, sensor); + + auto sensorNoise = sf.CreateSensor(navsatSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + // verify initial readings + EXPECT_DOUBLE_EQ(0.0, sensor->Latitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensor->Longitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensor->Altitude()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().X()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Y()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Z()); + + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Latitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Longitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Altitude()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().X()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().Y()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().Z()); + + // set state and verify readings + math::Angle lat{IGN_DTOR(20)}; + sensor->SetLatitude(lat); + sensorNoise->SetLatitude(lat); + EXPECT_EQ(lat, sensor->Latitude()); + EXPECT_EQ(lat, sensorNoise->Latitude()); + + math::Angle lon{IGN_DTOR(-20)}; + sensor->SetLongitude(lon); + sensorNoise->SetLongitude(lon); + EXPECT_EQ(lon, sensor->Longitude()); + EXPECT_EQ(lon, sensorNoise->Longitude()); + + double altitude{20.0}; + sensor->SetAltitude(altitude); + sensorNoise->SetAltitude(altitude); + EXPECT_DOUBLE_EQ(altitude, sensor->Altitude()); + EXPECT_DOUBLE_EQ(altitude, sensorNoise->Altitude()); + + lat += IGN_DTOR(20); + lon += IGN_DTOR(20); + altitude += 100; + sensor->SetPosition(lat, lon, altitude); + sensorNoise->SetPosition(lat, lon, altitude); + EXPECT_EQ(lat, sensor->Latitude()); + EXPECT_EQ(lat, sensorNoise->Latitude()); + EXPECT_EQ(lon, sensor->Longitude()); + EXPECT_EQ(lon, sensorNoise->Longitude()); + EXPECT_DOUBLE_EQ(altitude, sensor->Altitude()); + EXPECT_DOUBLE_EQ(altitude, sensorNoise->Altitude()); + + math::Vector3d velocity{1.0, 2.0, 3.0}; + sensor->SetVelocity(velocity); + sensorNoise->SetVelocity(velocity); + EXPECT_EQ(velocity, sensor->Velocity()); + EXPECT_EQ(velocity, sensorNoise->Velocity()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + sensor->Update(std::chrono::steady_clock::duration(1s)); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + EXPECT_DOUBLE_EQ(lat.Degree(), msg.latitude_deg()); + EXPECT_DOUBLE_EQ(lon.Degree(), msg.longitude_deg()); + EXPECT_DOUBLE_EQ(altitude, msg.altitude()); + EXPECT_DOUBLE_EQ(velocity.X(), msg.velocity_east()); + EXPECT_DOUBLE_EQ(velocity.Y(), msg.velocity_north()); + EXPECT_DOUBLE_EQ(velocity.Z(), msg.velocity_up()); + + // verify msg with noise received on the topic + WaitForMessageTestHelper msgHelperNoise(topicNoise); + sensorNoise->Update(std::chrono::steady_clock::duration(1s)); + EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; + auto msgNoise = msgHelperNoise.Message(); + EXPECT_EQ(1, msgNoise.header().stamp().sec()); + EXPECT_EQ(0, msgNoise.header().stamp().nsec()); + EXPECT_FALSE(math::equal(lat.Degree(), msgNoise.latitude_deg())); + EXPECT_FALSE(math::equal(lon.Degree(), msgNoise.longitude_deg())); + EXPECT_FALSE(math::equal(altitude, msgNoise.altitude())); + EXPECT_FALSE(math::equal(velocity.X(), msgNoise.velocity_east())); + EXPECT_FALSE(math::equal(velocity.Y(), msgNoise.velocity_north())); + EXPECT_FALSE(math::equal(velocity.Z(), msgNoise.velocity_up())); +} + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, Topic) +{ + const std::string name{"TestNavSat"}; + const double updateRate{30.0}; + const bool alwaysOn{true}; + const bool visualize{true}; + auto sensorPose = math::Pose3d(); + + // Factory + SensorFactory factory; + + // Default topic + { + const std::string topic; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsat = factory.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, navsat); + + EXPECT_EQ("/navsat", navsat->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsat = factory.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, navsat); + + EXPECT_EQ("/topic_with_spaces/characters", navsat->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto sensor = factory.CreateSensor< + NavSatSensor>(navsatSdf); + ASSERT_EQ(nullptr, sensor); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}