diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh new file mode 100644 index 0000000000..4daf7ee249 --- /dev/null +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -0,0 +1,46 @@ +/* + * 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. + * 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::WheelSlipParameters. + 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 36e5a2ff66..971b3f414e 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -25,9 +25,11 @@ #include #include #include +#include #include #include +#include #include #include @@ -42,6 +44,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,14 +55,18 @@ #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" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -377,6 +385,48 @@ 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::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief WheelSlip equality comparision function + public: std::function + wheelSlipEql { + []( + const msgs::WheelSlipParametersCmd &_a, + const msgs::WheelSlipParametersCmd &_b) + { + return + ( + ( + _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(), + 1e-6) && + math::equal( + _a.slip_compliance_longitudinal(), + _b.slip_compliance_longitudinal(), + 1e-6); + }}; +}; } } } @@ -460,6 +510,16 @@ 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::WheelSlipParametersCmd &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -634,6 +694,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/" + validWorldName + "/wheel_slip"}; + this->dataPtr->node.Advertise(wheelSlipService, + &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); + + ignmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -857,6 +925,25 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::WheelSlipService( + const msgs::WheelSlipParametersCmd &_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) @@ -1501,6 +1588,130 @@ bool VisualCommand::Execute() return true; } +////////////////////////////////////////////////// +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +// 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() +{ + auto wheelSlipMsg = dynamic_cast( + this->msg); + if (nullptr == wheelSlipMsg) + { + ignerr << "Internal error, null wheel slip message" << std::endl; + return false; + } + const auto & ecm = *this->iface->ecm; + Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + if (kNullEntity == entity) + { + return false; + } + + 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); + } + }; + const components::BaseComponent * component = + ecm.Component(entity); + + 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; + } + 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, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..78385295de 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; @@ -107,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++) { @@ -242,9 +245,32 @@ 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) + { + params.slipComplianceLateral = + wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = + wheelSlipCmdParams.slip_compliance_longitudinal(); + } + _ecm.RemoveComponent(linkSurface.first); + } // get user-defined normal force constant double force = params.wheelNormalForce; diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 81528beb64..d57012558d 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" @@ -1087,3 +1089,103 @@ 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, IGN_UTILS_TEST_DISABLED_ON_WIN32(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; + 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); + + 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 + entityMsg->set_name("trisphere_cycle1"); + entityMsg->set_type(msgs::Entity::MODEL); + 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);}