Add service and GUI to configure physics parameters (step size and re…

…al time factor) (#536) Signed-off-by: Maganty Rushyendra <[email protected]> Signed-off-by: Luca Della Vedova <[email protected]> Co-authored-by: mrushyendra <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Feb 2, 2021 · 9bc088d · 9bc088d
1 parent b4e152d
commit 9bc088d
Show file tree

Hide file tree

Showing 20 changed files with 776 additions and 7 deletions.
diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh
@@ -26,6 +26,7 @@
 #include <ignition/msgs/inertial.pb.h>
 #include <ignition/msgs/light.pb.h>
 #include <ignition/msgs/material.pb.h>
+#include <ignition/msgs/physics.pb.h>
 #include <ignition/msgs/scene.pb.h>
 #include <ignition/msgs/sensor.pb.h>
 #include <ignition/msgs/sensor_noise.pb.h>
@@ -46,6 +47,7 @@
 #include <sdf/Light.hh>
 #include <sdf/Material.hh>
 #include <sdf/Noise.hh>
+#include <sdf/Physics.hh>
 #include <sdf/Scene.hh>
 #include <sdf/Sensor.hh>
 
@@ -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<class Out>
+    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<class Out>
+    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
@@ -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 <ignition/msgs/physics.pb.h>
+
+#include <sdf/Physics.hh>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include "ignition/gazebo/components/Component.hh"
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/Conversions.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace serializers
+{
+  using PhysicsSerializer =
+      serializers::ComponentToMsgSerializer<sdf::Physics, msgs::Physics>;
+}
+namespace components
+{
+  /// \brief A component type that contains the physics properties of
+  /// the World entity.
+  using Physics = Component<sdf::Physics, class PhysicsTag,
+      serializers::PhysicsSerializer>;
+  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
@@ -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 <ignition/msgs/physics.pb.h>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#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<msgs::Physics, class PhysicsCmdTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsCmd",
+      PhysicsCmd)
+}
+}
+}
+}
+
+#endif
diff --git 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
@@ -28,6 +28,7 @@
 #include <sdf/Magnetometer.hh>
 #include <sdf/Mesh.hh>
 #include <sdf/Pbr.hh>
+#include <sdf/Physics.hh>
 #include <sdf/Plane.hh>
 #include <sdf/Root.hh>
 #include <sdf/Scene.hh>
@@ -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<sdf::Physics>(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<msgs::Physics>(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
@@ -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
@@ -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
@@ -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::World,
-           components::Name>(
+           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++;