From 57ba9255742d9dd3457fbcbfeffbe3d3345de656 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Aug 2024 10:53:20 +0200 Subject: [PATCH 1/4] add parameters to wheelslip system plugin Signed-off-by: knmcguire --- src/systems/wheel_slip/WheelSlip.cc | 65 +++++++++++++++++++++++++---- src/systems/wheel_slip/WheelSlip.hh | 6 +++ 2 files changed, 62 insertions(+), 9 deletions(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 31771e096f..955844baf5 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -27,6 +27,7 @@ #include "gz/sim/Link.hh" #include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/ChildLinkName.hh" #include "gz/sim/components/Collision.hh" @@ -35,6 +36,9 @@ #include "gz/sim/components/SlipComplianceCmd.hh" #include "gz/sim/components/WheelSlipCmd.hh" +#include + + using namespace gz; using namespace sim; using namespace systems; @@ -53,6 +57,9 @@ class gz::sim::systems::WheelSlipPrivate /// \brief Gazebo communication node public: transport::Node node; + /// \brief Parameters registry + public: transport::parameters::ParametersRegistry * registry; + /// \brief Joint Entity public: Entity jointEntity; @@ -248,29 +255,48 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) for (auto &linkSurface : this->mapLinkSurfaceParams) { auto ¶ms = linkSurface.second; - const auto * wheelSlipCmdComp = - _ecm.Component(linkSurface.first); - if (wheelSlipCmdComp) + std::string scopedName = ignition::gazebo::scopedName( + linkSurface.first, _ecm, ".", false); + + // TODO(ivanpauno): WHY THE SCOPED NAME CHANGES BETWEEN HERE AND + // ConfigureParameters()? + // Here the scoped name starts with "wheel_slip." + // In `ConfigureParameters()` that doesn't happen! + auto paramName = std::string("systems.") + scopedName; + transport::parameters::ParametersRegistry::ParameterValue value; + try { + value = this->registry->Parameter(paramName); + } catch (const std::exception & ex) { + ignerr << "WheelSlip system Update(): failed to get parameter [" + << paramName << "]: " << ex.what() << std::endl; + } + auto * msg = dynamic_cast(value.msg.get()); + if (msg) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = (!math::equal( params.slipComplianceLateral, - wheelSlipCmdParams.slip_compliance_lateral(), + msg->slip_compliance_lateral(), 1e-6)) || (!math::equal( params.slipComplianceLongitudinal, - wheelSlipCmdParams.slip_compliance_longitudinal(), + msg->slip_compliance_longitudinal(), 1e-6)); if (changed) { + gzdbg << "WheelSlip system Update(): parameter [" + << paramName << "] updated" + << std::endl; params.slipComplianceLateral = - wheelSlipCmdParams.slip_compliance_lateral(); + msg->slip_compliance_lateral(); params.slipComplianceLongitudinal = - wheelSlipCmdParams.slip_compliance_longitudinal(); + msg->slip_compliance_longitudinal(); } - _ecm.RemoveComponent(linkSurface.first); - } + } else if (value.msg.get()) { + gzerr << "WheelSlip system Update(): parameter [" + << paramName << "] is not of type [ign_msgs.WheelSlipParameters]" + << std::endl; } // get user-defined normal force constant double force = params.wheelNormalForce; @@ -346,6 +372,26 @@ void WheelSlip::Configure(const Entity &_entity, this->dataPtr->validConfig = this->dataPtr->Load(_ecm, sdfClone); } +void WheelSlip::ConfigureParameters( + ignition::transport::parameters::ParametersRegistry & _registry, + EntityComponentManager &_ecm) +{ + this->dataPtr->registry = &_registry; + for (const auto & linkParamsPair : this->dataPtr->mapLinkSurfaceParams) { + std::string scopedName = ignition::gazebo::scopedName( + linkParamsPair.first, _ecm, ".", false); + + auto paramName = std::string("systems.wheel_slip.") + scopedName; + auto wsParams = std::make_unique(); + wsParams->set_slip_compliance_lateral( + linkParamsPair.second.slipComplianceLateral); + wsParams->set_slip_compliance_longitudinal( + linkParamsPair.second.slipComplianceLongitudinal); + _registry.DeclareParameter(paramName, std::move(wsParams)); + } +} + + ////////////////////////////////////////////////// void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -388,6 +434,7 @@ void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) GZ_ADD_PLUGIN(WheelSlip, System, WheelSlip::ISystemConfigure, + WheelSlip::ISystemConfigureParameters, WheelSlip::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(WheelSlip, diff --git a/src/systems/wheel_slip/WheelSlip.hh b/src/systems/wheel_slip/WheelSlip.hh index fa34ff119f..4143e7157b 100644 --- a/src/systems/wheel_slip/WheelSlip.hh +++ b/src/systems/wheel_slip/WheelSlip.hh @@ -118,6 +118,7 @@ namespace systems class WheelSlip : public System, public ISystemConfigure, + public ISystemConfigureParameters, public ISystemPreUpdate { /// \brief Constructor @@ -132,6 +133,11 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) override; + public: void ConfigureParameters( + gz::transport::parameters::ParametersRegistry & + _registry, + EntityComponentManager &_ecm) override; + // Documentation inherited public: void PreUpdate( const gz::sim::UpdateInfo &_info, From a998454dbe06574e3618cf24fbf2b62066490c16 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Aug 2024 10:58:50 +0200 Subject: [PATCH 2/4] wheelslip update to latest gazebo changes Signed-off-by: knmcguire --- src/systems/wheel_slip/WheelSlip.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 955844baf5..4859ba90a6 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -36,7 +36,7 @@ #include "gz/sim/components/SlipComplianceCmd.hh" #include "gz/sim/components/WheelSlipCmd.hh" -#include +#include using namespace gz; @@ -255,7 +255,7 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) for (auto &linkSurface : this->mapLinkSurfaceParams) { auto ¶ms = linkSurface.second; - std::string scopedName = ignition::gazebo::scopedName( + std::string scopedName = gz::sim::scopedName( linkSurface.first, _ecm, ".", false); // TODO(ivanpauno): WHY THE SCOPED NAME CHANGES BETWEEN HERE AND @@ -270,7 +270,7 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) ignerr << "WheelSlip system Update(): failed to get parameter [" << paramName << "]: " << ex.what() << std::endl; } - auto * msg = dynamic_cast(value.msg.get()); + auto * msg = dynamic_cast(value.msg.get()); if (msg) { const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); @@ -373,16 +373,16 @@ void WheelSlip::Configure(const Entity &_entity, } void WheelSlip::ConfigureParameters( - ignition::transport::parameters::ParametersRegistry & _registry, + gz::transport::parameters::ParametersRegistry & _registry, EntityComponentManager &_ecm) { this->dataPtr->registry = &_registry; for (const auto & linkParamsPair : this->dataPtr->mapLinkSurfaceParams) { - std::string scopedName = ignition::gazebo::scopedName( + std::string scopedName = gz::sim::scopedName( linkParamsPair.first, _ecm, ".", false); auto paramName = std::string("systems.wheel_slip.") + scopedName; - auto wsParams = std::make_unique(); + auto wsParams = std::make_unique(); wsParams->set_slip_compliance_lateral( linkParamsPair.second.slipComplianceLateral); wsParams->set_slip_compliance_longitudinal( From b588acf5ae7667699ba9642b9e32fec107b02a3c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 6 Aug 2024 11:52:57 +0200 Subject: [PATCH 3/4] wheel slip different way to get parameter message Signed-off-by: knmcguire --- src/systems/wheel_slip/WheelSlip.cc | 31 +++++++++++------------------ 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 4859ba90a6..e16386c7da 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -58,7 +58,7 @@ class gz::sim::systems::WheelSlipPrivate public: transport::Node node; /// \brief Parameters registry - public: transport::parameters::ParametersRegistry * registry; + public: transport::parameters::ParametersInterface * registry; /// \brief Joint Entity public: Entity jointEntity; @@ -263,24 +263,20 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) // Here the scoped name starts with "wheel_slip." // In `ConfigureParameters()` that doesn't happen! auto paramName = std::string("systems.") + scopedName; - transport::parameters::ParametersRegistry::ParameterValue value; - try { - value = this->registry->Parameter(paramName); - } catch (const std::exception & ex) { - ignerr << "WheelSlip system Update(): failed to get parameter [" - << paramName << "]: " << ex.what() << std::endl; - } - auto * msg = dynamic_cast(value.msg.get()); - if (msg) + //transport::parameters::ParameterResult value; + msgs::WheelSlipParametersCmd msg; + + auto result = this->registry->Parameter(paramName, msg); + + if (result) { - const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); bool changed = (!math::equal( params.slipComplianceLateral, - msg->slip_compliance_lateral(), + msg.slip_compliance_lateral(), 1e-6)) || (!math::equal( params.slipComplianceLongitudinal, - msg->slip_compliance_longitudinal(), + msg.slip_compliance_longitudinal(), 1e-6)); if (changed) @@ -289,14 +285,11 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) << paramName << "] updated" << std::endl; params.slipComplianceLateral = - msg->slip_compliance_lateral(); + msg.slip_compliance_lateral(); params.slipComplianceLongitudinal = - msg->slip_compliance_longitudinal(); + msg.slip_compliance_longitudinal(); } - } else if (value.msg.get()) { - gzerr << "WheelSlip system Update(): parameter [" - << paramName << "] is not of type [ign_msgs.WheelSlipParameters]" - << std::endl; } + } // get user-defined normal force constant double force = params.wheelNormalForce; From 588482d0baeaabacf2017dd85317a84792f2dd98 Mon Sep 17 00:00:00 2001 From: "Kimberly N. McGuire" Date: Mon, 25 Nov 2024 15:56:24 +0100 Subject: [PATCH 4/4] Update src/systems/wheel_slip/WheelSlip.cc Co-authored-by: Steve Peters Signed-off-by: Kimberly N. McGuire --- src/systems/wheel_slip/WheelSlip.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index e16386c7da..3e6a016554 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -263,7 +263,6 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) // Here the scoped name starts with "wheel_slip." // In `ConfigureParameters()` that doesn't happen! auto paramName = std::string("systems.") + scopedName; - //transport::parameters::ParameterResult value; msgs::WheelSlipParametersCmd msg; auto result = this->registry->Parameter(paramName, msg);