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 ce24763e77..9385241702 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,7 @@ ign_find_package(ignition-sensors4 REQUIRED altimeter camera gpu_lidar + gps imu logical_camera magnetometer diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 967511ebe0..d116062c57 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"> + + @@ -146,12 +150,20 @@ + + 1 + 1 + true + gps + + 1 100 true imu + 1 100 @@ -169,7 +181,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 edbc024d77..5d14804978 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -849,6 +851,34 @@ 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()->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()->mutable_horizontal_noise(), + _in.GpsSensor()->VelocityNoise()); + ignition::gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(), + _in.GpsSensor()->VelocityNoise()); + } + } + 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 414e871cd8..5b0564b5bf 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -37,6 +37,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" @@ -627,6 +628,18 @@ 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)); + + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldLinearVelocity(math::Vector3d::Zero)); + } else if (_sensor->Type() == sdf::SensorType::IMU) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index cf899f8476..3e596a1c49 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -82,6 +82,7 @@ add_subdirectory(detachable_joint) add_subdirectory(diff_drive) add_subdirectory(follow_actor) 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..f0f89b1d56 --- /dev/null +++ b/src/systems/gps/Gps.cc @@ -0,0 +1,239 @@ +/* + * 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; + } + + + // 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))); + + igndbg << "Creating GPS Entity" << std::endl; + + 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, + 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(), + 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); + } + 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 + { + igndbg << "Removing GPS entity" << std::endl; + 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 bcb305e1b6..959efe4f72 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -3,6 +3,7 @@ set(TEST_TYPE "INTEGRATION") set(tests 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 + + + + + +