From db96ccdb620461bfb14b6493dc85df8124bcbccb Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Tue, 29 Dec 2020 19:15:49 +1100 Subject: [PATCH 01/15] start implement gps Signed-off-by: Dre Westcook --- .gitignore | 1 + CMakeLists.txt | 1 + examples/worlds/sensors.sdf | 14 +- include/ignition/gazebo/components/Gps.hh | 46 +++++ src/Conversions.cc | 26 +++ src/SdfEntityCreator.cc | 10 + src/systems/CMakeLists.txt | 1 + src/systems/gps/CMakeLists.txt | 9 + src/systems/gps/Gps.cc | 230 ++++++++++++++++++++++ src/systems/gps/Gps.hh | 66 +++++++ test/integration/CMakeLists.txt | 1 + test/integration/gps_system.cc | 209 ++++++++++++++++++++ test/worlds/gps.sdf | 78 ++++++++ 13 files changed, 691 insertions(+), 1 deletion(-) create mode 100644 include/ignition/gazebo/components/Gps.hh create mode 100644 src/systems/gps/CMakeLists.txt create mode 100644 src/systems/gps/Gps.cc create mode 100644 src/systems/gps/Gps.hh create mode 100644 test/integration/gps_system.cc create mode 100644 test/worlds/gps.sdf diff --git a/.gitignore b/.gitignore index 51c7dd858d..6133f58ce9 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,4 @@ build_* .ycm_extra_conf.py *.orig +.vscode diff --git a/CMakeLists.txt b/CMakeLists.txt index 29d9e21c3f..7add2495c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,6 +118,7 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 altimeter imu force_torque + gps logical_camera magnetometer diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index f6653ed506..678c5739d4 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -27,6 +27,10 @@ filename="ignition-gazebo-altimeter-system" name="ignition::gazebo::systems::Altimeter"> + + @@ -153,6 +157,13 @@ + + 1 + 100 + true + gps + + 1 100 @@ -160,6 +171,7 @@ imu true + 1 100 @@ -178,7 +190,7 @@ 0.0 0.1 - + 0.0 diff --git a/include/ignition/gazebo/components/Gps.hh b/include/ignition/gazebo/components/Gps.hh new file mode 100644 index 0000000000..f9b10a854a --- /dev/null +++ b/include/ignition/gazebo/components/Gps.hh @@ -0,0 +1,46 @@ +/* + * 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_COMPONENTS_GPS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_GPS_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 GPS sensor, + /// sdf::GPS, information. + using Gps = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Gps", Gps) +} +} +} +} +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index d7d308d27b..c9b4931e71 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -1064,6 +1065,31 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) << "sensor pointer is null.\n"; } } + else if (_in.Type() == sdf::SensorType::GPS) + { + if (_in.GpsSensor()) + { + /* + msgs::GPSSensor *sensor = out.mutable_gps(); + + if (_in.GpsSensor()->PositionNoise().Type() != sdf::NoiseType::NONE) + { + ignition::gazebo::set(sensor->mutable_position_noise(), + _in.GpsSensor()->PositionNoise()); + } + if (_in.GpsSensor()->VelocityNoise().Type() != sdf::NoiseType::NONE) + { + ignition::gazebo::set(sensor->mutable_velocity_noise(), + _in.GpsSensor()->VelocityNoise()); + }*/ //TODO + igndbg << "Attempting to convert a GPS SDF sensor but not sure what converting is\n"; + } + else + { + ignerr << "Attempting to convert a GPS 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..d2f5d6414d 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -40,6 +40,7 @@ #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Imu.hh" +#include "ignition/gazebo/components/Gps.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" @@ -876,6 +877,15 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldLinearVelocity(math::Vector3d::Zero)); } + else if (_sensor->Type() == sdf::SensorType::GPS) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::Gps(*_sensor)); + + // create components to be filled by physics + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldPose(math::Pose3d::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..e712b500bb 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -103,6 +103,7 @@ add_subdirectory(follow_actor) add_subdirectory(force_torque) add_subdirectory(hydrodynamics) add_subdirectory(imu) +add_subdirectory(gps) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) diff --git a/src/systems/gps/CMakeLists.txt b/src/systems/gps/CMakeLists.txt new file mode 100644 index 0000000000..a3f242c7b6 --- /dev/null +++ b/src/systems/gps/CMakeLists.txt @@ -0,0 +1,9 @@ +gz_add_system(gps + SOURCES + Gps.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + PRIVATE_LINK_LIBS + ignition-sensors${IGN_SENSORS_VER}::gps +) + diff --git a/src/systems/gps/Gps.cc b/src/systems/gps/Gps.cc new file mode 100644 index 0000000000..35eaf1d012 --- /dev/null +++ b/src/systems/gps/Gps.cc @@ -0,0 +1,230 @@ +/* + * 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. + * + */ + +#include "Gps.hh" + +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/Gps.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private Gps data class. +class ignition::gazebo::systems::GpsPrivate +{ + /// \brief A map of gps entity to its vertical reference + public: std::unordered_map> entitySensorMap; + + /// \brief Ign-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Create gps sensor + /// \param[in] _ecm Mutable reference to ECM. + public: void CreateGpsEntities(EntityComponentManager &_ecm); + + /// \brief Update gps sensor data based on physics data + /// \param[in] _ecm Immutable reference to ECM. + public: void UpdateGps(const EntityComponentManager &_ecm); + + /// \brief Remove gps sensors if their entities have been removed from + /// simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveGpsEntities(const EntityComponentManager &_ecm); +}; + +////////////////////////////////////////////////// +Gps::Gps() : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +Gps::~Gps() = default; + +////////////////////////////////////////////////// +void Gps::PreUpdate(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("Gps::PreUpdate"); + this->dataPtr->CreateGpsEntities(_ecm); +} + +////////////////////////////////////////////////// +void Gps::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("Gps::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; + } + + // Only update and publish if not paused. + if (!_info.paused) + { + this->dataPtr->UpdateGps(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + // Update measurement time + auto time = math::durationToSecNsec(_info.simTime); + dynamic_cast(it.second.get())->Update( + math::secNsecToDuration(time.first, time.second), false); + } + } + + this->dataPtr->RemoveGpsEntities(_ecm); +} + +////////////////////////////////////////////////// +void GpsPrivate::CreateGpsEntities(EntityComponentManager &_ecm) +{ + IGN_PROFILE("Gps::CreateGpsEntities"); + // Create gps + _ecm.EachNew( + [&](const Entity &_entity, + const components::Gps *_gps, + const components::ParentEntity *_parent)->bool + { + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _gps->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/gps"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::GpsSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return true; + } + + igndbg << "Creating GPS Entity" << std::endl; + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + sensor->SetLatitude(0.0); + sensor->SetLongitude(0.0); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); + + return true; + }); +} + +////////////////////////////////////////////////// +void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) +{ + IGN_PROFILE("Gps::UpdateGps"); + _ecm.Each( + [&](const Entity &_entity, + const components::Gps * /*_gps*/, + const components::WorldPose *_worldPose)->bool + { + auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) + { + math::Vector3d linearVel; + math::Pose3d worldPose = _worldPose->Data(); + it->second->SetLatitude(worldPose.Pos().X()); + it->second->SetLongitude(worldPose.Pos().Y()); + //it->second->SetPosition(worldPose.Pos().Z()); + //it->second->SetVerticalVelocity(_worldLinearVel->Data().Z()); + } + else + { + ignerr << "Failed to update gps: " << _entity << ". " + << "Entity not found." << std::endl; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void GpsPrivate::RemoveGpsEntities( + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("Gps::RemoveGpsEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::Gps *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + ignerr << "Internal error, missing gps sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +IGNITION_ADD_PLUGIN(Gps, System, + Gps::ISystemPreUpdate, + Gps::ISystemPostUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS(Gps, "ignition::gazebo::systems::Gps") diff --git a/src/systems/gps/Gps.hh b/src/systems/gps/Gps.hh new file mode 100644 index 0000000000..31550ab7ed --- /dev/null +++ b/src/systems/gps/Gps.hh @@ -0,0 +1,66 @@ +/* + * 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_GPS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_GPS_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class GpsPrivate; + + /// \class Gps Gps.hh ignition/gazebo/systems/Gps.hh + /// \brief An gps sensor that reports vertical position and velocity + /// readings over ign transport + class IGNITION_GAZEBO_VISIBLE Gps: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: explicit Gps(); + + /// \brief Destructor + public: ~Gps() override; + + /// 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. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index e09452bbad..19977afec6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -4,6 +4,7 @@ set(tests ackermann_steering_system.cc air_pressure_system.cc altimeter_system.cc + gps_system.cc apply_joint_force_system.cc battery_plugin.cc breadcrumbs.cc diff --git a/test/integration/gps_system.cc b/test/integration/gps_system.cc new file mode 100644 index 0000000000..a165587698 --- /dev/null +++ b/test/integration/gps_system.cc @@ -0,0 +1,209 @@ +/* + * 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. + * + */ + +#include + +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Gps.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.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/test_config.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test GpsTest system +class GpsTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + +std::mutex mutex; +std::vector gpsMsgs; + +///////////////////////////////////////////////// +void gpsCb(const msgs::GPS &_msg) +{ + mutex.lock(); + gpsMsgs.push_back(_msg); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the world pose and sensor readings of a falling gps +TEST_F(GpsTest, ModelFalling) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/gps.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "gps_sensor"; + + auto topic = "world/gps_sensor/" + "model/gps_model/link/link/sensor/gps_sensor/gps"; + + // Create a system that records gps data + test::Relay testSystem; + std::vector poses; + std::vector velocities; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Gps *, + const components::Name *_name, + const components::WorldPose *_worldPose) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + poses.push_back(_worldPose->Data()); + //velocities.push_back(_worldLinearVel->Data()); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // subscribe to gps topic + transport::Node node; + node.Subscribe(topic, &gpsCb); + + // Run server + size_t iters100 = 100u; + server.Run(true, iters100, false); + EXPECT_EQ(iters100, poses.size()); + + // Wait for messages to be received + double updateRate = 30; + double stepSize = 0.001; + size_t waitForMsgs = poses.size() * stepSize * updateRate + 1; + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + mutex.lock(); + bool received = gpsMsgs.size() == waitForMsgs; + mutex.unlock(); + + if (received) + break; + } + + mutex.lock(); + EXPECT_EQ(gpsMsgs.size(), waitForMsgs); + auto firstMsg = gpsMsgs.front(); + auto lastMsg = gpsMsgs.back(); + mutex.unlock(); + + // check gps world pose + // verify the gps model z pos is decreasing + EXPECT_GT(poses.front().Pos().Z(), poses.back().Pos().Z()); + EXPECT_GT(poses.back().Pos().Z(), 0.0); + // velocity should be negative in z + //EXPECT_GT(velocities.front().Z(), velocities.back().Z()); + //EXPECT_LT(velocities.back().Z(), 0.0); + + // check gps sensor data + + /* + // vertical position = world position - intial position + // so since gps is falling, vertical position should be negative + EXPECT_GT(firstMsg.vertical_position(), + lastMsg.vertical_position()); + EXPECT_LT(lastMsg.vertical_position(), 0.0); + // vertical velocity should be negative + EXPECT_GT(firstMsg.vertical_velocity(), + lastMsg.vertical_velocity()); + EXPECT_LT(lastMsg.vertical_velocity(), 0.0); + */ + // Run server for longer period of time so the gps falls then rests + // on the ground plane + size_t iters1000 = 1000u; + server.Run(true, iters1000, false); + EXPECT_EQ(iters100 + iters1000, poses.size()); + + // Wait for messages to be received + waitForMsgs = poses.size() * stepSize * updateRate + 1; + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + mutex.lock(); + bool received = gpsMsgs.size() == waitForMsgs; + mutex.unlock(); + + if (received) + break; + } + + mutex.lock(); + EXPECT_EQ(gpsMsgs.size(), waitForMsgs); + lastMsg = gpsMsgs.back(); + mutex.unlock(); + + // check gps world pose + // gps should be on the ground + // note that gps's parent link is at an 0.05 offset + // set high tol due to instablity + EXPECT_NEAR(poses.back().Pos().Z(), 0.05, 1e-2); + // velocity should now be zero as the model is resting on the ground + //EXPECT_NEAR(velocities.back().Z(), 0u, 1e-3); + + // check gps sensor data + // gps vertical position = 0.05 (world pos) - 3.05 (initial position) + //EXPECT_LT(lastMsg.vertical_position(), -3.0); + // velocity should be zero + //EXPECT_NEAR(lastMsg.vertical_velocity(), 0u, 1e-3); +} diff --git a/test/worlds/gps.sdf b/test/worlds/gps.sdf new file mode 100644 index 0000000000..e8ae3d45fa --- /dev/null +++ b/test/worlds/gps.sdf @@ -0,0 +1,78 @@ + + + + + 0.001 + 1.0 + + + + + + + + true + + + + + 0 0 1 + + + + + + + 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 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + + + + + From a25a90605cbcdaa52009913da852ad0fbb85b318 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 09:59:32 +1100 Subject: [PATCH 02/15] gps-modifications Signed-off-by: Dre Westcook --- src/systems/gps/Gps.cc | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/systems/gps/Gps.cc b/src/systems/gps/Gps.cc index 35eaf1d012..b663921deb 100644 --- a/src/systems/gps/Gps.cc +++ b/src/systems/gps/Gps.cc @@ -182,10 +182,21 @@ void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) auto it = this->entitySensorMap.find(_entity); if (it != this->entitySensorMap.end()) { - math::Vector3d linearVel; + math::Vector3d worldCoords; math::Pose3d worldPose = _worldPose->Data(); - it->second->SetLatitude(worldPose.Pos().X()); - it->second->SetLongitude(worldPose.Pos().Y()); + math::SphericalCoordinates sphericalCoords; + + //TODO apply position noise before converting to global frame + + worldCoords = sphericalCoords.SphericalFromLocalPosition( + worldPose.Pos() + ); + + it->second->SetLatitude(worldCoords.X()); + it->second->SetLongitude(worldCoords.Y()); + //TODO set altitude + //TODO set velocity + //it->second->SetPosition(worldPose.Pos().Z()); //it->second->SetVerticalVelocity(_worldLinearVel->Data().Z()); } From 369b3deb408f4c87eb9c26ee59a8094a7265285e Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 18:34:38 +1100 Subject: [PATCH 03/15] finish up Signed-off-by: Dre Westcook --- examples/worlds/sensors.sdf | 2 +- src/Conversions.cc | 14 +++++++++----- src/SdfEntityCreator.cc | 3 +++ src/systems/gps/Gps.cc | 34 ++++++++++++++++------------------ 4 files changed, 29 insertions(+), 24 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 678c5739d4..23882c28cc 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -159,7 +159,7 @@ 1 - 100 + 1 true gps diff --git a/src/Conversions.cc b/src/Conversions.cc index c9b4931e71..c7ed441b1f 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -1069,20 +1070,23 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) { if (_in.GpsSensor()) { - /* msgs::GPSSensor *sensor = out.mutable_gps(); if (_in.GpsSensor()->PositionNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_position_noise(), + ignition::gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(), _in.GpsSensor()->PositionNoise()); + ignition::gazebo::set(sensor->mutable_position()->mutable_vertical_noise(), + _in.GpsSensor()->PositionNoise()); + } if (_in.GpsSensor()->VelocityNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_velocity_noise(), + ignition::gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(), _in.GpsSensor()->VelocityNoise()); - }*/ //TODO - igndbg << "Attempting to convert a GPS SDF sensor but not sure what converting is\n"; + ignition::gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), + _in.GpsSensor()->VelocityNoise()); + } } else { diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d2f5d6414d..b620aaf8b2 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -885,6 +885,9 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) // create components to be filled by physics this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldPose(math::Pose3d::Zero)); + + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldLinearVelocity(math::Vector3d::Zero)); } else if (_sensor->Type() == sdf::SensorType::IMU) { diff --git a/src/systems/gps/Gps.cc b/src/systems/gps/Gps.cc index b663921deb..f0f89b1d56 100644 --- a/src/systems/gps/Gps.cc +++ b/src/systems/gps/Gps.cc @@ -150,22 +150,19 @@ void GpsPrivate::CreateGpsEntities(EntityComponentManager &_ecm) return true; } - igndbg << "Creating GPS Entity" << std::endl; // set sensor parent std::string parentName = _ecm.Component( _parent->Data())->Data(); sensor->SetParent(parentName); - - sensor->SetLatitude(0.0); - sensor->SetLongitude(0.0); - // Set topic _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + igndbg << "Creating GPS Entity" << std::endl; + return true; }); } @@ -174,31 +171,31 @@ void GpsPrivate::CreateGpsEntities(EntityComponentManager &_ecm) void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) { IGN_PROFILE("Gps::UpdateGps"); - _ecm.Each( + + _ecm.Each( [&](const Entity &_entity, const components::Gps * /*_gps*/, - const components::WorldPose *_worldPose)->bool + const components::WorldPose *_worldPose, + const components::WorldLinearVelocity *_worldLinearVel + )->bool { auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) { math::Vector3d worldCoords; - math::Pose3d worldPose = _worldPose->Data(); math::SphericalCoordinates sphericalCoords; + math::Vector3d worldvel = _worldLinearVel->Data(); - //TODO apply position noise before converting to global frame - - worldCoords = sphericalCoords.SphericalFromLocalPosition( - worldPose.Pos() - ); + worldCoords = sphericalCoords.PositionTransform(_worldPose->Data().Pos(), + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::SPHERICAL); it->second->SetLatitude(worldCoords.X()); it->second->SetLongitude(worldCoords.Y()); - //TODO set altitude - //TODO set velocity - - //it->second->SetPosition(worldPose.Pos().Z()); - //it->second->SetVerticalVelocity(_worldLinearVel->Data().Z()); + it->second->SetAltitude(worldCoords.Z()); + it->second->SetVelocity(worldvel); } else { @@ -219,6 +216,7 @@ void GpsPrivate::RemoveGpsEntities( [&](const Entity &_entity, const components::Gps *)->bool { + igndbg << "Removing GPS entity" << std::endl; auto sensorId = this->entitySensorMap.find(_entity); if (sensorId == this->entitySensorMap.end()) { From a34551872b9e94df1fa82771c65574195d9685be Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 13 Dec 2021 16:09:28 -0800 Subject: [PATCH 04/15] GPS -> NavSat Signed-off-by: Louise Poubel --- CMakeLists.txt | 1 + examples/worlds/sensors.sdf | 13 +-- .../gazebo/components/{Gps.hh => NavSat.hh} | 14 +-- src/Conversions.cc | 32 ++++-- src/SdfEntityCreator.cc | 7 +- src/systems/CMakeLists.txt | 2 +- src/systems/{gps => navsat}/CMakeLists.txt | 6 +- src/systems/{gps/Gps.cc => navsat/NavSat.cc} | 102 +++++++++--------- src/systems/{gps/Gps.hh => navsat/NavSat.hh} | 27 ++--- test/integration/CMakeLists.txt | 2 +- .../{gps_system.cc => navsat_system.cc} | 70 ++++++------ test/worlds/{gps.sdf => navsat.sdf} | 10 +- 12 files changed, 150 insertions(+), 136 deletions(-) rename include/ignition/gazebo/components/{Gps.hh => NavSat.hh} (73%) rename src/systems/{gps => navsat}/CMakeLists.txt (60%) rename src/systems/{gps/Gps.cc => navsat/NavSat.cc} (67%) rename src/systems/{gps/Gps.hh => navsat/NavSat.hh} (68%) rename test/integration/{gps_system.cc => navsat_system.cc} (76%) rename test/worlds/{gps.sdf => navsat.sdf} (89%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7add2495c1..4f754cf440 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,7 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 gps logical_camera magnetometer + navsat # rendering rendering diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 23882c28cc..2760e0ca34 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -8,6 +8,7 @@ Imu: ign topic -e -t /imu Magnetometer: ign topic -e -t /magnetometer Air pressure: ign topic -e -t /air_pressure + NavSat: ign topic -e -t /navsat --> @@ -27,10 +28,6 @@ filename="ignition-gazebo-altimeter-system" name="ignition::gazebo::systems::Altimeter"> - - @@ -43,6 +40,10 @@ filename="ignition-gazebo-forcetorque-system" name="ignition::gazebo::systems::ForceTorque"> + + @@ -157,11 +158,11 @@ - + 1 1 true - gps + navsat diff --git a/include/ignition/gazebo/components/Gps.hh b/include/ignition/gazebo/components/NavSat.hh similarity index 73% rename from include/ignition/gazebo/components/Gps.hh rename to include/ignition/gazebo/components/NavSat.hh index f9b10a854a..ae3358b9b4 100644 --- a/include/ignition/gazebo/components/Gps.hh +++ b/include/ignition/gazebo/components/NavSat.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_COMPONENTS_GPS_HH_ -#define IGNITION_GAZEBO_COMPONENTS_GPS_HH_ +#ifndef IGNITION_GAZEBO_COMPONENTS_NAVSAT_HH_ +#define IGNITION_GAZEBO_COMPONENTS_NAVSAT_HH_ #include @@ -34,11 +34,11 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component type that contains an GPS sensor, - /// sdf::GPS, information. - using Gps = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Gps", Gps) + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.NavSat", NavSat) } } } diff --git a/src/Conversions.cc b/src/Conversions.cc index c7ed441b1f..33a4eb1cff 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -56,12 +56,12 @@ #include #include #include -#include #include #include #include #include #include +#include #include #include #include @@ -1066,31 +1066,41 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) << "sensor pointer is null.\n"; } } - else if (_in.Type() == sdf::SensorType::GPS) + else if (_in.Type() == sdf::SensorType::GPS || + _in.Type() == sdf::SensorType::NAVSAT) { - if (_in.GpsSensor()) + if (_in.NavSatSensor()) { - msgs::GPSSensor *sensor = out.mutable_gps(); + auto sdfSensor = _in.NavSatSensor(); - if (_in.GpsSensor()->PositionNoise().Type() != sdf::NoiseType::NONE) + // \TODO(chapulina) Update to navsat on Garden + auto sensor = out.mutable_gps(); + + if (sdfSensor->HorizontalPositionNoise().Type() != sdf::NoiseType::NONE) { ignition::gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(), - _in.GpsSensor()->PositionNoise()); + sdfSensor->HorizontalPositionNoise()); + } + if (sdfSensor->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) + { ignition::gazebo::set(sensor->mutable_position()->mutable_vertical_noise(), - _in.GpsSensor()->PositionNoise()); + sdfSensor->VerticalPositionNoise()); } - if (_in.GpsSensor()->VelocityNoise().Type() != sdf::NoiseType::NONE) + if (sdfSensor->HorizontalVelocityNoise().Type() != sdf::NoiseType::NONE) { ignition::gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(), - _in.GpsSensor()->VelocityNoise()); + sdfSensor->HorizontalVelocityNoise()); + } + if (sdfSensor->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) + { ignition::gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), - _in.GpsSensor()->VelocityNoise()); + sdfSensor->VerticalVelocityNoise()); } } else { - ignerr << "Attempting to convert a GPS SDF sensor, but the " + ignerr << "Attempting to convert a NavSat SDF sensor, but the " << "sensor pointer is null.\n"; } } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index b620aaf8b2..8b6e7ffddf 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -40,7 +40,7 @@ #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Imu.hh" -#include "ignition/gazebo/components/Gps.hh" +#include "ignition/gazebo/components/NavSat.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" @@ -877,10 +877,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldLinearVelocity(math::Vector3d::Zero)); } - else if (_sensor->Type() == sdf::SensorType::GPS) + else if (_sensor->Type() == sdf::SensorType::GPS || + _sensor->Type() == sdf::SensorType::NAVSAT) { this->dataPtr->ecm->CreateComponent(sensorEntity, - components::Gps(*_sensor)); + components::NavSat(*_sensor)); // create components to be filled by physics this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index e712b500bb..17514029f8 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -103,7 +103,6 @@ add_subdirectory(follow_actor) add_subdirectory(force_torque) add_subdirectory(hydrodynamics) add_subdirectory(imu) -add_subdirectory(gps) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) @@ -119,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/gps/CMakeLists.txt b/src/systems/navsat/CMakeLists.txt similarity index 60% rename from src/systems/gps/CMakeLists.txt rename to src/systems/navsat/CMakeLists.txt index a3f242c7b6..247d38ccf1 100644 --- a/src/systems/gps/CMakeLists.txt +++ b/src/systems/navsat/CMakeLists.txt @@ -1,9 +1,9 @@ -gz_add_system(gps +gz_add_system(navsat SOURCES - Gps.cc + NavSat.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} PRIVATE_LINK_LIBS - ignition-sensors${IGN_SENSORS_VER}::gps + ignition-sensors${IGN_SENSORS_VER}::navsat ) diff --git a/src/systems/gps/Gps.cc b/src/systems/navsat/NavSat.cc similarity index 67% rename from src/systems/gps/Gps.cc rename to src/systems/navsat/NavSat.cc index f0f89b1d56..57a4bc3883 100644 --- a/src/systems/gps/Gps.cc +++ b/src/systems/navsat/NavSat.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -15,9 +15,9 @@ * */ -#include "Gps.hh" +#include "NavSat.hh" -#include +#include #include #include @@ -32,9 +32,9 @@ #include #include -#include +#include -#include "ignition/gazebo/components/Gps.hh" +#include "ignition/gazebo/components/NavSat.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" @@ -48,51 +48,51 @@ using namespace ignition; using namespace gazebo; using namespace systems; -/// \brief Private Gps data class. -class ignition::gazebo::systems::GpsPrivate +/// \brief Private NavSat data class. +class ignition::gazebo::systems::NavSatPrivate { - /// \brief A map of gps entity to its vertical reference + /// \brief A map of NavSat entity to its vertical reference public: std::unordered_map> entitySensorMap; + std::unique_ptr> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; - /// \brief Create gps sensor + /// \brief Create NavSat sensor /// \param[in] _ecm Mutable reference to ECM. - public: void CreateGpsEntities(EntityComponentManager &_ecm); + public: void CreateNavSatEntities(EntityComponentManager &_ecm); - /// \brief Update gps sensor data based on physics data + /// \brief Update NavSat sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. - public: void UpdateGps(const EntityComponentManager &_ecm); + public: void UpdateNavSat(const EntityComponentManager &_ecm); - /// \brief Remove gps sensors if their entities have been removed from + /// \brief Remove NavSat sensors if their entities have been removed from /// simulation. /// \param[in] _ecm Immutable reference to ECM. - public: void RemoveGpsEntities(const EntityComponentManager &_ecm); + public: void RemoveNavSatEntities(const EntityComponentManager &_ecm); }; ////////////////////////////////////////////////// -Gps::Gps() : System(), dataPtr(std::make_unique()) +NavSat::NavSat() : System(), dataPtr(std::make_unique()) { } ////////////////////////////////////////////////// -Gps::~Gps() = default; +NavSat::~NavSat() = default; ////////////////////////////////////////////////// -void Gps::PreUpdate(const UpdateInfo &/*_info*/, +void NavSat::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { - IGN_PROFILE("Gps::PreUpdate"); - this->dataPtr->CreateGpsEntities(_ecm); + IGN_PROFILE("NavSat::PreUpdate"); + this->dataPtr->CreateNavSatEntities(_ecm); } ////////////////////////////////////////////////// -void Gps::PostUpdate(const UpdateInfo &_info, +void NavSat::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { - IGN_PROFILE("Gps::PostUpdate"); + IGN_PROFILE("NavSat::PostUpdate"); // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) @@ -105,7 +105,7 @@ void Gps::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { - this->dataPtr->UpdateGps(_ecm); + this->dataPtr->UpdateNavSat(_ecm); for (auto &it : this->dataPtr->entitySensorMap) { @@ -116,33 +116,33 @@ void Gps::PostUpdate(const UpdateInfo &_info, } } - this->dataPtr->RemoveGpsEntities(_ecm); + this->dataPtr->RemoveNavSatEntities(_ecm); } ////////////////////////////////////////////////// -void GpsPrivate::CreateGpsEntities(EntityComponentManager &_ecm) +void NavSatPrivate::CreateNavSatEntities(EntityComponentManager &_ecm) { - IGN_PROFILE("Gps::CreateGpsEntities"); - // Create gps - _ecm.EachNew( + IGN_PROFILE("NavSat::CreateNavSatEntities"); + // Create NavSat + _ecm.EachNew( [&](const Entity &_entity, - const components::Gps *_gps, + const components::NavSat *_navsat, const components::ParentEntity *_parent)->bool { // create sensor std::string sensorScopedName = removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _gps->Data(); + sdf::Sensor data = _navsat->Data(); data.SetName(sensorScopedName); // check topic if (data.Topic().empty()) { - std::string topic = scopedName(_entity, _ecm) + "/gps"; + std::string topic = scopedName(_entity, _ecm) + "/navsat"; data.SetTopic(topic); } - std::unique_ptr sensor = + std::unique_ptr sensor = this->sensorFactory.CreateSensor< - sensors::GpsSensor>(data); + sensors::NavSatSensor>(data); if (nullptr == sensor) { ignerr << "Failed to create sensor [" << sensorScopedName << "]" @@ -161,34 +161,34 @@ void GpsPrivate::CreateGpsEntities(EntityComponentManager &_ecm) this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); - igndbg << "Creating GPS Entity" << std::endl; + igndbg << "Creating NavSat Entity" << std::endl; return true; }); } ////////////////////////////////////////////////// -void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) +void NavSatPrivate::UpdateNavSat(const EntityComponentManager &_ecm) { - IGN_PROFILE("Gps::UpdateGps"); + IGN_PROFILE("NavSat::UpdateNavSat"); - _ecm.Each( [&](const Entity &_entity, - const components::Gps * /*_gps*/, + const components::NavSat * /*_navsat*/, const components::WorldPose *_worldPose, const components::WorldLinearVelocity *_worldLinearVel )->bool { auto it = this->entitySensorMap.find(_entity); - + if (it != this->entitySensorMap.end()) { math::Vector3d worldCoords; math::SphericalCoordinates sphericalCoords; math::Vector3d worldvel = _worldLinearVel->Data(); - worldCoords = sphericalCoords.PositionTransform(_worldPose->Data().Pos(), + worldCoords = sphericalCoords.PositionTransform(_worldPose->Data().Pos(), math::SphericalCoordinates::GLOBAL, math::SphericalCoordinates::SPHERICAL); @@ -199,7 +199,7 @@ void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) } else { - ignerr << "Failed to update gps: " << _entity << ". " + ignerr << "Failed to update NavSat: " << _entity << ". " << "Entity not found." << std::endl; } @@ -208,19 +208,19 @@ void GpsPrivate::UpdateGps(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void GpsPrivate::RemoveGpsEntities( +void NavSatPrivate::RemoveNavSatEntities( const EntityComponentManager &_ecm) { - IGN_PROFILE("Gps::RemoveGpsEntities"); - _ecm.EachRemoved( + IGN_PROFILE("NavSat::RemoveNavSatEntities"); + _ecm.EachRemoved( [&](const Entity &_entity, - const components::Gps *)->bool + const components::NavSat *)->bool { - igndbg << "Removing GPS entity" << std::endl; + igndbg << "Removing NavSat entity" << std::endl; auto sensorId = this->entitySensorMap.find(_entity); if (sensorId == this->entitySensorMap.end()) { - ignerr << "Internal error, missing gps sensor for entity [" + ignerr << "Internal error, missing NavSat sensor for entity [" << _entity << "]" << std::endl; return true; } @@ -231,9 +231,9 @@ void GpsPrivate::RemoveGpsEntities( }); } -IGNITION_ADD_PLUGIN(Gps, System, - Gps::ISystemPreUpdate, - Gps::ISystemPostUpdate +IGNITION_ADD_PLUGIN(NavSat, System, + NavSat::ISystemPreUpdate, + NavSat::ISystemPostUpdate ) -IGNITION_ADD_PLUGIN_ALIAS(Gps, "ignition::gazebo::systems::Gps") +IGNITION_ADD_PLUGIN_ALIAS(NavSat, "ignition::gazebo::systems::NavSat") diff --git a/src/systems/gps/Gps.hh b/src/systems/navsat/NavSat.hh similarity index 68% rename from src/systems/gps/Gps.hh rename to src/systems/navsat/NavSat.hh index 31550ab7ed..ae054e0a55 100644 --- a/src/systems/gps/Gps.hh +++ b/src/systems/navsat/NavSat.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_GPS_HH_ -#define IGNITION_GAZEBO_SYSTEMS_GPS_HH_ +#ifndef IGNITION_GAZEBO_SYSTEMS_NAVSAT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_NAVSAT_HH_ #include #include @@ -31,33 +31,34 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Forward declarations. - class GpsPrivate; + class NavSatPrivate; - /// \class Gps Gps.hh ignition/gazebo/systems/Gps.hh - /// \brief An gps sensor that reports vertical position and velocity - /// readings over ign transport - class IGNITION_GAZEBO_VISIBLE Gps: + /// \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. + class IGNITION_GAZEBO_VISIBLE NavSat: public System, public ISystemPreUpdate, public ISystemPostUpdate { /// \brief Constructor - public: explicit Gps(); + public: explicit NavSat(); - /// \brief Destructor - public: ~Gps() override; + // \brief Destructor + public: ~NavSat() override; - /// Documentation inherited + // Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; - /// Documentation inherited + // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; }; } } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 19977afec6..6b712785a9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -4,7 +4,6 @@ set(tests ackermann_steering_system.cc air_pressure_system.cc altimeter_system.cc - gps_system.cc apply_joint_force_system.cc battery_plugin.cc breadcrumbs.cc @@ -39,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/gps_system.cc b/test/integration/navsat_system.cc similarity index 76% rename from test/integration/gps_system.cc rename to test/integration/navsat_system.cc index a165587698..6c810c2d38 100644 --- a/test/integration/gps_system.cc +++ b/test/integration/navsat_system.cc @@ -17,14 +17,14 @@ #include -#include +#include #include #include #include #include -#include "ignition/gazebo/components/Gps.hh" +#include "ignition/gazebo/components/NavSat.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -39,8 +39,8 @@ using namespace ignition; using namespace gazebo; -/// \brief Test GpsTest system -class GpsTest : public ::testing::Test +/// \brief Test NavSatTest system +class NavSatTest : public ::testing::Test { // Documentation inherited protected: void SetUp() override @@ -52,46 +52,46 @@ class GpsTest : public ::testing::Test }; std::mutex mutex; -std::vector gpsMsgs; +std::vector navSatMsgs; ///////////////////////////////////////////////// -void gpsCb(const msgs::GPS &_msg) +void navsatCb(const msgs::NavSat &_msg) { mutex.lock(); - gpsMsgs.push_back(_msg); + navSatMsgs.push_back(_msg); mutex.unlock(); } ///////////////////////////////////////////////// -// The test checks the world pose and sensor readings of a falling gps -TEST_F(GpsTest, ModelFalling) +// The test checks the world pose and sensor readings of a falling navsat +TEST_F(NavSatTest, ModelFalling) { // Start server ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/gps.sdf"; + "/test/worlds/navsat.sdf"; serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - const std::string sensorName = "gps_sensor"; + const std::string sensorName = "navsat_sensor"; - auto topic = "world/gps_sensor/" - "model/gps_model/link/link/sensor/gps_sensor/gps"; + auto topic = "world/navsat_sensor/" + "model/navsat_model/link/link/sensor/navsat_sensor/navsat"; - // Create a system that records gps data + // Create a system that records navsat data test::Relay testSystem; std::vector poses; std::vector velocities; testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { - _ecm.Each( [&](const ignition::gazebo::Entity &_entity, - const components::Gps *, + const components::NavSat *, const components::Name *_name, const components::WorldPose *_worldPose) -> bool { @@ -116,9 +116,9 @@ TEST_F(GpsTest, ModelFalling) server.AddSystem(testSystem.systemPtr); - // subscribe to gps topic + // subscribe to navsat topic transport::Node node; - node.Subscribe(topic, &gpsCb); + node.Subscribe(topic, &navsatCb); // Run server size_t iters100 = 100u; @@ -134,7 +134,7 @@ TEST_F(GpsTest, ModelFalling) std::this_thread::sleep_for(std::chrono::milliseconds(100)); mutex.lock(); - bool received = gpsMsgs.size() == waitForMsgs; + bool received = navsatMsgs.size() == waitForMsgs; mutex.unlock(); if (received) @@ -142,24 +142,24 @@ TEST_F(GpsTest, ModelFalling) } mutex.lock(); - EXPECT_EQ(gpsMsgs.size(), waitForMsgs); - auto firstMsg = gpsMsgs.front(); - auto lastMsg = gpsMsgs.back(); + EXPECT_EQ(navsatMsgs.size(), waitForMsgs); + auto firstMsg = navsatMsgs.front(); + auto lastMsg = navsatMsgs.back(); mutex.unlock(); - // check gps world pose - // verify the gps model z pos is decreasing + // check navsat world pose + // verify the navsat model z pos is decreasing EXPECT_GT(poses.front().Pos().Z(), poses.back().Pos().Z()); EXPECT_GT(poses.back().Pos().Z(), 0.0); // velocity should be negative in z //EXPECT_GT(velocities.front().Z(), velocities.back().Z()); //EXPECT_LT(velocities.back().Z(), 0.0); - // check gps sensor data + // check navsat sensor data /* // vertical position = world position - intial position - // so since gps is falling, vertical position should be negative + // so since navsat is falling, vertical position should be negative EXPECT_GT(firstMsg.vertical_position(), lastMsg.vertical_position()); EXPECT_LT(lastMsg.vertical_position(), 0.0); @@ -168,7 +168,7 @@ TEST_F(GpsTest, ModelFalling) lastMsg.vertical_velocity()); EXPECT_LT(lastMsg.vertical_velocity(), 0.0); */ - // Run server for longer period of time so the gps falls then rests + // Run server for longer period of time so the navsat falls then rests // on the ground plane size_t iters1000 = 1000u; server.Run(true, iters1000, false); @@ -181,7 +181,7 @@ TEST_F(GpsTest, ModelFalling) std::this_thread::sleep_for(std::chrono::milliseconds(100)); mutex.lock(); - bool received = gpsMsgs.size() == waitForMsgs; + bool received = navsatMsgs.size() == waitForMsgs; mutex.unlock(); if (received) @@ -189,20 +189,20 @@ TEST_F(GpsTest, ModelFalling) } mutex.lock(); - EXPECT_EQ(gpsMsgs.size(), waitForMsgs); - lastMsg = gpsMsgs.back(); + EXPECT_EQ(navsatMsgs.size(), waitForMsgs); + lastMsg = navsatMsgs.back(); mutex.unlock(); - // check gps world pose - // gps should be on the ground - // note that gps's parent link is at an 0.05 offset + // check navsat world pose + // navsat should be on the ground + // note that navsat's parent link is at an 0.05 offset // set high tol due to instablity EXPECT_NEAR(poses.back().Pos().Z(), 0.05, 1e-2); // velocity should now be zero as the model is resting on the ground //EXPECT_NEAR(velocities.back().Z(), 0u, 1e-3); - // check gps sensor data - // gps vertical position = 0.05 (world pos) - 3.05 (initial position) + // check navsat sensor data + // navsat vertical position = 0.05 (world pos) - 3.05 (initial position) //EXPECT_LT(lastMsg.vertical_position(), -3.0); // velocity should be zero //EXPECT_NEAR(lastMsg.vertical_velocity(), 0u, 1e-3); diff --git a/test/worlds/gps.sdf b/test/worlds/navsat.sdf similarity index 89% rename from test/worlds/gps.sdf rename to test/worlds/navsat.sdf index e8ae3d45fa..c4a346e55f 100644 --- a/test/worlds/gps.sdf +++ b/test/worlds/navsat.sdf @@ -1,6 +1,6 @@ - + 0.001 1.0 @@ -10,8 +10,8 @@ name="ignition::gazebo::systems::Physics"> + filename="ignition-gazebo-navsat-system" + name="ignition::gazebo::systems::NavSat"> @@ -40,7 +40,7 @@ - + 4 0 3.0 0 0.0 3.14 0.05 0.05 0.05 0 0 0 @@ -66,7 +66,7 @@ - + 1 30 true From dc6bf875aaa33b357531d017ce81fc3c06878eee Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 13:14:11 -0800 Subject: [PATCH 05/15] Make it work Signed-off-by: Louise Poubel --- CMakeLists.txt | 1 - examples/worlds/sensors.sdf | 11 +- src/Conversions.cc | 8 +- src/SdfEntityCreator.cc | 9 +- src/systems/navsat/NavSat.cc | 52 +++++---- src/systems/navsat/NavSat.hh | 15 ++- src/systems/physics/Physics.cc | 2 +- test/integration/navsat_system.cc | 172 ++++++++---------------------- test/worlds/navsat.sdf | 56 ++-------- 9 files changed, 106 insertions(+), 220 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f754cf440..553e661d29 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,7 +118,6 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 altimeter imu force_torque - gps logical_camera magnetometer navsat diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 2760e0ca34..a5477def57 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -53,6 +53,16 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + + true 0 0 10 0 0 0 @@ -161,7 +171,6 @@ 1 1 - true navsat diff --git a/src/Conversions.cc b/src/Conversions.cc index 33a4eb1cff..1e10fd2283 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1078,23 +1078,23 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) if (sdfSensor->HorizontalPositionNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(), + gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(), sdfSensor->HorizontalPositionNoise()); } if (sdfSensor->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_position()->mutable_vertical_noise(), + gazebo::set(sensor->mutable_position()->mutable_vertical_noise(), sdfSensor->VerticalPositionNoise()); } if (sdfSensor->HorizontalVelocityNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(), + gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(), sdfSensor->HorizontalVelocityNoise()); } if (sdfSensor->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), + gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), sdfSensor->VerticalVelocityNoise()); } } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 8b6e7ffddf..c9edcdb0ee 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -883,12 +883,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::NavSat(*_sensor)); - // create components to be filled by physics + // Create components to be filled by physics. + // Even though the sensor doesn't use WorldPose, that's required to fill + // WorldLinearVelocity this->dataPtr->ecm->CreateComponent(sensorEntity, - components::WorldPose(math::Pose3d::Zero)); - - this->dataPtr->ecm->CreateComponent(sensorEntity, - components::WorldLinearVelocity(math::Vector3d::Zero)); + components::WorldLinearVelocity(math::Vector3d::Zero)); } else if (_sensor->Type() == sdf::SensorType::IMU) { diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index 57a4bc3883..eca00a457d 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -49,7 +49,7 @@ using namespace gazebo; using namespace systems; /// \brief Private NavSat data class. -class ignition::gazebo::systems::NavSatPrivate +class ignition::gazebo::systems::NavSat::Implementation { /// \brief A map of NavSat entity to its vertical reference public: std::unordered_map()) +NavSat::NavSat() : System(), dataPtr(utils::MakeUniqueImpl()) { } -////////////////////////////////////////////////// -NavSat::~NavSat() = default; - ////////////////////////////////////////////////// void NavSat::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -120,7 +117,7 @@ void NavSat::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void NavSatPrivate::CreateNavSatEntities(EntityComponentManager &_ecm) +void NavSat::Implementation::CreateNavSatEntities(EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::CreateNavSatEntities"); // Create NavSat @@ -150,7 +147,6 @@ void NavSatPrivate::CreateNavSatEntities(EntityComponentManager &_ecm) return true; } - // set sensor parent std::string parentName = _ecm.Component( _parent->Data())->Data(); @@ -168,47 +164,47 @@ void NavSatPrivate::CreateNavSatEntities(EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void NavSatPrivate::UpdateNavSat(const EntityComponentManager &_ecm) +void NavSat::Implementation::UpdateNavSat(const EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::UpdateNavSat"); - _ecm.Each( + _ecm.Each( [&](const Entity &_entity, const components::NavSat * /*_navsat*/, - const components::WorldPose *_worldPose, const components::WorldLinearVelocity *_worldLinearVel )->bool { auto it = this->entitySensorMap.find(_entity); - if (it != this->entitySensorMap.end()) + if (it == this->entitySensorMap.end()) { - math::Vector3d worldCoords; - math::SphericalCoordinates sphericalCoords; - math::Vector3d worldvel = _worldLinearVel->Data(); - - worldCoords = sphericalCoords.PositionTransform(_worldPose->Data().Pos(), - math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::SPHERICAL); - - it->second->SetLatitude(worldCoords.X()); - it->second->SetLongitude(worldCoords.Y()); - it->second->SetAltitude(worldCoords.Z()); - it->second->SetVelocity(worldvel); + ignerr << "Failed to update NavSat sensor entity [" << _entity + << "]. Entity not found." << std::endl; + return true; } - else + + // Position + auto latLonEle = sphericalCoordinates(_entity, _ecm); + if (!latLonEle) { - ignerr << "Failed to update NavSat: " << _entity << ". " - << "Entity not found." << std::endl; + 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 NavSatPrivate::RemoveNavSatEntities( +void NavSat::Implementation::RemoveNavSatEntities( const EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::RemoveNavSatEntities"); diff --git a/src/systems/navsat/NavSat.hh b/src/systems/navsat/NavSat.hh index ae054e0a55..b9c45bfabf 100644 --- a/src/systems/navsat/NavSat.hh +++ b/src/systems/navsat/NavSat.hh @@ -17,7 +17,8 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_NAVSAT_HH_ #define IGNITION_GAZEBO_SYSTEMS_NAVSAT_HH_ -#include +#include + #include #include #include @@ -30,13 +31,14 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - // Forward declarations. - class NavSatPrivate; - /// \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 IGNITION_GAZEBO_VISIBLE NavSat: public System, public ISystemPreUpdate, @@ -45,9 +47,6 @@ namespace systems /// \brief Constructor public: explicit NavSat(); - // \brief Destructor - public: ~NavSat() override; - // Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; @@ -58,7 +57,7 @@ namespace systems const EntityComponentManager &_ecm) final; /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index ec96ed1bf0..0a5747e14e 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2870,7 +2870,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // 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. + // the component for the entity. // Populated components: // * WorldPose // * WorldLinearVelocity diff --git a/test/integration/navsat_system.cc b/test/integration/navsat_system.cc index 6c810c2d38..20d3021ccd 100644 --- a/test/integration/navsat_system.cc +++ b/test/integration/navsat_system.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -32,23 +32,18 @@ #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 ::testing::Test +class NavSatTest : public InternalFixture<::testing::Test> { - // Documentation inherited - protected: void SetUp() override - { - ignition::common::Console::SetVerbosity(4); - setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", - (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); - } }; std::mutex mutex; @@ -66,144 +61,67 @@ void navsatCb(const msgs::NavSat &_msg) // The test checks the world pose and sensor readings of a falling navsat TEST_F(NavSatTest, ModelFalling) { - // Start server - ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/navsat.sdf"; - serverConfig.SetSdfFile(sdfFile); - - Server server(serverConfig); - EXPECT_FALSE(server.Running()); - EXPECT_FALSE(*server.Running(0)); - - const std::string sensorName = "navsat_sensor"; + 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"; - // Create a system that records navsat data - test::Relay testSystem; - std::vector poses; - std::vector velocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const components::NavSat *, - const components::Name *_name, - const components::WorldPose *_worldPose) -> bool - { - EXPECT_EQ(_name->Data(), sensorName); - - poses.push_back(_worldPose->Data()); - //velocities.push_back(_worldLinearVel->Data()); - - auto sensorComp = _ecm.Component(_entity); - EXPECT_NE(nullptr, sensorComp); - - auto topicComp = _ecm.Component(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) - { - EXPECT_EQ(topic, topicComp->Data()); - } - - return true; - }); - }); - - server.AddSystem(testSystem.systemPtr); + 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 - size_t iters100 = 100u; - server.Run(true, iters100, false); - EXPECT_EQ(iters100, poses.size()); + fixture.Server()->Run(true, 300u, false); + EXPECT_TRUE(checkedComponents); // Wait for messages to be received - double updateRate = 30; - double stepSize = 0.001; - size_t waitForMsgs = poses.size() * stepSize * updateRate + 1; - for (int sleep = 0; sleep < 30; ++sleep) + int sleep{0}; + int maxSleep{30}; + for (; navSatMsgs.size() < 10 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - mutex.lock(); - bool received = navsatMsgs.size() == waitForMsgs; - mutex.unlock(); - - if (received) - break; } + EXPECT_LT(sleep, maxSleep); + EXPECT_LE(10u, navSatMsgs.size()); mutex.lock(); - EXPECT_EQ(navsatMsgs.size(), waitForMsgs); - auto firstMsg = navsatMsgs.front(); - auto lastMsg = navsatMsgs.back(); + auto lastMsg = navSatMsgs.back(); mutex.unlock(); - // check navsat world pose - // verify the navsat model z pos is decreasing - EXPECT_GT(poses.front().Pos().Z(), poses.back().Pos().Z()); - EXPECT_GT(poses.back().Pos().Z(), 0.0); - // velocity should be negative in z - //EXPECT_GT(velocities.front().Z(), velocities.back().Z()); - //EXPECT_LT(velocities.back().Z(), 0.0); - - // check navsat sensor data - - /* - // vertical position = world position - intial position - // so since navsat is falling, vertical position should be negative - EXPECT_GT(firstMsg.vertical_position(), - lastMsg.vertical_position()); - EXPECT_LT(lastMsg.vertical_position(), 0.0); - // vertical velocity should be negative - EXPECT_GT(firstMsg.vertical_velocity(), - lastMsg.vertical_velocity()); - EXPECT_LT(lastMsg.vertical_velocity(), 0.0); - */ - // Run server for longer period of time so the navsat falls then rests - // on the ground plane - size_t iters1000 = 1000u; - server.Run(true, iters1000, false); - EXPECT_EQ(iters100 + iters1000, poses.size()); - - // Wait for messages to be received - waitForMsgs = poses.size() * stepSize * updateRate + 1; - for (int sleep = 0; sleep < 30; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_EQ("navsat_model::link::navsat_sensor", lastMsg.frame_id()); - mutex.lock(); - bool received = navsatMsgs.size() == waitForMsgs; - mutex.unlock(); + // Located at world origin + EXPECT_DOUBLE_EQ(-22.9, lastMsg.latitude_deg()); + EXPECT_DOUBLE_EQ(-43.2, lastMsg.longitude_deg()); - if (received) - break; - } - - mutex.lock(); - EXPECT_EQ(navsatMsgs.size(), waitForMsgs); - lastMsg = navsatMsgs.back(); - mutex.unlock(); + // Not moving horizontally + EXPECT_DOUBLE_EQ(0.0, lastMsg.velocity_east()); + EXPECT_DOUBLE_EQ(0.0, lastMsg.velocity_north()); - // check navsat world pose - // navsat should be on the ground - // note that navsat's parent link is at an 0.05 offset - // set high tol due to instablity - EXPECT_NEAR(poses.back().Pos().Z(), 0.05, 1e-2); - // velocity should now be zero as the model is resting on the ground - //EXPECT_NEAR(velocities.back().Z(), 0u, 1e-3); - - // check navsat sensor data - // navsat vertical position = 0.05 (world pos) - 3.05 (initial position) - //EXPECT_LT(lastMsg.vertical_position(), -3.0); - // velocity should be zero - //EXPECT_NEAR(lastMsg.vertical_velocity(), 0u, 1e-3); + // 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 index c4a346e55f..46d984a18e 100644 --- a/test/worlds/navsat.sdf +++ b/test/worlds/navsat.sdf @@ -1,9 +1,9 @@ - - 0.001 - 1.0 + + 0.02 + 0 - - true - - - - - 0 0 1 - - - - - - - 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 - - - - + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + - 4 0 3.0 0 0.0 3.14 - 0.05 0.05 0.05 0 0 0 0.1 @@ -52,24 +33,9 @@ 0.000166667 - - - - 0.1 0.1 0.1 - - - - - - - 0.1 0.1 0.1 - - - 1 30 - true From 7fc2bbfd8f8913fcb1710d703af1ce9077719c04 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 13:26:39 -0800 Subject: [PATCH 06/15] tweaks Signed-off-by: Louise Poubel --- examples/worlds/sensors.sdf | 2 +- src/SdfEntityCreator.cc | 4 +--- src/systems/navsat/NavSat.cc | 4 ---- src/systems/physics/Physics.cc | 2 +- 4 files changed, 3 insertions(+), 9 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index a5477def57..c1148a6349 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -8,7 +8,7 @@ Imu: ign topic -e -t /imu Magnetometer: ign topic -e -t /magnetometer Air pressure: ign topic -e -t /air_pressure - NavSat: ign topic -e -t /navsat + NavSat (GPS): ign topic -e -t /navsat --> diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index c9edcdb0ee..ae98f41ec7 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -40,7 +40,6 @@ #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Imu.hh" -#include "ignition/gazebo/components/NavSat.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" @@ -58,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 @@ -884,8 +884,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) components::NavSat(*_sensor)); // Create components to be filled by physics. - // Even though the sensor doesn't use WorldPose, that's required to fill - // WorldLinearVelocity this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldLinearVelocity(math::Vector3d::Zero)); } diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index eca00a457d..a92f776529 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -156,9 +156,6 @@ void NavSat::Implementation::CreateNavSatEntities(EntityComponentManager &_ecm) this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); - - igndbg << "Creating NavSat Entity" << std::endl; - return true; }); } @@ -212,7 +209,6 @@ void NavSat::Implementation::RemoveNavSatEntities( [&](const Entity &_entity, const components::NavSat *)->bool { - igndbg << "Removing NavSat entity" << std::endl; auto sensorId = this->entitySensorMap.find(_entity); if (sensorId == this->entitySensorMap.end()) { diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0a5747e14e..18df36ba58 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2869,7 +2869,7 @@ 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 + // collisions. These get updated only if another system has created // the component for the entity. // Populated components: // * WorldPose From 9dd6d8e2a1dc2f1a5aedbefbada0a70315546a8e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 13:29:41 -0800 Subject: [PATCH 07/15] more tweaks Signed-off-by: Louise Poubel --- examples/worlds/sensors.sdf | 2 +- src/systems/navsat/NavSat.cc | 7 ++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index c1148a6349..4c9bde3922 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -200,7 +200,7 @@ 0.0 0.1 - + 0.0 diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index a92f776529..503f2583a6 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -23,11 +23,11 @@ #include #include +#include + #include #include -#include - #include #include @@ -38,9 +38,6 @@ #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/components/Sensor.hh" -#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" From a1feb5159c890c5be0401c8481cbb6f994bfb59f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 13:47:05 -0800 Subject: [PATCH 08/15] Apply PR #1104 fix Signed-off-by: Louise Poubel --- src/systems/navsat/NavSat.cc | 140 ++++++++++++++++++++++------------- 1 file changed, 89 insertions(+), 51 deletions(-) diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index 503f2583a6..b65c92f391 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -38,6 +38,7 @@ #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" @@ -55,18 +56,32 @@ class ignition::gazebo::systems::NavSat::Implementation /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; - /// \brief Create NavSat sensor + /// 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 Mutable reference to ECM. - public: void CreateNavSatEntities(EntityComponentManager &_ecm); + public: void CreateSensors(EntityComponentManager &_ecm); - /// \brief Update NavSat sensor data based on physics data + /// \brief Update sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. - public: void UpdateNavSat(const EntityComponentManager &_ecm); + public: void Update(const EntityComponentManager &_ecm); - /// \brief Remove NavSat sensors if their entities have been removed from - /// simulation. + /// \brief Remove sensors if their entities have been removed from simulation. /// \param[in] _ecm Immutable reference to ECM. - public: void RemoveNavSatEntities(const EntityComponentManager &_ecm); + public: void RemoveSensors(const EntityComponentManager &_ecm); + + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Sensor entity + /// \param[in] _navsat NavSat component. + /// \param[in] _parent Parent entity component. + public: void AddSensor( + EntityComponentManager &_ecm, + const Entity _entity, + const components::NavSat *_navSat, + const components::ParentEntity *_parent); }; ////////////////////////////////////////////////// @@ -79,7 +94,7 @@ void NavSat::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::PreUpdate"); - this->dataPtr->CreateNavSatEntities(_ecm); + this->dataPtr->CreateSensors(_ecm); } ////////////////////////////////////////////////// @@ -99,7 +114,7 @@ void NavSat::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { - this->dataPtr->UpdateNavSat(_ecm); + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) { @@ -110,57 +125,81 @@ void NavSat::PostUpdate(const UpdateInfo &_info, } } - this->dataPtr->RemoveNavSatEntities(_ecm); + this->dataPtr->RemoveSensors(_ecm); } ////////////////////////////////////////////////// -void NavSat::Implementation::CreateNavSatEntities(EntityComponentManager &_ecm) +void NavSat::Implementation::AddSensor( + EntityComponentManager &_ecm, + const Entity _entity, + const components::NavSat *_navsat, + const components::ParentEntity *_parent) { - IGN_PROFILE("NavSat::CreateNavSatEntities"); - // Create NavSat - _ecm.EachNew( - [&](const Entity &_entity, - const components::NavSat *_navsat, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _navsat->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) + // 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< + sensors::NavSatSensor>(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); + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + +////////////////////////////////////////////////// +void NavSat::Implementation::CreateSensors(EntityComponentManager &_ecm) +{ + IGN_PROFILE("NavSat::CreateSensors"); + if (!this->initialized) + { + _ecm.Each( + [&](const Entity &_entity, + const components::NavSat *_navSat, + const components::ParentEntity *_parent)->bool { - std::string topic = scopedName(_entity, _ecm) + "/navsat"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::NavSatSensor>(data); - if (nullptr == sensor) + 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 { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + this->AddSensor(_ecm, _entity, _navSat, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - return true; }); + } } ////////////////////////////////////////////////// -void NavSat::Implementation::UpdateNavSat(const EntityComponentManager &_ecm) +void NavSat::Implementation::Update(const EntityComponentManager &_ecm) { - IGN_PROFILE("NavSat::UpdateNavSat"); + IGN_PROFILE("NavSat::Update"); _ecm.Each( [&](const Entity &_entity, @@ -198,10 +237,9 @@ void NavSat::Implementation::UpdateNavSat(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void NavSat::Implementation::RemoveNavSatEntities( - const EntityComponentManager &_ecm) +void NavSat::Implementation::RemoveSensors(const EntityComponentManager &_ecm) { - IGN_PROFILE("NavSat::RemoveNavSatEntities"); + IGN_PROFILE("NavSat::RemoveSensors"); _ecm.EachRemoved( [&](const Entity &_entity, const components::NavSat *)->bool From 417887c6fbb0d08de9012ab28257a35783a5d706 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 20:09:37 -0800 Subject: [PATCH 09/15] Use NavSat plugin Signed-off-by: Louise Poubel --- examples/worlds/sensors.sdf | 21 -- examples/worlds/spherical_coordinates.sdf | 249 +++++++++++++++++++++- 2 files changed, 240 insertions(+), 30 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 4c9bde3922..ec15f663a8 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -8,7 +8,6 @@ Imu: ign topic -e -t /imu Magnetometer: ign topic -e -t /magnetometer Air pressure: ign topic -e -t /air_pressure - NavSat (GPS): ign topic -e -t /navsat --> @@ -40,10 +39,6 @@ filename="ignition-gazebo-forcetorque-system" name="ignition::gazebo::systems::ForceTorque"> - - @@ -53,16 +48,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - EARTH_WGS84 - ENU - -22.9 - -43.2 - 0 - 0 - - true 0 0 10 0 0 0 @@ -168,12 +153,6 @@ - - 1 - 1 - navsat - - 1 100 diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf index b8195168fb..d3835fb28c 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,19 @@ 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 + -6 0 6 0 0.5 0 + + + + + + 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 + + + + From 4644e7ef5d4433bd3320e9341ca3512a10dc059f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 30 Dec 2021 13:28:36 -0800 Subject: [PATCH 10/15] View aligned with map Signed-off-by: Louise Poubel --- examples/worlds/spherical_coordinates.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf index d3835fb28c..30aac2a57a 100644 --- a/examples/worlds/spherical_coordinates.sdf +++ b/examples/worlds/spherical_coordinates.sdf @@ -99,7 +99,7 @@ ign service -s /world/spherical_coordinates/set_spherical_coordinates \ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -6 0 6 0 0.5 0 + 0 -6 8 0 0.8 1.56 From 3221382b5a1c6552162b899c7989b38ef52fa46c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 6 Jan 2022 13:41:05 -0800 Subject: [PATCH 11/15] Bump required dependencies Signed-off-by: Louise Poubel --- CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 553e661d29..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 From e26e983ab742603ff9e9cdeef0d94515a7e5e395 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 10 Jan 2022 20:54:34 -0800 Subject: [PATCH 12/15] PR feedback, refactor not to call EachNew in PreUpdate Signed-off-by: Louise Poubel --- .gitignore | 1 - examples/worlds/sensors.sdf | 1 - examples/worlds/spherical_coordinates.sdf | 12 +++--- src/Conversions.cc | 6 +-- src/systems/navsat/NavSat.cc | 45 ++++++++++++++++------- test/integration/navsat_system.cc | 4 +- 6 files changed, 43 insertions(+), 26 deletions(-) diff --git a/.gitignore b/.gitignore index 6133f58ce9..51c7dd858d 100644 --- a/.gitignore +++ b/.gitignore @@ -12,4 +12,3 @@ build_* .ycm_extra_conf.py *.orig -.vscode diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index ec15f663a8..f6653ed506 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -160,7 +160,6 @@ imu true - 1 100 diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf index 30aac2a57a..58652d2757 100644 --- a/examples/worlds/spherical_coordinates.sdf +++ b/examples/worlds/spherical_coordinates.sdf @@ -57,12 +57,6 @@ ign service -s /world/spherical_coordinates/set_spherical_coordinates \ name="ignition::gazebo::systems::NavSat"> - - - - + + + + EARTH_WGS84 diff --git a/src/Conversions.cc b/src/Conversions.cc index 1e10fd2283..bdc67fd9cb 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -15,20 +15,20 @@ * */ +#include #include #include #include #include #include -#include #include +#include #include -#include #include +#include #include #include #include -#include #include #include #include diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index b65c92f391..c30cb279b9 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -34,9 +34,9 @@ #include #include -#include "ignition/gazebo/components/NavSat.hh" #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" @@ -49,20 +49,25 @@ using namespace systems; /// \brief Private NavSat data class. class ignition::gazebo::systems::NavSat::Implementation { - /// \brief A map of NavSat entity to its vertical reference + /// \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 Mutable reference to ECM. - public: void CreateSensors(EntityComponentManager &_ecm); + /// \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. @@ -73,12 +78,12 @@ class ignition::gazebo::systems::NavSat::Implementation public: void RemoveSensors(const EntityComponentManager &_ecm); /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \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( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::NavSat *_navSat, const components::ParentEntity *_parent); @@ -94,7 +99,21 @@ void NavSat::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::PreUpdate"); - this->dataPtr->CreateSensors(_ecm); + + // 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(); } ////////////////////////////////////////////////// @@ -111,6 +130,8 @@ void NavSat::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -130,7 +151,7 @@ void NavSat::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void NavSat::Implementation::AddSensor( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::NavSat *_navsat, const components::ParentEntity *_parent) @@ -147,8 +168,7 @@ void NavSat::Implementation::AddSensor( data.SetTopic(topic); } std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::NavSatSensor>(data); + this->sensorFactory.CreateSensor(data); if (nullptr == sensor) { ignerr << "Failed to create sensor [" << sensorScopedName << "]" @@ -160,15 +180,14 @@ void NavSat::Implementation::AddSensor( std::string parentName = _ecm.Component( _parent->Data())->Data(); sensor->SetParent(parentName); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void NavSat::Implementation::CreateSensors(EntityComponentManager &_ecm) +void NavSat::Implementation::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("NavSat::CreateSensors"); if (!this->initialized) diff --git a/test/integration/navsat_system.cc b/test/integration/navsat_system.cc index 20d3021ccd..f3b6208172 100644 --- a/test/integration/navsat_system.cc +++ b/test/integration/navsat_system.cc @@ -24,10 +24,10 @@ #include #include -#include "ignition/gazebo/components/NavSat.hh" #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" @@ -104,7 +104,7 @@ TEST_F(NavSatTest, ModelFalling) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - EXPECT_LT(sleep, maxSleep); + EXPECT_LE(sleep, maxSleep); EXPECT_LE(10u, navSatMsgs.size()); mutex.lock(); From 6b22f5c1d51204109bca4930980bcc54c58f28ff Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 10 Jan 2022 21:01:47 -0800 Subject: [PATCH 13/15] missing includes Signed-off-by: Louise Poubel --- src/systems/navsat/NavSat.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index c30cb279b9..181fc1d771 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -19,8 +19,10 @@ #include +#include #include #include +#include #include #include From d906b0e2d326f7581dfcdf576252327d4d14019a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 11 Jan 2022 13:33:02 -0800 Subject: [PATCH 14/15] Fix sensor update call Signed-off-by: Louise Poubel --- src/systems/navsat/NavSat.cc | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index 181fc1d771..faa8f4f38f 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -141,10 +141,7 @@ void NavSat::PostUpdate(const UpdateInfo &_info, for (auto &it : this->dataPtr->entitySensorMap) { - // Update measurement time - auto time = math::durationToSecNsec(_info.simTime); - dynamic_cast(it.second.get())->Update( - math::secNsecToDuration(time.first, time.second), false); + it.second.get()->sensors::Sensor::Update(_info.simTime, false); } } From c2c18c74c3adfc6fc559e739c98033cdf7844deb Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 11 Jan 2022 16:00:09 -0800 Subject: [PATCH 15/15] Fix Windows visibility Signed-off-by: Louise Poubel --- src/systems/navsat/NavSat.hh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/navsat/NavSat.hh b/src/systems/navsat/NavSat.hh index b9c45bfabf..e4ce4f34d6 100644 --- a/src/systems/navsat/NavSat.hh +++ b/src/systems/navsat/NavSat.hh @@ -20,7 +20,6 @@ #include #include -#include #include namespace ignition @@ -39,7 +38,7 @@ namespace systems /// 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 IGNITION_GAZEBO_VISIBLE NavSat: + class NavSat: public System, public ISystemPreUpdate, public ISystemPostUpdate