From 3466e4066a93e2a81bc083b0203c5fb97acaba67 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 3 Dec 2021 17:01:53 -0300 Subject: [PATCH 01/40] Add wheel slip command Signed-off-by: Ivan Santiago Paunovic --- .../gazebo/components/WheelSlipCmd.hh | 46 ++++++++ src/systems/user_commands/UserCommands.cc | 102 ++++++++++++++++++ 2 files changed, 148 insertions(+) create mode 100644 include/ignition/gazebo/components/WheelSlipCmd.hh diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh new file mode 100644 index 0000000000..07b4962ffb --- /dev/null +++ b/include/ignition/gazebo/components/WheelSlipCmd.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_WHEELSLIPCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains commanded wheel slip parameters of an + /// entity in the world frame represented by msgs::WheelSlip. + using WheelSlipCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", + WheelSlipCmd) +} +} +} +} +#endif diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 1bd29a7d96..39fdb008df 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -58,6 +59,7 @@ #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -352,6 +354,32 @@ class VisualCommand : public UserCommandBase aMaterial.emissive().a(), bMaterial.emissive().a(), 1e-6f); }}; }; + +/// \brief Command to modify a wheel entity from simulation. +class WheelSlipCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the wheel slip parameters. + /// \param[in] _iface Pointer to user commands interface. + public: WheelSlipCommand(msgs::WheelSlip *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief WheelSlip equality comparision function + public: std::function + wheelSlipEql { [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) + { + return + _a.name() == _b.name() && + _a.id() == _b.id() && + math::equal( + _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), 1e-6) && + math::equal( + _a.slip_compliance_longitudinal(), _b.slip_compliance_longitudinal(), 1e-6); + }}; +}; } } } @@ -428,6 +456,13 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); + /// \brief Callback for wheel slip service + /// \param[in] _req Request containing wheel slip parameter updates of an entity + /// \param[out] _res True if message sucessfully received and queued. + /// It does not mean that the wheel slip parameters will be successfully updated + /// \return True if successful. + public: bool WheelSlipService(const msgs::WheelSlip &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -574,6 +609,14 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::VisualService, this->dataPtr.get()); ignmsg << "Material service on [" << visualService << "]" << std::endl; + + // Wheel slip service + std::string wheelSlipService + {"/world/" + worldName + "/wheel_slip"}; + this->dataPtr->node.Advertise(wheelSlipService, + &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); + + ignmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -778,6 +821,24 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlip &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -1387,6 +1448,47 @@ bool VisualCommand::Execute() return true; } +////////////////////////////////////////////////// +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool WheelSlipCommand::Execute() +{ + auto wheelSlipMsg = dynamic_cast(this->msg); + if (nullptr == wheelSlipMsg) + { + ignerr << "Internal error, null wheel slip message" << std::endl; + return false; + } + + if (wheelSlipMsg->id() == kNullEntity) + { + ignerr << "Failed to find wheel entity" << std::endl; + return false; + } + + Entity wheelSlipCmdEntity = wheelSlipMsg->id(); + auto wheelSlipCmdComp = + this->iface->ecm->Component(wheelSlipCmdEntity); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + wheelSlipCmdEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + wheelSlipCmdEntity, components::WheelSlipCmd::typeId, state); + } + return true; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate From c3b422f7717be1ee1deccaba33351c7bd0b571f0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 6 Dec 2021 15:11:50 -0300 Subject: [PATCH 02/40] Handle WheelSlipCmd Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 45 ++++++++++++++++++++--- src/systems/wheel_slip/WheelSlip.cc | 20 +++++++++- 2 files changed, 57 insertions(+), 8 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 39fdb008df..e2644f07ba 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1465,26 +1465,59 @@ bool WheelSlipCommand::Execute() return false; } - if (wheelSlipMsg->id() == kNullEntity) + Entity wheelSlipEntity{wheelSlipMsg->id()}; + + if (kNullEntity == wheelSlipEntity && !wheelSlipMsg->name().empty()) + { + Entity wheelSlipParentEntity{wheelSlipMsg->parent_id()}; + if (kNullEntity != wheelSlipParentEntity) + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name()), + components::ParentEntity(wheelSlipParentEntity)); + } + else if (!wheelSlipMsg->parent_name().empty()) + { + auto possible_parents = this->iface->ecm->EntitiesByComponents( + components::Name(wheelSlipMsg->parent_name())); + for (const auto possible_parent : possible_parents) + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name()), + components::ParentEntity(possible_parent)); + if (kNullEntity != wheelSlipEntity) { + break; + } + } + } + else + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name())); + } + } + if (kNullEntity == wheelSlipEntity) { - ignerr << "Failed to find wheel entity" << std::endl; + ignerr << "Failed to find wheel slip with name [" << wheelSlipMsg->name() + << "], ID [" << wheelSlipMsg->id() << "], parent ID [" + << wheelSlipMsg->parent_id() << "] and parent name [" + << wheelSlipMsg->parent_name() << "]." << std::endl; return false; } - Entity wheelSlipCmdEntity = wheelSlipMsg->id(); auto wheelSlipCmdComp = - this->iface->ecm->Component(wheelSlipCmdEntity); + this->iface->ecm->Component(wheelSlipEntity); if (!wheelSlipCmdComp) { this->iface->ecm->CreateComponent( - wheelSlipCmdEntity, components::WheelSlipCmd(*wheelSlipMsg)); + wheelSlipEntity, components::WheelSlipCmd(*wheelSlipMsg)); } else { auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged( - wheelSlipCmdEntity, components::WheelSlipCmd::typeId, state); + wheelSlipEntity, components::WheelSlipCmd::typeId, state); } return true; } diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..59b31549db 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -242,9 +243,24 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, ///////////////////////////////////////////////// void WheelSlipPrivate::Update(EntityComponentManager &_ecm) { - for (const auto &linkSurface : this->mapLinkSurfaceParams) + for (auto &linkSurface : this->mapLinkSurfaceParams) { - const auto ¶ms = linkSurface.second; + auto ¶ms = linkSurface.second; + const auto * wheelSlipCmdComp = + _ecm.Component(linkSurface.first); + if (wheelSlipCmdComp) { + const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); + bool changed = math::equal( + params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), 1e-6) && + math::equal( + params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); + + if (changed) { + _ecm.RemoveComponent(linkSurface.first); + params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + } + } // get user-defined normal force constant double force = params.wheelNormalForce; From df40169ed851ca7dee206dec9fb2fab4772afe16 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 3 Jan 2022 16:53:12 -0300 Subject: [PATCH 03/40] Use validWorldName instead of worldName in topics MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Alejandro Hernández Cordero --- src/systems/user_commands/UserCommands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index e2644f07ba..7dc2538de5 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -612,7 +612,7 @@ void UserCommands::Configure(const Entity &_entity, // Wheel slip service std::string wheelSlipService - {"/world/" + worldName + "/wheel_slip"}; + {"/world/" + validWorldName + "/wheel_slip"}; this->dataPtr->node.Advertise(wheelSlipService, &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); From e9220f12e46b44a18932bd0269ab4e787d120309 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 10:11:29 -0300 Subject: [PATCH 04/40] linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 21 ++++++++++++++------- src/systems/wheel_slip/WheelSlip.cc | 16 ++++++++++++---- 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 7dc2538de5..8c11b902f0 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -369,15 +369,19 @@ class WheelSlipCommand : public UserCommandBase /// \brief WheelSlip equality comparision function public: std::function - wheelSlipEql { [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) - { + wheelSlipEql { + [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) { return _a.name() == _b.name() && _a.id() == _b.id() && math::equal( - _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), 1e-6) && + _a.slip_compliance_lateral(), + _b.slip_compliance_lateral(), + 1e-6) && math::equal( - _a.slip_compliance_longitudinal(), _b.slip_compliance_longitudinal(), 1e-6); + _a.slip_compliance_longitudinal(), + _b.slip_compliance_longitudinal(), + 1e-6); }}; }; } @@ -457,11 +461,14 @@ class ignition::gazebo::systems::UserCommandsPrivate public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); /// \brief Callback for wheel slip service - /// \param[in] _req Request containing wheel slip parameter updates of an entity + /// \param[in] _req Request containing wheel slip parameter updates of an + /// entity. /// \param[out] _res True if message sucessfully received and queued. - /// It does not mean that the wheel slip parameters will be successfully updated + /// It does not mean that the wheel slip parameters will be successfully + /// updated. /// \return True if successful. - public: bool WheelSlipService(const msgs::WheelSlip &_req, msgs::Boolean &_res); + public: bool WheelSlipService( + const msgs::WheelSlip &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 59b31549db..dcef56375b 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -108,7 +108,9 @@ class ignition::gazebo::systems::WheelSlipPrivate { if (_a.size() != _b.size() || _a.size() < 2 ||_b.size() < 2) + { return false; + } for (size_t i = 0; i < _a.size(); i++) { @@ -251,14 +253,20 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (wheelSlipCmdComp) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = math::equal( - params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), 1e-6) && + params.slipComplianceLateral, + wheelSlipCmdParams.slip_compliance_lateral(), + 1e-6) && math::equal( - params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); + params.slipComplianceLongitudinal, + wheelSlipCmdParams.slip_compliance_longitudinal(), + 1e-6); if (changed) { _ecm.RemoveComponent(linkSurface.first); - params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); - params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + params.slipComplianceLateral = + wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = + wheelSlipCmdParams.slip_compliance_longitudinal(); } } From a1030753f1fc695fc6954a836c8b3c79fd31f164 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 10:13:13 -0300 Subject: [PATCH 05/40] more linters Signed-off-by: Ivan Santiago Paunovic --- include/ignition/gazebo/components/WheelSlipCmd.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 07b4962ffb..11aa75645f 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -33,8 +33,8 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component type that contains commanded wheel slip parameters of an - /// entity in the world frame represented by msgs::WheelSlip. + /// \brief A component type that contains commanded wheel slip parameters of + /// an entity in the world frame represented by msgs::WheelSlip. using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", From 7c2b08713012e714036d7b6e63805a1a6ac3d83c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 13:25:13 -0300 Subject: [PATCH 06/40] msgs::WheelSlip -> msgs::WheelSlipParameters Signed-off-by: Ivan Santiago Paunovic --- .../ignition/gazebo/components/WheelSlipCmd.hh | 6 +++--- src/systems/user_commands/UserCommands.cc | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 11aa75645f..29b985b6bc 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ignition { @@ -34,8 +34,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief A component type that contains commanded wheel slip parameters of - /// an entity in the world frame represented by msgs::WheelSlip. - using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", WheelSlipCmd) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8c11b902f0..ad5d93488c 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -361,16 +361,16 @@ class WheelSlipCommand : public UserCommandBase /// \brief Constructor /// \param[in] _msg Message containing the wheel slip parameters. /// \param[in] _iface Pointer to user commands interface. - public: WheelSlipCommand(msgs::WheelSlip *_msg, + public: WheelSlipCommand(msgs::WheelSlipParameters *_msg, std::shared_ptr &_iface); // Documentation inherited public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) { + [](const msgs::WheelSlipParameters &_a, const msgs::WheelSlipParameters &_b) { return _a.name() == _b.name() && _a.id() == _b.id() && @@ -468,7 +468,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// updated. /// \return True if successful. public: bool WheelSlipService( - const msgs::WheelSlip &_req, msgs::Boolean &_res); + const msgs::WheelSlipParameters &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -829,7 +829,7 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlip &_req, +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParameters &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1456,7 +1456,7 @@ bool VisualCommand::Execute() } ////////////////////////////////////////////////// -WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, std::shared_ptr &_iface) : UserCommandBase(_msg, _iface) { @@ -1465,7 +1465,7 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast(this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; From 40949e2649b27ca0b9b96d53369c526da7ed37a4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 12 Jan 2022 11:07:12 -0300 Subject: [PATCH 07/40] Address peer review comment Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index ad5d93488c..6c7507cb1a 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -374,6 +374,8 @@ class WheelSlipCommand : public UserCommandBase return _a.name() == _b.name() && _a.id() == _b.id() && + _a.parent_id() == _b.parent_id() && + _a.parent_name() == _b.parent_name() && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), From 979676d3139af9b5c2f671a7639a66af9b4b0fcc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 2 Feb 2022 17:51:09 -0300 Subject: [PATCH 08/40] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- src/systems/wheel_slip/WheelSlip.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index dcef56375b..9d5d128055 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -250,7 +250,8 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) auto ¶ms = linkSurface.second; const auto * wheelSlipCmdComp = _ecm.Component(linkSurface.first); - if (wheelSlipCmdComp) { + if (wheelSlipCmdComp) + { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = math::equal( params.slipComplianceLateral, From 401352edb1f2011fcb6f479d92d35be4a7a07e8d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 2 Feb 2022 17:59:45 -0300 Subject: [PATCH 09/40] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- src/systems/wheel_slip/WheelSlip.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 9d5d128055..d63e02c82d 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -262,7 +262,8 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); - if (changed) { + if (changed) + { _ecm.RemoveComponent(linkSurface.first); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); From 530c8aea894fb521dc500e94da260ceff266b1aa Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:04:40 -0300 Subject: [PATCH 10/40] Make it work Signed-off-by: Ivan Santiago Paunovic --- .../gazebo/components/WheelSlipCmd.hh | 4 +- src/systems/user_commands/UserCommands.cc | 72 +++++++------------ src/systems/wheel_slip/WheelSlip.cc | 13 ++-- 3 files changed, 35 insertions(+), 54 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 29b985b6bc..25748dd82d 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ignition { @@ -35,7 +35,7 @@ namespace components { /// \brief A component type that contains commanded wheel slip parameters of /// an entity in the world frame represented by msgs::WheelSlipParameters. - using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", WheelSlipCmd) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 6c7507cb1a..c3e18e9a88 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -54,6 +54,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" @@ -361,21 +362,19 @@ class WheelSlipCommand : public UserCommandBase /// \brief Constructor /// \param[in] _msg Message containing the wheel slip parameters. /// \param[in] _iface Pointer to user commands interface. - public: WheelSlipCommand(msgs::WheelSlipParameters *_msg, + public: WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, std::shared_ptr &_iface); // Documentation inherited public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlipParameters &_a, const msgs::WheelSlipParameters &_b) { + [](const msgs::WheelSlipParametersCmd &_a, const msgs::WheelSlipParametersCmd &_b) { return - _a.name() == _b.name() && - _a.id() == _b.id() && - _a.parent_id() == _b.parent_id() && - _a.parent_name() == _b.parent_name() && + _a.model_name() == _b.model_name() && + _a.link_name() == _b.link_name() && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), @@ -470,7 +469,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// updated. /// \return True if successful. public: bool WheelSlipService( - const msgs::WheelSlipParameters &_req, msgs::Boolean &_res); + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -831,7 +830,7 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParameters &_req, +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1458,7 +1457,7 @@ bool VisualCommand::Execute() } ////////////////////////////////////////////////// -WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, std::shared_ptr &_iface) : UserCommandBase(_msg, _iface) { @@ -1467,66 +1466,45 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast(this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; return false; } - Entity wheelSlipEntity{wheelSlipMsg->id()}; - if (kNullEntity == wheelSlipEntity && !wheelSlipMsg->name().empty()) + Entity modelEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->model_name())); + if (kNullEntity == modelEntity) { - Entity wheelSlipParentEntity{wheelSlipMsg->parent_id()}; - if (kNullEntity != wheelSlipParentEntity) - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name()), - components::ParentEntity(wheelSlipParentEntity)); - } - else if (!wheelSlipMsg->parent_name().empty()) - { - auto possible_parents = this->iface->ecm->EntitiesByComponents( - components::Name(wheelSlipMsg->parent_name())); - for (const auto possible_parent : possible_parents) - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name()), - components::ParentEntity(possible_parent)); - if (kNullEntity != wheelSlipEntity) { - break; - } - } - } - else - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name())); - } + ignerr << "Failed to find model with name [" << wheelSlipMsg->model_name() + << "]." << std::endl; + return false; } - if (kNullEntity == wheelSlipEntity) + Model model{modelEntity}; + Entity linkEntity = model.LinkByName( + *this->iface->ecm, wheelSlipMsg->link_name()); + if (kNullEntity == linkEntity) { - ignerr << "Failed to find wheel slip with name [" << wheelSlipMsg->name() - << "], ID [" << wheelSlipMsg->id() << "], parent ID [" - << wheelSlipMsg->parent_id() << "] and parent name [" - << wheelSlipMsg->parent_name() << "]." << std::endl; + ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() + << "] for model [" << wheelSlipMsg->model_name() << "]." << std::endl; return false; } auto wheelSlipCmdComp = - this->iface->ecm->Component(wheelSlipEntity); + this->iface->ecm->Component(linkEntity); if (!wheelSlipCmdComp) { this->iface->ecm->CreateComponent( - wheelSlipEntity, components::WheelSlipCmd(*wheelSlipMsg)); + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); } else { auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged( - wheelSlipEntity, components::WheelSlipCmd::typeId, state); + linkEntity, components::WheelSlipCmd::typeId, state); } return true; } diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index d63e02c82d..27c6eb7109 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -253,23 +253,26 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (wheelSlipCmdComp) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); - bool changed = math::equal( + bool changed = (!math::equal( params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), - 1e-6) && - math::equal( + 1e-6)) || + (!math::equal( params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), - 1e-6); + 1e-6)); if (changed) { - _ecm.RemoveComponent(linkSurface.first); + fprintf(stderr, "changed wheel slip parameters!\n"); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + } else { + fprintf(stderr, "NOT changed wheel slip parameters!\n"); } + _ecm.RemoveComponent(linkSurface.first); } // get user-defined normal force constant From 866bd93fff75b39290f1f2d72d1a2d176afe2f98 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:05:25 -0300 Subject: [PATCH 11/40] Remove debugging messages Signed-off-by: Ivan Santiago Paunovic --- src/systems/wheel_slip/WheelSlip.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 27c6eb7109..78385295de 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -264,13 +264,10 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (changed) { - fprintf(stderr, "changed wheel slip parameters!\n"); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); - } else { - fprintf(stderr, "NOT changed wheel slip parameters!\n"); } _ecm.RemoveComponent(linkSurface.first); } From 8a9c7542db295021c8e0ab60301b348df36dd3b9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:32:29 -0300 Subject: [PATCH 12/40] linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c3e18e9a88..a7e54cae34 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -369,9 +369,13 @@ class WheelSlipCommand : public UserCommandBase public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlipParametersCmd &_a, const msgs::WheelSlipParametersCmd &_b) { + []( + const msgs::WheelSlipParametersCmd &_a, + const msgs::WheelSlipParametersCmd &_b) + { return _a.model_name() == _b.model_name() && _a.link_name() == _b.link_name() && @@ -830,7 +834,8 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParametersCmd &_req, +bool UserCommandsPrivate::WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1466,7 +1471,8 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast( + this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; @@ -1488,7 +1494,8 @@ bool WheelSlipCommand::Execute() if (kNullEntity == linkEntity) { ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." << std::endl; + << "] for model [" << wheelSlipMsg->model_name() << "]." + << std::endl; return false; } From b5d739e5280a31c9e9cdc3e4045b7c398d75ffa2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 1 Mar 2022 16:50:08 -0300 Subject: [PATCH 13/40] Add `link_name: '*'` option. Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 53 ++++++++++++++--------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index a7e54cae34..27d51fd660 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1489,29 +1489,40 @@ bool WheelSlipCommand::Execute() return false; } Model model{modelEntity}; - Entity linkEntity = model.LinkByName( - *this->iface->ecm, wheelSlipMsg->link_name()); - if (kNullEntity == linkEntity) - { - ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." - << std::endl; - return false; - } + auto linkName = wheelSlipMsg->link_name(); - auto wheelSlipCmdComp = + auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { + auto wheelSlipCmdComp = this->iface->ecm->Component(linkEntity); - if (!wheelSlipCmdComp) - { - this->iface->ecm->CreateComponent( - linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); - } - else - { - auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? - ComponentState::OneTimeChange : ComponentState::NoChange; - this->iface->ecm->SetChanged( - linkEntity, components::WheelSlipCmd::typeId, state); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + linkEntity, components::WheelSlipCmd::typeId, state); + } + }; + + if (linkName != "*") { + Entity linkEntity = model.LinkByName( + *this->iface->ecm, wheelSlipMsg->link_name()); + if (kNullEntity == linkEntity) + { + ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() + << "] for model [" << wheelSlipMsg->model_name() << "]." + << std::endl; + return false; + } + doForEachLink(linkEntity); + } else { + for (const auto & linkEntity : model.Links(*this->iface->ecm)) { + doForEachLink(linkEntity); + } } return true; } From cc6295a8ee03fc09bdd6ae3f8135686e2061ae6c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Mar 2022 15:41:08 -0300 Subject: [PATCH 14/40] Please linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 27d51fd660..3b92b8f412 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1501,8 +1501,9 @@ bool WheelSlipCommand::Execute() } else { - auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? - ComponentState::OneTimeChange : ComponentState::NoChange; + auto state = wheelSlipCmdComp->SetData( + *wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange + : ComponentState::NoChange; this->iface->ecm->SetChanged( linkEntity, components::WheelSlipCmd::typeId, state); } From 4377757a65e9c4a8a9390604503cf8fdea83ffc9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Mar 2022 15:41:34 -0300 Subject: [PATCH 15/40] add minimal test case Signed-off-by: Ivan Santiago Paunovic --- test/integration/user_commands.cc | 101 ++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 390f4a4a2e..6a980d72ea 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -33,7 +33,9 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -1012,3 +1014,102 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Physics)) EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, WheelSlip) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/trisphere_cycle_wheel_slip.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + ASSERT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + ASSERT_NE(kNullEntity, worldEntity); + Entity tc0 = ecm->EntityByComponents( + components::Name("trisphere_cycle0")); + ASSERT_NE(kNullEntity, tc0); + Entity tc1 = ecm->EntityByComponents( + components::Name("trisphere_cycle1")); + ASSERT_NE(kNullEntity, tc1); + + Model tcModel0{tc0}; + Model tcModel1{tc1}; + Entity wf0 = tcModel0.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf0); + Entity wrl0 = tcModel0.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl0); + Entity wrf0 = tcModel0.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf0); + Entity wf1 = tcModel1.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf1); + Entity wrl1 = tcModel1.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl1); + Entity wrf1 = tcModel1.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf1); + + Entity links[] = {wf0, wrl0, wrf0, wf1, wrl1, wrf1}; + for (auto link : links) { + EXPECT_EQ(nullptr, ecm->Component(link)); + } + + // modify wheel slip parameters of one link of model 0 + msgs::WheelSlipParametersCmd req; + req.set_model_name("trisphere_cycle0"); + req.set_link_name("wheel_front"); + req.set_slip_compliance_lateral(1); + req.set_slip_compliance_longitudinal(1); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/wheel_slip/wheel_slip"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 2, false); + + // modify wheel slip parameters of one link of model 1 + req.set_model_name("trisphere_cycle1"); + req.set_link_name("*"); + req.set_slip_compliance_lateral(2); + req.set_slip_compliance_longitudinal(1); + + result = false; + res = msgs::Boolean{}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 3, false);} From 8ea472fd7127a7684a26112ea592a0efdd69a7e1 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 11 Mar 2022 18:01:58 -0300 Subject: [PATCH 16/40] Use msgs::Entity instead of model_name/link_name Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 111 +++++++++++++++++----- test/integration/user_commands.cc | 9 +- 2 files changed, 92 insertions(+), 28 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 3b92b8f412..edbf6569b3 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -42,6 +42,8 @@ #include "ignition/common/Profiler.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/Link.hh" @@ -51,11 +53,13 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -377,8 +381,13 @@ class WheelSlipCommand : public UserCommandBase const msgs::WheelSlipParametersCmd &_b) { return - _a.model_name() == _b.model_name() && - _a.link_name() == _b.link_name() && + ( + (_a.entity().id() != kNullEntity && _a.entity().id() == _b.entity().id()) || + ( + _a.entity().name() == _b.entity().name() && + _a.entity().type() == _b.entity().type() + ) + ) && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), @@ -1468,6 +1477,67 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, { } +// TODO(ivanpauno): Move this somewhere else +Entity scopedEntityFromMsg(const msgs::Entity & _msg, const EntityComponentManager & _ecm) +{ + if (_msg.id() != kNullEntity) { + return _msg.id(); + } + std::unordered_set entities = entitiesFromScopedName(_msg.name(), _ecm); + if (entities.empty()) { + ignerr << "Failed to find entity with scoped name [" << _msg.name() + << "]." << std::endl; + return kNullEntity; + } + if (_msg.type() == msgs::Entity::NONE) { + return *entities.begin(); + } + const components::BaseComponent * component; + std::string componentType; + for (const auto entity : entities) { + switch (_msg.type()) { + case msgs::Entity::LIGHT: + component = _ecm.Component(entity); + componentType = "LIGHT"; + break; + case msgs::Entity::MODEL: + component = _ecm.Component(entity); + componentType = "MODEL"; + break; + case msgs::Entity::LINK: + component = _ecm.Component(entity); + componentType = "LINK"; + break; + case msgs::Entity::VISUAL: + component = _ecm.Component(entity); + componentType = "VISUAL"; + break; + case msgs::Entity::COLLISION: + component = _ecm.Component(entity); + componentType = "COLLISION"; + break; + case msgs::Entity::SENSOR: + component = _ecm.Component(entity); + componentType = "SENSOR"; + break; + case msgs::Entity::JOINT: + component = _ecm.Component(entity); + componentType = "JOINT"; + break; + default: + componentType = "unknown"; + break; + } + if (component != nullptr) { + return entity; + } + } + ignerr << "Found entity with scoped name [" << _msg.name() + << "], but it doesn't have a component of the required type [" + << componentType << "]." << std::endl; + return kNullEntity; +} + ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { @@ -1478,18 +1548,12 @@ bool WheelSlipCommand::Execute() ignerr << "Internal error, null wheel slip message" << std::endl; return false; } - - - Entity modelEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->model_name())); - if (kNullEntity == modelEntity) + const auto & ecm = *this->iface->ecm; + Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + if (kNullEntity == entity) { - ignerr << "Failed to find model with name [" << wheelSlipMsg->model_name() - << "]." << std::endl; return false; } - Model model{modelEntity}; - auto linkName = wheelSlipMsg->link_name(); auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { auto wheelSlipCmdComp = @@ -1508,24 +1572,23 @@ bool WheelSlipCommand::Execute() linkEntity, components::WheelSlipCmd::typeId, state); } }; + const components::BaseComponent * component = ecm.Component(entity); - if (linkName != "*") { - Entity linkEntity = model.LinkByName( - *this->iface->ecm, wheelSlipMsg->link_name()); - if (kNullEntity == linkEntity) - { - ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." - << std::endl; - return false; - } - doForEachLink(linkEntity); - } else { + if (nullptr != component) { + doForEachLink(entity); + return true; + } + component = ecm.Component(entity); + if (nullptr != component) { + Model model{entity}; for (const auto & linkEntity : model.Links(*this->iface->ecm)) { doForEachLink(linkEntity); } + return true; } - return true; + ignerr << "Found entity with scoped name [" << wheelSlipMsg->entity().name() + << "], is neither a model or a link." << std::endl; + return false; } IGNITION_ADD_PLUGIN(UserCommands, System, diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 6a980d72ea..9c29563e6d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1076,8 +1076,9 @@ TEST_F(UserCommandsTest, WheelSlip) // modify wheel slip parameters of one link of model 0 msgs::WheelSlipParametersCmd req; - req.set_model_name("trisphere_cycle0"); - req.set_link_name("wheel_front"); + auto * entityMsg = req.mutable_entity(); + entityMsg->set_name("trisphere_cycle0::wheel_front"); + entityMsg->set_type(msgs::Entity::LINK); req.set_slip_compliance_lateral(1); req.set_slip_compliance_longitudinal(1); @@ -1097,8 +1098,8 @@ TEST_F(UserCommandsTest, WheelSlip) server.Run(true, 2, false); // modify wheel slip parameters of one link of model 1 - req.set_model_name("trisphere_cycle1"); - req.set_link_name("*"); + entityMsg->set_name("trisphere_cycle1"); + entityMsg->set_type(msgs::Entity::MODEL); req.set_slip_compliance_lateral(2); req.set_slip_compliance_longitudinal(1); From 75ad4fb9a5a55ca402681669ddce68b8b35d1e0c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 15 Mar 2022 16:54:37 -0300 Subject: [PATCH 17/40] copyright notice year: 2021 -> 2022 Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Aditya Pande --- include/ignition/gazebo/components/WheelSlipCmd.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 25748dd82d..4daf7ee249 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 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. From cfb602022298d4a20b9b2bc713d8803e85a797ac Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 15 Mar 2022 18:07:18 -0300 Subject: [PATCH 18/40] Make codecheck pass Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index edbf6569b3..2683860a69 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -382,7 +383,10 @@ class WheelSlipCommand : public UserCommandBase { return ( - (_a.entity().id() != kNullEntity && _a.entity().id() == _b.entity().id()) || + ( + _a.entity().id() != kNullEntity && + _a.entity().id() == _b.entity().id() + ) || ( _a.entity().name() == _b.entity().name() && _a.entity().type() == _b.entity().type() @@ -1478,12 +1482,14 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, } // TODO(ivanpauno): Move this somewhere else -Entity scopedEntityFromMsg(const msgs::Entity & _msg, const EntityComponentManager & _ecm) +Entity scopedEntityFromMsg( + const msgs::Entity & _msg, const EntityComponentManager & _ecm) { if (_msg.id() != kNullEntity) { return _msg.id(); } - std::unordered_set entities = entitiesFromScopedName(_msg.name(), _ecm); + std::unordered_set entities = entitiesFromScopedName( + _msg.name(), _ecm); if (entities.empty()) { ignerr << "Failed to find entity with scoped name [" << _msg.name() << "]." << std::endl; @@ -1572,7 +1578,8 @@ bool WheelSlipCommand::Execute() linkEntity, components::WheelSlipCmd::typeId, state); } }; - const components::BaseComponent * component = ecm.Component(entity); + const components::BaseComponent * component = + ecm.Component(entity); if (nullptr != component) { doForEachLink(entity); From b1ea7eba99a9aa1070844a4654fe06ba4a1d3501 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 21 Mar 2022 15:09:48 -0300 Subject: [PATCH 19/40] Update include Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 2683860a69..5460d166a8 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include From f248c128b84e854d3fbb347275b5df72d6b3b331 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 3 Dec 2021 17:01:53 -0300 Subject: [PATCH 20/40] Add wheel slip command Signed-off-by: Ivan Santiago Paunovic --- .../gazebo/components/WheelSlipCmd.hh | 46 ++++++++ src/systems/user_commands/UserCommands.cc | 102 ++++++++++++++++++ 2 files changed, 148 insertions(+) create mode 100644 include/ignition/gazebo/components/WheelSlipCmd.hh diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh new file mode 100644 index 0000000000..07b4962ffb --- /dev/null +++ b/include/ignition/gazebo/components/WheelSlipCmd.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_WHEELSLIPCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains commanded wheel slip parameters of an + /// entity in the world frame represented by msgs::WheelSlip. + using WheelSlipCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", + WheelSlipCmd) +} +} +} +} +#endif diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 2b56e369c9..058403aef0 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -58,6 +59,7 @@ #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -374,6 +376,32 @@ class VisualCommand : public UserCommandBase aMaterial.emissive().a(), bMaterial.emissive().a(), 1e-6f); }}; }; + +/// \brief Command to modify a wheel entity from simulation. +class WheelSlipCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the wheel slip parameters. + /// \param[in] _iface Pointer to user commands interface. + public: WheelSlipCommand(msgs::WheelSlip *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief WheelSlip equality comparision function + public: std::function + wheelSlipEql { [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) + { + return + _a.name() == _b.name() && + _a.id() == _b.id() && + math::equal( + _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), 1e-6) && + math::equal( + _a.slip_compliance_longitudinal(), _b.slip_compliance_longitudinal(), 1e-6); + }}; +}; } } } @@ -450,6 +478,13 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); + /// \brief Callback for wheel slip service + /// \param[in] _req Request containing wheel slip parameter updates of an entity + /// \param[out] _res True if message sucessfully received and queued. + /// It does not mean that the wheel slip parameters will be successfully updated + /// \return True if successful. + public: bool WheelSlipService(const msgs::WheelSlip &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -596,6 +631,14 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::VisualService, this->dataPtr.get()); ignmsg << "Material service on [" << visualService << "]" << std::endl; + + // Wheel slip service + std::string wheelSlipService + {"/world/" + worldName + "/wheel_slip"}; + this->dataPtr->node.Advertise(wheelSlipService, + &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); + + ignmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -800,6 +843,24 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlip &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -1409,6 +1470,47 @@ bool VisualCommand::Execute() return true; } +////////////////////////////////////////////////// +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool WheelSlipCommand::Execute() +{ + auto wheelSlipMsg = dynamic_cast(this->msg); + if (nullptr == wheelSlipMsg) + { + ignerr << "Internal error, null wheel slip message" << std::endl; + return false; + } + + if (wheelSlipMsg->id() == kNullEntity) + { + ignerr << "Failed to find wheel entity" << std::endl; + return false; + } + + Entity wheelSlipCmdEntity = wheelSlipMsg->id(); + auto wheelSlipCmdComp = + this->iface->ecm->Component(wheelSlipCmdEntity); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + wheelSlipCmdEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + wheelSlipCmdEntity, components::WheelSlipCmd::typeId, state); + } + return true; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate From 6a6c400a5fb37a3c40155d1d24935dded521cd5b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 6 Dec 2021 15:11:50 -0300 Subject: [PATCH 21/40] Handle WheelSlipCmd Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 45 ++++++++++++++++++++--- src/systems/wheel_slip/WheelSlip.cc | 20 +++++++++- 2 files changed, 57 insertions(+), 8 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 058403aef0..4d3346b714 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1487,26 +1487,59 @@ bool WheelSlipCommand::Execute() return false; } - if (wheelSlipMsg->id() == kNullEntity) + Entity wheelSlipEntity{wheelSlipMsg->id()}; + + if (kNullEntity == wheelSlipEntity && !wheelSlipMsg->name().empty()) + { + Entity wheelSlipParentEntity{wheelSlipMsg->parent_id()}; + if (kNullEntity != wheelSlipParentEntity) + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name()), + components::ParentEntity(wheelSlipParentEntity)); + } + else if (!wheelSlipMsg->parent_name().empty()) + { + auto possible_parents = this->iface->ecm->EntitiesByComponents( + components::Name(wheelSlipMsg->parent_name())); + for (const auto possible_parent : possible_parents) + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name()), + components::ParentEntity(possible_parent)); + if (kNullEntity != wheelSlipEntity) { + break; + } + } + } + else + { + wheelSlipEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->name())); + } + } + if (kNullEntity == wheelSlipEntity) { - ignerr << "Failed to find wheel entity" << std::endl; + ignerr << "Failed to find wheel slip with name [" << wheelSlipMsg->name() + << "], ID [" << wheelSlipMsg->id() << "], parent ID [" + << wheelSlipMsg->parent_id() << "] and parent name [" + << wheelSlipMsg->parent_name() << "]." << std::endl; return false; } - Entity wheelSlipCmdEntity = wheelSlipMsg->id(); auto wheelSlipCmdComp = - this->iface->ecm->Component(wheelSlipCmdEntity); + this->iface->ecm->Component(wheelSlipEntity); if (!wheelSlipCmdComp) { this->iface->ecm->CreateComponent( - wheelSlipCmdEntity, components::WheelSlipCmd(*wheelSlipMsg)); + wheelSlipEntity, components::WheelSlipCmd(*wheelSlipMsg)); } else { auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged( - wheelSlipCmdEntity, components::WheelSlipCmd::typeId, state); + wheelSlipEntity, components::WheelSlipCmd::typeId, state); } return true; } diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..59b31549db 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -242,9 +243,24 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, ///////////////////////////////////////////////// void WheelSlipPrivate::Update(EntityComponentManager &_ecm) { - for (const auto &linkSurface : this->mapLinkSurfaceParams) + for (auto &linkSurface : this->mapLinkSurfaceParams) { - const auto ¶ms = linkSurface.second; + auto ¶ms = linkSurface.second; + const auto * wheelSlipCmdComp = + _ecm.Component(linkSurface.first); + if (wheelSlipCmdComp) { + const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); + bool changed = math::equal( + params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), 1e-6) && + math::equal( + params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); + + if (changed) { + _ecm.RemoveComponent(linkSurface.first); + params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + } + } // get user-defined normal force constant double force = params.wheelNormalForce; From 0b732683e2e71856b1cde7d1d5f67f1d5415d614 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 3 Jan 2022 16:53:12 -0300 Subject: [PATCH 22/40] Use validWorldName instead of worldName in topics MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Alejandro Hernández Cordero --- src/systems/user_commands/UserCommands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 4d3346b714..19efd69b12 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -634,7 +634,7 @@ void UserCommands::Configure(const Entity &_entity, // Wheel slip service std::string wheelSlipService - {"/world/" + worldName + "/wheel_slip"}; + {"/world/" + validWorldName + "/wheel_slip"}; this->dataPtr->node.Advertise(wheelSlipService, &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); From 1e7952c7ad50346dccc69a51a674cdd108c2174b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 10:11:29 -0300 Subject: [PATCH 23/40] linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 21 ++++++++++++++------- src/systems/wheel_slip/WheelSlip.cc | 16 ++++++++++++---- 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 19efd69b12..a0409e2488 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -391,15 +391,19 @@ class WheelSlipCommand : public UserCommandBase /// \brief WheelSlip equality comparision function public: std::function - wheelSlipEql { [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) - { + wheelSlipEql { + [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) { return _a.name() == _b.name() && _a.id() == _b.id() && math::equal( - _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), 1e-6) && + _a.slip_compliance_lateral(), + _b.slip_compliance_lateral(), + 1e-6) && math::equal( - _a.slip_compliance_longitudinal(), _b.slip_compliance_longitudinal(), 1e-6); + _a.slip_compliance_longitudinal(), + _b.slip_compliance_longitudinal(), + 1e-6); }}; }; } @@ -479,11 +483,14 @@ class ignition::gazebo::systems::UserCommandsPrivate public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); /// \brief Callback for wheel slip service - /// \param[in] _req Request containing wheel slip parameter updates of an entity + /// \param[in] _req Request containing wheel slip parameter updates of an + /// entity. /// \param[out] _res True if message sucessfully received and queued. - /// It does not mean that the wheel slip parameters will be successfully updated + /// It does not mean that the wheel slip parameters will be successfully + /// updated. /// \return True if successful. - public: bool WheelSlipService(const msgs::WheelSlip &_req, msgs::Boolean &_res); + public: bool WheelSlipService( + const msgs::WheelSlip &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 59b31549db..dcef56375b 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -108,7 +108,9 @@ class ignition::gazebo::systems::WheelSlipPrivate { if (_a.size() != _b.size() || _a.size() < 2 ||_b.size() < 2) + { return false; + } for (size_t i = 0; i < _a.size(); i++) { @@ -251,14 +253,20 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (wheelSlipCmdComp) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = math::equal( - params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), 1e-6) && + params.slipComplianceLateral, + wheelSlipCmdParams.slip_compliance_lateral(), + 1e-6) && math::equal( - params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); + params.slipComplianceLongitudinal, + wheelSlipCmdParams.slip_compliance_longitudinal(), + 1e-6); if (changed) { _ecm.RemoveComponent(linkSurface.first); - params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); - params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + params.slipComplianceLateral = + wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = + wheelSlipCmdParams.slip_compliance_longitudinal(); } } From 12713aa81ed5498785e7aeca38ddb08d0511c55e Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 10:13:13 -0300 Subject: [PATCH 24/40] more linters Signed-off-by: Ivan Santiago Paunovic --- include/ignition/gazebo/components/WheelSlipCmd.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 07b4962ffb..11aa75645f 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -33,8 +33,8 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component type that contains commanded wheel slip parameters of an - /// entity in the world frame represented by msgs::WheelSlip. + /// \brief A component type that contains commanded wheel slip parameters of + /// an entity in the world frame represented by msgs::WheelSlip. using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", From 5db555969f8db90d69737099c17ef57e3d5d58d2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Jan 2022 13:25:13 -0300 Subject: [PATCH 25/40] msgs::WheelSlip -> msgs::WheelSlipParameters Signed-off-by: Ivan Santiago Paunovic --- .../ignition/gazebo/components/WheelSlipCmd.hh | 6 +++--- src/systems/user_commands/UserCommands.cc | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 11aa75645f..29b985b6bc 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ignition { @@ -34,8 +34,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief A component type that contains commanded wheel slip parameters of - /// an entity in the world frame represented by msgs::WheelSlip. - using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", WheelSlipCmd) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index a0409e2488..5dbd5d7631 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -383,16 +383,16 @@ class WheelSlipCommand : public UserCommandBase /// \brief Constructor /// \param[in] _msg Message containing the wheel slip parameters. /// \param[in] _iface Pointer to user commands interface. - public: WheelSlipCommand(msgs::WheelSlip *_msg, + public: WheelSlipCommand(msgs::WheelSlipParameters *_msg, std::shared_ptr &_iface); // Documentation inherited public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlip &_a, const msgs::WheelSlip &_b) { + [](const msgs::WheelSlipParameters &_a, const msgs::WheelSlipParameters &_b) { return _a.name() == _b.name() && _a.id() == _b.id() && @@ -490,7 +490,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// updated. /// \return True if successful. public: bool WheelSlipService( - const msgs::WheelSlip &_req, msgs::Boolean &_res); + const msgs::WheelSlipParameters &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -851,7 +851,7 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlip &_req, +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParameters &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1478,7 +1478,7 @@ bool VisualCommand::Execute() } ////////////////////////////////////////////////// -WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, std::shared_ptr &_iface) : UserCommandBase(_msg, _iface) { @@ -1487,7 +1487,7 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlip *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast(this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; From d62a6b2e459dd7170d2cd3eb2fe228054b7dda40 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 12 Jan 2022 11:07:12 -0300 Subject: [PATCH 26/40] Address peer review comment Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 5dbd5d7631..78c93e11ec 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -396,6 +396,8 @@ class WheelSlipCommand : public UserCommandBase return _a.name() == _b.name() && _a.id() == _b.id() && + _a.parent_id() == _b.parent_id() && + _a.parent_name() == _b.parent_name() && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), From 96ce1a17992ceeae792892c54c94b6d54b3d90f5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 2 Feb 2022 17:51:09 -0300 Subject: [PATCH 27/40] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- src/systems/wheel_slip/WheelSlip.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index dcef56375b..9d5d128055 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -250,7 +250,8 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) auto ¶ms = linkSurface.second; const auto * wheelSlipCmdComp = _ecm.Component(linkSurface.first); - if (wheelSlipCmdComp) { + if (wheelSlipCmdComp) + { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = math::equal( params.slipComplianceLateral, From 84ab29053dbd43ca77b15aa43623eee99d4c9b8b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 2 Feb 2022 17:59:45 -0300 Subject: [PATCH 28/40] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- src/systems/wheel_slip/WheelSlip.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 9d5d128055..d63e02c82d 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -262,7 +262,8 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) wheelSlipCmdParams.slip_compliance_longitudinal(), 1e-6); - if (changed) { + if (changed) + { _ecm.RemoveComponent(linkSurface.first); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); From 8c00e8c5cbb375d826378ff5086a65943e681644 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:04:40 -0300 Subject: [PATCH 29/40] Make it work Signed-off-by: Ivan Santiago Paunovic --- .../gazebo/components/WheelSlipCmd.hh | 4 +- src/systems/user_commands/UserCommands.cc | 72 +++++++------------ src/systems/wheel_slip/WheelSlip.cc | 13 ++-- 3 files changed, 35 insertions(+), 54 deletions(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 29b985b6bc..25748dd82d 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ignition { @@ -35,7 +35,7 @@ namespace components { /// \brief A component type that contains commanded wheel slip parameters of /// an entity in the world frame represented by msgs::WheelSlipParameters. - using WheelSlipCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", WheelSlipCmd) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 78c93e11ec..c11ffe22ae 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -54,6 +54,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" @@ -383,21 +384,19 @@ class WheelSlipCommand : public UserCommandBase /// \brief Constructor /// \param[in] _msg Message containing the wheel slip parameters. /// \param[in] _iface Pointer to user commands interface. - public: WheelSlipCommand(msgs::WheelSlipParameters *_msg, + public: WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, std::shared_ptr &_iface); // Documentation inherited public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlipParameters &_a, const msgs::WheelSlipParameters &_b) { + [](const msgs::WheelSlipParametersCmd &_a, const msgs::WheelSlipParametersCmd &_b) { return - _a.name() == _b.name() && - _a.id() == _b.id() && - _a.parent_id() == _b.parent_id() && - _a.parent_name() == _b.parent_name() && + _a.model_name() == _b.model_name() && + _a.link_name() == _b.link_name() && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), @@ -492,7 +491,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// updated. /// \return True if successful. public: bool WheelSlipService( - const msgs::WheelSlipParameters &_req, msgs::Boolean &_res); + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res); /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -853,7 +852,7 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParameters &_req, +bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1480,7 +1479,7 @@ bool VisualCommand::Execute() } ////////////////////////////////////////////////// -WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, std::shared_ptr &_iface) : UserCommandBase(_msg, _iface) { @@ -1489,66 +1488,45 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParameters *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast(this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; return false; } - Entity wheelSlipEntity{wheelSlipMsg->id()}; - if (kNullEntity == wheelSlipEntity && !wheelSlipMsg->name().empty()) + Entity modelEntity = this->iface->ecm->EntityByComponents( + components::Name(wheelSlipMsg->model_name())); + if (kNullEntity == modelEntity) { - Entity wheelSlipParentEntity{wheelSlipMsg->parent_id()}; - if (kNullEntity != wheelSlipParentEntity) - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name()), - components::ParentEntity(wheelSlipParentEntity)); - } - else if (!wheelSlipMsg->parent_name().empty()) - { - auto possible_parents = this->iface->ecm->EntitiesByComponents( - components::Name(wheelSlipMsg->parent_name())); - for (const auto possible_parent : possible_parents) - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name()), - components::ParentEntity(possible_parent)); - if (kNullEntity != wheelSlipEntity) { - break; - } - } - } - else - { - wheelSlipEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->name())); - } + ignerr << "Failed to find model with name [" << wheelSlipMsg->model_name() + << "]." << std::endl; + return false; } - if (kNullEntity == wheelSlipEntity) + Model model{modelEntity}; + Entity linkEntity = model.LinkByName( + *this->iface->ecm, wheelSlipMsg->link_name()); + if (kNullEntity == linkEntity) { - ignerr << "Failed to find wheel slip with name [" << wheelSlipMsg->name() - << "], ID [" << wheelSlipMsg->id() << "], parent ID [" - << wheelSlipMsg->parent_id() << "] and parent name [" - << wheelSlipMsg->parent_name() << "]." << std::endl; + ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() + << "] for model [" << wheelSlipMsg->model_name() << "]." << std::endl; return false; } auto wheelSlipCmdComp = - this->iface->ecm->Component(wheelSlipEntity); + this->iface->ecm->Component(linkEntity); if (!wheelSlipCmdComp) { this->iface->ecm->CreateComponent( - wheelSlipEntity, components::WheelSlipCmd(*wheelSlipMsg)); + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); } else { auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged( - wheelSlipEntity, components::WheelSlipCmd::typeId, state); + linkEntity, components::WheelSlipCmd::typeId, state); } return true; } diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index d63e02c82d..27c6eb7109 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -253,23 +253,26 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (wheelSlipCmdComp) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); - bool changed = math::equal( + bool changed = (!math::equal( params.slipComplianceLateral, wheelSlipCmdParams.slip_compliance_lateral(), - 1e-6) && - math::equal( + 1e-6)) || + (!math::equal( params.slipComplianceLongitudinal, wheelSlipCmdParams.slip_compliance_longitudinal(), - 1e-6); + 1e-6)); if (changed) { - _ecm.RemoveComponent(linkSurface.first); + fprintf(stderr, "changed wheel slip parameters!\n"); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); + } else { + fprintf(stderr, "NOT changed wheel slip parameters!\n"); } + _ecm.RemoveComponent(linkSurface.first); } // get user-defined normal force constant From 480bd5c26e8ce703521f7a5f85611d4141a4583d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:05:25 -0300 Subject: [PATCH 30/40] Remove debugging messages Signed-off-by: Ivan Santiago Paunovic --- src/systems/wheel_slip/WheelSlip.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 27c6eb7109..78385295de 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -264,13 +264,10 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) if (changed) { - fprintf(stderr, "changed wheel slip parameters!\n"); params.slipComplianceLateral = wheelSlipCmdParams.slip_compliance_lateral(); params.slipComplianceLongitudinal = wheelSlipCmdParams.slip_compliance_longitudinal(); - } else { - fprintf(stderr, "NOT changed wheel slip parameters!\n"); } _ecm.RemoveComponent(linkSurface.first); } From 185c8e84ef4e3eb7f64611d969e07ead606592fd Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Feb 2022 11:32:29 -0300 Subject: [PATCH 31/40] linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c11ffe22ae..65aece90b6 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -391,9 +391,13 @@ class WheelSlipCommand : public UserCommandBase public: bool Execute() final; /// \brief WheelSlip equality comparision function - public: std::function + public: std::function wheelSlipEql { - [](const msgs::WheelSlipParametersCmd &_a, const msgs::WheelSlipParametersCmd &_b) { + []( + const msgs::WheelSlipParametersCmd &_a, + const msgs::WheelSlipParametersCmd &_b) + { return _a.model_name() == _b.model_name() && _a.link_name() == _b.link_name() && @@ -852,7 +856,8 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, } ////////////////////////////////////////////////// -bool UserCommandsPrivate::WheelSlipService(const msgs::WheelSlipParametersCmd &_req, +bool UserCommandsPrivate::WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res) { // Create command and push it to queue @@ -1488,7 +1493,8 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { - auto wheelSlipMsg = dynamic_cast(this->msg); + auto wheelSlipMsg = dynamic_cast( + this->msg); if (nullptr == wheelSlipMsg) { ignerr << "Internal error, null wheel slip message" << std::endl; @@ -1510,7 +1516,8 @@ bool WheelSlipCommand::Execute() if (kNullEntity == linkEntity) { ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." << std::endl; + << "] for model [" << wheelSlipMsg->model_name() << "]." + << std::endl; return false; } From dc17dae11a09f79690eec5e8a4852984d352aa8f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 1 Mar 2022 16:50:08 -0300 Subject: [PATCH 32/40] Add `link_name: '*'` option. Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 53 ++++++++++++++--------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 65aece90b6..3abcd5e848 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1511,29 +1511,40 @@ bool WheelSlipCommand::Execute() return false; } Model model{modelEntity}; - Entity linkEntity = model.LinkByName( - *this->iface->ecm, wheelSlipMsg->link_name()); - if (kNullEntity == linkEntity) - { - ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." - << std::endl; - return false; - } + auto linkName = wheelSlipMsg->link_name(); - auto wheelSlipCmdComp = + auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { + auto wheelSlipCmdComp = this->iface->ecm->Component(linkEntity); - if (!wheelSlipCmdComp) - { - this->iface->ecm->CreateComponent( - linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); - } - else - { - auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? - ComponentState::OneTimeChange : ComponentState::NoChange; - this->iface->ecm->SetChanged( - linkEntity, components::WheelSlipCmd::typeId, state); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + linkEntity, components::WheelSlipCmd::typeId, state); + } + }; + + if (linkName != "*") { + Entity linkEntity = model.LinkByName( + *this->iface->ecm, wheelSlipMsg->link_name()); + if (kNullEntity == linkEntity) + { + ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() + << "] for model [" << wheelSlipMsg->model_name() << "]." + << std::endl; + return false; + } + doForEachLink(linkEntity); + } else { + for (const auto & linkEntity : model.Links(*this->iface->ecm)) { + doForEachLink(linkEntity); + } } return true; } From a82ef556d269ebe0737e2caab7570cd8f36b69de Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Mar 2022 15:41:08 -0300 Subject: [PATCH 33/40] Please linters Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 3abcd5e848..b42c7a030d 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1523,8 +1523,9 @@ bool WheelSlipCommand::Execute() } else { - auto state = wheelSlipCmdComp->SetData(*wheelSlipMsg, this->wheelSlipEql) ? - ComponentState::OneTimeChange : ComponentState::NoChange; + auto state = wheelSlipCmdComp->SetData( + *wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange + : ComponentState::NoChange; this->iface->ecm->SetChanged( linkEntity, components::WheelSlipCmd::typeId, state); } From 18fa1e606f46c539622d59d356652abf33ee985c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Mar 2022 15:41:34 -0300 Subject: [PATCH 34/40] add minimal test case Signed-off-by: Ivan Santiago Paunovic --- test/integration/user_commands.cc | 101 ++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 390f4a4a2e..6a980d72ea 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -33,7 +33,9 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -1012,3 +1014,102 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Physics)) EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, WheelSlip) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/trisphere_cycle_wheel_slip.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + ASSERT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + ASSERT_NE(kNullEntity, worldEntity); + Entity tc0 = ecm->EntityByComponents( + components::Name("trisphere_cycle0")); + ASSERT_NE(kNullEntity, tc0); + Entity tc1 = ecm->EntityByComponents( + components::Name("trisphere_cycle1")); + ASSERT_NE(kNullEntity, tc1); + + Model tcModel0{tc0}; + Model tcModel1{tc1}; + Entity wf0 = tcModel0.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf0); + Entity wrl0 = tcModel0.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl0); + Entity wrf0 = tcModel0.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf0); + Entity wf1 = tcModel1.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf1); + Entity wrl1 = tcModel1.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl1); + Entity wrf1 = tcModel1.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf1); + + Entity links[] = {wf0, wrl0, wrf0, wf1, wrl1, wrf1}; + for (auto link : links) { + EXPECT_EQ(nullptr, ecm->Component(link)); + } + + // modify wheel slip parameters of one link of model 0 + msgs::WheelSlipParametersCmd req; + req.set_model_name("trisphere_cycle0"); + req.set_link_name("wheel_front"); + req.set_slip_compliance_lateral(1); + req.set_slip_compliance_longitudinal(1); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/wheel_slip/wheel_slip"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 2, false); + + // modify wheel slip parameters of one link of model 1 + req.set_model_name("trisphere_cycle1"); + req.set_link_name("*"); + req.set_slip_compliance_lateral(2); + req.set_slip_compliance_longitudinal(1); + + result = false; + res = msgs::Boolean{}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 3, false);} From bcab77b2335ffc83e812eaa8e2878bafdb31bac2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 11 Mar 2022 18:01:58 -0300 Subject: [PATCH 35/40] Use msgs::Entity instead of model_name/link_name Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 111 +++++++++++++++++----- test/integration/user_commands.cc | 9 +- 2 files changed, 92 insertions(+), 28 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index b42c7a030d..96a266452a 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -42,6 +42,8 @@ #include "ignition/common/Profiler.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/Link.hh" @@ -51,11 +53,13 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -399,8 +403,13 @@ class WheelSlipCommand : public UserCommandBase const msgs::WheelSlipParametersCmd &_b) { return - _a.model_name() == _b.model_name() && - _a.link_name() == _b.link_name() && + ( + (_a.entity().id() != kNullEntity && _a.entity().id() == _b.entity().id()) || + ( + _a.entity().name() == _b.entity().name() && + _a.entity().type() == _b.entity().type() + ) + ) && math::equal( _a.slip_compliance_lateral(), _b.slip_compliance_lateral(), @@ -1490,6 +1499,67 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, { } +// TODO(ivanpauno): Move this somewhere else +Entity scopedEntityFromMsg(const msgs::Entity & _msg, const EntityComponentManager & _ecm) +{ + if (_msg.id() != kNullEntity) { + return _msg.id(); + } + std::unordered_set entities = entitiesFromScopedName(_msg.name(), _ecm); + if (entities.empty()) { + ignerr << "Failed to find entity with scoped name [" << _msg.name() + << "]." << std::endl; + return kNullEntity; + } + if (_msg.type() == msgs::Entity::NONE) { + return *entities.begin(); + } + const components::BaseComponent * component; + std::string componentType; + for (const auto entity : entities) { + switch (_msg.type()) { + case msgs::Entity::LIGHT: + component = _ecm.Component(entity); + componentType = "LIGHT"; + break; + case msgs::Entity::MODEL: + component = _ecm.Component(entity); + componentType = "MODEL"; + break; + case msgs::Entity::LINK: + component = _ecm.Component(entity); + componentType = "LINK"; + break; + case msgs::Entity::VISUAL: + component = _ecm.Component(entity); + componentType = "VISUAL"; + break; + case msgs::Entity::COLLISION: + component = _ecm.Component(entity); + componentType = "COLLISION"; + break; + case msgs::Entity::SENSOR: + component = _ecm.Component(entity); + componentType = "SENSOR"; + break; + case msgs::Entity::JOINT: + component = _ecm.Component(entity); + componentType = "JOINT"; + break; + default: + componentType = "unknown"; + break; + } + if (component != nullptr) { + return entity; + } + } + ignerr << "Found entity with scoped name [" << _msg.name() + << "], but it doesn't have a component of the required type [" + << componentType << "]." << std::endl; + return kNullEntity; +} + ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { @@ -1500,18 +1570,12 @@ bool WheelSlipCommand::Execute() ignerr << "Internal error, null wheel slip message" << std::endl; return false; } - - - Entity modelEntity = this->iface->ecm->EntityByComponents( - components::Name(wheelSlipMsg->model_name())); - if (kNullEntity == modelEntity) + const auto & ecm = *this->iface->ecm; + Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + if (kNullEntity == entity) { - ignerr << "Failed to find model with name [" << wheelSlipMsg->model_name() - << "]." << std::endl; return false; } - Model model{modelEntity}; - auto linkName = wheelSlipMsg->link_name(); auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { auto wheelSlipCmdComp = @@ -1530,24 +1594,23 @@ bool WheelSlipCommand::Execute() linkEntity, components::WheelSlipCmd::typeId, state); } }; + const components::BaseComponent * component = ecm.Component(entity); - if (linkName != "*") { - Entity linkEntity = model.LinkByName( - *this->iface->ecm, wheelSlipMsg->link_name()); - if (kNullEntity == linkEntity) - { - ignerr << "Failed to find link with name [" << wheelSlipMsg->link_name() - << "] for model [" << wheelSlipMsg->model_name() << "]." - << std::endl; - return false; - } - doForEachLink(linkEntity); - } else { + if (nullptr != component) { + doForEachLink(entity); + return true; + } + component = ecm.Component(entity); + if (nullptr != component) { + Model model{entity}; for (const auto & linkEntity : model.Links(*this->iface->ecm)) { doForEachLink(linkEntity); } + return true; } - return true; + ignerr << "Found entity with scoped name [" << wheelSlipMsg->entity().name() + << "], is neither a model or a link." << std::endl; + return false; } IGNITION_ADD_PLUGIN(UserCommands, System, diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 6a980d72ea..9c29563e6d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1076,8 +1076,9 @@ TEST_F(UserCommandsTest, WheelSlip) // modify wheel slip parameters of one link of model 0 msgs::WheelSlipParametersCmd req; - req.set_model_name("trisphere_cycle0"); - req.set_link_name("wheel_front"); + auto * entityMsg = req.mutable_entity(); + entityMsg->set_name("trisphere_cycle0::wheel_front"); + entityMsg->set_type(msgs::Entity::LINK); req.set_slip_compliance_lateral(1); req.set_slip_compliance_longitudinal(1); @@ -1097,8 +1098,8 @@ TEST_F(UserCommandsTest, WheelSlip) server.Run(true, 2, false); // modify wheel slip parameters of one link of model 1 - req.set_model_name("trisphere_cycle1"); - req.set_link_name("*"); + entityMsg->set_name("trisphere_cycle1"); + entityMsg->set_type(msgs::Entity::MODEL); req.set_slip_compliance_lateral(2); req.set_slip_compliance_longitudinal(1); From c7b6db6615c369ab718a4612a5b4ef1bb56fdb00 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 15 Mar 2022 16:54:37 -0300 Subject: [PATCH 36/40] copyright notice year: 2021 -> 2022 Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Aditya Pande --- include/ignition/gazebo/components/WheelSlipCmd.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 25748dd82d..4daf7ee249 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 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. From def1f85bd068d0ecbb5f1f17c4409d0f00a27cdc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 15 Mar 2022 18:07:18 -0300 Subject: [PATCH 37/40] Make codecheck pass Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 96a266452a..ae6ba5746b 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -404,7 +405,10 @@ class WheelSlipCommand : public UserCommandBase { return ( - (_a.entity().id() != kNullEntity && _a.entity().id() == _b.entity().id()) || + ( + _a.entity().id() != kNullEntity && + _a.entity().id() == _b.entity().id() + ) || ( _a.entity().name() == _b.entity().name() && _a.entity().type() == _b.entity().type() @@ -1500,12 +1504,14 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, } // TODO(ivanpauno): Move this somewhere else -Entity scopedEntityFromMsg(const msgs::Entity & _msg, const EntityComponentManager & _ecm) +Entity scopedEntityFromMsg( + const msgs::Entity & _msg, const EntityComponentManager & _ecm) { if (_msg.id() != kNullEntity) { return _msg.id(); } - std::unordered_set entities = entitiesFromScopedName(_msg.name(), _ecm); + std::unordered_set entities = entitiesFromScopedName( + _msg.name(), _ecm); if (entities.empty()) { ignerr << "Failed to find entity with scoped name [" << _msg.name() << "]." << std::endl; @@ -1594,7 +1600,8 @@ bool WheelSlipCommand::Execute() linkEntity, components::WheelSlipCmd::typeId, state); } }; - const components::BaseComponent * component = ecm.Component(entity); + const components::BaseComponent * component = + ecm.Component(entity); if (nullptr != component) { doForEachLink(entity); From 05b88698a20233354d13202168cde1e273736e05 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 21 Mar 2022 15:09:48 -0300 Subject: [PATCH 38/40] Update include Signed-off-by: Ivan Santiago Paunovic --- src/systems/user_commands/UserCommands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index ae6ba5746b..b0f27086b9 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include From f9161d360f09b61c448437b54c7595cf026f0782 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 23 Mar 2022 15:57:55 -0300 Subject: [PATCH 39/40] Maybe fix CI issue Signed-off-by: Ivan Santiago Paunovic --- test/integration/user_commands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 9c29563e6d..dc3443a3a2 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1041,7 +1041,7 @@ TEST_F(UserCommandsTest, WheelSlip) // Run server and check we have the ECM EXPECT_EQ(nullptr, ecm); - server.Run(true, 1, false); + server.Run(true, 2, false); ASSERT_NE(nullptr, ecm); // Check that the physics properties are the ones specified in the sdf From 19606cdbb7a1cf9ebaebb56c8e943c0dfa7cc44f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 23 Mar 2022 16:59:33 -0300 Subject: [PATCH 40/40] Do what the other tests are doing: skip on windows Signed-off-by: Ivan Santiago Paunovic --- test/integration/user_commands.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index dc3443a3a2..95534160c6 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1016,7 +1016,7 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Physics)) } ///////////////////////////////////////////////// -TEST_F(UserCommandsTest, WheelSlip) +TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WheelSlip)) { // Start server ServerConfig serverConfig; @@ -1041,7 +1041,7 @@ TEST_F(UserCommandsTest, WheelSlip) // Run server and check we have the ECM EXPECT_EQ(nullptr, ecm); - server.Run(true, 2, false); + server.Run(true, 1, false); ASSERT_NE(nullptr, ecm); // Check that the physics properties are the ones specified in the sdf