diff --git a/Changelog.md b/Changelog.md index 73a4020d26..f57a1e757f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -6,6 +6,73 @@ ### Ignition Gazebo 6.X.X (202X-XX-XX) +### Ignition Gazebo 6.5.0 (202X-XX-XX) +1. New trajectory follower system + * [Pull request #1332](https://github.com/ignitionrobotics/ign-gazebo/pull/1332) + +1. Extend ShaderParam system to support textures + * [Pull request #1310](https://github.com/ignitionrobotics/ign-gazebo/pull/1310) + +1. Adds a `Link::SetLinearVelocity()` method + * [Pull request #1323](https://github.com/ignitionrobotics/ign-gazebo/pull/1323) + +1. Fix weird indentation in `Link.hh` + * [Pull request #1324](https://github.com/ignitionrobotics/ign-gazebo/pull/1324) + +1. Limit thruster system's input thrust cmd + * [Pull request #1318](https://github.com/ignitionrobotics/ign-gazebo/pull/1318) + +1. Load and run visual plugin (system) on GUI side + * [Pull request #1275](https://github.com/ignitionrobotics/ign-gazebo/pull/1275) + +1. Log an error if JointPositionController cannot find the joint. (citadel retarget) + * [Pull request #1314](https://github.com/ignitionrobotics/ign-gazebo/pull/1314) + +1. Update source install instructions + * [Pull request #1311](https://github.com/ignitionrobotics/ign-gazebo/pull/1311) + +1. Document the `` option for JointPositionController. + * [Pull request #1309](https://github.com/ignitionrobotics/ign-gazebo/pull/1309) + +1. Fix typo in EntityComponentManager + * [Pull request #1304](https://github.com/ignitionrobotics/ign-gazebo/pull/1304) + +1. Buoyancy: fix center of volume's reference frame + * [Pull request #1302](https://github.com/ignitionrobotics/ign-gazebo/pull/1302) + +1. Fix graded buoyancy problems + * [Pull request #1297](https://github.com/ignitionrobotics/ign-gazebo/pull/1297) + +1. Add surface to buoyancy engine. (retarget fortress) + * [Pull request #1298](https://github.com/ignitionrobotics/ign-gazebo/pull/1298) + +1. Remove EachNew calls from sensor PreUpdates + * [Pull request #1281](https://github.com/ignitionrobotics/ign-gazebo/pull/1281) + +1. Prevent GzScene3D 💥 if another scene is already loaded + * [Pull request #1294](https://github.com/ignitionrobotics/ign-gazebo/pull/1294) + +1. Fix various typos on API documentation + * [Pull request #1291](https://github.com/ignitionrobotics/ign-gazebo/pull/1291) + +1. Optional orientation when spawning entity using spherical coordinates + * [Pull request #1263](https://github.com/ignitionrobotics/ign-gazebo/pull/1263) + +1. Cleanup update call for non-rendering sensors + * [Pull request #1282](https://github.com/ignitionrobotics/ign-gazebo/pull/1282) + +1. Documentation Error + * [Pull request #1285](https://github.com/ignitionrobotics/ign-gazebo/pull/1285) + +1. Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. + * [Pull request #1229](https://github.com/ignitionrobotics/ign-gazebo/pull/1229) + +1. Add project() call to examples + * [Pull request #1274](https://github.com/ignitionrobotics/ign-gazebo/pull/1274) + +1. Implement /server_control::stop + * [Pull request #1240](https://github.com/ignitionrobotics/ign-gazebo/pull/1240) + ### Ignition Gazebo 6.4.0 (2021-01-13) 1. Disable more tests on Windows @@ -1009,6 +1076,21 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.14.0 (2021-12-20) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Disable user commands light test on macOS + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + ### Ignition Gazebo 4.13.0 (2021-11-15) 1. Prevent creation of spurious `` elements when saving worlds diff --git a/Migration.md b/Migration.md index 9b86dfa500..6307225661 100644 --- a/Migration.md +++ b/Migration.md @@ -146,6 +146,10 @@ in SDF by setting the `` SDF element. * **Deprecated**: `Entity EntityFromNode(const rendering::NodePtr &_node) const;` * **Replacement**: `Entity entity = std::get(visual->UserData("gazebo-entity"));` +## Ignition Gazebo 3.12.0 to 3.X.X + +* Some sensors will only have the `SensorTopic` component after the 1st iteration. + ## Ignition Gazebo 2.x to 3.x * Use ign-rendering3, ign-sensors3 and ign-gui3. diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf new file mode 100644 index 0000000000..25e8b12f14 --- /dev/null +++ b/examples/worlds/shader_param.sdf @@ -0,0 +1,247 @@ + + + + + + + + + ogre2 + 0.8 0.8 0.8 + + + + + + + + + + + + 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 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + docked + + + + + + + docked + + + + + + RGB camera + floating + 350 + 315 + + camera + false + + + + Depth camera + floating + 350 + 315 + 500 + + depth_camera + false + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + false + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.6 0.6 0.6 1 + -0.5 0.1 -0.9 + + + + deformable_sphere + 0 0 1.5 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere + + + waves + 0 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves + + + + true + 2.5 0 1.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + camera + + + 10 + depth_camera + + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + + + diff --git a/examples/worlds/trajectory_follower.sdf b/examples/worlds/trajectory_follower.sdf new file mode 100644 index 0000000000..128d0d1442 --- /dev/null +++ b/examples/worlds/trajectory_follower.sdf @@ -0,0 +1,220 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 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 + + + + + + + 2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + box_link + true + 10 + 10 + + 2 0 + 7 0 + + + + + + + 0 2 0.5 0 0 1.5708 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + box_link + true + 10 + 10 + + 0 + 5 + + + + + + + 0 -2 0.5 0 0 -1.5708 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + box_link + true + 10 + 10 + + 0 + 5 + + + + + + + diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 094567951b..9dd3d5ccc7 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -220,13 +220,20 @@ namespace ignition public: void EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable = true) const; - /// \brief Get the angular acceleration of the body in the world frame. - /// \param[in] _ecm Entity-component manager. - /// \return Angular acceleration of the body in the world frame or - /// nullopt if acceleration checks aren't enabled. - /// \sa EnableAccelerationChecks - public: std::optional WorldAngularAcceleration( - const EntityComponentManager &_ecm) const; + /// \brief Set the linear velocity on this link. If this is set, wrenches + /// on this link will be ignored for the current time step. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _vel Linear velocity to set in Link's Frame. + public: void SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const; + + /// \brief Get the angular acceleration of the body in the world frame. + /// \param[in] _ecm Entity-component manager. + /// \return Angular acceleration of the body in the world frame or + /// nullopt if acceleration checks aren't enabled. + /// \sa EnableAccelerationChecks + public: std::optional WorldAngularAcceleration( + const EntityComponentManager &_ecm) const; /// \brief Get the linear acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh index 4bd2389101..2096d2d5d0 100644 --- a/include/ignition/gazebo/components/CenterOfVolume.hh +++ b/include/ignition/gazebo/components/CenterOfVolume.hh @@ -33,7 +33,7 @@ namespace components /// \brief A component for an entity's center of volume. Units are in meters. /// The Vector3 value indicates center of volume of an entity. The /// position of the center of volume is relative to the pose of the parent - /// entity. + /// entity, which is usually a link. using CenterOfVolume = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume", CenterOfVolume) diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 9d90871a7b..de94818059 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -17,6 +17,11 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ #define IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ +#include + +#include +#include + #include #include #include @@ -27,11 +32,60 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class SdfElementSerializer + { + /// \brief Serialization for `sdf::Model`. + /// \param[in] _out Output stream. + /// \param[in] _elem Visual plugin elem to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::ElementPtr &_elem) + { + _out << "" + << "" + << _elem->ToString("") + << ""; + return _out; + } + + /// \brief Deserialization for `sdf::Element`. + /// \param[in] _in Input stream. + /// \param[out] _elem Visual plugin elem to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::ElementPtr &_elem) + { + std::string sdfStr(std::istreambuf_iterator(_in), {}); + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + bool result = sdf::readString(sdfStr, sdfParsed); + if (!result) + { + ignerr << "Unable to deserialize sdf::ElementPtr" << std::endl; + return _in; + } + + _elem = sdfParsed->Root()->GetFirstElement(); + return _in; + } + }; +} + namespace components { /// \brief A component that identifies an entity as being a visual. using Visual = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) + + /// \brief A component that contains a visual plugin's SDF element. + using VisualPlugin = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", + VisualPlugin) } } } diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 4407d68598..afbb751879 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -25,8 +25,11 @@ #include #include #include + #include #include +#include + #include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -211,6 +214,27 @@ namespace events IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief Event that notifies a visual plugin is to be loaded + class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \param[in] _elem Visual plugin SDF element + public: explicit VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem); + + /// \brief Get the entity to load the visual plugin for + public: ignition::gazebo::Entity Entity() const; + + /// \brief Get the sdf element of the visual plugin + public: sdf::ElementPtr Element() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh index d17b3922c5..683c66f529 100644 --- a/include/ignition/gazebo/rendering/Events.hh +++ b/include/ignition/gazebo/rendering/Events.hh @@ -32,6 +32,18 @@ namespace ignition /// more information about events. namespace events { + /// \brief The render event is emitted when the the scene manager is + /// updated with contents from the ECM. This event is emitted + /// before the PreRender event on the server side in the rendering + /// thread. It is also accessible on the GUI side. + /// + /// For example: + /// \code + /// eventManager.Emit(); + /// \endcode + using SceneUpdate = ignition::common::EventT; + /// \brief The render event is emitted before rendering updates. /// The event is emitted in the rendering thread so rendering /// calls can ben make in this event callback diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 6110b2083c..a757ec5378 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -214,6 +214,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _active True if active. public: void SetTransformActive(bool _active); + /// \brief Set the event manager to use + /// \param[in] _mgr Event manager to set to. + public: void SetEventManager(EventManager *_mgr); + /// \brief Private data pointer. private: std::unique_ptr dataPtr; }; diff --git a/src/Link.cc b/src/Link.cc index 855adf4352..0d07f17140 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -26,6 +26,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -246,6 +247,25 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable) _enable); } +////////////////////////////////////////////////// +void Link::SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const +{ + auto vel = + _ecm.Component(this->dataPtr->id); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LinearVelocityCmd(_vel)); + } + else + { + vel->Data() = _vel; + } +} + ////////////////////////////////////////////////// std::optional Link::WorldAngularAcceleration( const EntityComponentManager &_ecm) const diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index ae98f41ec7..d54e1f377f 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -749,6 +749,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) components::Material(*_visual->Material())); } + // store the plugin in a component + if (_visual->Element()) + { + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + if (pluginElem) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugin(pluginElem)); + } + } + // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. this->dataPtr->newVisuals[visualEntity] = _visual->Element(); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index a52210425e..5da194e209 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -495,9 +495,6 @@ namespace ignition /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; - /// \brief Systems implementing Configure - private: std::vector systemsConfigure; - /// \brief Systems implementing PreUpdate private: std::vector systemsPreupdate; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index cc9b33161a..17641a873c 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1234,6 +1234,18 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) }); EXPECT_NE(kNullEntity, sensorId); + // Get visual entity + Entity visualId{kNullEntity}; + runner.EntityCompMgr().Each([&]( + const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Visual *_visual)->bool + { + EXPECT_NE(nullptr, _visual); + visualId = _entity; + return true; + }); + EXPECT_NE(kNullEntity, visualId); + // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; auto worldComponentId = ignition::common::hash64(worldComponentName); @@ -1258,6 +1270,14 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, sensorComponentId)); + // Check component registered by visual plugin + std::string visualComponentName{"VisualPluginComponent"}; + auto visualComponentId = ignition::common::hash64(visualComponentName); + + EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId)); + EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId, + visualComponentId)); + // Clang re-registers components between tests. If we don't unregister them // beforehand, the new plugin tries to create a storage type from a previous // plugin, causing a crash. @@ -1268,6 +1288,7 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) components::Factory::Instance()->Unregister(worldComponentId); components::Factory::Instance()->Unregister(modelComponentId); components::Factory::Instance()->Unregister(sensorComponentId); + components::Factory::Instance()->Unregister(visualComponentId); #endif } diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index ed195212ed..a89a5d8254 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -50,6 +50,15 @@ class ignition::gazebo::gui::events::ModelEditorAddEntity::Implementation public: ignition::gazebo::Entity parent; }; +class ignition::gazebo::gui::events::VisualPlugin::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf element of the visual plugin + public: sdf::ElementPtr element; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -132,3 +141,24 @@ QMap &ModelEditorAddEntity::Data() { return this->dataPtr->data; } + +///////////////////////////////////////////////// +VisualPlugin::VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem) : + QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->element = _elem; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPlugin::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +sdf::ElementPtr VisualPlugin::Element() const +{ + return this->dataPtr->element; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 50a2c3f075..2a70c033ac 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include @@ -28,7 +32,9 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include #include "ignition/gazebo/gui/GuiSystem.hh" +#include "ignition/gazebo/SystemLoader.hh" #include "GuiRunner.hh" @@ -68,6 +74,31 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Name of WorldControl service public: std::string controlService; + + /// \brief System loader for loading ign-gazebo systems + public: std::unique_ptr systemLoader; + + /// \brief Mutex to protect systemLoader + public: std::mutex systemLoadMutex; + + /// \brief Events containing visual plugins to load + public: std::vector> + visualPlugins; + + /// \brief Systems implementing PreUpdate + public: std::vector systems; + + /// \brief Systems implementing PreUpdate + public: std::vector systemsPreupdate; + + /// \brief Systems implementing Update + public: std::vector systemsUpdate; + + /// \brief Systems implementing PostUpdate + public: std::vector systemsPostupdate; + + /// \brief Manager of all events. + public: EventManager eventMgr; }; ///////////////////////////////////////////////// @@ -92,7 +123,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) // so that an offset is not required this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2); - auto win = gui::App()->findChild(); + auto win = ignition::gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); win->setProperty("worldNames", winWorldNames); @@ -136,7 +167,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == ignition::gui::events::WorldControl::kType) { auto worldControlEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (worldControlEvent) { msgs::WorldControlState req; @@ -162,6 +193,20 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) + { + auto visualPluginEvent = + reinterpret_cast(_event); + if (visualPluginEvent) + { + std::lock_guard lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + sdf::ElementPtr pluginElem = visualPluginEvent->Element(); + this->dataPtr->visualPlugins.push_back( + std::make_pair(entity, pluginElem)); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -171,7 +216,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) void GuiRunner::RequestState() { // set up service for async state response callback - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv); @@ -225,7 +270,7 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) // todo(anyone) store reqSrv string in a member variable and use it here // and in RequestState() - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; this->dataPtr->node.UnadvertiseSrv(reqSrv); @@ -264,7 +309,8 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) ///////////////////////////////////////////////// void GuiRunner::UpdatePlugins() { - auto plugins = gui::App()->findChildren(); + // gui plugins + auto plugins = ignition::gui::App()->findChildren(); for (auto plugin : plugins) { plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); @@ -272,4 +318,92 @@ void GuiRunner::UpdatePlugins() this->dataPtr->ecm.ClearRemovedComponents(); this->dataPtr->ecm.ClearNewlyCreatedEntities(); this->dataPtr->ecm.ProcessRemoveEntityRequests(); + + // ign-gazebo systems + this->LoadSystems(); + this->UpdateSystems(); +} + +///////////////////////////////////////////////// +void GuiRunner::LoadSystems() +{ + std::lock_guard lock(this->dataPtr->systemLoadMutex); + // currently only support systems that are visual plugins + for (auto &visualPlugin : this->dataPtr->visualPlugins) + { + Entity entity = visualPlugin.first; + sdf::ElementPtr pluginElem = visualPlugin.second; + auto filename = pluginElem->Get("filename"); + auto name = pluginElem->Get("name"); + if (filename != "__default__" && name != "__default__") + { + std::optional system; + if (!this->dataPtr->systemLoader) + this->dataPtr->systemLoader = std::make_unique(); + system = this->dataPtr->systemLoader->LoadPlugin( + filename, name, pluginElem); + if (system) + { + SystemPluginPtr sys = system.value(); + this->dataPtr->systems.push_back(sys); + this->dataPtr->systemsPreupdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsUpdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsPostupdate.push_back( + sys->QueryInterface()); + + auto sysConfigure = sys->QueryInterface(); + if (sysConfigure) + { + sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm, + this->dataPtr->eventMgr); + } + igndbg << "Loaded system [" << name + << "] for entity [" << entity << "] in GUI" + << std::endl; + } + } + } + this->dataPtr->visualPlugins.clear(); +} + +///////////////////////////////////////////////// +void GuiRunner::UpdateSystems() +{ + IGN_PROFILE("GuiRunner::UpdateSystems"); + + { + IGN_PROFILE("PreUpdate"); + for (auto& system : this->dataPtr->systemsPreupdate) + { + if (system) + system->PreUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("Update"); + for (auto& system : this->dataPtr->systemsUpdate) + { + if (system) + system->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("PostUpdate"); + // \todo(anyone) Do PostUpdates in parallel + for (auto& system : this->dataPtr->systemsPostupdate) + { + if (system) + system->PostUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } +} + +///////////////////////////////////////////////// +EventManager &GuiRunner::GuiEventManager() const +{ + return this->dataPtr->eventMgr; } diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index 1a06098161..bc5a730553 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -25,6 +25,7 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -46,6 +47,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Destructor public: ~GuiRunner() override; + /// \brief Get the event manager for the gui + public: EventManager &GuiEventManager() const; + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; @@ -74,6 +78,12 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 private: Q_INVOKABLE void UpdatePlugins(); + /// \brief Load systems + private: void LoadSystems(); + + /// \brief Update systems + private: void UpdateSystems(); + /// \brief Pointer to private data. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index 4411fb5d7e..6dea55f64e 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -14,16 +14,18 @@ * limitations under the License. * */ +import IgnGazebo 1.0 as IgnGazebo +import QtGraphicalEffects 1.0 import QtQuick 2.9 import QtQuick.Controls 2.2 import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 import RenderWindow 1.0 -import QtGraphicalEffects 1.0 -import IgnGazebo 1.0 as IgnGazebo Rectangle { - width: 1000 - height: 800 + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + anchors.fill: parent /** * True to enable gamma correction @@ -38,6 +40,7 @@ Rectangle { anchors.fill: parent hoverEnabled: true acceptedButtons: Qt.NoButton + visible: GzScene3D.loadingError.length == 0 onEntered: { GzScene3D.OnFocusWindow() } @@ -50,6 +53,7 @@ Rectangle { id: renderWindow objectName: "renderWindow" anchors.fill: parent + visible: GzScene3D.loadingError.length == 0 /** * Message to be displayed over the render window @@ -120,4 +124,11 @@ Rectangle { standardButtons: Dialog.Ok } + Label { + anchors.fill: parent + anchors.margins: 10 + text: GzScene3D.loadingError + visible: (GzScene3D.loadingError.length > 0); + wrapMode: Text.WordWrap + } } diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e33d8be79f..b5097c15fb 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2098,10 +2098,18 @@ void IgnRenderer::HandleMouseViewControl() } ///////////////////////////////////////////////// -void IgnRenderer::Initialize() +std::string IgnRenderer::Initialize() { if (this->initialized) - return; + return std::string(); + + // Only one engine / scene / user camera is currently supported. + // Fail gracefully even before getting to renderUtil. + if (!rendering::loadedEngines().empty()) + { + return "Currently only one plugin providing a 3D scene is supported at a " + "time."; + } this->dataPtr->renderUtil.SetWinID(std::to_string( ignition::gui::App()->findChild()-> @@ -2111,7 +2119,7 @@ void IgnRenderer::Initialize() rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); if (!scene) - return; + return "Failed to create a 3D scene."; auto root = scene->RootVisual(); @@ -2136,6 +2144,7 @@ void IgnRenderer::Initialize() this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); this->initialized = true; + return std::string(); } ///////////////////////////////////////////////// @@ -2578,6 +2587,12 @@ RenderThread::RenderThread() qRegisterMetaType("RenderSync*"); } +///////////////////////////////////////////////// +void RenderThread::SetErrorCb(std::function _cb) +{ + this->errorCb = _cb; +} + ///////////////////////////////////////////////// void RenderThread::RenderNext(RenderSync *_renderSync) { @@ -2586,7 +2601,12 @@ void RenderThread::RenderNext(RenderSync *_renderSync) if (!this->ignRenderer.initialized) { // Initialize renderer - this->ignRenderer.Initialize(); + auto loadingError = this->ignRenderer.Initialize(); + if (!loadingError.empty()) + { + this->errorCb(QString::fromStdString(loadingError)); + return; + } } // check if engine has been successfully initialized @@ -2604,19 +2624,25 @@ void RenderThread::RenderNext(RenderSync *_renderSync) ///////////////////////////////////////////////// void RenderThread::ShutDown() { - this->context->makeCurrent(this->surface); + if (this->context && this->surface) + this->context->makeCurrent(this->surface); this->ignRenderer.Destroy(); - this->context->doneCurrent(); - delete this->context; + if (this->context) + { + this->context->doneCurrent(); + delete this->context; + } // schedule this to be deleted only after we're done cleaning up - this->surface->deleteLater(); + if (this->surface) + this->surface->deleteLater(); // Stop event processing, move the thread to GUI and make sure it is deleted. this->exit(); - this->moveToThread(QGuiApplication::instance()->thread()); + if (this->ignRenderer.initialized) + this->moveToThread(QGuiApplication::instance()->thread()); } @@ -2740,9 +2766,11 @@ void TextureNode::PrepareNode() RenderWindowItem::RenderWindowItem(QQuickItem *_parent) : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. + // TODO(chapulina) Forward-porting #1294 from ign-gazebo3 to ign-gazebo5 + // is non-trivial since the plugin's internals have changed. Keeping this + // shortcut here for now, and revisiting later specifically for ign-gazebo5 + // onwards. + // See https://github.com/ignitionrobotics/ign-gazebo/issues/1254 static bool done{false}; if (done) { @@ -2922,9 +2950,11 @@ Scene3D::~Scene3D() = default; ///////////////////////////////////////////////// void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. + // TODO(chapulina) Forward-porting #1294 from ign-gazebo3 to ign-gazebo5 + // is non-trivial since the plugin's internals have changed. Keeping this + // shortcut here for now, and revisiting later specifically for ign-gazebo5 + // onwards. + // See https://github.com/ignitionrobotics/ign-gazebo/issues/1254 static bool done{false}; if (done) { @@ -2941,6 +2971,8 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) << "Render window will not be created" << std::endl; return; } + renderWindow->SetErrorCb(std::bind(&Scene3D::SetLoadingError, this, + std::placeholders::_1)); if (this->title.empty()) this->title = "3D Scene"; @@ -3594,6 +3626,19 @@ void Scene3D::OnFocusWindow() renderWindow->forceActiveFocus(); } +///////////////////////////////////////////////// +QString Scene3D::LoadingError() const +{ + return this->loadingError; +} + +///////////////////////////////////////////////// +void Scene3D::SetLoadingError(const QString &_loadingError) +{ + this->loadingError = _loadingError; + this->LoadingErrorChanged(); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz) { @@ -3927,6 +3972,12 @@ void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); } +///////////////////////////////////////////////// +void RenderWindowItem::SetErrorCb(std::function _cb) +{ + this->dataPtr->renderThread->SetErrorCb(_cb); +} + ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 28c15227be..c21367c37f 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -56,10 +56,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class Scene3DPrivate; class RenderUtil; - /// \brief Creates a new ignition rendering scene or adds a user-camera to an - /// existing scene. It is possible to orbit the camera around the scene with + /// \brief Creates an ignition rendering scene and user camera. + /// It is possible to orbit the camera around the scene with /// the mouse. Use other plugins to manage objects in the scene. /// + /// Only one plugin displaying an Ignition Rendering scene can be used at a + /// time. + /// /// ## Configuration /// /// * \ : Optional render engine name, defaults to 'ogre'. @@ -88,6 +91,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { NOTIFY ErrorPopupTextChanged ) + /// \brief Loading error message + Q_PROPERTY( + QString loadingError + READ LoadingError + WRITE SetLoadingError + NOTIFY LoadingErrorChanged + ) + /// \brief Constructor public: Scene3D(); @@ -236,6 +247,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnViewControl(const msgs::StringMsg &_msg, msgs::Boolean &_res); + /// \brief Get the loading error string. + /// \return String explaining the loading error. If empty, there's no error. + public: Q_INVOKABLE QString LoadingError() const; + + /// \brief Set the loading error message. + /// \param[in] _loadingError Error message. + public: Q_INVOKABLE void SetLoadingError(const QString &_loadingError); + + /// \brief Notify that loading error has changed + signals: void LoadingErrorChanged(); + + /// \brief Loading error message + public: QString loadingError; + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -265,7 +290,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Render(RenderSync *_renderSync); /// \brief Initialize the render engine - public: void Initialize(); + /// \return Error message if initialization failed. If empty, no errors + /// occurred. + public: std::string Initialize(); /// \brief Destroy camera associated with this renderer public: void Destroy(); @@ -606,6 +633,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _size Size of the texture signals: void TextureReady(uint _id, const QSize &_size); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function _cb); + + /// \brief Function to be called if there are errors. + public: std::function errorCb; + /// \brief Offscreen surface to render to public: QOffscreenSurface *surface = nullptr; @@ -841,6 +875,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _entity Scoped name of entity. public slots: void OnContextMenuRequested(QString _entity); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function _cb); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index cccb6caaa4..48c0735f10 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -14,10 +14,15 @@ * limitations under the License. * */ + +#include #include +#include "../../GuiRunner.hh" #include "GzSceneManager.hh" +#include + #include #include #include @@ -28,6 +33,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -58,6 +64,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Mutex to protect gui event and system upate call race conditions /// for newEntities and removedEntities public: std::mutex newRemovedEntityMutex; + + /// \brief Indicates whether initial visual plugins have been loaded or not. + public: bool initializedVisualPlugins = false; }; } } @@ -102,6 +111,42 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + // load visual plugin on gui side + std::map pluginElems; + if (!this->dataPtr->initializedVisualPlugins) + { + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + this->dataPtr->initializedVisualPlugins = true; + } + else + { + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + } + for (const auto &it : pluginElems) + { + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, it.second); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &visualPluginEvent); + } + // Emit entities created / removed event for gui::Plugins which don't have // direct access to the ECM. std::set created; @@ -163,6 +208,16 @@ void GzSceneManagerPrivate::OnRender() return; this->renderUtil.SetScene(this->scene); + + auto runners = ignition::gui::App()->findChildren(); + if (runners.empty() || runners[0] == nullptr) + { + ignerr << "Internal error: no GuiRunner found." << std::endl; + } + else + { + this->renderUtil.SetEventManager(&runners[0]->GuiEventManager()); + } } this->renderUtil.Update(); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 3df544c588..0679db3769 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -89,6 +89,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/Events.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/SceneManager.hh" #include "ignition/gazebo/rendering/MarkerManager.hh" @@ -193,6 +194,9 @@ class ignition::gazebo::RenderUtilPrivate const components::Name *_name, const components::ParentEntity *_parent); + /// \brief Event manager used for emitting render / scene events + public: EventManager *eventManager{nullptr}; + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1583,6 +1587,9 @@ void RenderUtil::Update() } } } + + if (this->dataPtr->eventManager) + this->dataPtr->eventManager->Emit(); } ////////////////////////////////////////////////// @@ -3637,3 +3644,9 @@ void RenderUtilPrivate::CreateLight( this->newLights.push_back(std::make_tuple(_entity, _light->Data(), _name->Data(), _parent->Data())); } + +////////////////////////////////////////////////// +void RenderUtil::SetEventManager(EventManager *_mgr) +{ + this->dataPtr->eventManager = _mgr; +} diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 17514029f8..78ef2dc886 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -128,11 +128,13 @@ add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) +add_subdirectory(shader_param) add_subdirectory(thermal) add_subdirectory(thruster) add_subdirectory(touch_plugin) add_subdirectory(track_controller) add_subdirectory(tracked_vehicle) +add_subdirectory(trajectory_follower) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) add_subdirectory(velocity_control) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 4b70a091b3..6b8db56ba5 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -49,30 +50,35 @@ using namespace systems; /// \brief Private AirPressure data class. class ignition::gazebo::systems::AirPressurePrivate { - /// \brief A map of air pressure entity to its vertical reference + /// \brief A map of air pressure 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; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _airPressure AirPressureSensor component. /// \param[in] _parent Parent entity component. public: void AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent); /// \brief Create air pressure sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAirPressureEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update air pressure sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -98,7 +104,21 @@ void AirPressure::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("AirPressure::PreUpdate"); - this->dataPtr->CreateAirPressureEntities(_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(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + if (!_info.paused) { this->dataPtr->UpdateAirPressures(_ecm); @@ -132,7 +154,7 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AirPressurePrivate::AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent) @@ -169,15 +191,13 @@ void AirPressurePrivate::AddAirPressure( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) +void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities"); if (!this->initialized) @@ -188,7 +208,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); this->initialized = true; @@ -201,7 +221,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); } diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index adf9ec8345..36f97663bd 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -51,30 +52,35 @@ using namespace systems; /// \brief Private Altimeter data class. class ignition::gazebo::systems::AltimeterPrivate { - /// \brief A map of altimeter entity to its vertical reference + /// \brief A map of altimeter 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; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _altimeter Altimeter component. /// \param[in] _parent Parent entity component. public: void AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent); /// \brief Create altimeter sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAltimeterEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update altimeter sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -99,7 +105,21 @@ void Altimeter::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::PreUpdate"); - this->dataPtr->CreateAltimeterEntities(_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(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void Altimeter::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) { @@ -133,7 +155,7 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AltimeterPrivate::AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent) @@ -171,15 +193,13 @@ void AltimeterPrivate::AddAltimeter( sensor->SetVerticalReference(verticalReference); sensor->SetPosition(verticalReference); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) +void AltimeterPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::CreateAltimeterEntities"); if (!this->initialized) @@ -190,7 +210,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); this->initialized = true; @@ -203,7 +223,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); } diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 3a9a494919..12bf5bda7d 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -368,7 +368,8 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - math::Vector3d weightedPosSum = math::Vector3d::Zero; + ignition::math::Vector3d weightedPosInLinkSum = + ignition::math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision // elements and storing each geometry's volume. @@ -426,16 +427,15 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } volumeSum += volume; - math::Pose3d pose = worldPose(collision, _ecm); - weightedPosSum += volume * pose.Pos(); + auto poseInLink = _ecm.Component(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); } if (volumeSum > 0) { - // Store the center of volume - math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + // Store the center of volume expressed in the link frame _ecm.CreateComponent(_entity, components::CenterOfVolume( - weightedPosSum / volumeSum - linkWorldPose.Pos())); + weightedPosInLinkSum / volumeSum)); // Store the volume _ecm.CreateComponent(_entity, components::Volume(volumeSum)); diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 6c2c43de51..a49d4ba583 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -17,9 +17,10 @@ #include "Imu.hh" +#include #include +#include #include -#include #include @@ -56,6 +57,11 @@ class ignition::gazebo::systems::ImuPrivate /// \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; + /// \brief Keep track of world ID, which is equivalent to the scene's /// root visual. /// Defaults to zero, which is considered invalid by Ignition Gazebo. @@ -64,21 +70,21 @@ class ignition::gazebo::systems::ImuPrivate /// True if the rendering component is initialized public: bool initialized = false; - /// \brief Create IMU sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateImuEntities(EntityComponentManager &_ecm); + /// \brief Create IMU sensors in ign-sensors + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update IMU sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. public: void Update(const EntityComponentManager &_ecm); /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _imu IMU component. /// \param[in] _parent Parent entity component. - public: void addIMU( - EntityComponentManager &_ecm, + public: void AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent); @@ -102,7 +108,21 @@ void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Imu::PreUpdate"); - this->dataPtr->CreateImuEntities(_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(); } ////////////////////////////////////////////////// @@ -119,6 +139,8 @@ void Imu::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) { @@ -135,8 +157,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void ImuPrivate::addIMU( - EntityComponentManager &_ecm, +void ImuPrivate::AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent) @@ -184,9 +206,6 @@ void ImuPrivate::addIMU( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetOrientationReference(p.Rot()); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - // Set whether orientation is enabled if (data.ImuSensor()) { @@ -196,10 +215,11 @@ void ImuPrivate::addIMU( this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) +void ImuPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("ImuPrivate::CreateImuEntities"); // Get World Entity @@ -219,7 +239,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); this->initialized = true; @@ -232,7 +252,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index a8a507fd9d..fbb5a986d0 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -230,8 +230,17 @@ void JointPositionController::PreUpdate( this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); } + // If the joint is still not found then warn the user, they may have entered + // the wrong joint name. if (this->dataPtr->jointEntity == kNullEntity) + { + static bool warned = false; + if(!warned) + ignerr << "Could not find joint with name [" + << this->dataPtr->jointName <<"]\n"; + warned = true; return; + } // Nothing left to do if paused. if (_info.paused) diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index c8ae68c649..9a051f4114 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -59,23 +60,28 @@ class ignition::gazebo::systems::LogicalCameraPrivate /// \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; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _logicalCamera LogicalCamera component. /// \param[in] _parent Parent entity component. public: void AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent); /// \brief Create logicalCamera sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update logicalCamera sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -101,7 +107,21 @@ void LogicalCamera::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCamera::PreUpdate"); - this->dataPtr->CreateLogicalCameraEntities(_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(); } ////////////////////////////////////////////////// @@ -118,6 +138,8 @@ void LogicalCamera::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) { @@ -135,7 +157,7 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void LogicalCameraPrivate::AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent) @@ -170,16 +192,13 @@ void LogicalCameraPrivate::AddLogicalCamera( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void LogicalCameraPrivate::CreateLogicalCameraEntities( - EntityComponentManager &_ecm) +void LogicalCameraPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities"); if (!this->initialized) @@ -190,7 +209,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); this->initialized = true; @@ -204,7 +223,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); } diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index a941468db7..fd2963e35a 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -56,25 +57,30 @@ class ignition::gazebo::systems::MagnetometerPrivate /// \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; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _magnetometer Magnetometer component. /// \param[in] _worldField MagneticField component. /// \param[in] _parent Parent entity component. public: void AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, const components::ParentEntity *_parent); /// \brief Create magnetometer sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateMagnetometerEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update magnetometer sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -100,7 +106,21 @@ void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Magnetometer::PreUpdate"); - this->dataPtr->CreateMagnetometerEntities(_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(); } ////////////////////////////////////////////////// @@ -117,6 +137,8 @@ void Magnetometer::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) { @@ -134,7 +156,7 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void MagnetometerPrivate::AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, @@ -176,16 +198,13 @@ void MagnetometerPrivate::AddMagnetometer( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetWorldPose(p); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void MagnetometerPrivate::CreateMagnetometerEntities( - EntityComponentManager &_ecm) +void MagnetometerPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("MagnetometerPrivate::CreateMagnetometerEntities"); auto worldEntity = _ecm.EntityByComponents(components::World()); @@ -211,7 +230,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); this->initialized = true; @@ -224,7 +244,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 5d802189b1..d5309a82c7 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -242,7 +242,6 @@ void SensorsPrivate::RunOnce() this->renderUtil.Update(); } - if (!this->activeSensors.empty()) { this->sensorMaskMutex.lock(); @@ -422,8 +421,9 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->renderUtil.SetEnableSensors(true, std::bind(&Sensors::CreateSensor, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - this->dataPtr->renderUtil.SetRemoveSensorCb( + this->dataPtr->renderUtil.SetRemoveSensorCb( std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1)); + this->dataPtr->renderUtil.SetEventManager(&_eventMgr); // parse sensor-specific data auto worldEntity = _ecm.EntityByComponents(components::World()); diff --git a/src/systems/shader_param/CMakeLists.txt b/src/systems/shader_param/CMakeLists.txt new file mode 100644 index 0000000000..0d14af33f3 --- /dev/null +++ b/src/systems/shader_param/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(shader-param + SOURCES + ShaderParam.cc + PUBLIC_LINK_LIBS + ${rendering_target} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc new file mode 100644 index 0000000000..973a7cac08 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.cc @@ -0,0 +1,440 @@ +/* + * Copyright (C) 2022 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 "ShaderParam.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::ShaderParamPrivate +{ + /// \brief Data structure for storing shader param info + public: class ShaderParamValue + { + /// \brief shader type: vertex or fragment + public: std::string shader; + + /// \brief variable type: int, float, float_array, int_array, + /// texture, texture_cube + public: std::string type; + + /// \brief variable name of param + public: std::string name; + + /// \brief param value + public: std::string value; + + /// \brief Any additional arguments + public: std::vector args; + }; + + /// \brief Path to vertex shader + public: std::string vertexShaderUri; + + /// \brief Path to fragment shader + public: std::string fragmentShaderUri; + + /// \brief Mutex to protect sim time updates. + public: std::mutex mutex; + + /// \brief Connection to pre-render event callback + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Name of visual this plugin is attached to + public: std::string visualName; + + /// \brief Pointer to visual + public: rendering::VisualPtr visual; + + /// \brief Material used by this visual + public: rendering::MaterialPtr material; + + /// \brief Pointer to scene + public: rendering::ScenePtr scene; + + /// \brief Entity id of the visual + public: Entity entity = kNullEntity; + + /// \brief A list of shader params + public: std::vector shaderParams; + + /// \brief Time params that will be updated every iteration + public: std::vector timeParams; + + /// \brief Current sim time + public: std::chrono::steady_clock::duration currentSimTime; + + /// \brief Path to model + public: std::string modelPath; + + /// \brief All rendering operations must happen within this call + public: void OnUpdate(); +}; + +///////////////////////////////////////////////// +ShaderParam::ShaderParam() + : System(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ShaderParam::~ShaderParam() +{ +} + +///////////////////////////////////////////////// +void ShaderParam::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + IGN_PROFILE("ShaderParam::Configure"); + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto sdf = const_cast(_sdf.get()); + + if (sdf->HasElement("param")) + { + // loop and parse all shader params + sdf::ElementPtr paramElem = sdf->GetElement("param"); + while (paramElem) + { + if (!paramElem->HasElement("shader") || + !paramElem->HasElement("name")) + { + ignerr << " must have and sdf elements" + << std::endl; + paramElem = paramElem->GetNextElement("param"); + continue; + } + std::string shaderType = paramElem->Get("shader"); + std::string paramName = paramElem->Get("name"); + + std::string type = paramElem->Get("type", "float").first; + std::string value = paramElem->Get("value", "").first; + + ShaderParamPrivate::ShaderParamValue spv; + spv.shader = shaderType; + spv.name = paramName; + spv.value = value; + spv.type = type; + + if (paramElem->HasElement("arg")) + { + sdf::ElementPtr argElem = paramElem->GetElement("arg"); + while (argElem) + { + spv.args.push_back(argElem->Get()); + argElem = argElem->GetNextElement("arg"); + } + } + + this->dataPtr->shaderParams.push_back(spv); + paramElem = paramElem->GetNextElement("param"); + } + } + + if (this->dataPtr->modelPath.empty()) + { + auto modelEntity = topLevelModel(_entity, _ecm); + this->dataPtr->modelPath = + _ecm.ComponentData(modelEntity).value(); + } + + // parse path to shaders + if (sdf->HasElement("shader")) + { + sdf::ElementPtr shaderElem = sdf->GetElement("shader"); + if (!shaderElem->HasElement("vertex") || + !shaderElem->HasElement("fragment")) + { + ignerr << " must have and sdf elements" + << std::endl; + } + else + { + sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); + this->dataPtr->vertexShaderUri = common::findFile( + asFullPath(vertexElem->Get(), + this->dataPtr->modelPath)); + sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); + this->dataPtr->fragmentShaderUri = common::findFile( + asFullPath(fragmentElem->Get(), + this->dataPtr->modelPath)); + } + } + + this->dataPtr->entity = _entity; + auto nameComp = _ecm.Component(_entity); + this->dataPtr->visualName = nameComp->Data(); + + // connect to the SceneUpdate event + // the callback is executed in the rendering thread so do all + // rendering operations in that thread + this->dataPtr->connection = + _eventMgr.Connect( + std::bind(&ShaderParamPrivate::OnUpdate, this->dataPtr.get())); +} + +////////////////////////////////////////////////// +void ShaderParam::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &) +{ + IGN_PROFILE("ShaderParam::PreUpdate"); + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->currentSimTime = _info.simTime; +} + +////////////////////////////////////////////////// +void ShaderParamPrivate::OnUpdate() +{ + std::lock_guard lock(this->mutex); + if (this->visualName.empty()) + return; + + if (!this->scene) + this->scene = rendering::sceneFromFirstRenderEngine(); + + if (!this->scene) + return; + + if (!this->visual) + { + // this does a breadth first search for visual with the entity id + // \todo(anyone) provide a helper function in RenderUtil to search for + // visual by entity id? + auto rootVis = scene->RootVisual(); + std::list nodes; + nodes.push_back(rootVis); + while (!nodes.empty()) + { + auto n = nodes.front(); + nodes.pop_front(); + if (n && n->HasUserData("gazebo-entity")) + { + // RenderUti stores gazebo-entity user data as int + // \todo(anyone) Change this to uint64_t in Ignition H? + auto variant = n->UserData("gazebo-entity"); + const int *value = std::get_if(&variant); + if (value && *value == static_cast(this->entity)) + { + this->visual = std::dynamic_pointer_cast(n); + break; + } + } + for (unsigned int i = 0; i < n->ChildCount(); ++i) + nodes.push_back(n->ChildByIndex(i)); + } + } + + if (!this->visual) + return; + + // get the material and set shaders + if (!this->material) + { + auto mat = scene->CreateMaterial(); + mat->SetVertexShader(this->vertexShaderUri); + mat->SetFragmentShader(this->fragmentShaderUri); + this->visual->SetMaterial(mat); + scene->DestroyMaterial(mat); + this->material = this->visual->Material(); + } + + if (!this->material) + return; + + // set the shader params read from SDF + // this is only done once + for (const auto & spv : this->shaderParams) + { + // TIME is reserved keyword for sim time + if (spv.value == "TIME") + { + this->timeParams.push_back(spv); + continue; + } + + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + { + params = this->material->FragmentShaderParams(); + } + else if (spv.shader == "vertex") + { + params = this->material->VertexShaderParams(); + } + + // if no is specified, this could be a constant + if (spv.value.empty()) + { + // \todo handle args for constants in ign-rendering + (*params)[spv.name] = 1; + continue; + } + + // handle texture params + if (spv.type == "texture") + { + unsigned int uvSetIndex = spv.args.empty() ? 0u : + static_cast(std::stoul(spv.args[0])); + std::string texPath = common::findFile( + asFullPath(spv.value, this->modelPath)); + (*params)[spv.name].SetTexture(texPath, + rendering::ShaderParam::ParamType::PARAM_TEXTURE, uvSetIndex); + } + else if (spv.type == "texture_cube") + { + unsigned int uvSetIndex = spv.args.empty() ? 0u : + static_cast(std::stoul(spv.args[0])); + std::string texPath = common::findFile( + asFullPath(spv.value, this->modelPath)); + (*params)[spv.name].SetTexture(texPath, + rendering::ShaderParam::ParamType::PARAM_TEXTURE_CUBE, uvSetIndex); + } + // handle int, float, int_array, and float_array params + else + { + std::vector values = common::split(spv.value, " "); + + int intValue = 0; + float floatValue = 0; + std::vector floatArrayValue; + + rendering::ShaderParam::ParamType paramType = + rendering::ShaderParam::PARAM_NONE; + + // float / int + if (values.size() == 1u) + { + std::string str = values[0]; + + // if is not empty, respect the specified type + if (!spv.type.empty()) + { + if (spv.type == "int") + { + intValue = std::stoi(str); + paramType = rendering::ShaderParam::PARAM_INT; + } + else if (spv.type == "float") + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + } + // else do our best guess at what the type is + else + { + std::string::size_type sz; + int n = std::stoi(str, &sz); + if ( sz == str.size()) + { + intValue = n; + paramType = rendering::ShaderParam::PARAM_INT; + } + else + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + } + } + // arrays + else + { + // int array + if (!spv.type.empty() && spv.type == "int_array") + { + for (const auto &v : values) + floatArrayValue.push_back(std::stoi(v)); + paramType = rendering::ShaderParam::PARAM_INT_BUFFER; + } + // treat everything else as float_array + else + { + for (const auto &v : values) + floatArrayValue.push_back(std::stof(v)); + paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; + } + } + + // set the params + if (paramType == rendering::ShaderParam::PARAM_INT) + { + (*params)[spv.name] = intValue; + } + else if (paramType == rendering::ShaderParam::PARAM_FLOAT) + { + (*params)[spv.name] = floatValue; + } + else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || + paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) + { + (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); + float *fv = &floatArrayValue[0]; + (*params)[spv.name].UpdateBuffer(fv); + } + } + } + this->shaderParams.clear(); + + // time variables need to be updated every iteration + for (const auto & spv : this->timeParams) + { + float floatValue = (std::chrono::duration_cast( + this->currentSimTime).count()) * 1e-9; + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + params = this->material->FragmentShaderParams(); + else if (spv.shader == "vertex") + params = this->material->VertexShaderParams(); + (*params)[spv.name] = floatValue; + } +} + +IGNITION_ADD_PLUGIN(ShaderParam, + ignition::gazebo::System, + ShaderParam::ISystemConfigure, + ShaderParam::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ShaderParam, + "ignition::gazebo::systems::ShaderParam") diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh new file mode 100644 index 0000000000..d77e8bfd78 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 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_SHADER_PARAM_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ShaderParamPrivate; + + /// \brief A plugin for setting shaders to a visual and its params + /// + /// Plugin parameters: + /// + /// + /// Path to vertex program + /// Path to fragment program + /// Shader parameter - can be repeated within plugin SDF element + /// Name of uniform variable bound to the shader + /// Type of shader, i.e. vertex, fragment + /// Variable type: float, int, float_array, int_array + /// Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be 'TIME', in which case the value will + /// be bound to sim time. + /// + /// Example usage: + /// + /// \verbatim + /// + /// + /// materials/my_vs.glsl + /// materials/my_fs.glsl + /// + /// + /// + /// ambient + /// fragment + /// float_array + /// 1.0 0.0 0.0 1.0 + /// + /// + /// \endverbatim + class ShaderParam + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ShaderParam(); + + /// \brief Destructor + public: ~ShaderParam() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index d36be2b415..f971bb0200 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -92,12 +92,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// and writes the angular velocity directly to the joint. default: false public: bool velocityControl = false; - /// \brief Maximum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Maximum input force [N] for the propellerController, + /// default: 1000N public: double cmdMax = 1000; - /// \brief Minimum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Minimum input force [N] for the propellerController, + /// default: -1000N public: double cmdMin = -1000; /// \brief Thrust coefficient relating the propeller angular velocity to the @@ -266,6 +266,29 @@ void Thruster::Configure( enableComponent(_ecm, this->dataPtr->linkEntity); + double minThrustCmd = this->dataPtr->cmdMin; + double maxThrustCmd = this->dataPtr->cmdMax; + if (_sdf->HasElement("max_thrust_cmd")) + { + maxThrustCmd = _sdf->Get("max_thrust_cmd"); + } + if (_sdf->HasElement("min_thrust_cmd")) + { + minThrustCmd = _sdf->Get("min_thrust_cmd"); + } + if (maxThrustCmd < minThrustCmd) + { + ignerr << " must be greater than or equal to " + << ". Revert to using default values: " + << "min: " << this->dataPtr->cmdMin << ", " + << "max: " << this->dataPtr->cmdMax << std::endl; + } + else + { + this->dataPtr->cmdMax = maxThrustCmd; + this->dataPtr->cmdMin = minThrustCmd; + } + if (_sdf->HasElement("velocity_control")) { this->dataPtr->velocityControl = _sdf->Get("velocity_control"); diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index d62a8c9f1d..986b53a836 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -76,6 +76,10 @@ namespace systems /// no units, defaults to 0.0] /// - - Derivative gain for joint PID controller. [Optional, /// no units, defaults to 0.0] + /// - - Maximum thrust command. [Optional, + /// defaults to 1000N] + /// - - Minimum thrust command. [Optional, + /// defaults to -1000N] /// /// ## Example /// An example configuration is installed with Gazebo. The example diff --git a/src/systems/trajectory_follower/CMakeLists.txt b/src/systems/trajectory_follower/CMakeLists.txt new file mode 100644 index 0000000000..419aa55429 --- /dev/null +++ b/src/systems/trajectory_follower/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(trajectory-follower + SOURCES + TrajectoryFollower.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc new file mode 100644 index 0000000000..85c804e0d7 --- /dev/null +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -0,0 +1,359 @@ +/* + * Copyright (C) 2022 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 +#include +#include +#include + +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "TrajectoryFollower.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::TrajectoryFollowerPrivate +{ + /// \brief Initialize the plugin. + /// \param[in] _ecm Immutable reference to the EntityComponentManager. + /// \param[in] _sdf The SDF Element associated with this system plugin. + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); + + /// \brief The link entity + public: ignition::gazebo::Link link; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The initial pose of the model relative to the world frame. + public: ignition::math::Pose3 modelPose; + + /// \brief True if the model should continue looping though the waypoints. + public: bool loopForever = false; + + /// \brief Linear force to apply to the model in its X direction. + public: double forceToApply = 60; + + /// \brief Torque to apply to the model to align it with the next goal. + public: double torqueToApply = 50; + + /// \brief When the model is at this distance or closer we won't try to move. + /// Units are in meters. + public: double rangeTolerance = 0.5; + + /// \brief When the model is at this angle or closer we won't try to rotate. + /// Units are in degrees. + public: double bearingTolerance = 2.0; + + /// \brief Use to sample waypoints around a circle. + public: unsigned int numSamples = 8u; + + /// \brief The next position to reach. + public: ignition::math::Vector3d nextGoal; + + /// \brief Vector containing waypoints as 3D vectors of doubles representing + /// X Y, where X and Y are local (Gazebo) coordinates. + public: std::vector localWaypoints; + + /// \brief Initialization flag. + public: bool initialized{false}; + + /// \brief Copy of the sdf configuration used for this plugin + public: sdf::ElementPtr sdfConfig; +}; + +////////////////////////////////////////////////// +void TrajectoryFollowerPrivate::Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf) +{ + // Parse required elements. + if (!_sdf->HasElement("link_name")) + { + ignerr << "No specified" << std::endl; + return; + } + + std::string linkName = _sdf->Get("link_name"); + this->link = Link(this->model.LinkByName(_ecm, linkName)); + if (!this->link.Valid(_ecm)) + { + ignerr << "Could not find link named [" << linkName + << "] in model" << std::endl; + return; + } + + this->modelPose = ignition::gazebo::worldPose(this->link.Entity(), _ecm); + this->modelPose.Pos().Z() = 0; + + // Parse the optional element. + if (_sdf->HasElement("waypoints")) + { + auto waypointsElem = _sdf->GetElement("waypoints"); + + // We need at least one waypoint + if (!waypointsElem->HasElement("waypoint")) + { + ignerr << "TrajectoryFollower: Unable to find " + << "element in SDF." << std::endl; + return; + } + auto waypointElem = waypointsElem->GetElement("waypoint"); + while (waypointElem) + { + ignition::math::Vector2d position = + waypointElem->Get(); + + // Save the position. + this->localWaypoints.push_back(position); + + // Print some debugging messages + igndbg << "Waypoint, Local: X = " << position.X() + << " Y = " << position.Y() << std::endl; + + waypointElem = waypointElem->GetNextElement("waypoint"); + } + } + // If no waypoints present, check for the element and parse. + else if (_sdf->HasElement("circle")) + { + igndbg << "Circle element activated" << std::endl; + auto circleElem = _sdf->GetElement("circle"); + + if (!circleElem->HasElement("radius")) + { + ignerr << "No specified" << std::endl; + return; + } + + // Parse the required field. + double radius = circleElem->Get("radius"); + + // Get the current model position in global coordinates. Create + // local vectors that represent a path along a rough circle. + ignition::math::Vector2d position(this->modelPose.Pos().X(), + this->modelPose.Pos().Y()); + double angle = 0; + ignition::math::Vector2d vec(radius, 0); + for (unsigned int i = 0u; i < this->numSamples; ++i) + { + // Add the local vector to the current position. + // Store global position as a waypoint. + this->localWaypoints.push_back(position + vec); + angle += 2 * IGN_PI / this->numSamples; + vec.Set(radius * cos(angle), radius * sin(angle)); + igndbg << "Entered circle waypoint " << position + vec << std::endl; + } + } + // If no waypoints or circle, check for the element and parse. + else if (_sdf->HasElement("line")) + { + auto lineElem = _sdf->GetElement("line"); + // Parse the required field. + if (!lineElem->HasElement("direction")) + { + ignerr << "No specified" << std::endl; + return; + } + ignition::math::Angle direction = + lineElem->Get("direction"); + + // Parse the required field. + if (!lineElem->HasElement("length")) + { + ignerr << "No specified" << std::endl; + return; + } + auto length = lineElem->Get("length"); + + // Create a relative vector in the direction of "direction" and of + // length "length". + ignition::math::Vector3d lineVec( + length * cos(direction.Radian()), + length * sin(direction.Radian()), 0); + ignition::math::Vector2d position(this->modelPose.Pos().X(), + this->modelPose.Pos().Y()); + // Add the initial model position and calculated endpoint as waypoints. + this->localWaypoints.push_back(position); + ignition::math::Vector3d p = this->modelPose.CoordPositionAdd(lineVec); + ignition::math::Vector2d p2D = {p.X(), p.Y()}; + this->localWaypoints.push_back(p2D); + igndbg << "Entered line waypoints " << position << ", " << p2D << std::endl; + } + + // Parse the optional element. + if (_sdf->HasElement("loop")) + this->loopForever = _sdf->Get("loop"); + + // Parse the optional element. + if (_sdf->HasElement("force")) + this->forceToApply = _sdf->Get("force"); + + // Parse the optional element. + if (_sdf->HasElement("torque")) + this->torqueToApply = _sdf->Get("torque"); + + // Parse the optional element. + if (_sdf->HasElement("range_tolerance")) + this->rangeTolerance = _sdf->Get("range_tolerance"); + + // Parse the optional element. + if (_sdf->HasElement("bearing_tolerance")) + this->bearingTolerance = _sdf->Get("bearing_tolerance"); + + // If we have waypoints to visit, read the first one. + if (!this->localWaypoints.empty()) + { + this->nextGoal = + {this->localWaypoints.front().X(), this->localWaypoints.front().Y(), 0}; + } +} + +////////////////////////////////////////////////// +TrajectoryFollower::TrajectoryFollower() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void TrajectoryFollower::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + this->dataPtr->sdfConfig = _sdf->Clone(); +} + +////////////////////////////////////////////////// +void TrajectoryFollower::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrajectoryFollower::PreUpdate"); + + if (_info.paused) + return; + + if (!this->dataPtr->initialized) + { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called + this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + enableComponent(_ecm, this->dataPtr->link.Entity()); + this->dataPtr->initialized = true; + } + + // Nothing to do. + if (this->dataPtr->localWaypoints.empty()) + return; + + this->dataPtr->modelPose = ignition::gazebo::worldPose( + this->dataPtr->link.Entity(), _ecm); + this->dataPtr->modelPose.Pos().Z() = 0; + + // Direction vector to the goal from the model. + ignition::math::Vector3d direction = + this->dataPtr->nextGoal - this->dataPtr->modelPose.Pos(); + + // Direction vector in the local frame of the model. + ignition::math::Vector3d directionLocalFrame = + this->dataPtr->modelPose.Rot().RotateVectorReverse(direction); + + double range = directionLocalFrame.Length(); + ignition::math::Angle bearing( + atan2(directionLocalFrame.Y(), directionLocalFrame.X())); + bearing.Normalize(); + + // Waypoint reached! + if (range <= this->dataPtr->rangeTolerance) + { + // We always keep the last waypoint in the vector to keep the model + // "alive" in case it moves away from its goal. + if (this->dataPtr->localWaypoints.size() == 1) + return; + + if (this->dataPtr->loopForever) + { + // Rotate to the left. + std::rotate(this->dataPtr->localWaypoints.begin(), + this->dataPtr->localWaypoints.begin() + 1, + this->dataPtr->localWaypoints.end()); + } + else + { + // Remove the first waypoint. + this->dataPtr->localWaypoints.erase( + this->dataPtr->localWaypoints.begin()); + } + + this->dataPtr->nextGoal = { + this->dataPtr->localWaypoints.front().X(), + this->dataPtr->localWaypoints.front().Y(), 0}; + + return; + } + + // Transform from world to local frame. + auto comPose = this->dataPtr->link.WorldInertialPose(_ecm); + if (!comPose.has_value()) + { + ignerr << "Failed to get CoM pose for link [" + << this->dataPtr->link.Entity() << "]" << std::endl; + return; + } + + // Transform the force and torque to the world frame. + // Move commands. The vehicle always move forward (X direction). + ignition::math::Vector3d forceWorld; + if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance) + { + forceWorld = (*comPose).Rot().RotateVector( + ignition::math::Vector3d(this->dataPtr->forceToApply, 0, 0)); + } + + ignition::math::Vector3d torqueWorld; + if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance) + { + int sign = std::abs(bearing.Degree()) / bearing.Degree(); + torqueWorld = (*comPose).Rot().RotateVector( + ignition::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply)); + } + + // Apply the force and torque at COM. + this->dataPtr->link.AddWorldWrench(_ecm, forceWorld, torqueWorld); +} + +IGNITION_ADD_PLUGIN(TrajectoryFollower, + ignition::gazebo::System, + TrajectoryFollower::ISystemConfigure, + TrajectoryFollower::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrajectoryFollower, + "ignition::gazebo::systems::TrajectoryFollower") diff --git a/src/systems/trajectory_follower/TrajectoryFollower.hh b/src/systems/trajectory_follower/TrajectoryFollower.hh new file mode 100644 index 0000000000..c1d52aca06 --- /dev/null +++ b/src/systems/trajectory_follower/TrajectoryFollower.hh @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2022 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_TRAJECTORYFOLLOWER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRAJECTORYFOLLOWER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrajectoryFollowerPrivate; + + /// \brief A plugin that scripts the movement of a model based on waypoints. + /// Note that at this point, the trajectory is always in 2D (x, y). + /// The plugin applies a torque until the model is aligned with the next + /// waypoint. Then, it applies a force until the model is close enough to + /// the waypoint. + /// + /// This plugin loads a set of waypoints via SDF and traverse the waypoints + /// in sequence. The movement is generated applying force and torque to one of + /// of the model links. It's also possible to loop through the waypoints for + /// generating a never ending trajectory. + /// Waypoints may be inserted manually via the element, or + /// generated relative to the model's initial position via the or + /// elements. Only one of these options should be used. + /// + /// This plugin requires the following SDF parameters: + /// * Required parameters: + /// : The name of the link within the model where the force/torque + /// will be applied when moving the vehicle. + /// + /// * Optional parameters: + /// : When true, all waypoints will be visited continously in a + /// circular pattern. If false, the model will stop when the + /// last waypoint is reached. Note, that if the vehicle moves, + /// it will still try to reach the very last waypoint. + /// : Element specifying the set of waypoints that the + /// the model should navigate through. This block should contain + /// at least one of these blocks: + /// : This block should contain the X, Y of a + /// waypoint. + /// : The current waypoint is considered reached if the + /// distance to it is within +- this tolerance (m). + /// The default value is 0.5m. + /// : If the bearing to the current waypoint is within + /// +- this tolerance, a torque won't be applied (degree) + /// The default value is 2 deg. + /// : The force to apply at every plugin iteration in the X direction + /// of the link (N). The default value is 60. + /// : The torque to apply at every plugin iteration in the Yaw + /// direction of the link (Nm). The default value is 50. + /// : Element that indicates the model should travel in "line" mode. + /// The block should contain the relative direction and distance from + /// the initial position in which the vehicle should move, specified + /// in the world frame. + /// : Relative direction (radians) in the world frame for + /// the vehicle to travel. + /// : Distance (meters) for the vehicle to travel. + /// : Element that indicates the model should travel in "circle" mode. + /// The block should contain the desired radius of the circle about + /// the vehicle's initial position + /// : Radius (meters) of circular path to travel. + /// + /// Here are three examples: + // + // base_link + // true + // + // 25 0 + // 15 0 + // + // + // + // base_link + // true + // + // 0 + // 5 + // + // + // + // base_link + // true + // + // 2 + // + // + class TrajectoryFollower + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: TrajectoryFollower(); + + /// \brief Destructor + public: ~TrajectoryFollower() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 938836f669..ee4c22f45f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -75,6 +75,7 @@ set(tests_needing_display optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc + shader_param_system.cc thermal_system.cc thermal_sensor_system.cc ) diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 2036f92041..3db386e4d0 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -64,7 +64,7 @@ TEST_F(AirPressureTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(AirPressure)) // Create a system that checks sensor topic test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each( @@ -77,6 +77,10 @@ TEST_F(AirPressureTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(AirPressure)) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 0219862050..2c2f1dae26 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -82,7 +82,7 @@ TEST_F(AltimeterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) test::Relay testSystem; std::vector poses; std::vector velocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 39dc2a7bb4..807b2c475f 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -21,8 +21,10 @@ #include #include +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" #include "ignition/gazebo/components/CenterOfVolume.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" @@ -154,7 +156,8 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments)) EXPECT_NEAR(maxUniformRoll, 0.11, 1e-1); EXPECT_NEAR(minUniformRoll, minGradedRoll, 1e-1); // Emperically derived - EXPECT_NEAR(minUniformRoll, -0.15, 1e-1); + // added extra tol (3e-5) after fixing center of volume's reference frame + EXPECT_NEAR(minUniformRoll, -0.15, 1e-1 + 3e-5); } ///////////////////////////////////////////////// @@ -396,3 +399,77 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedBuoyancy)) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetAndRotation)) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "center_of_volume.sdf")); + + std::size_t iterations{0}; + fixture.OnPostUpdate([&]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get links + auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); + ASSERT_EQ(1u, noOffsets.size()); + auto noOffset = *noOffsets.begin(); + EXPECT_NE(kNullEntity, noOffset); + + auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", + _ecm); + ASSERT_EQ(1u, noOffsetRotateds.size()); + auto noOffsetRotated = *noOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, noOffsetRotated); + + auto withOffsets = entitiesFromScopedName("com_cov_offset::link", _ecm); + ASSERT_EQ(1u, withOffsets.size()); + auto withOffset = *withOffsets.begin(); + EXPECT_NE(kNullEntity, withOffset); + + auto withOffsetRotateds = entitiesFromScopedName( + "com_cov_offset_rotated::link", _ecm); + ASSERT_EQ(1u, withOffsetRotateds.size()); + auto withOffsetRotated = *withOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, withOffsetRotated); + + // Check CoVs have correct offsets + auto noOffsetCoV = _ecm.Component(noOffset); + ASSERT_NE(noOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data()); + + auto noOffsetRotatedCoV = _ecm.Component( + noOffsetRotated); + ASSERT_NE(noOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data()); + + auto withOffsetCoV = _ecm.Component(withOffset); + ASSERT_NE(withOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data()); + + auto withOffsetRotatedCoV = _ecm.Component( + withOffsetRotated); + ASSERT_NE(withOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data()); + + // Check that all objects are neutrally buoyant and stay still + auto noOffsetPose = worldPose(noOffset, _ecm); + EXPECT_EQ(math::Pose3d(), noOffsetPose); + + auto noOffsetRotatedPose = worldPose(noOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 0, 0, 0.1, 0.2, 0.3), noOffsetRotatedPose); + + auto withOffsetPose = worldPose(withOffset, _ecm); + EXPECT_EQ(math::Pose3d(0, 3, 0, 0, 0, 0), withOffsetPose); + + auto withOffsetRotatedPose = worldPose(withOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 3, 0, 0.1, 0.2, 0.3), withOffsetRotatedPose); + + iterations++; + }).Finalize(); + + std::size_t targetIterations{1000}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); +} diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 388da45bae..f6664d8135 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -91,7 +91,7 @@ TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) std::vector poses; std::vector accelerations; std::vector angularVelocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/link.cc b/test/integration/link.cc index da64000838..53a1dc9f7c 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -468,6 +469,39 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy) EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkSetVelocity) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // No LinearVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + math::Vector3d linVel(1, 0, 0); + link.SetLinearVelocity(ecm, linVel); + + // LinearVelocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_EQ(linVel, + ecm.Component(eLink)->Data()); + + // Make sure the linear velocity is updated + math::Vector3d linVel2(0, 0, 0); + link.SetLinearVelocity(ecm, linVel2); + EXPECT_EQ(linVel2, + ecm.Component(eLink)->Data()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkAddWorldForce) { diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index e87306172d..e9716dc52c 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -93,7 +93,7 @@ TEST_F(LogicalCameraTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox)) // Create a system that checks sensor topics test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each( @@ -107,11 +107,15 @@ TEST_F(LogicalCameraTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox)) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic1, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic1, topicComp->Data()); + } } update1Checked = true; } @@ -121,11 +125,15 @@ TEST_F(LogicalCameraTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox)) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic2, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic2, topicComp->Data()); + } } update2Checked = true; } diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index 1b3b53464e..9c332f24ba 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -83,7 +83,7 @@ TEST_F(MagnetometerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RotatedMagnetometer)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/shader_param_system.cc b/test/integration/shader_param_system.cc new file mode 100644 index 0000000000..906133e937 --- /dev/null +++ b/test/integration/shader_param_system.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 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/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test ShaderParamTest system +class ShaderParamTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + mutex.lock(); + unsigned int channels = 3u; + unsigned int imageSamples = _msg.width() * _msg.height(); + unsigned int imageBufferSize = imageSamples * sizeof(unsigned char) + * channels; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples * channels]; + memcpy(imageBuffer, _msg.data().c_str(), imageBufferSize); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks camera image data to verify that the sphere is using +// custom material shaders +TEST_F(ShaderParamTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ShaderParam)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "shader_param.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the image camera topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + // Run server and verify that we are receiving a message + // from the image camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + // shaders set the sphere color to red + unsigned int height = 320; + unsigned int width = 240; + + int mid = (height / 2 * width * 3u) + (width / 2 - 1) * 3u; + + // Lock access to buffer and don't release it + mutex.lock(); + int r = static_cast(imageBuffer[mid]); + int g = static_cast(imageBuffer[mid+1]); + int b = static_cast(imageBuffer[mid+2]); + EXPECT_GT(r, g); + EXPECT_GT(r, b); + EXPECT_EQ(g, b); + + delete[] imageBuffer; +} diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index abcc6c6707..749aaea60e 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -147,6 +147,22 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); + // input force cmd - this should be capped to 0 + double forceCmd{-1000.0}; + msgs::Double msg; + msg.set_data(forceCmd); + pub.Publish(msg); + + // Check no movement + fixture.Server()->Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); + EXPECT_EQ(100u, modelPoses.size()); + EXPECT_EQ(100u, propellerAngVels.size()); + modelPoses.clear(); + propellerAngVels.clear(); + + // max allowed force double force{300.0}; // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 @@ -154,7 +170,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4))); - msgs::Double msg; + msg.Clear(); if(!_useAngVelCmd) { msg.set_data(force); diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 19306775b4..554589094a 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -13,6 +13,7 @@ set (test_plugins EventTriggerSystem TestModelSystem TestSensorSystem + TestVisualSystem TestWorldSystem MockSystem Null diff --git a/test/plugins/TestVisualSystem.cc b/test/plugins/TestVisualSystem.cc new file mode 100644 index 0000000000..9cc3741117 --- /dev/null +++ b/test/plugins/TestVisualSystem.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2022 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 "TestVisualSystem.hh" + +#include + +IGNITION_ADD_PLUGIN(ignition::gazebo::TestVisualSystem, + ignition::gazebo::System, + ignition::gazebo::TestVisualSystem::ISystemConfigure) diff --git a/test/plugins/TestVisualSystem.hh b/test/plugins/TestVisualSystem.hh new file mode 100644 index 0000000000..e8cdf4a45f --- /dev/null +++ b/test/plugins/TestVisualSystem.hh @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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_TEST_TESTVISUALSYSTEM_HH_ +#define IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ +using VisualPluginComponent = components::Component; +IGN_GAZEBO_REGISTER_COMPONENT("VisualPluginComponent", + VisualPluginComponent) +} +} + +class TestVisualSystem : + public System, + public ISystemConfigure +{ + public: TestVisualSystem() + { + igndbg << "Constructing TestVisualSystem" << std::endl; + } + + public: ~TestVisualSystem() + { + igndbg << "Destroying TestVisualSystem" << std::endl; + } + + private: bool Service(msgs::StringMsg &_msg) + { + igndbg << "TestVisualSystem service called" << std::endl; + _msg.set_data("TestVisualSystem"); + return true; + } + + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventManager*/) override + { + igndbg << "Configuring TestVisualSystem" << std::endl; + auto value = _sdf->Get("visual_key"); + _ecm.CreateComponent(_entity, + components::VisualPluginComponent(value)); + + // Create a test service + this->node.Advertise("/test/service/visual", + &TestVisualSystem::Service, this); + } + + private: transport::Node node; +}; +} +} + +#endif diff --git a/test/worlds/center_of_volume.sdf b/test/worlds/center_of_volume.sdf new file mode 100644 index 0000000000..0972ca554d --- /dev/null +++ b/test/worlds/center_of_volume.sdf @@ -0,0 +1,160 @@ + + + + + + 0 + + + + + 1000 + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 0 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + 0 3 0 0 0 0 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 3 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + + diff --git a/test/worlds/plugins.sdf b/test/worlds/plugins.sdf index 466215d186..d4668e294d 100644 --- a/test/worlds/plugins.sdf +++ b/test/worlds/plugins.sdf @@ -25,6 +25,13 @@ 456 + + + 890 + + diff --git a/test/worlds/thruster_ang_vel_cmd.sdf b/test/worlds/thruster_ang_vel_cmd.sdf index c345259735..28185e1ec5 100644 --- a/test/worlds/thruster_ang_vel_cmd.sdf +++ b/test/worlds/thruster_ang_vel_cmd.sdf @@ -104,6 +104,8 @@ 0.2 true true + 300 + 0 diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf index c18becc7d0..540c73170b 100644 --- a/test/worlds/thruster_pid.sdf +++ b/test/worlds/thruster_pid.sdf @@ -104,6 +104,8 @@ 0.004 1000 0.2 + 300 + 0 diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf index f78236a8ca..1850eac8f4 100644 --- a/test/worlds/thruster_vel_cmd.sdf +++ b/test/worlds/thruster_vel_cmd.sdf @@ -103,6 +103,8 @@ 950 0.25 true + 300 + 0 diff --git a/tutorials/install.md b/tutorials/install.md index 97d4e598ac..3260f10154 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -59,33 +59,36 @@ feature which hasn't been released yet. ### Ubuntu 18.04 or above -1. Enable the Ignition software repositories: +1. Install tools + ``` + sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + ``` + +2. Enable the Ignition software repositories: ``` sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt-get update ``` -2. Install package dependencies: +3. Clone repository ``` git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> - export SYSTEM_VERSION=bionic - sudo apt -y install \ - $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/ignition\|sdf/d' | tr '\n' ' ') ``` -3. (Ubuntu 18.04 only) Configure gcc8 +4. Install package dependencies (including other Ignition libraries): ``` - sudo apt-get install g++-8 - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 + sudo apt -y install \ + $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | tr '\n' ' ')) ``` -4. Clone the repository if you haven't already. +5. (Ubuntu 18.04 only) Configure gcc8 ``` - git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> + sudo apt-get install g++-8 + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 ``` -5. Configure and build. +6. Configure and build. ``` cd ign-gazebo mkdir build