diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 781684a4ee..9cb5efc355 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -313,24 +313,29 @@ void SimulationRunner::UpdateCurrentInfo() ///////////////////////////////////////////////// void SimulationRunner::UpdatePhysicsParams() { - auto worldEntity = this->entityCompMgr.EntityByComponents(components::World()); + auto worldEntity = + this->entityCompMgr.EntityByComponents(components::World()); const auto physicsCmdComp = this->entityCompMgr.Component(worldEntity); if (!physicsCmdComp) { return; } - auto physicsComp = this->entityCompMgr.Component(worldEntity); + auto physicsComp = + this->entityCompMgr.Component(worldEntity); const auto& physicsParams = physicsCmdComp->Data(); - const auto newStepSize = std::chrono::duration(physicsParams.MaxStepSize()); + const auto newStepSize = + std::chrono::duration(physicsParams.MaxStepSize()); const double newRTF = physicsParams.RealTimeFactor(); const double eps = 0.00001; - if (newStepSize != this->stepSize || std::abs(newRTF - this->desiredRtf) > eps) + if (newStepSize != this->stepSize || + std::abs(newRTF - this->desiredRtf) > eps) { this->SetStepSize( - std::chrono::duration_cast(newStepSize)); + std::chrono::duration_cast( + newStepSize)); this->desiredRtf = newRTF; this->updatePeriod = std::chrono::nanoseconds( static_cast(this->stepSize.count() / this->desiredRtf)); @@ -343,7 +348,7 @@ void SimulationRunner::UpdatePhysicsParams() this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId, ComponentState::OneTimeChange); } - auto res = this->entityCompMgr.RemoveComponent(worldEntity); + this->entityCompMgr.RemoveComponent(worldEntity); } ///////////////////////////////////////////////// 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 ab2fc8f98c..0be57bb5ea 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -768,7 +768,7 @@ bool PhysicsCommand::Execute() this->iface->ecm->CreateComponent(worldEntity, components::PhysicsCmd(physics)); // HACK, the component is meant to be updated in SimulationRunner - // auto physicsComponent = + // auto physicsComponent = // this->iface->ecm->Component(worldEntity); // physicsComponent->Data() = components::Physics(physics).Data(); }