diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index f02c9b4fff..a8740a291e 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,7 @@ #include #include #include +#include #include #include @@ -417,6 +419,41 @@ namespace ignition sdf::Atmosphere convert(const msgs::Atmosphere &_in); + /// \brief Generic conversion from an SDF Physics to another type. + /// \param[in] _in SDF Physics. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Physics &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF physics to a physics + /// message. + /// \param[in] _in SDF physics. + /// \return Physics message. + template<> + msgs::Physics convert(const sdf::Physics &_in); + + /// \brief Generic conversion from a physics message to another type. + /// \param[in] _in Physics message. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Physics &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a physics message to a physics + /// SDF object. + /// \param[in] _in Physics message. + /// \return SDF physics. + template<> + sdf::Physics convert(const msgs::Physics &_in); + + /// \brief Generic conversion from an SDF Sensor to another type. /// \param[in] _in SDF Sensor. /// \return Conversion result. @@ -429,7 +466,7 @@ namespace ignition /// \brief Specialized conversion from an SDF sensor to a sensor /// message. - /// \param[in] _in SDF geometry. + /// \param[in] _in SDF sensor. /// \return Sensor message. template<> msgs::Sensor convert(const sdf::Sensor &_in); diff --git a/include/ignition/gazebo/components/Physics.hh b/include/ignition/gazebo/components/Physics.hh new file mode 100644 index 0000000000..804059dedc --- /dev/null +++ b/include/ignition/gazebo/components/Physics.hh @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ + +#include + +#include + +#include +#include + +#include +#include "ignition/gazebo/components/Component.hh" +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using PhysicsSerializer = + serializers::ComponentToMsgSerializer; +} +namespace components +{ + /// \brief A component type that contains the physics properties of + /// the World entity. + using Physics = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Physics", + Physics) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/PhysicsCmd.hh b/include/ignition/gazebo/components/PhysicsCmd.hh new file mode 100644 index 0000000000..f72dc328b0 --- /dev/null +++ b/include/ignition/gazebo/components/PhysicsCmd.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_PHYSICSCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_PHYSICSCMD_HH_ + +#include + +#include +#include + +#include +#include "ignition/gazebo/components/Component.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains the physics properties of + /// the World entity. + using PhysicsCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsCmd", + PhysicsCmd) +} +} +} +} + +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index edbc024d77..4f4d102191 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -721,6 +721,26 @@ void ignition::gazebo::set(msgs::WorldStatistics *_msg, _msg->set_paused(_in.paused); } +////////////////////////////////////////////////// +template<> +msgs::Physics ignition::gazebo::convert(const sdf::Physics &_in) +{ + msgs::Physics out; + out.set_max_step_size(_in.MaxStepSize()); + out.set_real_time_factor(_in.RealTimeFactor()); + return out; +} + +////////////////////////////////////////////////// +template<> +sdf::Physics ignition::gazebo::convert(const msgs::Physics &_in) +{ + sdf::Physics out; + out.SetRealTimeFactor(_in.real_time_factor()); + out.SetMaxStepSize(_in.max_step_size()); + return out; +} + ////////////////////////////////////////////////// void ignition::gazebo::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf) { diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index fe155e00a5..76fad52775 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -157,6 +158,28 @@ TEST(Conversions, Entity) EXPECT_EQ(msgs::Entity_Type_NONE, entityType2); } +///////////////////////////////////////////////// +TEST(Conversions, Physics) +{ + // Test conversion from msg to sdf + msgs::Physics msg; + msg.set_real_time_factor(1.23); + msg.set_max_step_size(0.12); + + auto physics = convert(msg); + EXPECT_DOUBLE_EQ(1.23, physics.RealTimeFactor()); + EXPECT_DOUBLE_EQ(0.12, physics.MaxStepSize()); + + // Test conversion from sdf to msg + sdf::Physics physSdf; + physSdf.SetMaxStepSize(0.34); + physSdf.SetRealTimeFactor(2.34); + + auto physMsg = convert(physSdf); + EXPECT_DOUBLE_EQ(2.34, physMsg.real_time_factor()); + EXPECT_DOUBLE_EQ(0.34, physMsg.max_step_size()); +} + ///////////////////////////////////////////////// TEST(Conversions, Pose) { diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 1b54f01848..74e2f71020 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -46,6 +46,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerLevels.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" @@ -102,6 +103,14 @@ void LevelManager::ReadLevelPerformerInfo() this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::Gravity(this->runner->sdfWorld->Gravity())); + auto physics = this->runner->sdfWorld->PhysicsByIndex(0); + if (!physics) + { + physics = this->runner->sdfWorld->PhysicsDefault(); + } + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::Physics(*physics)); + this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::MagneticField(this->runner->sdfWorld->MagneticField())); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 414e871cd8..cc9c2c0794 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -54,6 +54,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" @@ -202,6 +203,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->ecm->CreateComponent(worldEntity, components::Gravity(_world->Gravity())); + // Physics + // \todo(anyone) Support picking a specific physics profile + auto physics = _world->PhysicsByIndex(0); + if (!physics) + { + physics = _world->PhysicsDefault(); + } + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Physics(*physics)); + // MagneticField this->dataPtr->ecm->CreateComponent(worldEntity, components::MagneticField(_world->MagneticField())); diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc index b46eb95322..71c392c5e0 100644 --- a/src/SdfEntityCreator_TEST.cc +++ b/src/SdfEntityCreator_TEST.cc @@ -42,6 +42,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visibility.hh" @@ -111,15 +112,20 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) unsigned int worldCount{0}; Entity worldEntity = kNullEntity; this->ecm.Each( + components::Name, + components::Physics>( [&](const Entity &_entity, const components::World *_world, - const components::Name *_name)->bool + const components::Name *_name, + const components::Physics *_physics)->bool { EXPECT_NE(nullptr, _world); EXPECT_NE(nullptr, _name); + EXPECT_NE(nullptr, _physics); EXPECT_EQ("default", _name->Data()); + EXPECT_DOUBLE_EQ(0.001, _physics->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(1.0, _physics->Data().RealTimeFactor()); worldCount++; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index bdd21a1cb8..e3d0bc8a4b 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -27,6 +27,8 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/Physics.hh" +#include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/Util.hh" @@ -60,8 +62,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Keep system loader so plugins can be loaded at runtime this->systemLoader = _systemLoader; - // Get the first physics profile - // \todo(louise) Support picking a specific profile + // Get the physics profile + // TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager auto physics = _world->PhysicsByIndex(0); if (!physics) { @@ -76,7 +78,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, dur); // Desired real time factor - double desiredRtf = _world->PhysicsDefault()->RealTimeFactor(); + this->desiredRtf = physics->RealTimeFactor(); // The instantaneous real time factor is given as: // @@ -102,7 +104,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // // period = step_size / RTF this->updatePeriod = std::chrono::nanoseconds( - static_cast(this->stepSize.count() / desiredRtf)); + static_cast(this->stepSize.count() / this->desiredRtf)); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -330,6 +332,47 @@ void SimulationRunner::UpdateCurrentInfo() } } +///////////////////////////////////////////////// +void SimulationRunner::UpdatePhysicsParams() +{ + auto worldEntity = + this->entityCompMgr.EntityByComponents(components::World()); + const auto physicsCmdComp = + this->entityCompMgr.Component(worldEntity); + if (!physicsCmdComp) + { + return; + } + auto physicsComp = + this->entityCompMgr.Component(worldEntity); + + const auto& physicsParams = physicsCmdComp->Data(); + const auto newStepSize = + std::chrono::duration(physicsParams.max_step_size()); + const double newRTF = physicsParams.real_time_factor(); + + const double eps = 0.00001; + if (newStepSize != this->stepSize || + std::abs(newRTF - this->desiredRtf) > eps) + { + this->SetStepSize( + std::chrono::duration_cast( + newStepSize)); + this->desiredRtf = newRTF; + this->updatePeriod = std::chrono::nanoseconds( + static_cast(this->stepSize.count() / this->desiredRtf)); + + this->simTimes.clear(); + this->realTimes.clear(); + // Update physics components + physicsComp->Data().SetMaxStepSize(physicsParams.max_step_size()); + physicsComp->Data().SetRealTimeFactor(newRTF); + this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId, + ComponentState::OneTimeChange); + } + this->entityCompMgr.RemoveComponent(worldEntity); +} + ///////////////////////////////////////////////// void SimulationRunner::PublishStats() { @@ -647,6 +690,10 @@ bool SimulationRunner::Run(const uint64_t _iterations) processedIterations < _iterations)) { IGN_PROFILE("SimulationRunner::Run - Iteration"); + + // Update the step size and desired rtf + this->UpdatePhysicsParams(); + // Compute the time to sleep in order to match, as closely as possible, // the update period. sleepTime = 0ns; diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index faf8e3518b..e9d0316f63 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -372,6 +372,10 @@ namespace ignition /// \brief Set the next step to be blocking and paused. public: void SetNextStepAsBlockingPaused(const bool value); + /// \brief Updates the physics parameters of the simulation based on the + /// Physics component of the world, if any. + public: void UpdatePhysicsParams(); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -462,6 +466,9 @@ namespace ignition /// \brief Step size private: ignition::math::clock::duration stepSize{10ms}; + /// \brief Desired real time factor + private: double desiredRtf{1.0}; + /// \brief Connection to the pause event. private: ignition::common::ConnectionPtr pauseConn; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 64b75f6c1f..21f84aafc2 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -176,6 +176,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(1u, worldCount); EXPECT_NE(kNullEntity, worldEntity); + // Test step size, real time factor is not testable since it has no public API + auto stepSize = std::chrono::duration(runner.StepSize()).count(); + EXPECT_DOUBLE_EQ(0.001, stepSize); + // Check models unsigned int modelCount{0}; Entity boxModelEntity = kNullEntity; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index de0cf8afee..1c2de965b9 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -46,6 +46,7 @@ #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerAffinity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/SelfCollide.hh" @@ -174,6 +175,18 @@ void ignition::gazebo::setData(QStandardItem *_item, const double &_data) _item->setData(_data, ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) +{ + _item->setData(QString("Physics"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(_data.MaxStepSize()), + QVariant(_data.RealTimeFactor()) + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { @@ -521,6 +534,12 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::Physics::typeId) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); @@ -728,6 +747,30 @@ void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, this->dataPtr->node.Request(poseCmdService, req, cb); } +///////////////////////////////////////////////// +void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting physics parameters" << std::endl; + }; + + ignition::msgs::Physics req; + req.set_max_step_size(_stepSize); + req.set_real_time_factor(_realTimeFactor); + auto physicsCmdService = "/world/" + this->dataPtr->worldName + + "/set_physics"; + physicsCmdService = transport::TopicUtils::AsValidTopic(physicsCmdService); + if (physicsCmdService.empty()) + { + ignerr << "Invalid physics command service topic provided" << std::endl; + return; + } + this->dataPtr->node.Request(physicsCmdService, req, cb); +} + ///////////////////////////////////////////////// bool ComponentInspector::NestedModel() const { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 470c7b508f..8df6eb585b 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -28,6 +28,8 @@ #include #include +#include "ignition/gazebo/components/Physics.hh" + Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) namespace ignition @@ -75,6 +77,12 @@ namespace gazebo template<> void setData(QStandardItem *_item, const math::Vector3d &_data); + /// \brief Specialized to set Physics data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const sdf::Physics &_data); + /// \brief Specialized to set boolean data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -207,6 +215,12 @@ namespace gazebo public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); + /// \brief Callback in Qt thread when physics' properties change. + /// \param[in] _stepSize step size + /// \param[in] _realTimeFactor real time factor + public: Q_INVOKABLE void OnPhysics(double _stepSize, + double _realTimeFactor); + /// \brief Get whether the entity is a nested model or not /// \return True if the entity is a nested model, false otherwise public: Q_INVOKABLE bool NestedModel() const; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index aa058cae4f..e48bc4b393 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -85,6 +85,13 @@ Rectangle { ComponentInspector.OnPose(_x, _y, _z, _roll, _pitch, _yaw) } + /** + * Forward physics changes to C++ + */ + function onPhysics(_stepSize, _realTimeFactor) { + ComponentInspector.OnPhysics(_stepSize, _realTimeFactor) + } + Rectangle { id: header height: lockButton.height diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 43efec19d1..f50f8e7616 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -3,6 +3,7 @@ Boolean.qml ComponentInspector.qml NoData.qml + Physics.qml Pose3d.qml String.qml TypeHeader.qml diff --git a/src/gui/plugins/component_inspector/Physics.qml b/src/gui/plugins/component_inspector/Physics.qml new file mode 100644 index 0000000000..71f0a34fda --- /dev/null +++ b/src/gui/plugins/component_inspector/Physics.qml @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying physics information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Minimum parameter value, step size and RTF must be strictly positive + property double minPhysParam: 0.000001 + + // Maximum parameter value + property double maxPhysParam: 100000 + + // Horizontal margins + property int margin: 5 + + property int iconWidth: 20 + property int iconHeight: 20 + + // Loaded item for physics step size + property var stepSizeItem: {} + + // Loaded item for real time factor + property var realTimeFactorItem: {} + + // Send new physics data to C++ + function sendPhysics() { + // TODO(anyone) There's a loss of precision when these values get to C++ + componentInspector.onPhysics( + stepSizeItem.value, + realTimeFactorItem.value + ); + } + + FontMetrics { + id: fontMetrics + font.family: "Roboto" + } + + /** + * Used to create a spin box + */ + + Component { + id: writablePositiveNumber + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: minPhysParam + maximumValue: maxPhysParam + decimals: 6 + stepSize: 0.001 + onEditingFinished: { + sendPhysics() + } + } + } + + Component { + id: plotIcon + Image { + property string componentInfo: "" + source: "plottable_icon.svg" + anchors.top: parent.top + anchors.left: parent.left + + Drag.mimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + componentInfo + "," + model.shortName + } + Drag.dragType: Drag.Automatic + Drag.supportedActions : Qt.CopyAction + Drag.active: dragMouse.drag.active + // a point to drag from + Drag.hotSpot.x: 0 + Drag.hotSpot.y: y + MouseArea { + id: dragMouse + anchors.fill: parent + drag.target: (model === null) ? null : parent + onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) + onReleased: parent.Drag.drop(); + cursorShape: Qt.DragCopyCursor + } + } + } + + Column { + anchors.fill: parent + + // Header + Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + RowLayout { + anchors.fill: parent + Item { + width: margin + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: content.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + content.show = !content.show + } + onEntered: { + header.color = highlightColor + } + onExited: { + header.color = "transparent" + } + } + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? grid.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + GridLayout { + id: grid + width: parent.width + columns: 3 + + // Left spacer + Item { + Layout.rowSpan: 3 + width: margin + indentation + } + + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: stepSizeText.width + indentation*3 + Loader { + id: loaderStepSize + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderStepSize.item.componentInfo = "stepSize" + + Text { + id : stepSizeText + text: ' Step Size (s)' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: stepSizeLoader + anchors.fill: parent + property double numberValue: model.data[0] + sourceComponent: writablePositiveNumber + onLoaded: { + stepSizeItem = stepSizeLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: realTimeFactorText.width + indentation*3 + Loader { + id: loaderRealTimeFactor + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderRealTimeFactor.item.componentInfo = "realTimeFactor" + + Text { + id : realTimeFactorText + text: ' Real time factor' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: realTimeFactorLoader + anchors.fill: parent + property double numberValue: model.data[1] + sourceComponent: writablePositiveNumber + onLoaded: { + realTimeFactorItem = realTimeFactorLoader.item + } + } + } + } + } + } +} diff --git a/src/gui/plugins/plotting/Plotting.cc b/src/gui/plugins/plotting/Plotting.cc index ef20533382..876e3e613a 100644 --- a/src/gui/plugins/plotting/Plotting.cc +++ b/src/gui/plugins/plotting/Plotting.cc @@ -28,6 +28,7 @@ #include "ignition/gazebo/components/LinearVelocitySeed.hh" #include "ignition/gazebo/components/MagneticField.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/Static.hh" @@ -96,6 +97,11 @@ PlotComponent::PlotComponent(const std::string &_type, } else if (_type == "double") this->dataPtr->data["value"] = std::make_shared(); + else if (_type == "Physics") + { + this->dataPtr->data["stepSize"] = std::make_shared(); + this->dataPtr->data["realTimeFactor"] = std::make_shared(); + } else ignwarn << "Invalid Plot Component Type:" << _type << std::endl; } @@ -215,6 +221,15 @@ void Plotting::SetData(std::string _Id, const ignition::math::Pose3d &_pose) this->dataPtr->components[_Id]->SetAttributeValue("yaw", _pose.Rot().Yaw()); } +////////////////////////////////////////////////// +void Plotting::SetData(std::string _Id, const sdf::Physics &_physics) +{ + this->dataPtr->components[_Id]->SetAttributeValue("stepSize", + _physics.MaxStepSize()); + this->dataPtr->components[_Id]->SetAttributeValue("realTimeFactor", + _physics.RealTimeFactor()); +} + ////////////////////////////////////////////////// void Plotting::SetData(std::string _Id, const double &_value) { @@ -325,6 +340,12 @@ void Plotting ::Update(const ignition::gazebo::UpdateInfo &_info, if (comp) this->SetData(component.first, comp->Data()); } + else if (typeId == components::Physics::typeId) + { + auto comp = _ecm.Component(entity); + if (comp) + this->SetData(component.first, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(entity); diff --git a/src/gui/plugins/plotting/Plotting.hh b/src/gui/plugins/plotting/Plotting.hh index bf9c54c69b..24601bdc6b 100644 --- a/src/gui/plugins/plotting/Plotting.hh +++ b/src/gui/plugins/plotting/Plotting.hh @@ -24,6 +24,8 @@ #include #include +#include "sdf/Physics.hh" + #include #include #include @@ -119,6 +121,12 @@ class Plotting : public ignition::gazebo::GuiSystem public: void SetData(std::string _Id, const ignition::math::Pose3d &_pose); + /// \brief Set the Component data of giving id to the giving + /// physics properties + /// \param [in] _Id Component Key of the components map + /// \param [in] _value physics Data to be set to the component + public: void SetData(std::string _Id, const sdf::Physics &_physics); + /// \brief Set the Component data of giving id to the giving number /// \param [in] _Id Component Key of the components map /// \param [in] _value double Data to be set to the component diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index d334c246a2..8ff22c10c8 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -28,6 +29,7 @@ #include +#include #include #include @@ -42,7 +44,9 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -146,6 +150,19 @@ class PoseCommand : public UserCommandBase math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); }}; }; + +/// \brief Command to modify the physics parameters of a simulation. +class PhysicsCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the new physics parameters. + /// \param[in] _iface Pointer to user commands interface. + public: PhysicsCommand(msgs::Physics *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; } } } @@ -185,6 +202,13 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res); + /// \brief Callback for physics service + /// \param[in] _req Request containing updates to the physics parameters. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the physics parameters will be successfully updated. + /// \return True if successful. + public: bool PhysicsService(const msgs::Physics &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -258,6 +282,13 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::PoseService, this->dataPtr.get()); ignmsg << "Pose service on [" << poseService << "]" << std::endl; + + // Physics service + std::string physicsService{"/world/" + validWorldName + "/set_physics"}; + this->dataPtr->node.Advertise(physicsService, + &UserCommandsPrivate::PhysicsService, this->dataPtr.get()); + + ignmsg << "Physics service on [" << physicsService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -369,6 +400,24 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -715,6 +764,39 @@ bool PoseCommand::Execute() return true; } +////////////////////////////////////////////////// +PhysicsCommand::PhysicsCommand(msgs::Physics *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PhysicsCommand::Execute() +{ + auto physicsMsg = dynamic_cast(this->msg); + if (nullptr == physicsMsg) + { + ignerr << "Internal error, null physics message" << std::endl; + return false; + } + + auto worldEntity = this->iface->ecm->EntityByComponents(components::World()); + if (worldEntity == kNullEntity) + { + ignmsg << "Failed to find world entity" << std::endl; + return false; + } + + if (!this->iface->ecm->EntityHasComponentType(worldEntity, + components::PhysicsCmd().TypeId())) + { + this->iface->ecm->CreateComponent(worldEntity, + components::PhysicsCmd(*physicsMsg)); + } + + return true; +} IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index b87473e9b1..347135fc59 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -27,7 +27,9 @@ #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -690,3 +692,65 @@ TEST_F(UserCommandsTest, Pose) ASSERT_NE(nullptr, poseComp); EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, Physics) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/shapes.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + auto physicsComp = ecm->Component(worldEntity); + ASSERT_NE(nullptr, physicsComp); + EXPECT_DOUBLE_EQ(0.001, physicsComp->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(1.0, physicsComp->Data().RealTimeFactor()); + + // Set physics properties + msgs::Physics req; + req.set_max_step_size(0.123); + req.set_real_time_factor(4.567); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/default/set_physics"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the PhysicsCmd component is created + // in the second one it is processed + server.Run(true, 2, false); + + // Check updated physics properties + physicsComp = ecm->Component(worldEntity); + EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); +}