diff --git a/vrx_gz/models/dock_2022/model.sdf b/vrx_gz/models/dock_2022/model.sdf
index fc96d3273..df3868732 100644
--- a/vrx_gz/models/dock_2022/model.sdf
+++ b/vrx_gz/models/dock_2022/model.sdf
@@ -78,35 +78,6 @@
1
-
-
-
@@ -186,35 +157,6 @@
1
-
-
-
@@ -294,35 +236,6 @@
1
-
-
-
@@ -335,8 +248,8 @@
-
+
+
+
+
+
+ 5.95 15.59 2.83 0 0 1.5707963267948966
+
+
+ 0.5 0.5 0.5
+
+
+
+ 0 1 0
+ 0 1 0
+ 0 1 0 1
+
+
+
+
+ 5.95 14.39 2.83 0 0 1.5707963267948966
+
+
+ 0.25 0.5 0.25
+
+
+
+ 0 1 0
+ 0 1 0
+ 0 1 0 1
+
+ -->
+
+
+
@@ -495,6 +494,79 @@
+
+
+
+ /vrx/dock_2022_placard1_big_hole/contain
+ 5.95 2.59 2.83 0 0 1.5707963267948966
+
+
+ 0.5 0.5 0.5
+
+
+
+
+
+ vrx/dock_2022_placard1_small_hole/contain
+ 5.95 2.39 2.83 0 0 1.5707963267948966
+
+
+ 0.25 0.5 0.25
+
+
+
+
+
+ /vrx/dock_2022_placard2_big_hole/contain
+ 5.95 9.59 2.83 0 0 1.5707963267948966
+
+
+ 0.5 0.5 0.5
+
+
+
+
+
+ vrx/dock_2022_placard2_small_hole/contain
+ 5.95 8.39 2.83 0 0 1.5707963267948966
+
+
+ 0.25 0.5 0.25
+
+
+
+
+
+ /vrx/dock_2022_placard3_big_hole/contain
+ 5.95 15.59 2.83 0 0 1.5707963267948966
+
+
+ 0.5 0.5 0.5
+
+
+
+
+
+ vrx/dock_2022_placard3_small_hole/contain
+ 5.95 14.39 2.83 0 0 1.5707963267948966
+
+
+ 0.25 0.5 0.25
+
+
+
diff --git a/vrx_gz/src/ScanDockScoringPlugin.cc b/vrx_gz/src/ScanDockScoringPlugin.cc
index 3649e7106..51a49227e 100644
--- a/vrx_gz/src/ScanDockScoringPlugin.cc
+++ b/vrx_gz/src/ScanDockScoringPlugin.cc
@@ -183,13 +183,15 @@ class DockChecker
/// \param[in] _announceSymbol Symbol to announce via msgs.
/// E.g.: red_cross, blue_circle
/// \param[in] _symbolTopic Optional topic to announce the symbol.
+ /// \param[in] _vehicleName Name of the vehicle.
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);
+ const std::string &_symbolTopic,
+ const std::string &_vehicleName);
/// \brief The name of this checker.
public: std::string name;
@@ -255,6 +257,9 @@ class DockChecker
/// \brief Topic where the target symbol will be published.
private: std::string symbolTopic = "/vrx/scan_dock/placard_symbol";
+ /// \brief Vehicle name.
+ private: std::string vehicleName;
+
/// \brief The transport node.
private: transport::Node node;
@@ -267,13 +272,15 @@ 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)
+ const std::string &_announceSymbol, const std::string &_symbolTopic,
+ const std::string &_vehicleName)
: name(_name),
internalActivationTopic(_internalActivationTopic),
externalActivationTopic(_externalActivationTopic),
minDockTime(_minDockTime),
correctDock(_correctDock),
- symbolTopic(_symbolTopic)
+ symbolTopic(_symbolTopic),
+ vehicleName(_vehicleName)
{
// Override the docks own sdf parameters
this->dockPlacardPub = this->node.Advertise(
@@ -349,6 +356,11 @@ void DockChecker::Update(const sim::UpdateInfo &_info)
/////////////////////////////////////////////////
void DockChecker::OnInternalActivationEvent(const msgs::Pose &_msg)
{
+ // Sanity check: We're only interested in a performer that matches our
+ // vehicle name.
+ if (_msg.name() != this->vehicleName)
+ return;
+
// Get the state from the header.
std::string state;
for (const auto &data : _msg.header().data())
@@ -395,6 +407,11 @@ void DockChecker::OnInternalActivationEvent(const msgs::Pose &_msg)
/////////////////////////////////////////////////
void DockChecker::OnExternalActivationEvent(const msgs::Pose &_msg)
{
+ // Sanity check: We're only interested in a performer that matches our
+ // vehicle name.
+ if (_msg.name() != this->vehicleName)
+ return;
+
// Get the state from the header.
std::string state;
for (const auto &data : _msg.header().data())
@@ -432,8 +449,10 @@ class ScanDockScoringPlugin::Implementation
/// \brief Parse all SDF parameters.
/// \param[in] _sdf SDF elements.
+ /// \param[in] _vehicleName Name of the vehicle.
/// \return True when all params were successfully parsed or false otherwise.
- public: bool ParseSDF(sdf::ElementPtr _sdf);
+ public: bool ParseSDF(sdf::ElementPtr _sdf,
+ const std::string _vehicleName);
/// \brief In charge of receiving the team submission and compare it with
/// the color sequence from the light buoy.
@@ -467,10 +486,14 @@ class ScanDockScoringPlugin::Implementation
/// \brief A mutex.
public: std::mutex mutex;
+
+ /// \brief The shooting bonus.
+ public: double shootingBonus = 0.0;
};
//////////////////////////////////////////////////
-bool ScanDockScoringPlugin::Implementation::ParseSDF(sdf::ElementPtr _sdf)
+bool ScanDockScoringPlugin::Implementation::ParseSDF(sdf::ElementPtr _sdf,
+ const std::string _vehicleName)
{
// Enable color checker - default is true
this->enableColorChecker = true;
@@ -596,7 +619,7 @@ bool ScanDockScoringPlugin::Implementation::ParseSDF(sdf::ElementPtr _sdf)
std::unique_ptr dockChecker(
new DockChecker(bayName, internalActivationTopic,
externalActivationTopic, std::chrono::duration(minDockTime),
- correctDock, announceSymbol, symbolTopic));
+ correctDock, announceSymbol, symbolTopic, _vehicleName));
// Add the dock checker.
this->dockCheckers.push_back(std::move(dockChecker));
@@ -619,6 +642,65 @@ bool ScanDockScoringPlugin::Implementation::ParseSDF(sdf::ElementPtr _sdf)
_sdf->GetElement("correct_dock_bonus_points")->Get();
}
+ // Optional: the shooting targets.
+ if (_sdf->HasElement("targets"))
+ {
+ // We require at least one element.
+ auto targetsElem = _sdf->GetElement("targets");
+ if (!targetsElem->HasElement("target"))
+ {
+ gzerr << " not found" << std::endl;
+ return false;
+ }
+
+ auto targetElem = targetsElem->GetElement("target");
+ while (targetElem)
+ {
+ if (!targetElem->HasElement("topic"))
+ {
+ gzerr << " not found" << std::endl;
+ return false;
+ }
+ std::string topic = targetElem->Get("topic");
+
+ if (!targetElem->HasElement("bonus_points"))
+ {
+ gzerr << " not found" << std::endl;
+ return false;
+ }
+ double bonusPoints = targetElem->Get("bonus_points");
+
+ std::function subCb =
+ [this, bonusPoints](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;
+ }
+ }
+ // The projectile hit the target!
+ if (state == "1")
+ {
+ std::lock_guard lock(this->mutex);
+ this->shootingBonus = bonusPoints;
+
+ gzmsg << "Target hit!" << std::endl;
+ std::cout << std::flush;
+ }
+ };
+
+ this->node.Subscribe(topic, subCb);
+
+ // Process the next target.
+ targetElem = targetElem->GetNextElement();
+ }
+ }
+
return true;
}
@@ -641,7 +723,7 @@ void ScanDockScoringPlugin::Configure(const sim::Entity &_entity,
_ecm.Component(worldEntity)->Data();
auto sdf = _sdf->Clone();
- if (!this->dataPtr->ParseSDF(sdf))
+ if (!this->dataPtr->ParseSDF(sdf, this->VehicleName()))
{
gzerr << "Error parsing SDF, plugin disabled." << std::endl;
return;
@@ -723,6 +805,16 @@ void ScanDockScoringPlugin::PreUpdate(const sim::UpdateInfo &_info,
this->Exit();
break;
}
+
+ // Check the shooting targets.
+ {
+ std::lock_guard lock(this->dataPtr->mutex);
+ if (this->dataPtr->shootingBonus > 0)
+ {
+ this->SetScore(this->Score() + this->dataPtr->shootingBonus);
+ this->dataPtr->shootingBonus = 0;
+ }
+ }
}
//////////////////////////////////////////////////
diff --git a/vrx_gz/src/ScanDockScoringPlugin.hh b/vrx_gz/src/ScanDockScoringPlugin.hh
index c481a525c..c2aa865e8 100644
--- a/vrx_gz/src/ScanDockScoringPlugin.hh
+++ b/vrx_gz/src/ScanDockScoringPlugin.hh
@@ -42,6 +42,7 @@ 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:
@@ -63,6 +64,13 @@ namespace vrx
/// 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.
+ ///
+ /// : Contains at least one of the following blocks:
+ /// : A shooting target. It has the following elements:
+ /// : The name of the topic where the PerformerDectorPlugin
+ /// publishes information for this target.
+ /// : The points awarded for hitting this target.
+ ///
/// Here's an example:
///
@@ -111,6 +119,38 @@ namespace vrx
/// yellow_circle
///
///
+ ///
+ ///
+ ///
+ ///
+ /// vrx/dock_2022_placard1_big_hole/contain
+ /// 5
+ ///
+ ///
+ /// vrx/dock_2022_placard1_small_hole/contain
+ /// 10
+ ///
+
+ ///
+ ///
+ /// vrx/dock_2022_placard2_big_hole/contain
+ /// 5
+ ///
+ ///
+ /// vrx/dock_2022_placard2_small_hole/contain
+ /// 10
+ ///
+
+ ///
+ ///
+ /// vrx/dock_2022_placard3_big_hole/contain
+ /// 5
+ ///
+ ///
+ /// vrx/dock_2022_placard3_small_hole/contain
+ /// 10
+ ///
+ ///
///
class ScanDockScoringPlugin : public ScoringPlugin
{
diff --git a/vrx_gz/worlds/scan_dock_deliver_task.sdf b/vrx_gz/worlds/scan_dock_deliver_task.sdf
index c0317449d..30090073c 100644
--- a/vrx_gz/worlds/scan_dock_deliver_task.sdf
+++ b/vrx_gz/worlds/scan_dock_deliver_task.sdf
@@ -487,6 +487,18 @@
+
+
+
+ [blue_projectile]
+
+
+ 0.057 0.057 0.057
+
+
+
+
+
0 0 0
@@ -582,6 +594,36 @@
yellow_circle
+
+
+
+
+ /vrx/dock_2022_placard1_big_hole/contain
+ 5
+
+
+ /vrx/dock_2022_placard1_small_hole/contain
+ 10
+
+
+
+ /vrx/dock_2022_placard2_big_hole/contain
+ 5
+
+
+ /vrx/dock_2022_placard2_small_hole/contain
+ 10
+
+
+
+ /vrx/dock_2022_placard3_big_hole/contain
+ 5
+
+
+ /vrx/dock_2022_placard3_small_hole/contain
+ 10
+
+
diff --git a/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro b/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro
index fc65e13b6..59e8aae20 100644
--- a/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro
+++ b/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro
@@ -71,7 +71,7 @@
0.14 0 0 0 0 0
4
- 300
+ 65
${namespace}/${shooter_namespace}${name}/fire