From 989cd28896fb3786f1c7947eff10d09c7ef388bf Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 12 Jan 2022 10:36:44 -0800 Subject: [PATCH] Add NavSat sensor (GPS) (#1248) Signed-off-by: Louise Poubel --- CMakeLists.txt | 7 +- examples/worlds/spherical_coordinates.sdf | 237 +++++++++++++++- include/ignition/gazebo/components/NavSat.hh | 46 +++ src/Conversions.cc | 44 ++- src/SdfEntityCreator.cc | 11 + src/systems/CMakeLists.txt | 1 + src/systems/navsat/CMakeLists.txt | 9 + src/systems/navsat/NavSat.cc | 284 +++++++++++++++++++ src/systems/navsat/NavSat.hh | 65 +++++ src/systems/physics/Physics.cc | 4 +- test/integration/CMakeLists.txt | 1 + test/integration/navsat_system.cc | 127 +++++++++ test/worlds/navsat.sdf | 44 +++ 13 files changed, 870 insertions(+), 10 deletions(-) create mode 100644 include/ignition/gazebo/components/NavSat.hh create mode 100644 src/systems/navsat/CMakeLists.txt create mode 100644 src/systems/navsat/NavSat.cc create mode 100644 src/systems/navsat/NavSat.hh create mode 100644 test/integration/navsat_system.cc create mode 100644 test/worlds/navsat.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 29d9e21c3f..1d5e598b94 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) #-------------------------------------- # 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}) #-------------------------------------- @@ -87,7 +87,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools7_VERSION_MAJOR}) #-------------------------------------- # Find ignition-gui -ign_find_package(ignition-gui6 REQUIRED VERSION 6.1) +ign_find_package(ignition-gui6 REQUIRED VERSION 6.3) set(IGN_GUI_VER ${ignition-gui6_VERSION_MAJOR}) ign_find_package (Qt5 COMPONENTS @@ -110,7 +110,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 +ign_find_package(ignition-sensors6 REQUIRED VERSION 6.1 # component order is important COMPONENTS # non-rendering @@ -120,6 +120,7 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 force_torque logical_camera magnetometer + navsat # rendering rendering diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf index b8195168fb..58652d2757 100644 --- a/examples/worlds/spherical_coordinates.sdf +++ b/examples/worlds/spherical_coordinates.sdf @@ -5,6 +5,9 @@ The world origin's spherical coordinates can be inspected using the Component Inspector GUI plugin. + It has a model with a NavSat (GPS) sensor, whose reading is displayed on a + NavSatMap plugin. + Try changing the world origin coordinates with: ign service \ @@ -46,7 +49,13 @@ ign service -s /world/spherical_coordinates/set_spherical_coordinates \ --> - + + + + + EARTH_WGS84 ENU - -22.9 - -43.2 + -22.986687 + -43.202501 0 0 + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 0 -6 8 0 0.8 1.56 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 250 + 50 + floating + false + #777777 + + + + false + + + + + + docked_collapsed + + + + + + + docked + + + + + + + NavSat Map + docked + + /navsat + true + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.05 0 0.0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1 + 1 + navsat + + + + diff --git a/include/ignition/gazebo/components/NavSat.hh b/include/ignition/gazebo/components/NavSat.hh new file mode 100644 index 0000000000..ae3358b9b4 --- /dev/null +++ b/include/ignition/gazebo/components/NavSat.hh @@ -0,0 +1,46 @@ +/* + * 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_GAZEBO_COMPONENTS_NAVSAT_HH_ +#define IGNITION_GAZEBO_COMPONENTS_NAVSAT_HH_ + +#include + +#include +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains an NavSat sensor, + /// sdf::NavSat, information. + using NavSat = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.NavSat", NavSat) +} +} +} +} +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index d7d308d27b..bdc67fd9cb 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -15,19 +15,20 @@ * */ +#include #include #include #include #include #include -#include #include +#include #include +#include #include #include #include #include -#include #include #include #include @@ -60,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -1064,6 +1066,44 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) << "sensor pointer is null.\n"; } } + else if (_in.Type() == sdf::SensorType::GPS || + _in.Type() == sdf::SensorType::NAVSAT) + { + if (_in.NavSatSensor()) + { + auto sdfSensor = _in.NavSatSensor(); + + // \TODO(chapulina) Update to navsat on Garden + auto sensor = out.mutable_gps(); + + if (sdfSensor->HorizontalPositionNoise().Type() != sdf::NoiseType::NONE) + { + gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(), + sdfSensor->HorizontalPositionNoise()); + } + if (sdfSensor->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) + { + gazebo::set(sensor->mutable_position()->mutable_vertical_noise(), + sdfSensor->VerticalPositionNoise()); + + } + if (sdfSensor->HorizontalVelocityNoise().Type() != sdf::NoiseType::NONE) + { + gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(), + sdfSensor->HorizontalVelocityNoise()); + } + if (sdfSensor->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) + { + gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), + sdfSensor->VerticalVelocityNoise()); + } + } + else + { + ignerr << "Attempting to convert a NavSat SDF sensor, but the " + << "sensor pointer is null.\n"; + } + } else if (_in.Type() == sdf::SensorType::ALTIMETER) { if (_in.AltimeterSensor()) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 17c8155765..ae98f41ec7 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -57,6 +57,7 @@ #include "ignition/gazebo/components/Material.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/NavSat.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include @@ -876,6 +877,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldLinearVelocity(math::Vector3d::Zero)); } + else if (_sensor->Type() == sdf::SensorType::GPS || + _sensor->Type() == sdf::SensorType::NAVSAT) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::NavSat(*_sensor)); + + // Create components to be filled by physics. + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldLinearVelocity(math::Vector3d::Zero)); + } else if (_sensor->Type() == sdf::SensorType::IMU) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c00277aadf..17514029f8 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -118,6 +118,7 @@ add_subdirectory(magnetometer) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) +add_subdirectory(navsat) add_subdirectory(odometry_publisher) add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) diff --git a/src/systems/navsat/CMakeLists.txt b/src/systems/navsat/CMakeLists.txt new file mode 100644 index 0000000000..247d38ccf1 --- /dev/null +++ b/src/systems/navsat/CMakeLists.txt @@ -0,0 +1,9 @@ +gz_add_system(navsat + SOURCES + NavSat.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + PRIVATE_LINK_LIBS + ignition-sensors${IGN_SENSORS_VER}::navsat +) + diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc new file mode 100644 index 0000000000..faa8f4f38f --- /dev/null +++ b/src/systems/navsat/NavSat.cc @@ -0,0 +1,284 @@ +/* + * 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 "NavSat.hh" + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/NavSat.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private NavSat data class. +class ignition::gazebo::systems::NavSat::Implementation +{ + /// \brief A map of NavSat entity to its sensor + public: std::unordered_map> entitySensorMap; + + /// \brief Ign-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + + /// When the system is just loaded, we loop over all entities to create + /// sensors. After this initialization, we only check inserted entities. + public: bool initialized = false; + + /// \brief Create sensors in ign-sensors + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); + + /// \brief Update sensor data based on physics data + /// \param[in] _ecm Immutable reference to ECM. + public: void Update(const EntityComponentManager &_ecm); + + /// \brief Remove sensors if their entities have been removed from simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveSensors(const EntityComponentManager &_ecm); + + /// \brief Create sensor + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _entity Sensor entity + /// \param[in] _navsat NavSat component. + /// \param[in] _parent Parent entity component. + public: void AddSensor( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::NavSat *_navSat, + const components::ParentEntity *_parent); +}; + +////////////////////////////////////////////////// +NavSat::NavSat() : System(), dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void NavSat::PreUpdate(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::PreUpdate"); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); +} + +////////////////////////////////////////////////// +void NavSat::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::PostUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + this->dataPtr->CreateSensors(_ecm); + + // Only update and publish if not paused. + if (!_info.paused) + { + this->dataPtr->Update(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + it.second.get()->sensors::Sensor::Update(_info.simTime, false); + } + } + + this->dataPtr->RemoveSensors(_ecm); +} + +////////////////////////////////////////////////// +void NavSat::Implementation::AddSensor( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::NavSat *_navsat, + const components::ParentEntity *_parent) +{ + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _navsat->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/navsat"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); +} + +////////////////////////////////////////////////// +void NavSat::Implementation::CreateSensors(const EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::CreateSensors"); + if (!this->initialized) + { + _ecm.Each( + [&](const Entity &_entity, + const components::NavSat *_navSat, + const components::ParentEntity *_parent)->bool + { + this->AddSensor(_ecm, _entity, _navSat, _parent); + return true; + }); + this->initialized = true; + } + else + { + _ecm.EachNew( + [&](const Entity &_entity, + const components::NavSat *_navSat, + const components::ParentEntity *_parent)->bool + { + this->AddSensor(_ecm, _entity, _navSat, _parent); + return true; + }); + } +} + +////////////////////////////////////////////////// +void NavSat::Implementation::Update(const EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::Update"); + + _ecm.Each( + [&](const Entity &_entity, + const components::NavSat * /*_navsat*/, + const components::WorldLinearVelocity *_worldLinearVel + )->bool + { + auto it = this->entitySensorMap.find(_entity); + + if (it == this->entitySensorMap.end()) + { + ignerr << "Failed to update NavSat sensor entity [" << _entity + << "]. Entity not found." << std::endl; + return true; + } + + // Position + auto latLonEle = sphericalCoordinates(_entity, _ecm); + if (!latLonEle) + { + ignwarn << "Failed to update NavSat sensor enity [" << _entity + << "]. Spherical coordinates not set." << std::endl; + return true; + } + + it->second->SetLatitude(IGN_DTOR(latLonEle.value().X())); + it->second->SetLongitude(IGN_DTOR(latLonEle.value().Y())); + it->second->SetAltitude(latLonEle.value().Z()); + + // Velocity in ENU frame + it->second->SetVelocity(_worldLinearVel->Data()); + + return true; + }); +} + +////////////////////////////////////////////////// +void NavSat::Implementation::RemoveSensors(const EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::RemoveSensors"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::NavSat *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + ignerr << "Internal error, missing NavSat sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +IGNITION_ADD_PLUGIN(NavSat, System, + NavSat::ISystemPreUpdate, + NavSat::ISystemPostUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS(NavSat, "ignition::gazebo::systems::NavSat") diff --git a/src/systems/navsat/NavSat.hh b/src/systems/navsat/NavSat.hh new file mode 100644 index 0000000000..e4ce4f34d6 --- /dev/null +++ b/src/systems/navsat/NavSat.hh @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2019 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_GAZEBO_SYSTEMS_NAVSAT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_NAVSAT_HH_ + +#include + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \class NavSat NavSat.hh ignition/gazebo/systems/NavSat.hh + /// \brief System that handles navigation satellite sensors, such as GPS, + /// that reports position and velocity in spherical coordinates (latitude / + /// longitude) over Ignition Transport. + /// + /// The NavSat sensors rely on the world origin's spherical coordinates + /// being set, for example through SDF's `` tag + /// or the `/world/world_name/set_spherical_coordinates` service. + class NavSat: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: explicit NavSat(); + + // Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} +#endif diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index ec96ed1bf0..18df36ba58 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2869,8 +2869,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, IGN_PROFILE_END(); // pose/velocity/acceleration of non-link entities such as sensors / - // collisions. These get updated only if another system has created a - // components::WorldPose component for the entity. + // collisions. These get updated only if another system has created + // the component for the entity. // Populated components: // * WorldPose // * WorldLinearVelocity diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index e09452bbad..6b712785a9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -38,6 +38,7 @@ set(tests model.cc multicopter.cc multiple_servers.cc + navsat_system.cc nested_model_physics.cc network_handshake.cc odometry_publisher.cc diff --git a/test/integration/navsat_system.cc b/test/integration/navsat_system.cc new file mode 100644 index 0000000000..f3b6208172 --- /dev/null +++ b/test/integration/navsat_system.cc @@ -0,0 +1,127 @@ +/* + * 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 + +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/NavSat.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test NavSatTest system +class NavSatTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +std::vector navSatMsgs; + +///////////////////////////////////////////////// +void navsatCb(const msgs::NavSat &_msg) +{ + mutex.lock(); + navSatMsgs.push_back(_msg); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the world pose and sensor readings of a falling navsat +TEST_F(NavSatTest, ModelFalling) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "navsat.sdf")); + + auto topic = "world/navsat_sensor/" + "model/navsat_model/link/link/sensor/navsat_sensor/navsat"; + + bool checkedComponents{false}; + fixture.OnPostUpdate( + [&]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, + const components::Sensor *, + const components::NavSat *, + const components::Name *_name, + const components::SensorTopic *_topic) -> bool + { + EXPECT_EQ(_name->Data(), "navsat_sensor"); + EXPECT_EQ(topic, _topic->Data()); + + checkedComponents = true; + return true; + }); + }).Finalize(); + + // subscribe to navsat topic + transport::Node node; + node.Subscribe(topic, &navsatCb); + + // Run server + fixture.Server()->Run(true, 300u, false); + EXPECT_TRUE(checkedComponents); + + // Wait for messages to be received + int sleep{0}; + int maxSleep{30}; + for (; navSatMsgs.size() < 10 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_LE(sleep, maxSleep); + EXPECT_LE(10u, navSatMsgs.size()); + + mutex.lock(); + auto lastMsg = navSatMsgs.back(); + mutex.unlock(); + + EXPECT_EQ("navsat_model::link::navsat_sensor", lastMsg.frame_id()); + + // Located at world origin + EXPECT_DOUBLE_EQ(-22.9, lastMsg.latitude_deg()); + EXPECT_DOUBLE_EQ(-43.2, lastMsg.longitude_deg()); + + // Not moving horizontally + EXPECT_DOUBLE_EQ(0.0, lastMsg.velocity_east()); + EXPECT_DOUBLE_EQ(0.0, lastMsg.velocity_north()); + + // Falling due to gravity + EXPECT_GT(0.0, lastMsg.altitude()); + EXPECT_GT(0.0, lastMsg.velocity_up()); +} diff --git a/test/worlds/navsat.sdf b/test/worlds/navsat.sdf new file mode 100644 index 0000000000..46d984a18e --- /dev/null +++ b/test/worlds/navsat.sdf @@ -0,0 +1,44 @@ + + + + + 0.02 + 0 + + + + + + + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + + + + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + 1 + 30 + + + + + +