diff --git a/Migration.md b/Migration.md index 4fbe67cda4..58a4e5c815 100644 --- a/Migration.md +++ b/Migration.md @@ -135,6 +135,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/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/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/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 84a4a9443d..6f4188ede7 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 3b5bf3b1da..d72ca9847a 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 acda248ec0..843421ba6e 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 a1b5a84e4d..b573f5214b 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 20822d1c73..b46b77a424 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/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 6211434bc9..885f5ca209 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -79,12 +79,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 @@ -206,6 +206,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 ec1a607d21..78eafb849b 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -64,6 +64,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/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..9b2ca5d40d 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" @@ -396,3 +398,77 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedBuoyancy)) 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/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/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/thruster.cc b/test/integration/thruster.cc index af1ec159b8..d2333bc3f1 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); - double force{300.0}; + // input force cmd - this should be capped to 0 + double forceCmd{-1000.0}; msgs::Double msg; - msg.set_data(force); + 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(); + + // input force cmd this should be capped to 300 + forceCmd = 1000.0; + msg.set_data(forceCmd); pub.Publish(msg); // Check movement @@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + // max allowed force + double force{300.0}; + // F = m * a // s = a * t^2 / 2 // F = m * 2 * s / t^2 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/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