diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 400dd761..062200a6 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(ignition-common4 REQUIRED COMPONENTS graphics) find_package(ignition-fuel_tools7 REQUIRED) find_package(ignition-math6 REQUIRED) find_package(ignition-msgs8 REQUIRED) +find_package(ignition-sensors6 REQUIRED) find_package(ignition-transport11 REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS loader register) find_package(ignition-launch5 REQUIRED) @@ -118,6 +119,7 @@ target_link_libraries(SubtCommon ignition-common4::graphics ignition-transport11::ignition-transport11 ignition-transport11::ignition-transport11-log + ignition-sensors6::ignition-sensors6 ignition-launch5::ignition-launch5 ignition-fuel_tools7::ignition-fuel_tools7 ${catkin_LIBRARIES} @@ -133,6 +135,7 @@ list(APPEND SUBT_IGN_PLUGINS ControllerPlugin GameLogicPlugin GasEmitterDetectorPlugin + LocalWindPlugin VisibilityPlugin ) diff --git a/subt_ign/include/subt_ign/LocalWindPlugin.hh b/subt_ign/include/subt_ign/LocalWindPlugin.hh new file mode 100644 index 00000000..5b41a9b3 --- /dev/null +++ b/subt_ign/include/subt_ign/LocalWindPlugin.hh @@ -0,0 +1,59 @@ +/* + * 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 SUBT_IGN_LOCALWINDPLUGIN_HH_ +#define SUBT_IGN_LOCALWINDPLUGIN_HH_ + +#include + +namespace subt +{ + class LocalWindPrivate; + /// \class LocalWind + /// \brief Iterates over all the models and adds wind accordingly. + /// + /// The LocalWind plugin loads a pre-computed lookup table to + /// determine the tile where a robot is based in its position. + /// In each simulation step, checks all the robots tiles + /// Then checks if the tile has wind defined, and if that's the case + /// Add wind to the links of the robot. + + class LocalWind: + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate + { + /// \brief Constructor + public: LocalWind(); + + /// \brief Destructor + public: ~LocalWind(); + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; +} +#endif diff --git a/subt_ign/src/LocalWindPlugin.cc b/subt_ign/src/LocalWindPlugin.cc new file mode 100644 index 00000000..11493328 --- /dev/null +++ b/subt_ign/src/LocalWindPlugin.cc @@ -0,0 +1,301 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include "ignition/gazebo/components/Inertial.hh" +#include +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/WindMode.hh" + +#include + +#include +#include +#include + +#include "ignition/gazebo/Link.hh" + +#include "subt_ign/LocalWindPlugin.hh" +#include "subt_ign/VisibilityTable.hh" + +IGNITION_ADD_PLUGIN( + subt::LocalWind, + ignition::gazebo::System, + subt::LocalWind::ISystemConfigure, + subt::LocalWind::ISystemPreUpdate) + +using namespace ignition; +using namespace subt; + +/// \brief Wind properties for a tile. +class WindTile +{ + /// \brief Constructor + public: WindTile() = default; + + /// \brief Load based on SDF. + public: void Load(sdf::ElementPtr _elem); + + /// \brief Tile number + public: uint64_t number{0}; + + /// \brief Linear velocity for the tile + public: ignition::math::Vector3d linearVelocity; + + /// \brief Noise added to linear velocity. + public: sensors::NoisePtr noise; +}; + +////////////////////////////////////////////////// +class subt::LocalWindPrivate +{ + /// \brief Initialize the system. + /// \param[in] _ecm Mutable reference to the EntityComponentManager. + /// \param[in] _sdf Pointer to sdf::Element that contains configuration + /// parameters for the system. + public: void Load(gazebo::EntityComponentManager &_ecm, + const std::shared_ptr &_sdf); + + /// \brief World entity to which this system is attached. + public: gazebo::Entity worldEntity; + + /// \brief A map that stores the tiles that have wind defined + // public: std::map windTiles; + public: std::map windTiles; + + /// \brief Factor to determine wind strength + public: double forceApproximationScalingFactor{1.0}; + + /// \brief Table to store the map xyz position -> tile + public: subt::VisibilityTable visibilityTable; + + /// \brief boolean to confirm if the plugin is properly configured + public: bool initialized{false}; +}; + +////////////////////////////////////////////////// +LocalWind::LocalWind() : + dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +LocalWind::~LocalWind() = default; + +////////////////////////////////////////////////// +void WindTile::Load(sdf::ElementPtr _elem) +{ + this->number = _elem->Get("tile", + std::numeric_limits::max()).first; + + this->linearVelocity = _elem->Get( + "linear_velocity", ignition::math::Vector3d::Zero).first; + + if (_elem->HasElement("noise")) + { + this->noise = ignition::sensors::NoiseFactory::NewNoiseModel( + _elem->GetElementImpl("noise")); + } +} + +////////////////////////////////////////////////// +void LocalWindPrivate::Load( + gazebo::EntityComponentManager &_ecm, + const std::shared_ptr &_sdf + ) +{ + this->forceApproximationScalingFactor = + _sdf->Get("scaling", this->forceApproximationScalingFactor).first; + + // Iterate over all the local_wind instances defined + // inside the plugin + auto sdfClone = _sdf->Clone(); + sdf::ElementPtr localWindElement = sdfClone->GetElement("local_wind"); + while (localWindElement) + { + // Check if valid + if (localWindElement->HasElement("tile") && + localWindElement->HasElement("linear_velocity")) + { + WindTile tile; + tile.Load(localWindElement); + + // Avoid duplicates in the plugin definition + if (this->windTiles.find(tile.number) == this->windTiles.end()) + { + this->windTiles[tile.number] = tile; + } + else + { + ignerr << "Tile #" << tile.number + << " was already added. Remove duplicated tiles\n"; + return; + } + } + else + { + ignerr << "Parsing problem. Each local_wind element requires a tile" + << "and a magnitude\n"; + return; + } + + // parse next localWind element + localWindElement = localWindElement->GetNextElement("local_wind"); + } + + if(this->windTiles.size() == 0) + { + ignwarn << "Empty LocalWindPlugin, not using LocalWindPlugin\n"; + return; + } + + auto worldName = + _ecm.Component(this->worldEntity); + if (!worldName){ + ignerr << "Unable to get world name\n"; + return; + } + + if (!this->visibilityTable.Load(worldName->Data())) + { + ignerr << "Unable to load visibility table data files\n"; + return; + } + + this->initialized = true; +} + +////////////////////////////////////////////////// +void LocalWind::Configure( + const gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager & /*_eventMgr*/) +{ + this->dataPtr->worldEntity = _entity; + this->dataPtr->Load(_ecm, _sdf); +} + +////////////////////////////////////////////////// +void LocalWind::PreUpdate( + const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) +{ + if (!this->dataPtr->initialized) + return; + + if (_info.paused) + return; + + // Init each new link added to the world + _ecm.EachNew( + [&](const gazebo::Entity &_entity, + gazebo::components::Link *, + const gazebo::components::WindMode *_windMode) -> bool + { + if (_windMode->Data()) + { + // Create a WorldLinearVelocity component on the link so that + // physics can populate it + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + gazebo::components::WorldLinearVelocity()); + } + } + return true; + }); + + gazebo::Link link; + + _ecm.Each( + [&](const gazebo::Entity &_entity, const gazebo::components::Link *, + const gazebo::components::Inertial *_inertial, + const gazebo::components::WindMode *_windMode, + const gazebo::components::WorldLinearVelocity *_linkVel + ) -> bool + { + // Skip links for which the wind is disabled + // This assumption also implies links with windMode enabled + // were populated with + if (!_windMode->Data()) + { + return true; + } + + // Get XYZ of each link + math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + math::Vector3d linkPosition = linkWorldPose.Pos(); + + int32_t x = std::round(linkPosition.X()); + int32_t y = std::round(linkPosition.Y()); + int32_t z = std::round(linkPosition.Z()); + auto roundedPos = std::make_tuple(x, y, z); + + // Get Tile defined for that XYZ + const auto &tilesMap = this->dataPtr->visibilityTable.Vertices(); + + // Skip if the robot position is not in the tile. + if (tilesMap.find(roundedPos) == tilesMap.end()) + return true; + + const auto linkTile = tilesMap.at(roundedPos); + + // If Tile is in the tiles defined by the plugin, apply force + auto tileLocalWind = this->dataPtr->windTiles.find(linkTile); + if (tileLocalWind != this->dataPtr->windTiles.end()) + { + + link.ResetEntity(_entity); + + ignition::math::Vector3d linearVelocity = + tileLocalWind->second.linearVelocity; + + // Apply noise + if (tileLocalWind->second.noise) + { + linearVelocity.X() = + tileLocalWind->second.noise->Apply(linearVelocity.X()); + linearVelocity.Y() = + tileLocalWind->second.noise->Apply(linearVelocity.Y()); + linearVelocity.Z() = + tileLocalWind->second.noise->Apply(linearVelocity.Z()); + } + + // Debugging: + // igndbg << "Position: " << std::get<0>(roundedPos) << " " + // << std::get<1>(roundedPos) << " " + // << std::get<2>(roundedPos) << " " + // << " current tile: " << linkTile + // << " local_wind:" << linearVelocity << "\n"; + + math::Vector3d windForce = + _inertial->Data().MassMatrix().Mass() * + this->dataPtr->forceApproximationScalingFactor * + (linearVelocity - _linkVel->Data()); + + // Apply force at center of mass + link.AddWorldForce(_ecm, windForce); + } + return true; + }); +} diff --git a/subt_ign/worlds/finals_qual_demo.dat b/subt_ign/worlds/finals_qual_demo.dat index 2c53c10c..7673ea13 100644 Binary files a/subt_ign/worlds/finals_qual_demo.dat and b/subt_ign/worlds/finals_qual_demo.dat differ diff --git a/subt_ign/worlds/finals_qual_demo.dot b/subt_ign/worlds/finals_qual_demo.dot index 8c255348..a7ca6b27 100644 --- a/subt_ign/worlds/finals_qual_demo.dot +++ b/subt_ign/worlds/finals_qual_demo.dot @@ -63,6 +63,8 @@ graph { 83 [label="83::Cave Cap Type A::tile_83"]; 84 [label="84::Cave Cap Type A::tile_84"]; 85 [label="85::Cave Cap Type A::tile_85"]; + 87 [label="87::Urban Service Room::tile_87"]; + 1 [label="1::Universal Straight 2.5::universal_with_door"]; /* ==== Edges ==== */ @@ -75,6 +77,7 @@ graph { 22 -- 57 [label=3]; 23 -- 59 [label=6]; 24 -- 31 [label=6]; + 24 -- 1 [label=3]; 25 -- 32 [label=3]; 26 -- 66 [label=1]; 27 -- 68 [label=3]; @@ -125,4 +128,5 @@ graph { 72 -- 74 [label=3]; 75 -- 76 [label=1]; 76 -- 77 [label=3]; + 87 -- 1 [label=1]; } diff --git a/subt_ign/worlds/finals_qual_demo.sdf b/subt_ign/worlds/finals_qual_demo.sdf index 134e9c1e..5f165643 100644 --- a/subt_ign/worlds/finals_qual_demo.sdf +++ b/subt_ign/worlds/finals_qual_demo.sdf @@ -715,7 +715,6 @@ - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae @@ -1197,6 +1196,94 @@ + + + + + 0.8 + + 30 + + 1 1 0 + + 0 + 0.0002 + + + + + 29 + + 0 2 0 + + 0 + 0.0002 + + + + + 28 + + 0 4 0 + + 0 + 0.0002 + + + + + 64 + + 0 5 1 + + 0 + 0.0002 + + + + + 38 + + 0 3 2 + + 0 + 0.0002 + + + + + 37 + + 0 4 1 + + 0 + 0.0002 + + + + + 63 + + 1 0 0 + + 0 + 0.0002 + + + + + 77 + + 0 1 0 + + 0 + 0.0002 + + + +