From 6f58c919a95e8ab03864d25af022599e64bc090f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 18 Jan 2022 08:57:19 -0800 Subject: [PATCH 1/4] =?UTF-8?q?Prevent=20GzScene3D=20=F0=9F=92=A5=20if=20a?= =?UTF-8?q?nother=20scene=20is=20already=20loaded=20(#1294)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- src/gui/plugins/scene3d/GzScene3D.qml | 19 ++++-- src/gui/plugins/scene3d/Scene3D.cc | 87 +++++++++++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 44 +++++++++++++- 3 files changed, 112 insertions(+), 38 deletions(-) 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 09ae8c5919..429294f7d4 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1643,17 +1643,25 @@ 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.SetUseCurrentGLContext(true); this->dataPtr->renderUtil.Init(); rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); if (!scene) - return; + return "Failed to create a 3D scene."; auto root = scene->RootVisual(); @@ -1674,6 +1682,7 @@ void IgnRenderer::Initialize() this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); this->initialized = true; + return std::string(); } ///////////////////////////////////////////////// @@ -2066,6 +2075,12 @@ RenderThread::RenderThread() qRegisterMetaType(); } +///////////////////////////////////////////////// +void RenderThread::SetErrorCb(std::function _cb) +{ + this->errorCb = _cb; +} + ///////////////////////////////////////////////// void RenderThread::RenderNext() { @@ -2074,7 +2089,12 @@ void RenderThread::RenderNext() 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 @@ -2092,18 +2112,24 @@ void RenderThread::RenderNext() ///////////////////////////////////////////////// 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->moveToThread(QGuiApplication::instance()->thread()); + if (this->ignRenderer.initialized) + this->moveToThread(QGuiApplication::instance()->thread()); } @@ -2203,16 +2229,6 @@ 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. - static bool done{false}; - if (done) - { - return; - } - done = true; - this->setAcceptedMouseButtons(Qt::AllButtons); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); @@ -2378,18 +2394,6 @@ 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. - static bool done{false}; - if (done) - { - ignerr << "Only one Scene3D is supported per process at the moment." - << std::endl; - return; - } - done = true; - auto renderWindow = this->PluginItem()->findChild(); if (!renderWindow) { @@ -2397,6 +2401,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"; @@ -2891,6 +2897,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) { @@ -3180,6 +3199,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 1f743a4323..37b95f5db7 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(); @@ -185,6 +196,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// the connection to work on the QML side signals: void popupError(); + /// \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; @@ -210,7 +235,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Render(); /// \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(); @@ -519,6 +546,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _size Size of the texture signals: void TextureReady(int _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; @@ -726,6 +760,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; From 650b74658d0b8713de169b25a2e24c6d9c3e7ec3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 18 Jan 2022 08:59:02 -0800 Subject: [PATCH 2/4] Remove EachNew calls from sensor PreUpdates (#1281) Signed-off-by: Louise Poubel --- Migration.md | 4 ++ src/systems/air_pressure/AirPressure.cc | 46 ++++++++++++------ src/systems/altimeter/Altimeter.cc | 46 ++++++++++++------ src/systems/imu/Imu.cc | 52 ++++++++++++++------- src/systems/logical_camera/LogicalCamera.cc | 45 ++++++++++++------ src/systems/magnetometer/Magnetometer.cc | 47 +++++++++++++------ test/integration/air_pressure_system.cc | 6 ++- test/integration/altimeter_system.cc | 6 ++- test/integration/imu_system.cc | 6 ++- test/integration/logical_camera_system.cc | 30 +++++++----- test/integration/magnetometer_system.cc | 6 ++- 11 files changed, 211 insertions(+), 83 deletions(-) diff --git a/Migration.md b/Migration.md index 012dbafdb8..52e498a41b 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,10 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## 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/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index cff6ea8858..4346cf1f1b 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); @@ -134,7 +156,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) @@ -171,15 +193,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) @@ -190,7 +210,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; @@ -203,7 +223,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 4cc12f12bc..1b34fc63c1 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) { @@ -135,7 +157,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) @@ -173,15 +195,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) @@ -192,7 +212,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; @@ -205,7 +225,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/imu/Imu.cc b/src/systems/imu/Imu.cc index d883f6cc91..a6a3ecf7d9 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) { @@ -137,8 +159,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) @@ -186,15 +208,13 @@ void ImuPrivate::addIMU( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetOrientationReference(p.Rot()); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - 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 @@ -214,7 +234,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; @@ -227,7 +247,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/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 3b53fb4c0c..fbed671f66 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) { @@ -137,7 +159,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) @@ -172,16 +194,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) @@ -192,7 +211,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; @@ -206,7 +225,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 ade0e3dd08..bf974caa82 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) { @@ -136,7 +158,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, @@ -178,16 +200,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()); @@ -213,7 +232,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; @@ -226,7 +246,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/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 51e9000d90..52ad96ab50 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -62,7 +62,7 @@ TEST_F(AirPressureTest, 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( @@ -75,6 +75,10 @@ TEST_F(AirPressureTest, 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 4b391963ac..bac80b7c25 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -80,7 +80,7 @@ TEST_F(AltimeterTest, 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/imu_system.cc b/test/integration/imu_system.cc index 68147f4e30..c38c3cf6c7 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -89,7 +89,7 @@ TEST_F(ImuTest, 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/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 32bae02ef5..4abcf00700 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -91,7 +91,7 @@ TEST_F(LogicalCameraTest, 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( @@ -105,11 +105,15 @@ TEST_F(LogicalCameraTest, 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; } @@ -119,11 +123,15 @@ TEST_F(LogicalCameraTest, 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 cf29f976f8..e17be9b9f7 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -81,7 +81,7 @@ TEST_F(MagnetometerTest, 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) From 749884db3f031f6159101c1d98880df6ac2deb46 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 21 Jan 2022 09:10:55 -0800 Subject: [PATCH 3/4] Buoyancy: fix center of volume's reference frame (#1302) Signed-off-by: Louise Poubel --- .../gazebo/components/CenterOfVolume.hh | 2 +- src/systems/buoyancy/Buoyancy.cc | 11 +- test/integration/buoyancy.cc | 76 +++++++++ test/worlds/center_of_volume.sdf | 160 ++++++++++++++++++ 4 files changed, 242 insertions(+), 7 deletions(-) create mode 100644 test/worlds/center_of_volume.sdf 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/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 81a880a21a..0e7a286583 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -151,7 +151,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosSum = + ignition::math::Vector3d weightedPosInLinkSum = ignition::math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision @@ -210,16 +210,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/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 77445fdd61..7acfee7a5e 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -20,8 +20,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" @@ -194,3 +196,77 @@ TEST_F(BuoyancyTest, Movement) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, 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/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 + + + + + + + + + From ebb3a9ad8a7484eeb97bf2585f7731e7d4225f9b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 28 Jan 2022 16:48:17 +0800 Subject: [PATCH 4/4] Log an error if JointPositionController cannot find the joint. (citadel retarget) (#1314) In the event a user enters the wrong name for a certain joint, the JointPositionController system will silently fail. This PR adds a simple error message that tells the user that the joint was not found. Signed-off-by: Arjo Chakravarty --- .../joint_position_controller/JointPositionController.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 067c622161..9530256eff 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -192,8 +192,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)