From b147cf440460732833b792bdff8e01c163070852 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 23 Nov 2022 21:37:29 +0100 Subject: [PATCH] [Garden] Port scan, dock, deliver - Part 2 (#541) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero Co-authored-by: M1chaelM --- vrx_gz/config/wamv.yaml | 4 +- vrx_gz/models/dock_2022/model.sdf | 323 +++++++++------- vrx_gz/src/ScanDockScoringPlugin.cc | 458 ++++++++++++++++++++++- vrx_gz/src/ScanDockScoringPlugin.hh | 56 +++ vrx_gz/worlds/scan_dock_deliver_task.sdf | 32 ++ 5 files changed, 728 insertions(+), 145 deletions(-) diff --git a/vrx_gz/config/wamv.yaml b/vrx_gz/config/wamv.yaml index c0030155a..e8ad417da 100644 --- a/vrx_gz/config/wamv.yaml +++ b/vrx_gz/config/wamv.yaml @@ -83,7 +83,7 @@ joy_teleop: axis_mappings: data: axis: 1 - scale: 1000.0 + scale: 400.0 offset: 0 right_thrust: @@ -94,7 +94,7 @@ joy_teleop: axis_mappings: data: axis: 3 - scale: 1000.0 + scale: 400.0 offset: 0 left_pos: diff --git a/vrx_gz/models/dock_2022/model.sdf b/vrx_gz/models/dock_2022/model.sdf index 1018b801d..fc96d3273 100644 --- a/vrx_gz/models/dock_2022/model.sdf +++ b/vrx_gz/models/dock_2022/model.sdf @@ -87,10 +87,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 @@ -102,10 +101,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 --> @@ -117,45 +115,6 @@ link_symbols - - - - @@ -236,10 +195,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 @@ -251,10 +209,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 --> @@ -266,45 +223,6 @@ link_symbols - - - - @@ -385,10 +303,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 @@ -400,10 +317,9 @@ - + 0 1 0 + 0 1 0 + 0 1 0 1 --> @@ -415,47 +331,170 @@ link_symbols - - - - - + + + + + + /vrx/dock_2022/bay_1/contain + 1.5 3 0.25 0 0 1.5707963267948966 + + + 1.5 4 2 + + + + + + /vrx/dock_2022/bay_2/contain + 1.5 9 0.25 0 0 1.5707963267948966 + + + 1.5 4 2 + + + + + + /vrx/dock_2022/bay_3/contain + 1.5 15 0.25 0 0 1.5707963267948966 + + + 1.5 4 2 + + + + + + /vrx/dock_2022/bay_1_external/contain + -3.5 3 0.25 0 0 1.5707963267948966 + + + 5.5 1.5 2 + + + + + + /vrx/dock_2022/bay_2_external/contain + -3.5 9 0.25 0 0 1.5707963267948966 + + + 5.5 1.5 2 + + + + + + /vrx/dock_2022/bay_3_external/contain + -3.5 15 0.25 0 0 1.5707963267948966 + + + 5.5 1.5 2 + + + diff --git a/vrx_gz/src/ScanDockScoringPlugin.cc b/vrx_gz/src/ScanDockScoringPlugin.cc index 392d0fb46..3649e7106 100644 --- a/vrx_gz/src/ScanDockScoringPlugin.cc +++ b/vrx_gz/src/ScanDockScoringPlugin.cc @@ -15,11 +15,16 @@ * */ +#include +#include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -162,9 +167,269 @@ void ColorSequenceChecker::OnColorSequence(const msgs::StringMsg_V &_sequence) std::cout << std::flush; } +/// \brief A class to monitor if the vehicle docked in a given bay. +class DockChecker +{ + /// \brief Constructor. + /// \param[in] _name The name of the checker (only used for debugging). + /// \param[in] _internalActivationTopic The gazebo topic used to receive + /// notifications about the internal activation zone. + /// \param[in] _externalActivationTopic The gazebo topic used to receive + /// notifications about the external activation zone. + /// from the "contain" plugin. + /// \param[in] _minDockTime Minimum amount of seconds to stay docked to be + /// considered a fully successfull dock. + /// \param[in] _correctDock Whether this is the correct bay to dock in. + /// \param[in] _announceSymbol Symbol to announce via msgs. + /// E.g.: red_cross, blue_circle + /// \param[in] _symbolTopic Optional topic to announce the symbol. + public: DockChecker(const std::string &_name, + const std::string &_internalActivationTopic, + const std::string &_exteriorActivationTopic, + const std::chrono::duration _minDockTime, + const bool _correctDock, + const std::string &_announceSymbol, + const std::string &_symbolTopic); + + /// \brief The name of this checker. + public: std::string name; + + /// \brief Whether the robot has been successfully docked in this bay or not. + /// \return True when the robot has been docked or false otherwise. + public: bool AnytimeDocked() const; + + /// \brief Whether the robot is currently at the entrance of the bay. + /// \return True when the robot is at the entrance or false othwerwise. + public: bool AtEntrance() const; + + /// \brief Whether this is the correct bay to dock in. + public: bool Correct() const; + + /// \brief Announce the symbol of the bay. + public: void AnnounceSymbol(); + + /// \brief Update function that needs to be executed periodically. + /// \param[in] _info Update information. + public: void Update(const sim::UpdateInfo &_info); + + /// \brief Callback triggered when the vehicle enters or exits the activation + /// zone. + /// \param[in] _msg The current state (0: exiting, 1: entering). + private: void OnInternalActivationEvent(const msgs::Pose &_msg); + + /// \brief Callback triggered when the vehicle enters or exits the activation + /// zone. + /// \param[in] _msg The current state (0: exiting, 1: entering). + private: void OnExternalActivationEvent(const msgs::Pose &_msg); + + /// \brief The topic used to receive notifications from the internal + /// activation zone. + private: std::string internalActivationTopic; + + /// \brief The topic used to receive notifications from the external + /// activation zone. + private: std::string externalActivationTopic; + + /// \brief Minimum amount of seconds to stay docked to be considered a fully + /// successfull dock. + private: std::chrono::duration minDockTime; + + /// \brief Current simulation time. + private: std::chrono::duration simTime; + + /// \brief Whether this is the correct bay to dock in. + private: bool correctDock; + + /// \brief Timer used to calculate the elapsed time docked in the bay. + private: std::chrono::duration timer; + + /// \brief Whether the vehicle has successfully docked or not. + private: bool anytimeDocked = false; + + /// \brief Whether the vehicle is at the entrance of the bay or not. + private: bool atEntrance = false; + + /// \brief Shape and color of symbol to announce. E.g.: ["red", "cross"] + private: msgs::StringMsg_V symbol; + + /// \brief Topic where the target symbol will be published. + private: std::string symbolTopic = "/vrx/scan_dock/placard_symbol"; + + /// \brief The transport node. + private: transport::Node node; + + /// \brief Publish the placard symbols. + private: transport::Node::Publisher dockPlacardPub; +}; + +///////////////////////////////////////////////// +DockChecker::DockChecker(const std::string &_name, + const std::string &_internalActivationTopic, + const std::string &_externalActivationTopic, + const std::chrono::duration _minDockTime, const bool _correctDock, + const std::string &_announceSymbol, const std::string &_symbolTopic) + : name(_name), + internalActivationTopic(_internalActivationTopic), + externalActivationTopic(_externalActivationTopic), + minDockTime(_minDockTime), + correctDock(_correctDock), + symbolTopic(_symbolTopic) +{ + // Override the docks own sdf parameters + this->dockPlacardPub = this->node.Advertise( + this->symbolTopic); + + // Add the shape. + this->symbol.add_data(_announceSymbol.substr(_announceSymbol.find("_") + 1)); + // Add the color. + this->symbol.add_data(_announceSymbol.substr(0, _announceSymbol.find("_"))); + + this->timer = + std::chrono::duration(std::numeric_limits::max()); + + // Subscriber to receive ContainPlugin updates. + if (!this->node.Subscribe(this->internalActivationTopic, + &DockChecker::OnInternalActivationEvent, this)) + { + gzerr << "Error subscribing to topic [" << this->internalActivationTopic + << "]" << std::endl; + return; + } + if (!this->node.Subscribe(this->externalActivationTopic, + &DockChecker::OnExternalActivationEvent, this)) + { + gzerr << "Error subscribing to topic [" << this->externalActivationTopic + << "]" << std::endl; + return; + } +} + +///////////////////////////////////////////////// +bool DockChecker::AnytimeDocked() const +{ + return this->anytimeDocked; +} + +///////////////////////////////////////////////// +bool DockChecker::AtEntrance() const +{ + return this->atEntrance; +} + +///////////////////////////////////////////////// +bool DockChecker::Correct() const +{ + return this->correctDock; +} + +///////////////////////////////////////////////// +void DockChecker::AnnounceSymbol() +{ + this->dockPlacardPub.Publish(this->symbol); +} + +///////////////////////////////////////////////// +void DockChecker::Update(const sim::UpdateInfo &_info) +{ + this->simTime = _info.simTime; + + if (this->anytimeDocked) + return; + + auto elapsed = _info.simTime - this->timer; + this->anytimeDocked = elapsed >= this->minDockTime; + + if (this->anytimeDocked) + { + gzmsg << "Successfully stayed in dock for " << this->minDockTime.count() + << " seconds, transitioning to state" << std::endl; + } +} + +///////////////////////////////////////////////// +void DockChecker::OnInternalActivationEvent(const msgs::Pose &_msg) +{ + // Get the state from the header. + std::string state; + for (const auto &data : _msg.header().data()) + { + if (data.key() == "state" && !data.value().empty()) + { + state = data.value(0); + break; + } + } + + // Currently docked. + if (state == "1") + { + this->timer = this->simTime; + gzmsg << "Entering internal dock activation zone, transitioning to " + << " state in [" << this->name << "]." << std::endl; + } + + // Currently undocked. + if (state == "0") + { + this->timer = + std::chrono::duration(std::numeric_limits::max()); + if (this->AnytimeDocked()) + { + gzmsg << "Leaving internal dock activation zone in [" << this->name + << "] after required time - transitioning to state." + << std::endl; + } + else + { + gzmsg << "Leaving internal dock activation zone in [" << this->name + << "] early - transitioning back to state." + << std::endl; + } + } + + gzdbg << "[" << this->name << "] OnInternalActivationEvent(): " + << state << std::endl; + std::cout << std::flush; +} + +///////////////////////////////////////////////// +void DockChecker::OnExternalActivationEvent(const msgs::Pose &_msg) +{ + // Get the state from the header. + std::string state; + for (const auto &data : _msg.header().data()) + { + if (data.key() == "state" && !data.value().empty()) + { + state = data.value(0); + break; + } + } + + this->atEntrance = state == "1"; + + if (this->atEntrance) + { + gzmsg << "Entering external dock activation zone in [" << this->name + << "]" << std::endl; + } + else + { + gzmsg << "Leaving external dock activation zone in [" << this->name + << "]" << std::endl; + } + + gzdbg << "[" << this->name << "] OnExternalActivationEvent(): " + << state << std::endl; + std::cout << std::flush; +} + /// \brief Private ScanDockScoringPlugin data class. class ScanDockScoringPlugin::Implementation { + /// \brief World entity. + public: std::string worldName; + /// \brief Parse all SDF parameters. /// \param[in] _sdf SDF elements. /// \return True when all params were successfully parsed or false otherwise. @@ -174,6 +439,9 @@ class ScanDockScoringPlugin::Implementation /// the color sequence from the light buoy. public: std::unique_ptr colorChecker; + /// \brief Monitor all the available bays to decide when the vehicle docks. + public: std::vector> dockCheckers; + /// \brief To check colors or not. public: bool enableColorChecker = true; @@ -183,11 +451,22 @@ class ScanDockScoringPlugin::Implementation /// \brief Points granted when the color sequence is correct. public: double colorBonusPoints = 10.0; + /// \brief Points granted when the vehicle successfully + /// dock-and-undock in any bay + public: double dockBonusPoints = 10.0; + + /// \brief Points granted when the vehicle successfully + /// dock-and-undock in the specified bay. + public: double correctDockBonusPoints = 10.0; + /// \brief Expected color sequence. public: std::vector expectedSequence; + /// \brief A transport node. + public: transport::Node node; + /// \brief A mutex. - private: std::mutex mutex; + public: std::mutex mutex; }; ////////////////////////////////////////////////// @@ -239,6 +518,107 @@ bool ScanDockScoringPlugin::Implementation::ParseSDF(sdf::ElementPtr _sdf) new ColorSequenceChecker(this->expectedSequence, colorSequenceTopic)); } + // Required: Parse the bays. + if (!_sdf->HasElement("bays")) + { + gzerr << " missing" << std::endl; + return false; + } + + auto baysElem = _sdf->GetElement("bays"); + if (!baysElem->HasElement("bay")) + { + gzerr << " missing" << std::endl; + return false; + } + + auto bayElem = baysElem->GetElement("bay"); + while (bayElem) + { + // Required: bay name. + if (!bayElem->GetElement("name")) + { + gzerr << " missing" << std::endl; + return false; + } + std::string bayName = bayElem->Get("name"); + + // Required: internal_activation topic. + if (!bayElem->GetElement("internal_activation_topic")) + { + gzerr << " missing" << std::endl; + return false; + } + std::string internalActivationTopic = + bayElem->Get("internal_activation_topic"); + + // Required: external_activation topic. + if (!bayElem->GetElement("external_activation_topic")) + { + gzerr << " missing" << std::endl; + return false; + } + std::string externalActivationTopic = + bayElem->Get("external_activation_topic"); + + // Required: gazebo symbol topic. + if (!bayElem->GetElement("symbol_topic")) + { + gzerr << " missing" << std::endl; + return false; + } + std::string symbolTopic = bayElem->Get("symbol_topic"); + + // Required: minimum time to be considered "docked". + if (!bayElem->GetElement("min_dock_time")) + { + gzerr << " missing" << std::endl; + return false; + } + double minDockTime = bayElem->Get("min_dock_time"); + + // Required: correct dock . + if (!bayElem->GetElement("correct_dock")) + { + gzerr << " missing" << std::endl; + return false; + } + bool correctDock = bayElem->Get("correct_dock"); + + std::string announceSymbol = ""; + if (!bayElem->HasElement("symbol")) + { + gzerr << " not found" << std::endl; + } + announceSymbol = bayElem->GetElement("symbol")->Get(); + + // Create a new dock checker. + std::unique_ptr dockChecker( + new DockChecker(bayName, internalActivationTopic, + externalActivationTopic, std::chrono::duration(minDockTime), + correctDock, announceSymbol, symbolTopic)); + + // Add the dock checker. + this->dockCheckers.push_back(std::move(dockChecker)); + + // Process the next checker. + bayElem = bayElem->GetNextElement(); + } + + // Optional: the points granted when the vehicle docks in any bay. + if (_sdf->HasElement("dock_bonus_points")) + { + this->dockBonusPoints = + _sdf->GetElement("dock_bonus_points")->Get(); + } + + // Optional: the points granted when the vehicle docks in the expected bay. + if (_sdf->HasElement("correct_dock_bonus_points")) + { + this->correctDockBonusPoints = + _sdf->GetElement("correct_dock_bonus_points")->Get(); + } + return true; } @@ -256,6 +636,10 @@ void ScanDockScoringPlugin::Configure(const sim::Entity &_entity, { ScoringPlugin::Configure(_entity, _sdf, _ecm, _eventMgr); + sim::Entity worldEntity = _ecm.EntityByComponents(sim::components::World()); + this->dataPtr->worldName = + _ecm.Component(worldEntity)->Data(); + auto sdf = _sdf->Clone(); if (!this->dataPtr->ParseSDF(sdf)) { @@ -293,13 +677,85 @@ void ScanDockScoringPlugin::PreUpdate(const sim::UpdateInfo &_info, this->dataPtr->colorSubmissionProcessed = true; } } + + // Verify the dock checkers. + for (auto &dockChecker : this->dataPtr->dockCheckers) + { + // We always need to update the checkers. + dockChecker->Update(_info); + + // Nothing to do if nobody ever docked or we're still inside the bay. + if (!dockChecker->AnytimeDocked() || !dockChecker->AtEntrance()) + continue; + + // Points granted for docking! + this->SetScore(this->Score() + this->dataPtr->dockBonusPoints); + if (this->TaskState() == "running") + { + gzmsg << "Successfully docked in [" << dockChecker->name << "]" + << ". Awarding " << this->dataPtr->dockBonusPoints << " points." + << std::endl; + } + + // Is this the right bay? + if (dockChecker->Correct()) + { + this->SetScore(this->Score() + this->dataPtr->correctDockBonusPoints); + if (this->TaskState() == "running") + { + gzmsg << "Docked in correct dock [" << dockChecker->name << "]" + << ". Awarding " << this->dataPtr->correctDockBonusPoints + << " more points." << std::endl; + } + } + else + { + if (this->TaskState() == "running") + { + gzmsg << "Docked in incorrect dock [" << dockChecker->name << "]" + << ". No additional points." << std::endl; + } + } + + // Time to finish the task as the vehicle docked. + // Note that we only allow to dock one time. This is to prevent teams + // docking in all possible bays. + this->Exit(); + break; + } +} + +////////////////////////////////////////////////// +void ScanDockScoringPlugin::OnReady() +{ + // Announce the symbol if needed. + for (auto &dockChecker : this->dataPtr->dockCheckers) + dockChecker->AnnounceSymbol(); } ////////////////////////////////////////////////// void ScanDockScoringPlugin::OnRunning() { + std::function f = + [](const msgs::Boolean &_res, const bool) + { + }; + + std::string performerTopic = + "/world/" + this->dataPtr->worldName + "/level/set_performer"; + msgs::StringMsg performer; + performer.set_data(this->VehicleName()); + + // Register the vehicle as a performer. + this->dataPtr->node.Request( + performerTopic, performer, f); + if (this->dataPtr->enableColorChecker) this->dataPtr->colorChecker->Enable(); + + // Announce the symbol if needed. + for (auto &dockChecker : this->dataPtr->dockCheckers) + dockChecker->AnnounceSymbol(); } GZ_ADD_PLUGIN(ScanDockScoringPlugin, diff --git a/vrx_gz/src/ScanDockScoringPlugin.hh b/vrx_gz/src/ScanDockScoringPlugin.hh index 52bf24914..c481a525c 100644 --- a/vrx_gz/src/ScanDockScoringPlugin.hh +++ b/vrx_gz/src/ScanDockScoringPlugin.hh @@ -42,6 +42,27 @@ namespace vrx /// : Expected 3rd color of the sequence (RED, GREEN, BLUE, YELLOW). /// : Points granted when the color sequence is correct. /// Default value is 10. + /// : Contains at least one of the following blocks: + /// : A bay represents a potential place where a vehicle can dock. + /// It has the following required elements: + /// : The name of the bay. This is used for debugging only. + /// : The topic used to receive + /// notifications from the internal activation zone. + /// : The topic used to receive + /// notifications from the external activation zone. + /// : Minimum amount of seconds to stay docked to be + /// considered a fully successfull dock. + /// : True if this is the correct bay to dock in. + /// : Required string with format _, where + /// color can be "red", "green", "blue", "yellow" and shape can be + /// "triangle", "circle", "cross", "rectangle". If this parameter is + /// present, a message will be sent in OnReady(). + /// The vehicle should dock in the bay matching this color and shape. + /// Topic to publish the symbol announcement. + /// : Points granted when the vehicle successfully + /// docks and undocks in any bay. Default value is 10. + /// : Points granted when the vehicle successfully + /// docks and undocks in the specified bay. Default value is 10. /// Here's an example: /// @@ -58,6 +79,38 @@ namespace vrx /// red /// green /// blue + /// + /// 15 + /// 5 + /// + /// + /// bay1 + /// /vrx/dock_2022/bay_1/contain + /// /vrx/dock_2022/bay_1_external/contain + /// /vrx/dock_2022_placard1/symbol + /// 10.0 + /// False + /// red_rectangle + /// + /// + /// bay2 + /// /vrx/dock_2022/bay_2/contain + /// /vrx/dock_2022/bay_2_external/contain + /// /vrx/dock_2022_placard2/symbol + /// 10.0 + /// True + /// blue_triangle + /// + /// + /// bay3 + /// /vrx/dock_2022/bay_3/contain + /// /vrx/dock_2022/bay_3_external/contain + /// /vrx/dock_2022_placard3/symbol + /// 10.0 + /// False + /// yellow_circle + /// + /// /// class ScanDockScoringPlugin : public ScoringPlugin { @@ -77,6 +130,9 @@ namespace vrx public: void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + // Documentation inherited. + private: void OnReady() override; + // Documentation inherited. private: void OnRunning() override; diff --git a/vrx_gz/worlds/scan_dock_deliver_task.sdf b/vrx_gz/worlds/scan_dock_deliver_task.sdf index 34c03b38d..c0317449d 100644 --- a/vrx_gz/worlds/scan_dock_deliver_task.sdf +++ b/vrx_gz/worlds/scan_dock_deliver_task.sdf @@ -550,6 +550,38 @@ red green blue + + 15 + 5 + + + bay1 + /vrx/dock_2022/bay_1/contain + /vrx/dock_2022/bay_1_external/contain + /vrx/dock_2022_placard1/symbol + 10.0 + False + red_rectangle + + + bay2 + /vrx/dock_2022/bay_2/contain + /vrx/dock_2022/bay_2_external/contain + /vrx/dock_2022_placard2/symbol + 10.0 + True + blue_triangle + + + bay3 + /vrx/dock_2022/bay_3/contain + /vrx/dock_2022/bay_3_external/contain + /vrx/dock_2022_placard3/symbol + 10.0 + False + yellow_circle + +