From 9f0c918a9404b8b6ac594b419c749c9f5dae4bff Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Tue, 30 Mar 2021 16:40:13 -0300 Subject: [PATCH 1/7] tested halt motion feature Signed-off-by: Tomas Lorente --- .../ignition/gazebo/components/HaltMotion.hh | 42 ++++++++++++++ .../KineticEnergyMonitor.cc | 58 +++++++++++++++++-- .../KineticEnergyMonitor.hh | 9 ++- src/systems/physics/Physics.cc | 12 +++- 4 files changed, 113 insertions(+), 8 deletions(-) create mode 100644 include/ignition/gazebo/components/HaltMotion.hh diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh new file mode 100644 index 0000000000..5a8fa8bde1 --- /dev/null +++ b/include/ignition/gazebo/components/HaltMotion.hh @@ -0,0 +1,42 @@ +/* + * 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_HALTMOTION_HH_ +#define IGNITION_GAZEBO_COMPONENTS_HALTMOTION_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 indicate that an entity casts shadows + /// e.g. visual entities + 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..8453864459 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -27,10 +27,12 @@ #include #include #include +#include #include #include #include #include +#include #include @@ -62,6 +64,9 @@ class ignition::gazebo::systems::KineticEnergyMonitorPrivate /// \brief The model this plugin is attached to. public: Model model; + + /// \brief This model halt motion state. + public: bool haltMotionState {false}; }; ////////////////////////////////////////////////// @@ -161,12 +166,53 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldAngularVelocity()); } + + // Create a halt motion component if one is not present. + if (!_ecm.Component( + _ecm.ParentEntity(this->dataPtr->linkEntity))) + { + _ecm.CreateComponent(_ecm.ParentEntity(this->dataPtr->linkEntity), + components::HaltMotion(false)); + } } ////////////////////////////////////////////////// -void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/, +void KineticEnergyMonitor::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("KineticEnergyMonitor::Update"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + if (_info.paused) + return; + + // Update Halt Motion component + auto *haltMotionComp = + _ecm.Component( + _ecm.ParentEntity(this->dataPtr->linkEntity)); + + if (haltMotionComp->Data() != this->dataPtr->haltMotionState) + { + haltMotionComp->Data() = this->dataPtr->haltMotionState; + } +} + +////////////////////////////////////////////////// +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); @@ -186,15 +232,17 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/, msgs::Double msg; msg.set_data(deltaKE); this->dataPtr->pub.Publish(msg); + this->dataPtr->haltMotionState = true; } } } } -IGNITION_ADD_PLUGIN(KineticEnergyMonitor, System, - KineticEnergyMonitor::ISystemConfigure, - KineticEnergyMonitor::ISystemPostUpdate -) +IGNITION_ADD_PLUGIN(KineticEnergyMonitor, + ignition::gazebo::System, + KineticEnergyMonitor::ISystemConfigure, + KineticEnergyMonitor::ISystemUpdate, + 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..97da5ea494 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -97,9 +97,10 @@ namespace systems \endverbatim */ - class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor: - public System, + class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor + : public System, public ISystemConfigure, + public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -114,6 +115,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + /// Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 5a3daf3357..156f48fe58 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" @@ -1232,8 +1233,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) From 19acc8de41d076c93827c001ce3898450ae38752 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Wed, 31 Mar 2021 17:06:56 -0300 Subject: [PATCH 2/7] added flag to turn off movement halting Signed-off-by: Tomas Lorente --- .../kinetic_energy_monitor/KineticEnergyMonitor.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 8453864459..14ad3b4ea9 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -67,6 +67,9 @@ class ignition::gazebo::systems::KineticEnergyMonitorPrivate /// \brief This model halt motion state. public: bool haltMotionState {false}; + + /// \brief Halting motion Mode + public: bool haltMode; }; ////////////////////////////////////////////////// @@ -129,6 +132,8 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, ignmsg << "KineticEnergyMonitor publishing messages on " << "[" << topic << "]" << std::endl; + this->dataPtr->haltMode = sdfClone->Get("halt_mode", false).first; + transport::Node node; this->dataPtr->pub = node.Advertise(topic); @@ -198,7 +203,8 @@ void KineticEnergyMonitor::Update(const UpdateInfo &_info, _ecm.Component( _ecm.ParentEntity(this->dataPtr->linkEntity)); - if (haltMotionComp->Data() != this->dataPtr->haltMotionState) + if (this->dataPtr->haltMode && + haltMotionComp->Data() != this->dataPtr->haltMotionState) { haltMotionComp->Data() = this->dataPtr->haltMotionState; } From 67c262b407627c5cb3312dd40dcad920ca1b28f9 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Wed, 21 Apr 2021 12:39:43 -0300 Subject: [PATCH 3/7] Moved logic into gamelogic Signed-off-by: Tomas Lorente --- .../ignition/gazebo/components/HaltMotion.hh | 8 ++-- .../KineticEnergyMonitor.cc | 38 +------------------ .../KineticEnergyMonitor.hh | 5 --- 3 files changed, 5 insertions(+), 46 deletions(-) diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh index 5a8fa8bde1..2b44fa5bb8 100644 --- a/include/ignition/gazebo/components/HaltMotion.hh +++ b/include/ignition/gazebo/components/HaltMotion.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_COMPONENTS_HALTMOTION_HH_ -#define IGNITION_GAZEBO_COMPONENTS_HALTMOTION_HH_ +#ifndef IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_ +#define IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_ #include #include @@ -29,8 +29,7 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component used to indicate that an entity casts shadows - /// e.g. visual entities + /// \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) @@ -38,5 +37,4 @@ namespace components } } } - #endif diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 14ad3b4ea9..900db3c8b4 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -65,9 +65,6 @@ class ignition::gazebo::systems::KineticEnergyMonitorPrivate /// \brief The model this plugin is attached to. public: Model model; - /// \brief This model halt motion state. - public: bool haltMotionState {false}; - /// \brief Halting motion Mode public: bool haltMode; }; @@ -172,8 +169,8 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, components::WorldAngularVelocity()); } - // Create a halt motion component if one is not present. - if (!_ecm.Component( + // Create a halt motion component if halt mode is true and one is not present. + if (this->dataPtr->haltMode && !_ecm.Component( _ecm.ParentEntity(this->dataPtr->linkEntity))) { _ecm.CreateComponent(_ecm.ParentEntity(this->dataPtr->linkEntity), @@ -181,35 +178,6 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, } } -////////////////////////////////////////////////// -void KineticEnergyMonitor::Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ - IGN_PROFILE("KineticEnergyMonitor::Update"); - - // \TODO(anyone) Support rewind - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - ignwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - if (_info.paused) - return; - - // Update Halt Motion component - auto *haltMotionComp = - _ecm.Component( - _ecm.ParentEntity(this->dataPtr->linkEntity)); - - if (this->dataPtr->haltMode && - haltMotionComp->Data() != this->dataPtr->haltMotionState) - { - haltMotionComp->Data() = this->dataPtr->haltMotionState; - } -} - ////////////////////////////////////////////////// void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) @@ -238,7 +206,6 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info, msgs::Double msg; msg.set_data(deltaKE); this->dataPtr->pub.Publish(msg); - this->dataPtr->haltMotionState = true; } } } @@ -247,7 +214,6 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info, IGNITION_ADD_PLUGIN(KineticEnergyMonitor, ignition::gazebo::System, KineticEnergyMonitor::ISystemConfigure, - KineticEnergyMonitor::ISystemUpdate, KineticEnergyMonitor::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(KineticEnergyMonitor, diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index 97da5ea494..ad01b1a60d 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -100,7 +100,6 @@ namespace systems class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor : public System, public ISystemConfigure, - public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -115,10 +114,6 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; - /// Documentation inherited - public: void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; From f5c472e50ec404db05773307ebd2f190c3a45fc2 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Wed, 21 Apr 2021 12:45:36 -0300 Subject: [PATCH 4/7] docs Signed-off-by: Tomas Lorente --- src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index ad01b1a60d..ce8b1e1a72 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -52,6 +52,10 @@ namespace systems /// energy surpasses the threshold. This element if optional, and the /// default value is `/model/{name_of_model}/kinetic_energy`. /// + /// ``: Boolean that decides if a Halt Motion Component for that + /// model is created. If created, a plugin can use this to disable the model's + /// joints. This element if optional, and the default value is `false`. + /// /// # Example Usage /// /** \verbatim From 4a9f5518712fee29bfb9a8069e5df10e75d970a6 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Fri, 23 Apr 2021 11:12:41 -0300 Subject: [PATCH 5/7] moved everything into gamelogic Signed-off-by: Tomas Lorente --- .../kinetic_energy_monitor/KineticEnergyMonitor.cc | 14 -------------- .../kinetic_energy_monitor/KineticEnergyMonitor.hh | 4 ---- 2 files changed, 18 deletions(-) diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 900db3c8b4..3726eb69e9 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include @@ -64,9 +63,6 @@ class ignition::gazebo::systems::KineticEnergyMonitorPrivate /// \brief The model this plugin is attached to. public: Model model; - - /// \brief Halting motion Mode - public: bool haltMode; }; ////////////////////////////////////////////////// @@ -129,8 +125,6 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, ignmsg << "KineticEnergyMonitor publishing messages on " << "[" << topic << "]" << std::endl; - this->dataPtr->haltMode = sdfClone->Get("halt_mode", false).first; - transport::Node node; this->dataPtr->pub = node.Advertise(topic); @@ -168,14 +162,6 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldAngularVelocity()); } - - // Create a halt motion component if halt mode is true and one is not present. - if (this->dataPtr->haltMode && !_ecm.Component( - _ecm.ParentEntity(this->dataPtr->linkEntity))) - { - _ecm.CreateComponent(_ecm.ParentEntity(this->dataPtr->linkEntity), - components::HaltMotion(false)); - } } ////////////////////////////////////////////////// diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index ce8b1e1a72..ad01b1a60d 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -52,10 +52,6 @@ namespace systems /// energy surpasses the threshold. This element if optional, and the /// default value is `/model/{name_of_model}/kinetic_energy`. /// - /// ``: Boolean that decides if a Halt Motion Component for that - /// model is created. If created, a plugin can use this to disable the model's - /// joints. This element if optional, and the default value is `false`. - /// /// # Example Usage /// /** \verbatim From 45044f7ae16468d197bb42619cfd0ceb5509e551 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Fri, 14 May 2021 13:06:14 -0300 Subject: [PATCH 6/7] test added Signed-off-by: Tomas Lorente --- test/integration/CMakeLists.txt | 1 + test/integration/halt_motion.cc | 149 ++++++++++++++++++++++++++++++++ 2 files changed, 150 insertions(+) create mode 100644 test/integration/halt_motion.cc diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 683b7a9472..5b438bf461 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..ea06d5b1c7 --- /dev/null +++ b/test/integration/halt_motion.cc @@ -0,0 +1,149 @@ +#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 (long unsigned int 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)); From 6b19cd7296db06988a095dd37dfa8270556c6f38 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 17 May 2021 17:10:45 -0700 Subject: [PATCH 7/7] Fix codecheck Signed-off-by: Nate Koenig --- test/integration/halt_motion.cc | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/test/integration/halt_motion.cc b/test/integration/halt_motion.cc index ea06d5b1c7..ec3b6c0159 100644 --- a/test/integration/halt_motion.cc +++ b/test/integration/halt_motion.cc @@ -1,3 +1,20 @@ +/* + * 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 @@ -71,7 +88,7 @@ class HaltMotionTest : public ::testing::TestWithParam EXPECT_NE(kNullEntity, model); if (!_ecm.Component(model)) { - _ecm.CreateComponent(model,components::HaltMotion(false)); + _ecm.CreateComponent(model, components::HaltMotion(false)); } if (poses.size() == 4000u && !_ecm.Component(model)->Data()) @@ -129,7 +146,7 @@ class HaltMotionTest : public ::testing::TestWithParam EXPECT_EQ(9000u, poses.size()); - for (long unsigned int i = 4001; i < poses.size(); i++) + for (uint64_t i = 4001; i < poses.size(); ++i) { EXPECT_EQ(poses[3999], poses[i]); }