From 2d20432e3953c5c352599e5bbe757541ad252aa7 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 14 Nov 2022 21:53:25 -0800 Subject: [PATCH 01/11] first pass --- vrx_gz/CMakeLists.txt | 1 + vrx_gz/src/GymkhanaScoringPlugin.cc | 234 ++++++++++++++++++++++++++++ vrx_gz/src/GymkhanaScoringPlugin.hh | 61 ++++++++ 3 files changed, 296 insertions(+) create mode 100644 vrx_gz/src/GymkhanaScoringPlugin.cc create mode 100644 vrx_gz/src/GymkhanaScoringPlugin.hh diff --git a/vrx_gz/CMakeLists.txt b/vrx_gz/CMakeLists.txt index 2ba453a29..0b1d1ff51 100644 --- a/vrx_gz/CMakeLists.txt +++ b/vrx_gz/CMakeLists.txt @@ -102,6 +102,7 @@ list(APPEND VRX_GZ_PLUGINS LightBuoyPlugin PerceptionScoringPlugin NavigationScoringPlugin + GymkhanaScoringPlugin SimpleHydrodynamics StationkeepingScoringPlugin Surface diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc new file mode 100644 index 000000000..5d4604d22 --- /dev/null +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -0,0 +1,234 @@ +/* + * Copyright (C) 2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include "GymkhanaScoringPlugin.hh" + +using namespace gz; +using namespace vrx; + +/// \brief Private GymkhanaScoringPlugin data class. +class GymkhanaScoringPlugin::Implementation +{ + /// \brief Callback for channel navigation portion's scoring plugin + public: void ChannelCallback(const msgs::Param &_msg); + + /// \brief Callback for black box station-keeping portion's scoring plugin + public: void BlackboxCallback(const msgs::Param &_msg); + + /// \brief Set the pinger location. + public: void SetPingerPosition(); + + /// \brief Transport node + public: transport::Node node; + + /// \brief ROS publisher to set the pinger position. + public: transport::Node::Publisher setPingerPub; + + /// \brief Whether buoy channel portion has been completed + public: bool channelCrossed = false; + + /// \brief Cumulative error from black box station keeping portion + public: double blackboxScore = 0.0; + + /// \brief Penalty added per collision. + public: double obstaclePenalty = 0.1; + + // TODO: compare with acousticpinger + /// \brief Position of the pinger. + public: math::Vector3d pingerPosition = {0, 0, 0}; + + /// \brief Last time we published a pinger position. + public: std::chrono::duration lastSetPingerPositionTime = + std::chrono::duration::zero(); + + /// \brief Pointer to the SDF plugin element. + public: sdf::ElementPtr sdf; + +}; + +///////////////////////////////////////////////// +void GymkhanaScoringPlugin::Implementation::ChannelCallback( + const msgs::Param &_msg) +{ + if (_msg.IsInitialized()) // TODO: Check + { + + // Determine whether channel has been crossed before timeout + if (!this->channelCrossed) + { + if (_msg.params().at("state").string_value() == "finished") + { + if (_msg.params().at("score").double_value() == 200) + this->ScoringPlugin::Finish(); + else + this->channelCrossed = true; + } + } + } +} + +///////////////////////////////////////////////// +void GymkhanaScoringPlugin::Implementation::BlackboxCallback( + const msgs::Param &_msg) +{ + if (_msg.IsInitialized()) //TODO: Check + { + this->blackboxScore = _msg.params().at("score").double_value(); + } +} + +///////////////////////////////////////////////// +void GymkhanaScoringPlugin::Implementation::SetPingerPosition() +{ + msgs::Vector3d position; + position.set_x(this->pingerPosition.X()); + position.set_y(this->pingerPosition.Y()); + position.set_z(this->pingerPosition.Z()); + this->setPingerPub.Publish(position); +} + +///////////////////////////////////////////////// +GymkhanaScoringPlugin::GymkhanaScoringPlugin() + : ScoringPlugin(), + dataPtr(utils::MakeUniqueImpl()) +{ + gzmsg << "GymkhanaScoringPlugin loaded" << std::endl; +} + +///////////////////////////////////////////////// +//TODO +void GymkhanaScoringPlugin::Configure(const sim::Entity &_entity, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &_ecm, sim::EventManager &_eventMgr) +{ + // Base class + ScoringPlugin::Configure(_entity, _sdf, _ecm, _eventMgr); + gzmsg << "Task [" << this->TaskName() << "]" << std::endl; + + this->dataPtr->sdf = _sdf->Clone(); + + // Optional element. + if (_sdf->HasElement("obstacle_penalty")) + this->dataPtr->obstaclePenalty = + this->dataPtr->sdf->Get("obstacle_penalty"); + + // Throttle messages to 1Hz + transport::AdvertiseMessageOptions opts; + opts.SetMsgsPerSec(1u); + + // Set the topic to be used to publish the sensor message. + std::string setPositionTopicName = "/pinger/set_pinger_position"; + if (_sdf->HasElement("set_position_topic_name")) + { + setPositionTopicName = + this->dataPtr->sdf->GetElement("set_position_topic_name")->Get(); + } + + // Set the pinger position. + if (_sdf->HasElement("pinger_position")) + { + this->dataPtr->pingerPosition = + _sdf->Get("pinger_position"); + } + + this->dataPtr->setPingerPub = + this->dataPtr->node.Advertise( + setPositionTopicName, opts); + + this->dataPtr->node.Subscribe("/vrx/gymkhana_channel/task/info", + &GymkhanaScoringPlugin::Implementation::ChannelCallback, + this->dataPtr.get()); + + this->dataPtr->node.Subscribe("/vrx/gymkhana_blackbox/task/info", + &GymkhanaScoringPlugin::Implementation::BlackboxCallback, + this->dataPtr.get()); +} + +///////////////////////////////////////////////// +//TODO +void GymkhanaScoringPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + + ScoringPlugin::PreUpdate(_info, _ecm); + + std::chrono::duration now = _info.simTime; + + // Set pinger position for the first time if needed + if (this->dataPtr->lastSetPingerPositionTime == + std::chrono::duration::zero()) + { + this->dataPtr->SetPingerPosition(); + this->dataPtr->lastSetPingerPositionTime = now; + } + + // Set the pinger position again every 10 seconds. + std::chrono::duration elapsedTime = + now - this->dataPtr->lastSetPingerPositionTime; + + if (elapsedTime >= std::chrono::duration(10.0)) + { + this->dataPtr->SetPingerPosition(); + this->dataPtr->lastSetPingerPositionTime = now; + } + + // Nothing to do if the task is not in "running" state. + if (this->ScoringPlugin::TaskState() != "running") + return; + + // Check channel navigation is finished + // If not, invalidate black box station-keeping score + if (this->dataPtr->channelCrossed) + { + this->ScoringPlugin::SetScore(this->dataPtr->blackboxScore); + } + else + { + this->ScoringPlugin::SetScore(200); + } +} + + + +////////////////////////////////////////////////// +void GymkhanaScoringPlugin::OnFinished() +{ + // TODO: fix obstacle penalty (Navigation Plugin Implementation) + double penalty = this->NumCollisions() * this->obstaclePenalty; + this->SetTimeoutScore(this->Score() + penalty); + + ScoringPlugin::OnFinished(); +} + +GZ_ADD_PLUGIN(GymkhanaScoringPlugin, + sim::System, + ScoringPlugin::ISystemConfigure, + ScoringPlugin::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(vrx::GymkhanaScoringPlugin, + "vrx::GymkhanaScoringPlugin") diff --git a/vrx_gz/src/GymkhanaScoringPlugin.hh b/vrx_gz/src/GymkhanaScoringPlugin.hh new file mode 100644 index 000000000..a111a3836 --- /dev/null +++ b/vrx_gz/src/GymkhanaScoringPlugin.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2020 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 VRX_GAZEBO_GYMKHANA_SCORING_PLUGIN_HH_ +#define VRX_GAZEBO_GYMKHANA_SCORING_PLUGIN_HH_ + +#include +#include +#include +#include + +#include "ScoringPlugin.hh" + +namespace vrx +{ + class GymkhanaScoringPlugin : public ScoringPlugin + { + /// \brief Constructor. + public: GymkhanaScoringPlugin(); + + /// \brief Destructor. + public: virtual ~GymkhanaScoringPlugin() override = default; + + // Documentation inherited. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + // Documentation inherited. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + // Documentation inherited. + private: void OnRunning() override; + + // Documentation inherited. + protected: void OnFinished() override; + + // Documentation inherited. + private: void ReleaseVehicle() override; + + /// \brief Private data pointer. + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +#endif From 8569014d282d83e73e7232d80466d670bc3f3519 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 16 Nov 2022 09:03:53 -0800 Subject: [PATCH 02/11] move channel callback to main class --- vrx_gz/src/GymkhanaScoringPlugin.cc | 17 +++++++---------- vrx_gz/src/GymkhanaScoringPlugin.hh | 4 ++++ 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index 5d4604d22..4de202f89 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -34,9 +34,6 @@ using namespace vrx; /// \brief Private GymkhanaScoringPlugin data class. class GymkhanaScoringPlugin::Implementation { - /// \brief Callback for channel navigation portion's scoring plugin - public: void ChannelCallback(const msgs::Param &_msg); - /// \brief Callback for black box station-keeping portion's scoring plugin public: void BlackboxCallback(const msgs::Param &_msg); @@ -72,21 +69,21 @@ class GymkhanaScoringPlugin::Implementation }; ///////////////////////////////////////////////// -void GymkhanaScoringPlugin::Implementation::ChannelCallback( +void GymkhanaScoringPlugin::ChannelCallback( const msgs::Param &_msg) { if (_msg.IsInitialized()) // TODO: Check { // Determine whether channel has been crossed before timeout - if (!this->channelCrossed) + if (!this->dataPtr->channelCrossed) { if (_msg.params().at("state").string_value() == "finished") { if (_msg.params().at("score").double_value() == 200) - this->ScoringPlugin::Finish(); + ScoringPlugin::Finish(); else - this->channelCrossed = true; + this->dataPtr->channelCrossed = true; } } } @@ -161,8 +158,8 @@ void GymkhanaScoringPlugin::Configure(const sim::Entity &_entity, setPositionTopicName, opts); this->dataPtr->node.Subscribe("/vrx/gymkhana_channel/task/info", - &GymkhanaScoringPlugin::Implementation::ChannelCallback, - this->dataPtr.get()); + &GymkhanaScoringPlugin::ChannelCallback, + this); this->dataPtr->node.Subscribe("/vrx/gymkhana_blackbox/task/info", &GymkhanaScoringPlugin::Implementation::BlackboxCallback, @@ -219,7 +216,7 @@ void GymkhanaScoringPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, void GymkhanaScoringPlugin::OnFinished() { // TODO: fix obstacle penalty (Navigation Plugin Implementation) - double penalty = this->NumCollisions() * this->obstaclePenalty; + double penalty = this->NumCollisions() * this->dataPtr->obstaclePenalty; this->SetTimeoutScore(this->Score() + penalty); ScoringPlugin::OnFinished(); diff --git a/vrx_gz/src/GymkhanaScoringPlugin.hh b/vrx_gz/src/GymkhanaScoringPlugin.hh index a111a3836..638a2e665 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.hh +++ b/vrx_gz/src/GymkhanaScoringPlugin.hh @@ -18,6 +18,7 @@ #ifndef VRX_GAZEBO_GYMKHANA_SCORING_PLUGIN_HH_ #define VRX_GAZEBO_GYMKHANA_SCORING_PLUGIN_HH_ +#include #include #include #include @@ -45,6 +46,9 @@ namespace vrx public: void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + /// \brief Callback for channel navigation portion's scoring plugin + public: void ChannelCallback(const gz::msgs::Param &_msg); + // Documentation inherited. private: void OnRunning() override; From 42f6a9051ce5962a4d7bac2deb91adacfd3916bb Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 16 Nov 2022 11:17:23 -0800 Subject: [PATCH 03/11] plugins working together --- vrx_gz/src/GymkhanaScoringPlugin.cc | 14 +++ vrx_gz/src/GymkhanaScoringPlugin.hh | 3 - vrx_gz/src/NavigationScoringPlugin.cc | 1 - vrx_gz/worlds/gymkhana_task.sdf | 118 ++++++++++++++++---------- 4 files changed, 87 insertions(+), 49 deletions(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index 4de202f89..fe4a4fa51 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -66,6 +66,9 @@ class GymkhanaScoringPlugin::Implementation /// \brief Pointer to the SDF plugin element. public: sdf::ElementPtr sdf; + /// \brief Display or suppress state changes + public: bool silent = false; + }; ///////////////////////////////////////////////// @@ -210,6 +213,17 @@ void GymkhanaScoringPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, } } +////////////////////////////////////////////////// +void GymkhanaScoringPlugin::OnRunning() +{ + if (!this->dataPtr->silent) + { + gzmsg << "GymkhanaScoringPlugin::OnRunning" << std::endl; + std::cout << std::flush; + } + ScoringPlugin::OnRunning(); +} + ////////////////////////////////////////////////// diff --git a/vrx_gz/src/GymkhanaScoringPlugin.hh b/vrx_gz/src/GymkhanaScoringPlugin.hh index 638a2e665..896f03683 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.hh +++ b/vrx_gz/src/GymkhanaScoringPlugin.hh @@ -55,9 +55,6 @@ namespace vrx // Documentation inherited. protected: void OnFinished() override; - // Documentation inherited. - private: void ReleaseVehicle() override; - /// \brief Private data pointer. GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/vrx_gz/src/NavigationScoringPlugin.cc b/vrx_gz/src/NavigationScoringPlugin.cc index c7626004e..7c4ea1d9c 100644 --- a/vrx_gz/src/NavigationScoringPlugin.cc +++ b/vrx_gz/src/NavigationScoringPlugin.cc @@ -128,7 +128,6 @@ class NavigationScoringPlugin::Implementation /// \brief Number of points deducted per collision. public: double obstaclePenalty = 10.0; - //TODO: should this be set in ScoringPlugin? /// \brief Display or suppress state changes public: bool silent = false; }; diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf index 71fe3584a..b3f00a5a2 100644 --- a/vrx_gz/worlds/gymkhana_task.sdf +++ b/vrx_gz/worlds/gymkhana_task.sdf @@ -358,6 +358,12 @@ platform + + + model://short_navigation_course_0 + -524 186 0 0 0 -1.44 + + post_0 @@ -405,46 +411,6 @@ blue_projectile --> - - mb_marker_buoy_red - -528 191 0 0 1.57 0 - https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_red - - - 1000 - 0.0 - 25.0 - 2.0 - - link - 0 0 -0.3 0 0 0 - - - 0.325 - 0.1 - - - - - 1000 1000 - 50 50> - - PMS - 5.0 - 3 - 1.1 - 0.3 - 1 0 - 0.4 - 2.0 - 0.0 - 0.0 - - - - - 0 0 0 @@ -486,20 +452,82 @@ + + + wamv + gymkhana + /vrx/task/info + /vrx/debug/contact + 10 + 10 + 300 + + true + 1 + /wamv/sensors/pingers/pinger/set_pinger_position + -483 295.5 0 + + + + + wamv + gymkhana_channel + /vrx/gymkhana_channel/task/info + /vrx/gymkhana_channel/debug/contact + + false + + 10 + 10 + 300 + 10 + /vrx/release + + short_navigation_course_0 + 10.0 + + + red_bound_0 + green_bound_0 + + + red_bound_1 + green_bound_1 + + + red_bound_2 + green_bound_2 + + + true + + wamv - station_keeping - /vrx/task/info + gymkhana_blackbox + /vrx/gymkhana_blackbox/mean_pose_error + /vrx/gymkhana_blackbox/pose_error + /vrx/gymkhana_blackbox/goal + /vrx/gymkhana_blackbox/task/info + /vrx/gymkhana_blackbox/debug/contact + + false + + false + + -483 295.5 0 10 10 300 /vrx/release - - -33.722718 150.674031 0 - From 52e1564322c21afd3831bb23f00ad5f22a69a789 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 16 Nov 2022 14:53:32 -0800 Subject: [PATCH 04/11] obstacle course fix --- vrx_gz/models/obstacle_course/model.config | 13 + vrx_gz/models/obstacle_course/model.sdf | 577 +++++++++++++++++++++ vrx_gz/worlds/gymkhana_task.sdf | 7 + 3 files changed, 597 insertions(+) create mode 100644 vrx_gz/models/obstacle_course/model.config create mode 100644 vrx_gz/models/obstacle_course/model.sdf diff --git a/vrx_gz/models/obstacle_course/model.config b/vrx_gz/models/obstacle_course/model.config new file mode 100644 index 000000000..c059adbb0 --- /dev/null +++ b/vrx_gz/models/obstacle_course/model.config @@ -0,0 +1,13 @@ + + + obstacle_course + 1.0 + model.sdf + + Carlos Agüero + caguero@openrobotics.org + + + An obstacle field. + + diff --git a/vrx_gz/models/obstacle_course/model.sdf b/vrx_gz/models/obstacle_course/model.sdf new file mode 100644 index 000000000..40a14f916 --- /dev/null +++ b/vrx_gz/models/obstacle_course/model.sdf @@ -0,0 +1,577 @@ + + + + + + + mb_round_buoy_black_0 + 56.84848062 59.2614612521 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_1 + 56.9804067204 8.39663411782 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_2 + -39.9569402425 -59.6380114707 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_3 + 40.5935528521 0.420855757631 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_4 + 45.1471787907 -1.58774897319 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_5 + 37.4966413916 -23.6251778906 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_6 + 46.2135293014 -5.59469145311 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_7 + 24.576913737 30.3481238647 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_8 + 37.5737439619 -18.8335819857 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_black_9 + -2.49572219989 13.1619102364 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_orange_0 + 59.628595207 -32.2043953768 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_orange_1 + 51.0184408248 -44.8771143523 0.0 0 0 0 + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange + + + + mb_round_buoy_orange_2 + -37.1237799886 -55.0780758672 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_orange_3 + 4.82024240714 5.70282780335 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + mb_round_buoy_orange_4 + -10.7269893388 -31.6245175746 0.0 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 0 0 0 0 + + + 0.25 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf index b3f00a5a2..3bcb883f5 100644 --- a/vrx_gz/worlds/gymkhana_task.sdf +++ b/vrx_gz/worlds/gymkhana_task.sdf @@ -364,6 +364,13 @@ -524 186 0 0 0 -1.44 + + + buoys + model://obstacle_course + -477 275 0 0 0 2.54 + + post_0 From 759acc9f1003be0bd1659348ef24e8d914e1d8dd Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Tue, 22 Nov 2022 12:56:57 -0800 Subject: [PATCH 05/11] made sdf topics match bridge topic names --- vrx_gz/src/vrx_gz/bridges.py | 28 ++++++++++++++++++++++++++++ vrx_gz/src/vrx_gz/launch.py | 6 ++++++ vrx_gz/worlds/gymkhana_task.sdf | 2 +- 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/vrx_gz/src/vrx_gz/bridges.py b/vrx_gz/src/vrx_gz/bridges.py index bb52d4475..9a7bf92ca 100644 --- a/vrx_gz/src/vrx_gz/bridges.py +++ b/vrx_gz/src/vrx_gz/bridges.py @@ -185,3 +185,31 @@ def perception_reports(): gz_type='ignition.msgs.Pose', ros_type='geometry_msgs/msg/PoseStamped', direction=BridgeDirection.ROS_TO_GZ) + +def gymkhana_blackbox_goal(): + return Bridge( + gz_topic=f'/vrx/gymkhana_blackbox/goal', + ros_topic=f'/vrx/gymkhana_blackbox/goal', + gz_type='ignition.msgs.Pose', + ros_type='geometry_msgs/msg/PoseStamped', + direction=BridgeDirection.GZ_TO_ROS) + + +def gymkhana_blackbox_mean_pose_error(): + return Bridge( + gz_topic=f'/vrx/gymkhana_blackbox/mean_pose_error', + ros_topic=f'/vrx/gymkhana_blackbox/mean_pose_error', + gz_type='ignition.msgs.Float', + ros_type='std_msgs/msg/Float32', + direction=BridgeDirection.GZ_TO_ROS) + + +def gymkhana_blackbox_pose_error(): + return Bridge( + gz_topic=f'/vrx/gymkhana_blackbox/pose_error', + ros_topic=f'/vrx/gymkhana_blackbox/pose_error', + gz_type='ignition.msgs.Float', + ros_type='std_msgs/msg/Float32', + direction=BridgeDirection.GZ_TO_ROS) + + diff --git a/vrx_gz/src/vrx_gz/launch.py b/vrx_gz/src/vrx_gz/launch.py index b3e20691e..a173f2257 100644 --- a/vrx_gz/src/vrx_gz/launch.py +++ b/vrx_gz/src/vrx_gz/launch.py @@ -111,6 +111,12 @@ def competition_bridges(world_name): vrx_gz.bridges.wayfinding_mean_error(), vrx_gz.bridges.wayfinding_min_errors(), ] + elif world_name in GYMKHANA_WORLDS: + task_bridges = [ + vrx_gz.bridges.gymkhana_blackbox_goal(), + vrx_gz.bridges.gymkhana_blackbox_mean_pose_error(), + vrx_gz.bridges.gymkhana_blackbox_pose_error(), + ] elif world_name in WILDLIFE_WORLDS: task_bridges = [ ] diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf index 3bcb883f5..be71087dc 100644 --- a/vrx_gz/worlds/gymkhana_task.sdf +++ b/vrx_gz/worlds/gymkhana_task.sdf @@ -474,7 +474,7 @@ Respect top-level plugin finished status. --> true 1 - /wamv/sensors/pingers/pinger/set_pinger_position + /wamv/pingers/pinger/set_pinger_position -483 295.5 0 From 5f4c6ea8ea71d07f4bffbce4d0e6ce3c63ab7a8d Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Nov 2022 11:58:29 -0800 Subject: [PATCH 06/11] telling plugins not to update when the simulation is paused --- vrx_gz/src/GymkhanaScoringPlugin.cc | 3 +++ vrx_gz/src/NavigationScoringPlugin.cc | 5 ++++- vrx_gz/src/PerceptionScoringPlugin.cc | 1 + vrx_gz/src/StationkeepingScoringPlugin.cc | 4 ++++ vrx_gz/src/WayfindingScoringPlugin.cc | 4 ++++ 5 files changed, 16 insertions(+), 1 deletion(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index fe4a4fa51..f155cfd6d 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -174,6 +174,9 @@ void GymkhanaScoringPlugin::Configure(const sim::Entity &_entity, void GymkhanaScoringPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) { + // don't update when paused + if (_info.paused) + return; ScoringPlugin::PreUpdate(_info, _ecm); diff --git a/vrx_gz/src/NavigationScoringPlugin.cc b/vrx_gz/src/NavigationScoringPlugin.cc index 7c4ea1d9c..c544d5748 100644 --- a/vrx_gz/src/NavigationScoringPlugin.cc +++ b/vrx_gz/src/NavigationScoringPlugin.cc @@ -341,8 +341,11 @@ void NavigationScoringPlugin::Configure(const sim::Entity &_entity, void NavigationScoringPlugin::PreUpdate( const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) { - ScoringPlugin::PreUpdate(_info, _ecm); + // don't update when paused + if (_info.paused) + return; + ScoringPlugin::PreUpdate(_info, _ecm); // The vehicle might not be ready yet, let's try to get it. if (!this->dataPtr->vehicleEntity) { diff --git a/vrx_gz/src/PerceptionScoringPlugin.cc b/vrx_gz/src/PerceptionScoringPlugin.cc index 5c419e95b..15f8796a7 100644 --- a/vrx_gz/src/PerceptionScoringPlugin.cc +++ b/vrx_gz/src/PerceptionScoringPlugin.cc @@ -417,6 +417,7 @@ void PerceptionScoringPlugin::Configure(const sim::Entity &_entity, void PerceptionScoringPlugin::PreUpdate(const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) { + // don't update when paused if (_info.paused) return; diff --git a/vrx_gz/src/StationkeepingScoringPlugin.cc b/vrx_gz/src/StationkeepingScoringPlugin.cc index a4c2dd5a9..1543657e1 100644 --- a/vrx_gz/src/StationkeepingScoringPlugin.cc +++ b/vrx_gz/src/StationkeepingScoringPlugin.cc @@ -244,6 +244,10 @@ void StationkeepingScoringPlugin::Configure(const sim::Entity &_entity, void StationkeepingScoringPlugin::PreUpdate( const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) { + // don't update when paused + if (_info.paused) + return; + ScoringPlugin::PreUpdate(_info, _ecm); // Check to see if we need to publish the marker(s) diff --git a/vrx_gz/src/WayfindingScoringPlugin.cc b/vrx_gz/src/WayfindingScoringPlugin.cc index e44167d23..52ed833c2 100644 --- a/vrx_gz/src/WayfindingScoringPlugin.cc +++ b/vrx_gz/src/WayfindingScoringPlugin.cc @@ -213,6 +213,10 @@ void WayfindingScoringPlugin::Configure(const sim::Entity &_entity, void WayfindingScoringPlugin::PreUpdate(const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) { + // don't update when paused + if (_info.paused) + return; + ScoringPlugin::PreUpdate(_info,_ecm); // Start publishing the goal once in "ready" state From 33b9eb170c22da588cdf9ef60d334c838fe6278d Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 28 Nov 2022 10:25:12 -0800 Subject: [PATCH 07/11] documenting _msg parameter and taskMsg definition --- vrx_gz/src/GymkhanaScoringPlugin.cc | 1 + vrx_gz/src/ScoringPlugin.cc | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index f155cfd6d..959843124 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -35,6 +35,7 @@ using namespace vrx; class GymkhanaScoringPlugin::Implementation { /// \brief Callback for black box station-keeping portion's scoring plugin + /// \param[in] _msg Task message as defined in ScoringPlugin::Implementation public: void BlackboxCallback(const msgs::Param &_msg); /// \brief Set the pinger location. diff --git a/vrx_gz/src/ScoringPlugin.cc b/vrx_gz/src/ScoringPlugin.cc index fe3b32d46..80e2abcea 100644 --- a/vrx_gz/src/ScoringPlugin.cc +++ b/vrx_gz/src/ScoringPlugin.cc @@ -130,6 +130,14 @@ class ScoringPlugin::Implementation public: std::string taskState = "initial"; /// \brief The next task message to be published. + /// This is a generic message holding name-value pairs. + /// The following name-value pairs are defined: + /// state: string containing the current taskState + /// elapsed_time: double representing current elapsed time + /// remaining_time: double representing current remaining time + /// timed_out: boolean indicating whether the task has timed out + /// num_collisions: integer representing the number of recorded collisions + /// score: double representing the current task score public: msgs::Param taskMsg; /// \brief Score in case of timeout - added for Navigation task From 853e1ff939aa8ae5ee05c200b76c82a33995f173 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 28 Nov 2022 10:45:58 -0800 Subject: [PATCH 08/11] style tweaks --- vrx_gz/src/GymkhanaScoringPlugin.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index 959843124..530b13ee2 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -44,7 +44,7 @@ class GymkhanaScoringPlugin::Implementation /// \brief Transport node public: transport::Node node; - /// \brief ROS publisher to set the pinger position. + /// \brief publisher to set the pinger position. public: transport::Node::Publisher setPingerPub; /// \brief Whether buoy channel portion has been completed @@ -69,7 +69,6 @@ class GymkhanaScoringPlugin::Implementation /// \brief Display or suppress state changes public: bool silent = false; - }; ///////////////////////////////////////////////// @@ -135,8 +134,10 @@ void GymkhanaScoringPlugin::Configure(const sim::Entity &_entity, // Optional element. if (_sdf->HasElement("obstacle_penalty")) + { this->dataPtr->obstaclePenalty = this->dataPtr->sdf->Get("obstacle_penalty"); + } // Throttle messages to 1Hz transport::AdvertiseMessageOptions opts; @@ -171,7 +172,6 @@ void GymkhanaScoringPlugin::Configure(const sim::Entity &_entity, } ///////////////////////////////////////////////// -//TODO void GymkhanaScoringPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) { From 321df570636c670b3f0d609487a0da7fc7ed05e0 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 28 Nov 2022 11:23:51 -0800 Subject: [PATCH 09/11] set Stationkeeping to silent --- vrx_gz/worlds/gymkhana_task.sdf | 1 + 1 file changed, 1 insertion(+) diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf index be71087dc..763ab3778 100644 --- a/vrx_gz/worlds/gymkhana_task.sdf +++ b/vrx_gz/worlds/gymkhana_task.sdf @@ -536,6 +536,7 @@ 300 /vrx/release + true From 2c1a08bc8454b22d109b222321b7988aca968e5c Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 28 Nov 2022 11:24:14 -0800 Subject: [PATCH 10/11] add documentation for GymkhanaScoringPlugin --- vrx_gz/src/GymkhanaScoringPlugin.hh | 39 ++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.hh b/vrx_gz/src/GymkhanaScoringPlugin.hh index 896f03683..1944fb1db 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.hh +++ b/vrx_gz/src/GymkhanaScoringPlugin.hh @@ -28,7 +28,44 @@ namespace vrx { - class GymkhanaScoringPlugin : public ScoringPlugin + /// \brief A plugin for computing the score of the gymkhana task. + /// This plugin derives from the generic ScoringPlugin class. Refer to that + /// plugin for an explanation of the four states defined (Initial, Ready, + /// Running and Finished) as well as other required SDF elements. + /// + /// The gymkhana task is divided into two parts: a navigation channel and + /// an obstacle field. The vehicle should first traverse the navigation + /// channel, then minimize its distance to an acoustic pinger "blackbox" + /// located in the obstacle field. + /// + /// This plugin accepts the following optional SDF parameters: + /// + /// : Specifies how many points are deducted per collision. + /// : Specifies the location of the pinger in cartesian + /// coordinates. The first two coordinates represent cartesian X and Y. These + /// default to 0. The third value is always ignored. + /// : Specifies the topic used to set the pinger + /// position. The default is /pinger/set_pinger_position. Note this topic is + /// for debugging only; the pinger position will not be changed dynamically + /// during an actual competition trial. + /// + /// To manage the navigation channel and obstacle field parts of the task, + /// this plugin also relies on the NavigationScoringPlugin and the + /// StationKeepingScoringPlugin. These must be included in the sdf and + /// configured according to their documentation, with the following + /// additional constraints: + /// + /// NavigationScoringPlugin: + /// must be set to false + /// is recommended to be set to true to reduce redundant messages + /// + /// StationkeepingScoringPlugin: + /// must have the same value as the + /// set in the GymKhanaScoringPlugin. + /// must be set to false + /// must be set to false + /// is recommended to be set to true to reduce redundant messages + class GymkhanaScoringPlugin : public ScoringPlugin { /// \brief Constructor. public: GymkhanaScoringPlugin(); From 714b8b306c1095a555ab46d4caa686a858cf9f33 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Mon, 28 Nov 2022 16:37:11 -0800 Subject: [PATCH 11/11] remove unnessary _msg check --- vrx_gz/src/GymkhanaScoringPlugin.cc | 23 ++++++++--------------- 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/vrx_gz/src/GymkhanaScoringPlugin.cc b/vrx_gz/src/GymkhanaScoringPlugin.cc index 530b13ee2..0a8c31482 100644 --- a/vrx_gz/src/GymkhanaScoringPlugin.cc +++ b/vrx_gz/src/GymkhanaScoringPlugin.cc @@ -75,19 +75,15 @@ class GymkhanaScoringPlugin::Implementation void GymkhanaScoringPlugin::ChannelCallback( const msgs::Param &_msg) { - if (_msg.IsInitialized()) // TODO: Check + // Determine whether channel has been crossed before timeout + if (!this->dataPtr->channelCrossed) { - - // Determine whether channel has been crossed before timeout - if (!this->dataPtr->channelCrossed) + if (_msg.params().at("state").string_value() == "finished") { - if (_msg.params().at("state").string_value() == "finished") - { - if (_msg.params().at("score").double_value() == 200) - ScoringPlugin::Finish(); - else - this->dataPtr->channelCrossed = true; - } + if (_msg.params().at("score").double_value() == 200) + ScoringPlugin::Finish(); + else + this->dataPtr->channelCrossed = true; } } } @@ -96,10 +92,7 @@ void GymkhanaScoringPlugin::ChannelCallback( void GymkhanaScoringPlugin::Implementation::BlackboxCallback( const msgs::Param &_msg) { - if (_msg.IsInitialized()) //TODO: Check - { - this->blackboxScore = _msg.params().at("score").double_value(); - } + this->blackboxScore = _msg.params().at("score").double_value(); } /////////////////////////////////////////////////