Adding HaltMotion (Or new suggested name) to physics plugin (#728)

* tested halt motion feature Signed-off-by: Tomas Lorente <[email protected]> * added flag to turn off movement halting Signed-off-by: Tomas Lorente <[email protected]> * Moved logic into gamelogic Signed-off-by: Tomas Lorente <[email protected]> * docs Signed-off-by: Tomas Lorente <[email protected]> * moved everything into gamelogic Signed-off-by: Tomas Lorente <[email protected]> * test added Signed-off-by: Tomas Lorente <[email protected]> * Fix codecheck Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · May 18, 2021 · 27ef720 · 27ef720
1 parent 86fe608
commit 27ef720
Show file tree

Hide file tree

Showing 6 changed files with 231 additions and 8 deletions.
diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2019 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_HALT_MOTION_HH_
+#define IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component used to turn off a model's joint's movement.
+  using HaltMotion = Component<bool, class HaltMotionTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.HaltMotion",
+      HaltMotion)
+}
+}
+}
+}
+#endif
diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc
@@ -31,6 +31,7 @@
 #include <ignition/gazebo/Link.hh>
 #include <ignition/gazebo/Model.hh>
 #include <ignition/gazebo/Util.hh>
+#include <ignition/common/Profiler.hh>
 
 #include <ignition/plugin/Register.hh>
 
@@ -164,9 +165,14 @@ void KineticEnergyMonitor::Configure(const Entity &_entity,
 }
 
 //////////////////////////////////////////////////
-void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/,
+void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info,
     const EntityComponentManager &_ecm)
 {
+  IGN_PROFILE("KineticEnergyMonitor::PostUpdate");
+  // Nothing left to do if paused or the publisher wasn't created.
+  if (_info.paused || !this->dataPtr->pub)
+    return;
+
   if (this->dataPtr->linkEntity != kNullEntity)
   {
     Link link(this->dataPtr->linkEntity);
@@ -191,10 +197,10 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/,
   }
 }
 
-IGNITION_ADD_PLUGIN(KineticEnergyMonitor, System,
-  KineticEnergyMonitor::ISystemConfigure,
-  KineticEnergyMonitor::ISystemPostUpdate
-)
+IGNITION_ADD_PLUGIN(KineticEnergyMonitor,
+                    ignition::gazebo::System,
+                    KineticEnergyMonitor::ISystemConfigure,
+                    KineticEnergyMonitor::ISystemPostUpdate)
 
 IGNITION_ADD_PLUGIN_ALIAS(KineticEnergyMonitor,
   "ignition::gazebo::systems::KineticEnergyMonitor")
diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh
@@ -97,8 +97,8 @@ namespace systems
       </plugin>
     </model>
   \endverbatim */
-  class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor:
-    public System,
+  class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor
+  : public System,
     public ISystemConfigure,
     public ISystemPostUpdate
   {

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
@@ -118,6 +118,7 @@
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/ThreadPitch.hh"
 #include "ignition/gazebo/components/World.hh"
+#include "ignition/gazebo/components/HaltMotion.hh"
 
 #include "EntityFeatureMap.hh"
 
@@ -1268,8 +1269,17 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         if (nullptr == jointPhys)
           return true;
 
+        auto haltMotionComp = _ecm.Component<components::HaltMotion>(
+            _ecm.ParentEntity(_entity));
+        bool haltMotion = false;
+        if (haltMotionComp)
+        {
+          haltMotion = haltMotionComp->Data();
+        }
+
+
         // Model is out of battery
-        if (this->entityOffMap[_ecm.ParentEntity(_entity)])
+        if (this->entityOffMap[_ecm.ParentEntity(_entity)] || haltMotion)
         {
           std::size_t nDofs = jointPhys->GetDegreesOfFreedom();
           for (std::size_t i = 0; i < nDofs; ++i)

diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
@@ -19,6 +19,7 @@ set(tests
   examples_build.cc
   follow_actor_system.cc
   fuel_cached_server.cc
+  halt_motion.cc
   imu_system.cc
   joint_controller_system.cc
   joint_position_controller_system.cc

diff --git a/test/integration/halt_motion.cc b/test/integration/halt_motion.cc
@@ -0,0 +1,166 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/transport/Node.hh>
+
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/HaltMotion.hh"
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/test_config.hh"
+
+#include "../helpers/Relay.hh"
+
+#define tol 10e-4
+
+using namespace ignition;
+using namespace gazebo;
+using namespace std::chrono_literals;
+
+/// \brief Test DiffDrive system
+class HaltMotionTest : public ::testing::TestWithParam<int>
+{
+  // Documentation inherited
+  protected: void SetUp() override
+  {
+    common::Console::SetVerbosity(1);
+    ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH",
+           (std::string(PROJECT_BINARY_PATH) + "/lib").c_str());
+  }
+
+  /// \param[in] _sdfFile SDF file to load.
+  /// \param[in] _cmdVelTopic Command velocity topic.
+  /// \param[in] _odomTopic Odometry topic.
+  protected: void TestPublishCmd(const std::string &_sdfFile,
+                                 const std::string &_cmdVelTopic)
+  {
+    // Start server
+    ServerConfig serverConfig;
+    serverConfig.SetSdfFile(_sdfFile);
+
+    Server server(serverConfig);
+    EXPECT_FALSE(server.Running());
+    EXPECT_FALSE(*server.Running(0));
+
+    // Create a system that records the vehicle poses
+    test::Relay testSystem;
+
+    std::vector<math::Pose3d> poses;
+    testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
+      const gazebo::EntityComponentManager &_ecm)
+      {
+        auto id = _ecm.EntityByComponents(
+          components::Model(),
+          components::Name("vehicle"));
+        EXPECT_NE(kNullEntity, id);
+
+        auto poseComp = _ecm.Component<components::Pose>(id);
+        ASSERT_NE(nullptr, poseComp);
+
+        poses.push_back(poseComp->Data());
+      });
+    testSystem.OnPreUpdate([&poses](const gazebo::UpdateInfo &,
+      gazebo::EntityComponentManager & _ecm)
+      {
+        auto model = _ecm.EntityByComponents(
+          components::Model(),
+          components::Name("vehicle"));;
+        EXPECT_NE(kNullEntity, model);
+        if (!_ecm.Component<components::HaltMotion>(model))
+        {
+          _ecm.CreateComponent(model, components::HaltMotion(false));
+        }
+        if (poses.size() == 4000u &&
+            !_ecm.Component<components::HaltMotion>(model)->Data())
+        {
+          _ecm.Component<components::HaltMotion>(model)->Data() = true;
+        }
+      });
+    server.AddSystem(testSystem.systemPtr);
+
+    // Run server and check that vehicle didn't move
+    server.Run(true, 1000, false);
+
+    EXPECT_EQ(1000u, poses.size());
+
+    for (const auto &pose : poses)
+    {
+      EXPECT_EQ(poses[0], pose);
+    }
+
+    // Publish command and check that vehicle moved
+    transport::Node node;
+    auto pub = node.Advertise<msgs::Twist>(_cmdVelTopic);
+
+    msgs::Twist msg;
+
+    const double desiredLinVel = 10.5;
+    msgs::Set(msg.mutable_linear(),
+              math::Vector3d(desiredLinVel, 0, 0));
+    msgs::Set(msg.mutable_angular(),
+              math::Vector3d(0.0, 0, 0));
+    pub.Publish(msg);
+
+    server.Run(true, 1000, false);
+
+    msgs::Set(msg.mutable_linear(),
+              math::Vector3d(0, 0, 0));
+    msgs::Set(msg.mutable_angular(),
+              math::Vector3d(0.0, 0, 0));
+    pub.Publish(msg);
+
+    server.Run(true, 2000, false);
+
+    // Poses for 4s
+    ASSERT_EQ(4000u, poses.size());
+
+    server.Run(true, 1000, false);
+
+    msgs::Set(msg.mutable_linear(),
+              math::Vector3d(desiredLinVel, 0, 0));
+    msgs::Set(msg.mutable_angular(),
+              math::Vector3d(0.0, 0, 0));
+    pub.Publish(msg);
+
+    server.Run(true, 4000, false);
+
+    EXPECT_EQ(9000u, poses.size());
+
+    for (uint64_t i = 4001; i < poses.size(); ++i)
+    {
+      EXPECT_EQ(poses[3999], poses[i]);
+    }
+  }
+};
+
+/////////////////////////////////////////////////
+TEST_P(HaltMotionTest, PublishCmd)
+{
+  TestPublishCmd(
+      std::string(PROJECT_SOURCE_PATH) + "/test/worlds/diff_drive.sdf",
+      "/model/vehicle/cmd_vel");
+}
+
+// Run multiple times
+INSTANTIATE_TEST_SUITE_P(ServerRepeat, HaltMotionTest,
+    ::testing::Range(1, 2));