diff --git a/CMakeLists.txt b/CMakeLists.txt index 64899d8cf6..8e01f53d83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,7 +134,7 @@ set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-math -ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.8) +ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.9) set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf new file mode 100644 index 0000000000..24492c8213 --- /dev/null +++ b/examples/worlds/spherical_coordinates.sdf @@ -0,0 +1,106 @@ + + + + + + + + + + + + + + + + + + + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 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 + + + + + + docked + + + + + + + diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 758ac65c00..9b6f83b92a 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -238,6 +238,15 @@ namespace ignition return changed; } + /// \brief Get the spherical coordinates for an entity. + /// \param[in] _entity Entity whose coordinates we want. + /// \param[in] _ecm Entity component manager + /// \return The entity's latitude (deg), longitude (deg) and elevation (m). + /// If the entity doesn't have a pose, or the world's spherical coordinates + /// haven't been defined, this will return nullopt. + std::optional IGNITION_GAZEBO_VISIBLE sphericalCoordinates( + Entity _entity, const EntityComponentManager &_ecm); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/include/ignition/gazebo/World.hh b/include/ignition/gazebo/World.hh index 55d98ea54b..9e6d8ccfb0 100644 --- a/include/ignition/gazebo/World.hh +++ b/include/ignition/gazebo/World.hh @@ -24,6 +24,7 @@ #include #include +#include #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -116,11 +117,24 @@ namespace ignition /// \brief Get atmosphere information. /// \param[in] _ecm Entity-component manager. - /// \return Magnetic field vector or nullopt if the entity does not + /// \return Atmosphere or nullopt if the entity does not /// have a components::Atmosphere component. public: std::optional Atmosphere( const EntityComponentManager &_ecm) const; + /// \brief Get spherical coordinates for the world origin. + /// \param[in] _ecm Entity-component manager. + /// \return Spherical coordinates or nullopt if the entity does not + /// have a components::SphericalCoordinates component. + public: std::optional SphericalCoordinates( + const EntityComponentManager &_ecm) const; + + /// \brief Set spherical coordinates for the world origin. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _sphericalCoordinates New spherical coordinates. + public: void SetSphericalCoordinates(EntityComponentManager &_ecm, + const math::SphericalCoordinates &_sphericalCoordinates); + /// \brief Get the ID of a light entity which is an immediate child of /// this world. /// \param[in] _ecm Entity-component manager. diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 7fd3ee7765..1c3e144033 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -25,6 +25,7 @@ #include #include +#include // This header holds serialization operators which are shared among several // components @@ -35,6 +36,31 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace traits +{ + /// \brief Type trait that determines if an ignition::gazebo::convert from In + /// to Out is defined. + /// Usage: + /// \code + /// constexpr bool hasGazeboConvert = + /// HasGazeboConvert::value + /// \endcode + template + class HasGazeboConvert + { + private: template + static auto Test(int _test) + -> decltype(std::declval() = + ignition::gazebo::convert(std::declval()), + std::true_type()); + + private: template + static auto Test(...) -> std::false_type; + + public: static constexpr bool value = // NOLINT + decltype(Test(true))::value; + }; +} /// \brief A Serializer class is used to serialize and deserialize a component. /// It is passed in as the third template parameter to components::Component. /// Eg. @@ -78,7 +104,16 @@ namespace serializers public: static std::ostream &Serialize(std::ostream &_out, const DataType &_data) { - auto msg = ignition::gazebo::convert(_data); + MsgType msg; + if constexpr (traits::HasGazeboConvert::value) + { + msg = ignition::gazebo::convert(_data); + } + else + { + msg = ignition::msgs::Convert(_data); + } + msg.SerializeToOstream(&_out); return _out; } @@ -93,7 +128,14 @@ namespace serializers MsgType msg; msg.ParseFromIstream(&_in); - _data = ignition::gazebo::convert(msg); + if constexpr (traits::HasGazeboConvert::value) + { + _data = ignition::gazebo::convert(msg); + } + else + { + _data = ignition::msgs::Convert(msg); + } return _in; } }; diff --git a/include/ignition/gazebo/components/SphericalCoordinates.hh b/include/ignition/gazebo/components/SphericalCoordinates.hh new file mode 100644 index 0000000000..93ebd43fa8 --- /dev/null +++ b/include/ignition/gazebo/components/SphericalCoordinates.hh @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_SPHERICALCOORDINATES_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SPHERICALCOORDINATES_HH_ + +#include +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using SphericalCoordinatesSerializer = + serializers::ComponentToMsgSerializer; +} + +namespace components +{ + /// \brief This component holds the spherical coordinates of the world origin. + using SphericalCoordinates = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.SphericalCoordinates", SphericalCoordinates) +} +} +} +} + +#endif diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 00f9e6c4ab..6aa156dacc 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include "ignition/gazebo/Events.hh" @@ -53,6 +54,7 @@ #include "ignition/gazebo/components/RenderEngineServerHeadless.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Wind.hh" #include "ignition/gazebo/components/World.hh" @@ -185,6 +187,14 @@ void LevelManager::ReadLevelPerformerInfo() components::Atmosphere(*this->runner->sdfWorld->Atmosphere())); } + // spherical coordinates + if (this->runner->sdfWorld->SphericalCoordinates()) + { + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::SphericalCoordinates( + *this->runner->sdfWorld->SphericalCoordinates())); + } + // TODO(anyone) This should probably go somewhere else as it is a global // constant. const std::string kPluginName{"ignition::gazebo"}; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 72a0588924..29b9b968b1 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -66,6 +66,7 @@ #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/ThreadPitch.hh" @@ -237,6 +238,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) components::Atmosphere(*_world->Atmosphere())); } + // spherical coordinates + if (_world->SphericalCoordinates()) + { + this->dataPtr->ecm->CreateComponent(worldEntity, + components::SphericalCoordinates(*_world->SphericalCoordinates())); + } + // Models for (uint64_t modelIndex = 0; modelIndex < _world->ModelCount(); ++modelIndex) diff --git a/src/Util.cc b/src/Util.cc index 9681512d85..ec6efd57dc 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -47,6 +47,7 @@ #include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -554,6 +555,30 @@ std::string validTopic(const std::vector &_topics) } return std::string(); } + +////////////////////////////////////////////////// +std::optional sphericalCoordinates(Entity _entity, + const EntityComponentManager &_ecm) +{ + auto sphericalCoordinatesComp = + _ecm.Component( + worldEntity(_entity, _ecm)); + if (nullptr == sphericalCoordinatesComp) + { + return std::nullopt; + } + + auto xyzPose = worldPose(_entity, _ecm); + + // lat / lon / elevation in rad / rad / m + auto rad = sphericalCoordinatesComp->Data().PositionTransform( + xyzPose.Pos(), + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::SPHERICAL); + + // Return degrees + return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z()); +} } } } diff --git a/src/World.cc b/src/World.cc index 333a0f430c..2d1514f79c 100644 --- a/src/World.cc +++ b/src/World.cc @@ -15,6 +15,7 @@ * */ +#include #include #include "ignition/gazebo/components/Actor.hh" @@ -25,6 +26,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/World.hh" @@ -91,6 +93,34 @@ std::optional World::Atmosphere( return _ecm.ComponentData(this->dataPtr->id); } +////////////////////////////////////////////////// +std::optional World::SphericalCoordinates( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +void World::SetSphericalCoordinates(EntityComponentManager &_ecm, + const math::SphericalCoordinates &_sphericalCoordinates) +{ + auto sphericalCoordinatesComp = + _ecm.Component(this->dataPtr->id); + if (!sphericalCoordinatesComp) + { + _ecm.CreateComponent(this->dataPtr->id, + components::SphericalCoordinates(_sphericalCoordinates)); + return; + } + + sphericalCoordinatesComp->SetData(_sphericalCoordinates, + [](const math::SphericalCoordinates &, + const math::SphericalCoordinates &){return false;}); + _ecm.SetChanged(this->dataPtr->id, + components::SphericalCoordinates::typeId, ComponentState::OneTimeChange); +} + ////////////////////////////////////////////////// std::optional World::Gravity( const EntityComponentManager &_ecm) const diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 3255416565..158dde677d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -57,6 +57,7 @@ #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/WindMode.hh" @@ -267,6 +268,26 @@ void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) }), ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, + const math::SphericalCoordinates &_data) +{ + if (nullptr == _item) + return; + + _item->setData(QString("SphericalCoordinates"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(QString::fromStdString(math::SphericalCoordinates::Convert( + _data.Surface()))), + QVariant(_data.LatitudeReference().Degree()), + QVariant(_data.LongitudeReference().Degree()), + QVariant(_data.ElevationReference()), + QVariant(_data.HeadingOffset().Degree()), + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { @@ -695,6 +716,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::SphericalCoordinates::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::WindMode::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); @@ -954,6 +982,44 @@ void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) this->dataPtr->node.Request(physicsCmdService, req, cb); } +///////////////////////////////////////////////// +void ComponentInspector::OnSphericalCoordinates(QString _surface, + double _latitude, double _longitude, double _elevation, + double _heading) +{ + if (_surface != QString("EARTH_WGS84")) + { + ignerr << "Surface [" << _surface.toStdString() << "] not supported." + << std::endl; + return; + } + + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting spherical coordinates." << std::endl; + }; + + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(_latitude); + req.set_longitude_deg(_longitude); + req.set_elevation(_elevation); + req.set_heading_deg(_heading); + + auto sphericalCoordsCmdService = "/world/" + this->dataPtr->worldName + + "/set_spherical_coordinates"; + sphericalCoordsCmdService = + transport::TopicUtils::AsValidTopic(sphericalCoordsCmdService); + if (sphericalCoordsCmdService.empty()) + { + ignerr << "Invalid spherical coordinates service" << std::endl; + return; + } + this->dataPtr->node.Request(sphericalCoordsCmdService, req, cb); +} + ///////////////////////////////////////////////// bool ComponentInspector::NestedModel() const { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 193f7b4ddd..18b0ef7d66 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -21,6 +21,9 @@ #include #include #include + +#include + #include #include @@ -28,8 +31,6 @@ #include #include -#include "ignition/gazebo/components/Physics.hh" - #include Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) @@ -91,6 +92,12 @@ namespace gazebo template<> void setData(QStandardItem *_item, const sdf::Physics &_data); + /// \brief Specialized to set Spherical Coordinates data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const math::SphericalCoordinates &_data); + /// \brief Specialized to set boolean data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -260,6 +267,16 @@ namespace gazebo public: Q_INVOKABLE void OnPhysics(double _stepSize, double _realTimeFactor); + /// \brief Callback in Qt thread when spherical coordinates change. + /// \param[in] _surface Surface model + /// \param[in] _latitude Latitude in degrees + /// \param[in] _longitude Longitude in degrees + /// \param[in] _elevation Elevation in meters + /// \param[in] _heading Heading in degrees + public: Q_INVOKABLE void OnSphericalCoordinates(QString _surface, + double _latitude, double _longitude, double _elevation, + double _heading); + /// \brief Get whether the entity is a nested model or not /// \return True if the entity is a nested model, false otherwise public: Q_INVOKABLE bool NestedModel() const; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index bb59610008..7a416788a6 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -107,6 +107,14 @@ Rectangle { ComponentInspector.OnPhysics(_stepSize, _realTimeFactor) } + /* + * Forward spherical coordinate changes to C++ + */ + function onSphericalCoordinates(_surface, _lat, _lon, _elevation, _heading) { + ComponentInspector.OnSphericalCoordinates(_surface, _lat, _lon, _elevation, + _heading); + } + Rectangle { id: header height: lockButton.height diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 1996720cbc..b27a184945 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -6,6 +6,7 @@ NoData.qml Physics.qml Pose3d.qml + SphericalCoordinates.qml String.qml TypeHeader.qml Vector3d.qml diff --git a/src/gui/plugins/component_inspector/SphericalCoordinates.qml b/src/gui/plugins/component_inspector/SphericalCoordinates.qml new file mode 100644 index 0000000000..01561ac049 --- /dev/null +++ b/src/gui/plugins/component_inspector/SphericalCoordinates.qml @@ -0,0 +1,350 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying spherical coordinates information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + // Icon size + property int iconWidth: 20 + property int iconHeight: 20 + + // Send new data to C++ + function sendSphericalCoordinates() { + // TODO(anyone) There's a loss of precision when these values get to C++ + componentInspector.onSphericalCoordinates( + surfaceText.text, + latSpin.value, + lonSpin.value, + elevationSpin.value, + headingSpin.value + ); + } + + Component { + id: plotIcon + Image { + property string componentInfo: "" + source: "plottable_icon.svg" + anchors.top: parent.top + anchors.left: parent.left + + Drag.mimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + componentInfo + "," + model.shortName + } + Drag.dragType: Drag.Automatic + Drag.supportedActions : Qt.CopyAction + Drag.active: dragMouse.drag.active + // a point to drag from + Drag.hotSpot.x: 0 + Drag.hotSpot.y: y + MouseArea { + id: dragMouse + anchors.fill: parent + drag.target: (model === null) ? null : parent + onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) + onReleased: parent.Drag.drop(); + cursorShape: Qt.DragCopyCursor + } + } + } + + Column { + anchors.fill: parent + + // Header + Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + RowLayout { + anchors.fill: parent + Item { + width: margin + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: content.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + content.show = !content.show + } + onEntered: { + header.color = highlightColor + } + onExited: { + header.color = "transparent" + } + } + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? grid.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + GridLayout { + id: grid + width: parent.width + columns: 4 + + // Left spacer + Item { + Layout.rowSpan: 5 + width: margin + indentation + } + + // Surface type + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: latText.width + indentation*3 + Rectangle { + color: "transparent" + id: surfaceSpacer + height: iconHeight + width: iconWidth + y: 10 + } + + Text { + text: 'Surface' + leftPadding: 12 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.left: surfaceSpacer.right + anchors.verticalCenter: parent.verticalCenter + } + } + Text { + id: surfaceText + text: model.data[0] + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: Material.theme == Material.Light ? "black" : "white" + font.pointSize: 12 + elide: Text.ElideLeft + } + + // Right spacer + Item { + Layout.rowSpan: 5 + width: margin + } + + // Latitude + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: latText.width + indentation*3 + Loader { + id: loaderPlotLat + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotLat.item.componentInfo = "latitude" + + Text { + id : latText + text: 'Latitude (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: latSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[1] + value: latSpin.activeFocus ? latSpin.value : numberValue + minimumValue: -90 + maximumValue: 90 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Longitude + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: lonText.width + indentation*3 + Loader { + id: loaderPlotLon + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotLon.item.componentInfo = "longitude" + + Text { + id : lonText + text: 'Longitude (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: lonSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[2] + value: lonSpin.activeFocus ? lonSpin.value : numberValue + minimumValue: -180 + maximumValue: 180 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Elevation + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: elevationText.width + indentation*3 + Loader { + id: loaderPlotElevation + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotElevation.item.componentInfo = "elevation" + + Text { + id : elevationText + text: 'Elevation (m)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: elevationSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[3] + value: elevationSpin.activeFocus ? elevationSpin.value : numberValue + minimumValue: -100000 + maximumValue: 100000 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Heading + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: headingText.width + indentation*3 + Loader { + id: loaderPlotHeading + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotHeading.item.componentInfo = "heading" + + Text { + id : headingText + text: 'Heading (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: headingSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[4] + value: headingSpin.activeFocus ? headingSpin.value : numberValue + minimumValue: -180 + maximumValue: 180 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + } + } + } +} diff --git a/src/gui/plugins/plotting/Plotting.cc b/src/gui/plugins/plotting/Plotting.cc index 94d91357ba..a6f5228ae5 100644 --- a/src/gui/plugins/plotting/Plotting.cc +++ b/src/gui/plugins/plotting/Plotting.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" @@ -72,6 +73,7 @@ namespace ignition::gazebo }; } +using namespace ignition; using namespace ignition::gazebo; using namespace ignition::gui; @@ -130,6 +132,13 @@ PlotComponent::PlotComponent(const std::string &_type, this->dataPtr->data["stepSize"] = std::make_shared(); this->dataPtr->data["realTimeFactor"] = std::make_shared(); } + else if (_type == "SphericalCoordinates") + { + this->dataPtr->data["latitude"] = std::make_shared(); + this->dataPtr->data["longitude"] = std::make_shared(); + this->dataPtr->data["elevation"] = std::make_shared(); + this->dataPtr->data["heading"] = std::make_shared(); + } else ignwarn << "Invalid Plot Component Type:" << _type << std::endl; } @@ -306,6 +315,20 @@ void Plotting::SetData(std::string _Id, const ignition::math::Pose3d &_pose) this->dataPtr->components[_Id]->SetAttributeValue("yaw", _pose.Rot().Yaw()); } +////////////////////////////////////////////////// +void Plotting::SetData(std::string _Id, const math::SphericalCoordinates &_sc) +{ + std::lock_guard lock(this->dataPtr->componentsMutex); + this->dataPtr->components[_Id]->SetAttributeValue("latitude", + _sc.LatitudeReference().Degree()); + this->dataPtr->components[_Id]->SetAttributeValue("longitude", + _sc.LongitudeReference().Degree()); + this->dataPtr->components[_Id]->SetAttributeValue("elevation", + _sc.ElevationReference()); + this->dataPtr->components[_Id]->SetAttributeValue("heading", + _sc.HeadingOffset().Degree()); +} + ////////////////////////////////////////////////// void Plotting::SetData(std::string _Id, const sdf::Physics &_physics) { @@ -448,6 +471,12 @@ void Plotting::Update(const ignition::gazebo::UpdateInfo &_info, if (comp) this->SetData(component.first, comp->Data()); } + else if (typeId == components::SphericalCoordinates::typeId) + { + auto comp = _ecm.Component(entity); + if (comp) + this->SetData(component.first, comp->Data()); + } else if (typeId == components::TrajectoryPose::typeId) { auto comp = _ecm.Component(entity); diff --git a/src/gui/plugins/plotting/Plotting.hh b/src/gui/plugins/plotting/Plotting.hh index 534d9a5008..9e85c62264 100644 --- a/src/gui/plugins/plotting/Plotting.hh +++ b/src/gui/plugins/plotting/Plotting.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include "sdf/Physics.hh" @@ -110,31 +111,37 @@ class Plotting : public ignition::gazebo::GuiSystem public: void Update(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; - /// \brief Set the Component data of giving id to the giving vector + /// \brief Set the Component data of given id to the given vector /// \param [in] _Id Component Key of the components map /// \param [in] _vector Vector Data to be set to the component public: void SetData(std::string _Id, const ignition::math::Vector3d &_vector); - /// \brief Set the Component data of giving id to the giving light + /// \brief Set the Component data of given id to the given light /// \param [in] _Id Component Key of the components map /// \param [in] _light Vector Data to be set to the component public: void SetData(std::string _Id, const msgs::Light &_light); - /// \brief Set the Component data of giving id to the giving pose + /// \brief Set the Component data of given id to the given pose /// \param [in] _Id Component Key of the components map /// \param [in] _pose Position Data to be set to the component public: void SetData(std::string _Id, const ignition::math::Pose3d &_pose); - /// \brief Set the Component data of giving id to the giving + /// \brief Set the Component data of given id to the given spherical + /// coordinates. + /// \param [in] _Id Component Key of the components map + /// \param [in] _sc Data to be set to the component + public: void SetData(std::string _Id, const math::SphericalCoordinates &_sc); + + /// \brief Set the Component data of given id to the given /// physics properties /// \param [in] _Id Component Key of the components map /// \param [in] _value physics Data to be set to the component public: void SetData(std::string _Id, const sdf::Physics &_physics); - /// \brief Set the Component data of giving id to the giving number + /// \brief Set the Component data of given id to the given number /// \param [in] _Id Component Key of the components map /// \param [in] _value double Data to be set to the component /// valid for types (double, float, int, bool) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c395c4c6ff..e43066004c 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -49,10 +50,12 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/World.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -68,6 +71,74 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { + +/// \brief Helper function to get an entity from an entity message. +/// +/// \TODO(anyone) Move to Util.hh and generalize for all entities, +/// not only top level +/// +/// The message is used as follows: +/// +/// if id not null +/// use id +/// else if name not null and type not null +/// use name + type +/// else +/// error +/// end +/// \param[in] _ecm Entity component manager +/// \param[in] _msg Entity message +/// \return Entity ID, or kNullEntity if a matching entity couldn't be +/// found. +Entity topLevelEntityFromMessage(const EntityComponentManager &_ecm, + const msgs::Entity &_msg) +{ + if (_msg.id() != kNullEntity) + { + return _msg.id(); + } + + if (!_msg.name().empty() && _msg.type() != msgs::Entity::NONE) + { + Entity entity{kNullEntity}; + if (_msg.type() == msgs::Entity::MODEL) + { + entity = _ecm.EntityByComponents(components::Model(), + components::Name(_msg.name())); + } + else if (_msg.type() == msgs::Entity::LIGHT) + { + entity = _ecm.EntityByComponents( + components::Name(_msg.name())); + + auto lightComp = _ecm.Component(entity); + if (nullptr == lightComp) + entity = kNullEntity; + } + else + { + ignerr << "Failed to handle entity type [" << _msg.type() << "]" + << std::endl; + } + return entity; + } + + ignerr << "Message missing either entity's ID or name + type" << std::endl; + return kNullEntity; +} + +/// \brief Pose3d equality comparison function. +/// \param[in] _a A pose to compare +/// \param[in] _b Another pose to compare +bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b) +{ + return _a.Pos().Equal(_b.Pos(), 1e-6) && + math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && + math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && + math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && + math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); +} + /// \brief This class is passed to every command and contains interfaces that /// can be shared among all commands. For example, all create and remove /// commands can use the `creator` object. @@ -220,17 +291,6 @@ class PoseCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; - - /// \brief Pose3d equality comparison function. - public: std::function - pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b) - { - return _a.Pos().Equal(_b.Pos(), 1e-6) && - math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && - math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && - math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && - math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); - }}; }; /// \brief Command to modify the physics parameters of a simulation. @@ -246,6 +306,19 @@ class PhysicsCommand : public UserCommandBase public: bool Execute() final; }; +/// \brief Command to modify the spherical coordinates of a simulation. +class SphericalCoordinatesCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the new coordinates. + /// \param[in] _iface Pointer to user commands interface. + public: SphericalCoordinatesCommand(msgs::SphericalCoordinates *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; + /// \brief Command to enable a collision component. class EnableCollisionCommand : public UserCommandBase { @@ -324,6 +397,14 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PhysicsService(const msgs::Physics &_req, msgs::Boolean &_res); + /// \brief Callback for spherical coordinates service + /// \param[in] _req Request containing updates to the spherical coordinates. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the physics parameters will be successfully updated. + /// \return True if successful. + public: bool SphericalCoordinatesService( + const msgs::SphericalCoordinates &_req, msgs::Boolean &_res); + /// \brief Callback for enable collision service /// \param[in] _req Request containing collision entity. /// \param[in] _res True if message successfully received and queued. @@ -461,6 +542,15 @@ void UserCommands::Configure(const Entity &_entity, ignmsg << "Physics service on [" << physicsService << "]" << std::endl; + // Spherical coordinates service + std::string sphericalCoordinatesService{"/world/" + validWorldName + + "/set_spherical_coordinates"}; + this->dataPtr->node.Advertise(sphericalCoordinatesService, + &UserCommandsPrivate::SphericalCoordinatesService, this->dataPtr.get()); + + ignmsg << "SphericalCoordinates service on [" << sphericalCoordinatesService + << "]" << std::endl; + // Enable collision service std::string enableCollisionService{ "/world/" + validWorldName + "/enable_collision"}; @@ -664,6 +754,24 @@ bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::SphericalCoordinatesService( + const msgs::SphericalCoordinates &_req, msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -861,6 +969,35 @@ bool CreateCommand::Execute() auto poseComp = this->iface->ecm->Component(entity); *poseComp = components::Pose(msgs::Convert(createMsg->pose())); } + // Spherical coordinates + else if (createMsg->has_spherical_coordinates()) + { + auto scComp = this->iface->ecm->Component( + this->iface->worldEntity); + if (nullptr == scComp) + { + ignwarn << "Trying to create entity [" << desiredName + << "] with spherical coordinates, but world's spherical " + << "coordinates aren't set. Entity will be created at the world " + << "origin." << std::endl; + } + else + { + // deg to rad + math::Vector3d latLonEle{ + IGN_DTOR(createMsg->spherical_coordinates().latitude_deg()), + IGN_DTOR(createMsg->spherical_coordinates().longitude_deg()), + createMsg->spherical_coordinates().elevation()}; + + auto pos = scComp->Data().PositionTransform(latLonEle, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + + auto poseComp = this->iface->ecm->Component(entity); + *poseComp = components::Pose({pos.X(), pos.Y(), pos.Z(), 0, 0, + IGN_DTOR(createMsg->spherical_coordinates().heading_deg())}); + } + } igndbg << "Created entity [" << entity << "] named [" << desiredName << "]" << std::endl; @@ -885,42 +1022,7 @@ bool RemoveCommand::Execute() return false; } - Entity entity{kNullEntity}; - if (removeMsg->id() != kNullEntity) - { - entity = removeMsg->id(); - } - else if (!removeMsg->name().empty() && - removeMsg->type() != msgs::Entity::NONE) - { - if (removeMsg->type() == msgs::Entity::MODEL) - { - entity = this->iface->ecm->EntityByComponents(components::Model(), - components::Name(removeMsg->name())); - } - else if (removeMsg->type() == msgs::Entity::LIGHT) - { - entity = this->iface->ecm->EntityByComponents( - components::Name(removeMsg->name())); - - auto lightComp = this->iface->ecm->Component(entity); - if (nullptr == lightComp) - entity = kNullEntity; - } - else - { - ignerr << "Deleting entities of type [" << removeMsg->type() - << "] is not supported." << std::endl; - return false; - } - } - else - { - ignerr << "Remove command missing either entity's ID or name + type" - << std::endl; - return false; - } - + auto entity = topLevelEntityFromMessage(*this->iface->ecm, *removeMsg); if (entity == kNullEntity) { ignerr << "Entity named [" << removeMsg->name() << "] of type [" @@ -1086,7 +1188,7 @@ bool PoseCommand::Execute() else { /// \todo(anyone) Moving an object is not captured in a log file. - auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), this->pose3Eql) ? + auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), pose3Eql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, @@ -1130,6 +1232,86 @@ bool PhysicsCommand::Execute() return true; } +////////////////////////////////////////////////// +SphericalCoordinatesCommand::SphericalCoordinatesCommand( + msgs::SphericalCoordinates *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool SphericalCoordinatesCommand::Execute() +{ + auto sphericalCoordinatesMsg = + dynamic_cast(this->msg); + if (nullptr == sphericalCoordinatesMsg) + { + ignerr << "Internal error, null SphericalCoordinates message" << std::endl; + return false; + } + + // World + if (!sphericalCoordinatesMsg->has_entity()) + { + World world(this->iface->worldEntity); + world.SetSphericalCoordinates(*this->iface->ecm, + msgs::Convert(*sphericalCoordinatesMsg)); + return true; + } + + // Entity + auto entity = topLevelEntityFromMessage(*this->iface->ecm, + sphericalCoordinatesMsg->entity()); + + if (!this->iface->ecm->HasEntity(entity)) + { + ignerr << "Unable to update the pose for entity [" << entity + << "]: entity doesn't exist." << std::endl; + return false; + } + + auto scComp = this->iface->ecm->Component( + this->iface->worldEntity); + if (nullptr == scComp) + { + ignerr << "Trying to move entity [" << entity + << "] using spherical coordinates, but world's spherical " + << "coordinates aren't set." << std::endl; + return false; + } + + // deg to rad + math::Vector3d latLonEle{ + IGN_DTOR(sphericalCoordinatesMsg->latitude_deg()), + IGN_DTOR(sphericalCoordinatesMsg->longitude_deg()), + sphericalCoordinatesMsg->elevation()}; + + auto pos = scComp->Data().PositionTransform(latLonEle, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + + math::Pose3d pose{pos.X(), pos.Y(), pos.Z(), 0, 0, + IGN_DTOR(sphericalCoordinatesMsg->heading_deg())}; + + auto poseCmdComp = + this->iface->ecm->Component(entity); + if (!poseCmdComp) + { + this->iface->ecm->CreateComponent(entity, components::WorldPoseCmd(pose)); + } + else + { + auto state = poseCmdComp->SetData(pose, pose3Eql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, + state); + } + + return true; +} + ////////////////////////////////////////////////// EnableCollisionCommand::EnableCollisionCommand(msgs::Entity *_msg, std::shared_ptr &_iface) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index d4fb16a10a..41621be78a 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -50,6 +50,7 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc + spherical_coordinates.cc touch_plugin.cc triggered_publisher.cc user_commands.cc diff --git a/test/integration/spherical_coordinates.cc b/test/integration/spherical_coordinates.cc new file mode 100644 index 0000000000..aff21645c7 --- /dev/null +++ b/test/integration/spherical_coordinates.cc @@ -0,0 +1,339 @@ +/* + * Copyright (C) 2018 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 "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test SphericalCoordinates system +class SphericalCoordinatesTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, InitialFromSDF) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + auto scComp = _ecm.Component(entity); + EXPECT_NE(nullptr, scComp); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + int expectedIterations{10}; + fixture.Server()->Run(true, expectedIterations, false); + + // Check values from SDF + EXPECT_DOUBLE_EQ(expectedIterations, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(-22.9, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(-43.2, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(0.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(0.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, SetWorldOriginFromTransport) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + // Set through transport and check + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(35.6); + req.set_longitude_deg(140.1); + req.set_elevation(456.0); + req.set_heading_deg(-20.0); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/set_spherical_coordinates"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; latest.LatitudeReference().Degree() < 1.0 && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + EXPECT_DOUBLE_EQ(expectedIterations, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(35.6, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(140.1, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(456.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(-20.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, SetWorldOriginFromComponent) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + // Set throught C++ API and check + fixture.OnPreUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + world.SetSphericalCoordinates(_ecm, math::SphericalCoordinates( + math::SphericalCoordinates::EARTH_WGS84, IGN_DTOR(52.2), + IGN_DTOR(21.0), 789.0, 0)); + }); + + fixture.Server()->Run(true, 1, false); + EXPECT_DOUBLE_EQ(1, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(52.2, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(21.0, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(789.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(0.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, MoveEntity) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + Entity modelEntity{kNullEntity}; + math::SphericalCoordinates worldLatLon; + math::Vector3d modelLatLon; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + World world(worldEntity(_ecm)); + + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + worldLatLon = world.SphericalCoordinates(_ecm).value(); + + modelEntity = _ecm.EntityByComponents(components::Model(), + components::Name("north")); + auto modelCoord = sphericalCoordinates(modelEntity, _ecm); + EXPECT_TRUE(modelCoord); + modelLatLon = modelCoord.value(); + + iterations++; + }).Finalize(); + + // An entity on +Y (North) has higher Latitude than the origin + fixture.Server()->Run(true, 1, false); + EXPECT_NE(kNullEntity, modelEntity); + EXPECT_GT(modelLatLon.X(), worldLatLon.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(modelLatLon.Y(), worldLatLon.LongitudeReference().Degree()); + + // Move entity through transport and check + double desiredLat{-23.0}; + double desiredLon{-43.3}; + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(desiredLat); + req.set_longitude_deg(desiredLon); + auto entityMsg = req.mutable_entity(); + entityMsg->set_id(modelEntity); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/set_spherical_coordinates"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; modelLatLon.X() > worldLatLon.LatitudeReference().Degree() + && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + EXPECT_NEAR(modelLatLon.X(), desiredLat, 1e-6); + EXPECT_NEAR(modelLatLon.Y(), desiredLon, 1e-6); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, CreateEntity) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + Entity modelEntity{kNullEntity}; + math::SphericalCoordinates worldLatLon; + math::Vector3d modelLatLon; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + World world(worldEntity(_ecm)); + + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + worldLatLon = world.SphericalCoordinates(_ecm).value(); + + // Get model once it's spawned + modelEntity = _ecm.EntityByComponents(components::Model(), + components::Name("spawned")); + if (kNullEntity != modelEntity) + { + auto modelCoord = sphericalCoordinates(modelEntity, _ecm); + EXPECT_TRUE(modelCoord); + modelLatLon = modelCoord.value(); + } + + iterations++; + }).Finalize(); + + // Create entity at spherical coordinates + auto modelStr = std::string("") + + "" + + "" + + "" + + "" + + "" + + ""; + + double desiredLat{-23.0}; + double desiredLon{-43.3}; + + msgs::EntityFactory req; + req.set_sdf(modelStr); + + auto scMsg = req.mutable_spherical_coordinates(); + scMsg->set_latitude_deg(desiredLat); + scMsg->set_longitude_deg(desiredLon); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/create"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; kNullEntity == modelEntity && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + EXPECT_NE(kNullEntity, modelEntity); + EXPECT_NEAR(modelLatLon.X(), desiredLat, 1e-6); + EXPECT_NEAR(modelLatLon.Y(), desiredLon, 1e-6); +} diff --git a/test/integration/world.cc b/test/integration/world.cc index 9657d53eb7..9e0147924c 100644 --- a/test/integration/world.cc +++ b/test/integration/world.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -111,6 +113,24 @@ TEST_F(WorldIntegrationTest, Atmosphere) EXPECT_EQ(math::Temperature(288.15), atmosphere.Temperature()); } +////////////////////////////////////////////////// +TEST_F(WorldIntegrationTest, SphericalCoordinates) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::World()); + + World world(id); + + EXPECT_EQ(std::nullopt, world.SphericalCoordinates(ecm)); + + world.SetSphericalCoordinates(ecm, math::SphericalCoordinates()); + + auto sphericalCoordinates = world.SphericalCoordinates(ecm).value(); + EXPECT_DOUBLE_EQ(0.0, sphericalCoordinates.LatitudeReference().Degree()); +} + ////////////////////////////////////////////////// TEST_F(WorldIntegrationTest, Gravity) { diff --git a/test/worlds/spherical_coordinates.sdf b/test/worlds/spherical_coordinates.sdf new file mode 100644 index 0000000000..7e8edba5ca --- /dev/null +++ b/test/worlds/spherical_coordinates.sdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + + + + 0 1000 0 0 0 0 + + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index a035bb9f84..fbdc4b52c0 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -34,6 +34,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests +* \subpage spherical_coordinates "Spherical coordinates": Working with latitude and longitude **Migration from Gazebo classic** diff --git a/tutorials/files/spherical_coordinates/ENU.svg b/tutorials/files/spherical_coordinates/ENU.svg new file mode 100644 index 0000000000..c97ca18197 --- /dev/null +++ b/tutorials/files/spherical_coordinates/ENU.svg @@ -0,0 +1,348 @@ + + + +Z + + + + + + +Y + + + + + + +X + + + + + + +North + + + + + + +East + + + + + + +Up + + + + + + +ecef + + +ecef + + +ecef + + + +φ + +λ + + \ No newline at end of file diff --git a/tutorials/files/spherical_coordinates/inspector.png b/tutorials/files/spherical_coordinates/inspector.png new file mode 100644 index 0000000000..f96ac82d2a Binary files /dev/null and b/tutorials/files/spherical_coordinates/inspector.png differ diff --git a/tutorials/spherical_coordinates.md b/tutorials/spherical_coordinates.md new file mode 100644 index 0000000000..c5f1f283c6 --- /dev/null +++ b/tutorials/spherical_coordinates.md @@ -0,0 +1,150 @@ +\page spherical_coordinates Spherical coordinates + +Gazebo supports the use of real world latitude and longitude coordinates in its +simulations using the +[WGS84](https://en.wikipedia.org/wiki/World_Geodetic_System#1984_version) +geodetic system. + +Gazebo's simulation is always performed in Cartesian coordinates (good old XYZ). +Therefore, in order to use spherical coordinates, it's necessary to project +coordinates expressed in the `WGS84` frame to Cartesian and back. + +This tutorial will go over how to: + +* Define what latitude / longitude the world origin corresponds to +* Query the world origin or an entity's coordinates in latitude / longitude +* Spawn entities at given latitude / longitude +* Teleport entities based on a given latitude / longitude + +## Coordinates for the world origin + +The world's Cartesian coordinates correspond to a +[local tangent plane](https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates) +at a given point on the planetary surface. By default, this plane follows the +ENU (East-North-Up) convention, as shown on the image below: + +@image html files/spherical_coordinates/ENU.svg + +Users can define where the origin of the ENU coordinates sits on the surface +of the planet in three different ways: through the SDF file, the GUI, or through +an Ignition Transport service call. + +Changing the world origin will only affect operations performed afterwards. For +example, models already in the world will not be moved when the world origin's +coordinates change. + +### SDF file + +On the SDF file, it's possible to define the world origin's spherical +coordinates with the +[`/world/spherical_coordinates`](http://sdformat.org/spec?ver=1.8&elem=world#world_spherical_coordinates) +element. + +For example: + +```.xml + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + +``` + +At the moment, the only surface model supported is WGS84 and the only world +frame orientation is ENU. + +Try out an example world that ships with Gazebo and has the coordinates above +as follows: + +``` +ign gazebo spherical_coordinates.sdf +``` + +On the component inspector, expand the `Spherical Coordinates` component to see +that the coordinates were set correctly: + +@image html files/spherical_coordinates/inspector.png + +### GUI + +To change the world origin through the GUI, edit values within the component +inspector shown above, and these changes will be reflected on simulation. + +### Service + +It's also possible to change the coordinates for the world origin at runtime +through the `/world//set_spherical_coordinates` service. + +Loading the world above, try calling for example: + +```.bash +ign service \ +-s /world/spherical_coordinates/set_spherical_coordinates \ +--reqtype ignition.msgs.SphericalCoordinates \ +--reptype ignition.msgs.Boolean \ +--timeout 2000 \ +--req "surface_model: EARTH_WGS84, latitude_deg: 35.6, longitude_deg: 140.1, elevation: 10.0" +``` + +You should see the coordinates changing on the GUI. + +## Creating entities + +See the [Entity creation](entity_creation.html) tutorial for an introduction +to creating entities. + +In order to create an entity at a given spherical coordinate using the +`/world//create` service, omit the `pose` field from the +`ignition::msgs::EntityFactory` message and use the `spherical_coordinates` +field instead. + +For example, you can spawn a sphere into the `spherical_coordinates.sdf` world +as follows: + +```.bash +ign service -s /world/spherical_coordinates/create \ +--reqtype ignition.msgs.EntityFactory \ +--reptype ignition.msgs.Boolean \ +--timeout 300 \ +--req 'sdf: '\ +'"'\ +''\ +''\ +''\ +''\ +'1.0'\ +''\ +''\ +'1.0'\ +''\ +''\ +''\ +'" '\ +'spherical_coordinates: {latitude_deg: 35.59999, longitude_deg: 140.09999, elevation: 11.0} ' +``` + +## Moving entities + +It's also possible to move entities to a given spherical coordinate. Just use the +`set_spherical_coordinate` service that was used to set the world origin, but +specify the entity to be moved. + +For example, to move the sphere created above: + +```.bash +ign service -s /world/spherical_coordinates/set_spherical_coordinates \ +--reqtype ignition.msgs.SphericalCoordinates \ +--reptype ignition.msgs.Boolean \ +--timeout 300 \ +--req 'entity: {name: "spawned_model", type: MODEL}, latitude_deg: 35.59990, longitude_deg: 140.09990' +``` + +## Querying entities' coordinates + +When writing plugins, developers can use the +`ignition::gazebo::sphericalCoordinates` helper function to query the current +coordinates for any entity that has a pose. +