diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh new file mode 100644 index 0000000000..2b44fa5bb8 --- /dev/null +++ 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 +#include +#include + +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; + 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 index 60b1ab327e..3726eb69e9 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include @@ -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 index de037608bd..ad01b1a60d 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -97,8 +97,8 @@ namespace systems \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 index 90447526f9..617b58e2f3 100644 --- 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( + _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 index d3c20875e3..f95d17fd03 100644 --- 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 new file mode 100644 index 0000000000..ec3b6c0159 --- /dev/null +++ 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 +#include +#include +#include +#include + +#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 +{ + // 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 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(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(model)) + { + _ecm.CreateComponent(model, components::HaltMotion(false)); + } + if (poses.size() == 4000u && + !_ecm.Component(model)->Data()) + { + _ecm.Component(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(_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));