Fix codecheck, add plotting of physics properties

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
gazebosim · Jan 13, 2021 · b474cab · b474cab
1 parent f7afc38
commit b474cab
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 7 deletions.
diff --git 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<components::PhysicsCmd>(worldEntity);
   if (!physicsCmdComp)
   {
     return;
   }
-  auto physicsComp = this->entityCompMgr.Component<components::Physics>(worldEntity);
+  auto physicsComp =
+    this->entityCompMgr.Component<components::Physics>(worldEntity);
 
   const auto& physicsParams = physicsCmdComp->Data();
-  const auto newStepSize = std::chrono::duration<double>(physicsParams.MaxStepSize());
+  const auto newStepSize =
+    std::chrono::duration<double>(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<std::chrono::steady_clock::duration>(newStepSize));
+      std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+        newStepSize));
     this->desiredRtf = newRTF;
     this->updatePeriod = std::chrono::nanoseconds(
         static_cast<int>(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<components::PhysicsCmd>(worldEntity);
+  this->entityCompMgr.RemoveComponent<components::PhysicsCmd>(worldEntity);
 }
 
 /////////////////////////////////////////////////

diff --git 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<PlotData>();
+  else if (_type == "Physics")
+  {
+    this->dataPtr->data["stepSize"] = std::make_shared<PlotData>();
+    this->dataPtr->data["realTimeFactor"] = std::make_shared<PlotData>();
+  }
   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<components::Physics>(entity);
+      if (comp)
+        this->SetData(component.first, comp->Data());
+    }
     else if (typeId == components::Pose::typeId)
     {
       auto comp = _ecm.Component<components::Pose>(entity);

diff --git a/src/gui/plugins/plotting/Plotting.hh b/src/gui/plugins/plotting/Plotting.hh
@@ -24,6 +24,8 @@
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/Pose3.hh>
 
+#include "sdf/Physics.hh"
+
 #include <map>
 #include <string>
 #include <memory>
@@ -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
@@ -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<components::Physics>(worldEntity);
     // physicsComponent->Data() = components::Physics(physics).Data();
   }