From 87a00752f1f9461db03aaaff4e085b97e663c9a0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 2 Dec 2021 07:26:44 -0800 Subject: [PATCH 1/3] Working on pose modification Signed-off-by: Nate Koenig --- .../component_inspector/AirPressure.cc | 4 +- .../plugins/component_inspector/Altimeter.cc | 4 +- .../component_inspector/ComponentInspector.cc | 16 +- src/gui/plugins/component_inspector/Imu.cc | 12 +- .../plugins/component_inspector/JointType.cc | 4 +- src/gui/plugins/component_inspector/Lidar.cc | 4 +- .../component_inspector/Magnetometer.cc | 6 +- .../component_inspector/ModelEditor.cc | 4 +- .../plugins/component_inspector/Pose3d.qml | 204 +++++++++--------- .../plugins/scene_manager/GzSceneManager.cc | 4 +- 10 files changed, 132 insertions(+), 130 deletions(-) diff --git a/src/gui/plugins/component_inspector/AirPressure.cc b/src/gui/plugins/component_inspector/AirPressure.cc index d9b9656598..c6487f53ef 100644 --- a/src/gui/plugins/component_inspector/AirPressure.cc +++ b/src/gui/plugins/component_inspector/AirPressure.cc @@ -68,7 +68,7 @@ Q_INVOKABLE void AirPressure::OnAirPressureNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); @@ -100,7 +100,7 @@ Q_INVOKABLE void AirPressure::OnAirPressureReferenceAltitude( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); diff --git a/src/gui/plugins/component_inspector/Altimeter.cc b/src/gui/plugins/component_inspector/Altimeter.cc index fca5263475..0179e81c39 100644 --- a/src/gui/plugins/component_inspector/Altimeter.cc +++ b/src/gui/plugins/component_inspector/Altimeter.cc @@ -72,7 +72,7 @@ Q_INVOKABLE void Altimeter::OnAltimeterPositionNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Altimeter *altimeter = comp->Data().AltimeterSensor(); @@ -106,7 +106,7 @@ Q_INVOKABLE void Altimeter::OnAltimeterVelocityNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Altimeter *altimeter = comp->Data().AltimeterSensor(); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index cb29338b8d..a4f6449961 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1127,8 +1127,8 @@ void ComponentInspector::SetPaused(bool _paused) void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + /*std::function cb = + [](const ignition::msgs::Boolean &_rep, const bool _result) { if (!_result) ignerr << "Error setting pose" << std::endl; @@ -1141,6 +1141,18 @@ void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, auto poseCmdService = "/world/" + this->dataPtr->worldName + "/set_pose"; this->dataPtr->node.Request(poseCmdService, req, cb); + */ + + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component(this->GetEntity()); + if (comp) + comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw); + else + ignerr << "Unable to get the pose component.\n"; + }; + this->AddUpdateCallback(cb); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/Imu.cc b/src/gui/plugins/component_inspector/Imu.cc index 6949f9ba1d..18e4969493 100644 --- a/src/gui/plugins/component_inspector/Imu.cc +++ b/src/gui/plugins/component_inspector/Imu.cc @@ -103,7 +103,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -137,7 +137,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -171,7 +171,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -205,7 +205,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -239,7 +239,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -273,7 +273,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); diff --git a/src/gui/plugins/component_inspector/JointType.cc b/src/gui/plugins/component_inspector/JointType.cc index ffcc757f3e..63d9dfe5d6 100644 --- a/src/gui/plugins/component_inspector/JointType.cc +++ b/src/gui/plugins/component_inspector/JointType.cc @@ -81,10 +81,10 @@ Q_INVOKABLE void JointType::OnJointType(QString _jointType) [=](EntityComponentManager &_ecm) { components::JointType *comp = - _ecm.Component(this->inspector->Entity()); + _ecm.Component(this->inspector->GetEntity()); components::ParentEntity *parentComp = - _ecm.Component(this->inspector->Entity()); + _ecm.Component(this->inspector->GetEntity()); if (comp && parentComp) { diff --git a/src/gui/plugins/component_inspector/Lidar.cc b/src/gui/plugins/component_inspector/Lidar.cc index 48ad47b2fc..8fbf6e95c1 100644 --- a/src/gui/plugins/component_inspector/Lidar.cc +++ b/src/gui/plugins/component_inspector/Lidar.cc @@ -82,7 +82,7 @@ Q_INVOKABLE void Lidar::OnLidarNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Lidar *lidar = comp->Data().LidarSensor(); @@ -120,7 +120,7 @@ Q_INVOKABLE void Lidar::OnLidarChange( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Lidar *lidar = comp->Data().LidarSensor(); diff --git a/src/gui/plugins/component_inspector/Magnetometer.cc b/src/gui/plugins/component_inspector/Magnetometer.cc index c6f2ca59be..3c789582bc 100644 --- a/src/gui/plugins/component_inspector/Magnetometer.cc +++ b/src/gui/plugins/component_inspector/Magnetometer.cc @@ -81,7 +81,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); @@ -115,7 +115,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); @@ -149,7 +149,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index c6547cac73..d34ed099a7 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -251,10 +251,10 @@ void ModelEditor::Update(const UpdateInfo &, entities.push_back(child); } - // use tmp AddedRemovedEntities event to update other gui plugins + // use tmp GuiNewRemovedEntities event to update other gui plugins // note this event will be removed in Ignition Garden std::set removedEntities; - ignition::gazebo::gui::events::AddedRemovedEntities event( + ignition::gazebo::gui::events::GuiNewRemovedEntities event( newEntities, removedEntities); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 45784f9ac9..a300ee5d6b 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -38,43 +38,73 @@ Rectangle { // Maximum spinbox value property double spinMax: 1000000 - // Read-only / write - property bool readOnly: { - var isModel = entityType == "model" - return !(isModel) || nestedModel - } - property int iconWidth: 20 property int iconHeight: 20 // Loaded item for X - property var xItem: {} + property var xItem: model.data[0] // Loaded item for Y - property var yItem: {} + property var yItem: model.data[1] // Loaded item for Z - property var zItem: {} + property var zItem: model.data[2] // Loaded item for roll - property var rollItem: {} + property var rollItem: model.data[3] // Loaded item for pitch - property var pitchItem: {} + property var pitchItem: model.data[4] // Loaded item for yaw - property var yawItem: {} + property var yawItem: model.data[5] + + // Callback when x value is changed + function onXChange(_value) { + xItem = _value + sendPose() + } + + // Callback when y value is changed + function onYChange(_value) { + yItem = _value + sendPose() + } + + // Callback when z value is changed + function onZChange(_value) { + zItem = _value + sendPose() + } + + // Callback when roll value is changed + function onRollChange(_value) { + rollItem = _value + sendPose() + } + + // Callback when pitch value is changed + function onPitchChange(_value) { + pitchItem = _value + sendPose() + } + + // Callback when yaw value is changed + function onYawChange(_value) { + yawItem = _value + sendPose() + } // Send new pose to C++ function sendPose() { // TODO(anyone) There's a loss of precision when these values get to C++ componentInspector.onPose( - xItem.value, - yItem.value, - zItem.value, - rollItem.value, - pitchItem.value, - yawItem.value + xItem, + yItem, + zItem, + rollItem, + pitchItem, + yawItem ); } @@ -83,40 +113,6 @@ Rectangle { font.family: "Roboto" } - /** - * Used to create a spin box - */ - Component { - id: writableNumber - IgnSpinBox { - id: writableSpin - value: writableSpin.activeFocus ? writableSpin.value : numberValue - minimumValue: -spinMax - maximumValue: spinMax - decimals: getDecimalsAdjustValue(writableSpin, numberValue) - onEditingFinished: { - sendPose() - } - } - } - - /** - * Used to create a read-only number - */ - Component { - id: readOnlyNumber - Text { - id: numberText - anchors.fill: parent - horizontalAlignment: Text.AlignRight - verticalAlignment: Text.AlignVCenter - text: { - var decimals = getDecimals(numberText.width) - return numberValue.toFixed(decimals) - } - } - } - Column { anchors.fill: parent @@ -206,19 +202,18 @@ Rectangle { font.pointSize: 12 anchors.centerIn: parent } - } + } - Item { + StateAwareSpin { + id: xSpin Layout.fillWidth: true height: 40 - Loader { - id: xLoader - anchors.fill: parent - property double numberValue: model.data[0] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - xItem = xLoader.item - } + numberValue: xItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + xSpin.onChange.connect(onXChange) } } @@ -245,17 +240,16 @@ Rectangle { } } - Item { + StateAwareSpin { + id: rollSpin Layout.fillWidth: true height: 40 - Loader { - id: rollLoader - anchors.fill: parent - property double numberValue: model.data[3] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - rollItem = rollLoader.item - } + numberValue: rollItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + rollSpin.onChange.connect(onRollChange) } } @@ -288,17 +282,16 @@ Rectangle { } } - Item { + StateAwareSpin { + id: ySpin Layout.fillWidth: true height: 40 - Loader { - id: yLoader - anchors.fill: parent - property double numberValue: model.data[1] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - yItem = yLoader.item - } + numberValue: yItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + ySpin.onChange.connect(onYChange) } } @@ -324,17 +317,16 @@ Rectangle { } } - Item { + StateAwareSpin { + id: pitchSpin Layout.fillWidth: true height: 40 - Loader { - id: pitchLoader - anchors.fill: parent - property double numberValue: model.data[4] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - pitchItem = pitchLoader.item - } + numberValue: pitchItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + pitchSpin.onChange.connect(onPitchChange) } } @@ -360,17 +352,16 @@ Rectangle { } } - Item { + StateAwareSpin { + id: zSpin Layout.fillWidth: true height: 40 - Loader { - id: zLoader - anchors.fill: parent - property double numberValue: model.data[2] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - zItem = zLoader.item - } + numberValue: zItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + zSpin.onChange.connect(onZChange) } } @@ -396,17 +387,16 @@ Rectangle { } } - Item { + StateAwareSpin { + id: yawSpin Layout.fillWidth: true height: 40 - Loader { - id: yawLoader - anchors.fill: parent - property double numberValue: model.data[5] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - yawItem = yawLoader.item - } + numberValue: yawItem + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + Component.onCompleted: { + yawSpin.onChange.connect(onYawChange) } } } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 6014d14b9d..b114fb1dbe 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -136,11 +136,11 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->OnRender(); } else if (_event->type() == - ignition::gazebo::gui::events::AddedRemovedEntities::kType) + ignition::gazebo::gui::events::GuiNewRemovedEntities::kType) { std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); auto addedRemovedEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (addedRemovedEvent) { for (auto entity : addedRemovedEvent->NewEntities()) From 7e1e8e8dfc16153647e0ed7473315577f6c91a9c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 2 Dec 2021 15:25:56 -0800 Subject: [PATCH 2/3] Support setting pose on links, visuals, collision via the GUI Signed-off-by: Nate Koenig --- .../component_inspector/CMakeLists.txt | 2 + .../component_inspector/ComponentInspector.cc | 74 ++---------- .../component_inspector/ComponentInspector.hh | 17 --- .../ComponentInspector.qml | 7 -- src/gui/plugins/component_inspector/Pose3d.cc | 89 ++++++++++++++ src/gui/plugins/component_inspector/Pose3d.hh | 61 ++++++++++ .../plugins/component_inspector/Pose3d.qml | 109 ++++++------------ 7 files changed, 195 insertions(+), 164 deletions(-) create mode 100644 src/gui/plugins/component_inspector/Pose3d.cc create mode 100644 src/gui/plugins/component_inspector/Pose3d.hh diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 450cc9929b..283ce62a54 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -8,6 +8,7 @@ gz_add_gui_plugin(ComponentInspector Lidar.cc Magnetometer.cc ModelEditor.cc + Pose3d.cc Types.cc QT_HEADERS AirPressure.hh @@ -18,5 +19,6 @@ gz_add_gui_plugin(ComponentInspector Lidar.hh Magnetometer.hh ModelEditor.hh + Pose3d.hh Types.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index bb8768610f..20b245f8d2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -61,7 +61,7 @@ #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/SelfCollide.hh" @@ -77,6 +77,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/Util.hh" #include "AirPressure.hh" #include "Altimeter.hh" @@ -86,6 +87,7 @@ #include "Lidar.hh" #include "Magnetometer.hh" #include "ModelEditor.hh" +#include "Pose3d.hh" namespace ignition::gazebo { @@ -148,6 +150,9 @@ namespace ignition::gazebo /// \brief Magnetometer inspector elements public: std::unique_ptr magnetometer; + /// \brief Pose inspector elements + public: std::unique_ptr pose3d; + /// \brief Set of callbacks to execute during the Update function. public: std::vector< std::function> updateCallbacks; @@ -160,25 +165,6 @@ namespace ignition::gazebo using namespace ignition; using namespace gazebo; -////////////////////////////////////////////////// -template<> -void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data) -{ - if (nullptr == _item) - return; - - _item->setData(QString("Pose3d"), - ComponentsModel::RoleNames().key("dataType")); - _item->setData(QList({ - QVariant(_data.Pos().X()), - QVariant(_data.Pos().Y()), - QVariant(_data.Pos().Z()), - QVariant(_data.Rot().Roll()), - QVariant(_data.Rot().Pitch()), - QVariant(_data.Rot().Yaw()) - }), ComponentsModel::RoleNames().key("data")); -} - ////////////////////////////////////////////////// template<> void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data) @@ -519,6 +505,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Create the magnetometer this->dataPtr->magnetometer = std::make_unique(this); + + // Create the pose3d + this->dataPtr->pose3d = std::make_unique(this); } ////////////////////////////////////////////////// @@ -820,12 +809,6 @@ void ComponentInspector::Update(const UpdateInfo &_info, if (comp) setData(item, comp->Data()); } - else if (typeId == components::Pose::typeId) - { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); - } else if (typeId == components::RenderEngineGuiPlugin::typeId) { auto comp = _ecm.Component( @@ -953,13 +936,6 @@ void ComponentInspector::Update(const UpdateInfo &_info, if (comp) setData(item, comp->Data()); } - else if (typeId == components::WorldPoseCmd::typeId) - { - auto comp = _ecm.Component( - this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); - } else if (typeId == components::Material::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); @@ -1123,38 +1099,6 @@ void ComponentInspector::SetPaused(bool _paused) this->PausedChanged(); } -///////////////////////////////////////////////// -void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, - double _pitch, double _yaw) -{ - /*std::function cb = - [](const ignition::msgs::Boolean &_rep, const bool _result) - { - if (!_result) - ignerr << "Error setting pose" << std::endl; - }; - - ignition::msgs::Pose req; - req.set_id(this->dataPtr->entity); - msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); - msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); - auto poseCmdService = "/world/" + this->dataPtr->worldName - + "/set_pose"; - this->dataPtr->node.Request(poseCmdService, req, cb); - */ - - ignition::gazebo::UpdateCallback cb = - [=](EntityComponentManager &_ecm) - { - auto comp = _ecm.Component(this->GetEntity()); - if (comp) - comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw); - else - ignerr << "Unable to get the pose component.\n"; - }; - this->AddUpdateCallback(cb); -} - ///////////////////////////////////////////////// void ComponentInspector::OnLight( double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 0686a89d04..5d5a688475 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -26,7 +26,6 @@ #include #include -#include #include #include @@ -70,12 +69,6 @@ namespace gazebo template<> void setData(QStandardItem *_item, const std::string &_data); - /// \brief Specialized to set pose data. - /// \param[in] _item Item whose data will be set. - /// \param[in] _data Data to set. - template<> - void setData(QStandardItem *_item, const math::Pose3d &_data); - /// \brief Specialized to set light data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -244,16 +237,6 @@ namespace gazebo // Documentation inherited public: void Update(const UpdateInfo &, EntityComponentManager &) override; - /// \brief Callback in Qt thread when pose changes. - /// \param[in] _x X - /// \param[in] _y Y - /// \param[in] _z Z - /// \param[in] _roll Roll - /// \param[in] _pitch Pitch - /// \param[in] _yaw Yaw - public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, - double _roll, double _pitch, double _yaw); - /// \brief Callback in Qt thread when specular changes. /// \param[in] _rSpecular specular red /// \param[in] _gSpecular specular green diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 39038d65d3..034a33549f 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -113,13 +113,6 @@ Rectangle { return getDecimals(_widgetId.width) } - /** - * Forward pose changes to C++ - */ - function onPose(_x, _y, _z, _roll, _pitch, _yaw) { - ComponentInspector.OnPose(_x, _y, _z, _roll, _pitch, _yaw) - } - /** * Forward light changes to C++ */ diff --git a/src/gui/plugins/component_inspector/Pose3d.cc b/src/gui/plugins/component_inspector/Pose3d.cc new file mode 100644 index 0000000000..4eac570e21 --- /dev/null +++ b/src/gui/plugins/component_inspector/Pose3d.cc @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include +#include +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Recreate.hh" +#include + +#include "ComponentInspector.hh" +#include "Pose3d.hh" +#include "Types.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Pose3d::Pose3d(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("Pose3dImpl", this); + this->inspector = _inspector; + + ComponentCreator creator = + [=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item) + { + auto comp = _ecm.Component(_entity); + if (nullptr == _item || nullptr == comp) + return; + math::Pose3d pose = comp->Data(); + + _item->setData(QString("Pose3d"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(pose.Pos().X()), + QVariant(pose.Pos().Y()), + QVariant(pose.Pos().Z()), + QVariant(pose.Rot().Roll()), + QVariant(pose.Rot().Pitch()), + QVariant(pose.Rot().Yaw()) + }), ComponentsModel::RoleNames().key("data")); + + // Notify the qml component when the pose has changed, such as when + // a new entity is selected. + if (this->currentPose != pose) + { + this->currentPose = pose; + this->poseChanged(); + } + }; + + this->inspector->RegisterComponentCreator(components::Pose::typeId, creator); +} + +///////////////////////////////////////////////// +Q_INVOKABLE void Pose3d::PoseUpdate( + double _x, double _y, double _z, double _roll, double _pitch, double _yaw) +{ + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component(this->inspector->GetEntity()); + if (comp) + { + comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw); + // Recreate the model. Changing link poses can cause the kinematic + // tree to need regreneration. + Entity modelEntity = topLevelModel(this->inspector->GetEntity(), _ecm); + _ecm.CreateComponent(modelEntity, components::Recreate()); + } + else + ignerr << "Unable to get the pose component.\n"; + }; + this->inspector->AddUpdateCallback(cb); +} diff --git a/src/gui/plugins/component_inspector/Pose3d.hh b/src/gui/plugins/component_inspector/Pose3d.hh new file mode 100644 index 0000000000..b2a95e7295 --- /dev/null +++ b/src/gui/plugins/component_inspector/Pose3d.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ + class ComponentInspector; + + /// \brief A class that handles Pose3d changes. + class Pose3d : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit Pose3d(ComponentInspector *_inspector); + + /// \brief Handle pose updates from the GUI. + /// \param[in] _x New x value. + /// \param[in] _y New y value. + /// \param[in] _z New z value. + /// \param[in] _roll New roll value. + /// \param[in] _pitch New pitch value. + /// \param[in] _yaw New yaw value. + public: Q_INVOKABLE void PoseUpdate( + double _x, double _y, double _z, double _roll, + double _pitch, double _yaw); + + /// \brief Notify that simulation paused state has changed. + signals: void poseChanged(); + + /// \brief Pointer to the component inspector. This is used to add + /// update callbacks that modify the ECM. + private: ComponentInspector *inspector{nullptr}; + + /// \brief Keep track of current pose so that we can update the pose + /// spin boxes correctly. + private: math::Pose3d currentPose; + }; +} +} +#endif diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index a300ee5d6b..31f658b237 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -29,6 +29,20 @@ Rectangle { width: componentInspector.width color: index % 2 == 0 ? lightGrey : darkGrey + Connections { + target: Pose3dImpl + onPoseChanged: { + // Update the spinner to show to correct values when the pose component + // has changed, such as when a new entity is selected. + xSpin.numberValue = model.data[0] + ySpin.numberValue = model.data[1] + zSpin.numberValue = model.data[2] + rollSpin.numberValue = model.data[3] + pitchSpin.numberValue = model.data[4] + yawSpin.numberValue = model.data[5] + } + } + // Left indentation property int indentation: 10 @@ -41,70 +55,15 @@ Rectangle { property int iconWidth: 20 property int iconHeight: 20 - // Loaded item for X - property var xItem: model.data[0] - - // Loaded item for Y - property var yItem: model.data[1] - - // Loaded item for Z - property var zItem: model.data[2] - - // Loaded item for roll - property var rollItem: model.data[3] - - // Loaded item for pitch - property var pitchItem: model.data[4] - - // Loaded item for yaw - property var yawItem: model.data[5] - - // Callback when x value is changed - function onXChange(_value) { - xItem = _value - sendPose() - } - - // Callback when y value is changed - function onYChange(_value) { - yItem = _value - sendPose() - } - - // Callback when z value is changed - function onZChange(_value) { - zItem = _value - sendPose() - } - - // Callback when roll value is changed - function onRollChange(_value) { - rollItem = _value - sendPose() - } - - // Callback when pitch value is changed - function onPitchChange(_value) { - pitchItem = _value - sendPose() - } - - // Callback when yaw value is changed - function onYawChange(_value) { - yawItem = _value - sendPose() - } - // Send new pose to C++ - function sendPose() { - // TODO(anyone) There's a loss of precision when these values get to C++ - componentInspector.onPose( - xItem, - yItem, - zItem, - rollItem, - pitchItem, - yawItem + function sendPose(_value) { + Pose3dImpl.PoseUpdate( + xSpin.numberValue, + ySpin.numberValue, + zSpin.numberValue, + rollSpin.numberValue, + pitchSpin.numberValue, + yawSpin.numberValue ); } @@ -208,12 +167,12 @@ Rectangle { id: xSpin Layout.fillWidth: true height: 40 - numberValue: xItem + numberValue: model.data[0] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - xSpin.onChange.connect(onXChange) + xSpin.onChange.connect(sendPose) } } @@ -244,12 +203,12 @@ Rectangle { id: rollSpin Layout.fillWidth: true height: 40 - numberValue: rollItem + numberValue: model.data[3] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - rollSpin.onChange.connect(onRollChange) + rollSpin.onChange.connect(sendPose) } } @@ -286,12 +245,12 @@ Rectangle { id: ySpin Layout.fillWidth: true height: 40 - numberValue: yItem + numberValue: model.data[1] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - ySpin.onChange.connect(onYChange) + ySpin.onChange.connect(sendPose) } } @@ -321,12 +280,12 @@ Rectangle { id: pitchSpin Layout.fillWidth: true height: 40 - numberValue: pitchItem + numberValue: model.data[4] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - pitchSpin.onChange.connect(onPitchChange) + pitchSpin.onChange.connect(sendPose) } } @@ -356,12 +315,12 @@ Rectangle { id: zSpin Layout.fillWidth: true height: 40 - numberValue: zItem + numberValue: model.data[2] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - zSpin.onChange.connect(onZChange) + zSpin.onChange.connect(sendPose) } } @@ -391,12 +350,12 @@ Rectangle { id: yawSpin Layout.fillWidth: true height: 40 - numberValue: yawItem + numberValue: model.data[5] minValue: -100000 maxValue: 100000 stepValue: 0.1 Component.onCompleted: { - yawSpin.onChange.connect(onYawChange) + yawSpin.onChange.connect(sendPose) } } } From 1802027ea9c990c11e4bf5954abfd7a83e226b7b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 3 Dec 2021 08:12:18 -0800 Subject: [PATCH 3/3] Fix test Signed-off-by: Nate Koenig --- src/ModelCommandAPI_TEST.cc | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 81beeb6b32..76d19ef24b 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -462,7 +462,32 @@ TEST(ModelCommandAPI, MagnetometerSensor) " - Parent: magnetometer_model [8]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 0.000000 0.000000]\n" - " [0.000000 0.000000 0.000000]\n"; + " [0.000000 0.000000 0.000000]\n" + " - X-axis noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n" + " - Y-axis noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n" + " - Z-axis noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n"; + EXPECT_EQ(expectedOutput, output); } }