From 7c4a4d9be3416b0c914192f78424e89bc835c4e7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 17 Mar 2021 12:31:51 -0500 Subject: [PATCH 1/3] Add the ability to score an artifact via service. Signed-off-by: Michael Carroll --- subt_ign/launch/validate_artifacts.ign | 60 +++++++++++++ subt_ign/src/ArtifactValidator.cc | 120 +++++++++++++++++++++++-- 2 files changed, 172 insertions(+), 8 deletions(-) diff --git a/subt_ign/launch/validate_artifacts.ign b/subt_ign/launch/validate_artifacts.ign index a19fd1a9..0997b162 100644 --- a/subt_ign/launch/validate_artifacts.ign +++ b/subt_ign/launch/validate_artifacts.ign @@ -103,6 +103,26 @@ ign service -s /artifact/move_to \ name="ignition::gazebo::systems::SceneBroadcaster"> + + + + <%= $worldName %> + false + + <%= $durationSec %> + + + + /tmp/ign/logs + subt_<%= $circuit %> + 5 + + + <%end%> + + + true + <%= $worldName %> + + visibility_range + + + 500.0 + 0 + 40 + 10.0 + 1 + 2.0 + + + + 0.2 + 10 + + + + 1000000 + 20 + -90 + QPSK + + + + + + + + validator diff --git a/subt_ign/src/ArtifactValidator.cc b/subt_ign/src/ArtifactValidator.cc index 4ef08a12..618f4957 100644 --- a/subt_ign/src/ArtifactValidator.cc +++ b/subt_ign/src/ArtifactValidator.cc @@ -64,6 +64,8 @@ ign service -s /artifact/move_to \ #include #include "subt_ign/Common.hh" +#include "subt_communication_broker/subt_communication_client.h" +#include "subt_ign/protobuf/artifact.pb.h" IGNITION_ADD_PLUGIN( subt::ArtifactValidator, @@ -107,9 +109,26 @@ class subt::ArtifactValidatorPrivate public: bool OnPrev(const ignition::msgs::StringMsg &_msg, ignition::msgs::StringMsg &_rep); + /// \brief Callback fired when the score service is called. + /// \param[in] _msg Service request + /// \param[out] _rep Service response + public: bool OnScore(const ignition::msgs::StringMsg &_msg, + ignition::msgs::StringMsg &_rep); + + public: bool ReportArtifact(const ArtifactType _type, + const ignition::msgs::Pose &_pose); + + public: void OnArtifactAck(const std::string &/*_srcAddress*/, + const std::string &/*_dstAddress*/, + const uint32_t /*_dstPort*/, + const std::string &_data); + /// \brief Map of artifact names to additional artifact data. public: std::map artifacts; + /// \brief Queue of artifacts to report + public: std::deque toReport; + /// \brief Iterator to current position in map public: std::map::iterator artifactsIter; @@ -121,6 +140,9 @@ class subt::ArtifactValidatorPrivate /// \brief World name of current world. public: std::string worldName; + + /// \brief Communication client. + public: std::unique_ptr client {nullptr}; }; ///////////////////////////////////////////////// @@ -145,12 +167,13 @@ bool ArtifactValidatorPrivate::MoveTo(const ignition::msgs::StringMsg &_msg, req.mutable_position()->set_z(artifact.pose.Pos().Z()); _rep.set_data(_msg.data()); - msgs::Boolean res; + ignition::msgs::Boolean res; bool result; unsigned int timeout = 1000; std::string service {"/world/" + this->worldName + "/set_pose"}; node.Request(service, req, timeout, res, result); igndbg << "Result: " << res.data() << " " << result << std::endl; + return result; } @@ -163,6 +186,69 @@ bool ArtifactValidatorPrivate::MoveToString(const std::string &_name, return this->MoveTo(msg, _rep); } +///////////////////////////////////////////////// +bool ArtifactValidatorPrivate::OnScore(const ignition::msgs::StringMsg& /*_req*/, + ignition::msgs::StringMsg& _rep) +{ + // Report the artifact that we are currently at + + auto artifact = this->artifactsIter->second; + + if (artifact.name == "artifact_origin") + { + std::cerr << "Attempted to score artifact_origin" << std::endl; + return false; + } + + std::cout << "Reporting: " << artifact.String() << std::endl; + + ignition::msgs::Pose reportPose; + reportPose.mutable_position()->set_x(artifact.pose.Pos().X()); + reportPose.mutable_position()->set_y(artifact.pose.Pos().Y()); + reportPose.mutable_position()->set_z(artifact.pose.Pos().Z()); + + return this->ReportArtifact(artifact.type, + reportPose); +} + +///////////////////////////////////////////////// +bool ArtifactValidatorPrivate::ReportArtifact(const ArtifactType _type, + const ignition::msgs::Pose &_pose) +{ + subt::msgs::Artifact artifact; + artifact.set_type(static_cast(_type)); + artifact.mutable_pose()->CopyFrom(_pose); + + // Serialize the artifact. + std::string serializedData; + + if (!artifact.SerializeToString(&serializedData)) { + std::cerr + << "ReportArtifact(): Error serializing message\n" + << artifact.DebugString() << std::endl; + return false; + } + + this->client->SendTo(serializedData, subt::kBaseStationName); + return true; +} + +///////////////////////////////////////////////// +void ArtifactValidatorPrivate::OnArtifactAck(const std::string &/*_srcAddress*/, + const std::string &/*_dstAddress*/, + const uint32_t /*_dstPort*/, + const std::string &_data) +{ + subt::msgs::ArtifactScore ack; + if (!ack.ParseFromString(_data)) + { + std::cerr << "Error parsing artifact score response" << std::endl; + } + else + { + } +} + ///////////////////////////////////////////////// bool ArtifactValidatorPrivate::OnNext(const ignition::msgs::StringMsg& /*_req*/, ignition::msgs::StringMsg& _rep) @@ -264,17 +350,13 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, std::string fullPath; subt::FullWorldPath(worldName, fullPath); + ignmsg << "Loading SDF world file[" << fullPath + ".sdf" << "].\n"; - common::SystemPaths systemPaths; - systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); - std::string filePath = systemPaths.FindFile(fullPath); - ignmsg << "Loading SDF world file[" << filePath << "].\n"; - - auto errors = this->dataPtr->sdfRoot.Load(filePath); + auto errors = this->dataPtr->sdfRoot.Load(fullPath + ".sdf"); if (!errors.empty()) { + ignerr << "Couldn't load SDF" << std::endl; for (auto &err : errors) ignerr << err << "\n"; return; @@ -288,6 +370,20 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, &ArtifactValidatorPrivate::OnNext, this->dataPtr.get()); this->dataPtr->node.Advertise("/artifact/prev", &ArtifactValidatorPrivate::OnPrev, this->dataPtr.get()); + this->dataPtr->node.Advertise("/artifact/score", + &ArtifactValidatorPrivate::OnScore, this->dataPtr.get()); + + { + // Start the SubT competition + ignition::msgs::Boolean req; + ignition::msgs::Boolean rep; + unsigned int timeout = 5000; + bool result; + req.set_data(true); + this->dataPtr->node.Request("/subt/start", req, timeout, rep, result); + } + + } ///////////////////////////////////////////////// @@ -295,6 +391,14 @@ void ArtifactValidator::PostUpdate( const ignition::gazebo::UpdateInfo &/*_info*/, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { + + if (!this->dataPtr->client) + { + this->dataPtr->client.reset(new subt::CommsClient("X2", false, true)); + this->dataPtr->client->Bind(&ArtifactValidatorPrivate::OnArtifactAck, + this->dataPtr.get()); + } + if (this->dataPtr->artifactsIter == this->dataPtr->artifacts.end()) { this->dataPtr->artifactsIter = this->dataPtr->artifacts.begin(); From 12c844affe491744682574a0911f801f4447387b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 17 Mar 2021 12:56:48 -0500 Subject: [PATCH 2/3] Local updates Signed-off-by: Michael Carroll --- subt_ign/src/ArtifactValidator.cc | 36 +++++++++++++++++++------------ 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/subt_ign/src/ArtifactValidator.cc b/subt_ign/src/ArtifactValidator.cc index 618f4957..73012a09 100644 --- a/subt_ign/src/ArtifactValidator.cc +++ b/subt_ign/src/ArtifactValidator.cc @@ -187,25 +187,28 @@ bool ArtifactValidatorPrivate::MoveToString(const std::string &_name, } ///////////////////////////////////////////////// -bool ArtifactValidatorPrivate::OnScore(const ignition::msgs::StringMsg& /*_req*/, +bool ArtifactValidatorPrivate::OnScore(const ignition::msgs::StringMsg& _req, ignition::msgs::StringMsg& _rep) { // Report the artifact that we are currently at - - auto artifact = this->artifactsIter->second; - - if (artifact.name == "artifact_origin") + // + auto newIter = this->artifacts.find(_req.data()); + if (newIter == this->artifacts.end()) { - std::cerr << "Attempted to score artifact_origin" << std::endl; + ignerr << "Artifact: " << _req.data() << " does not exist" << std::endl; return false; } + this->artifactsIter = newIter; + auto artifact = this->artifactsIter->second; std::cout << "Reporting: " << artifact.String() << std::endl; + auto origin = this->artifacts.find("artifact_origin")->second; + ignition::msgs::Pose reportPose; - reportPose.mutable_position()->set_x(artifact.pose.Pos().X()); - reportPose.mutable_position()->set_y(artifact.pose.Pos().Y()); - reportPose.mutable_position()->set_z(artifact.pose.Pos().Z()); + reportPose.mutable_position()->set_x(artifact.pose.Pos().X() - origin.pose.Pos().X()); + reportPose.mutable_position()->set_y(artifact.pose.Pos().Y() - origin.pose.Pos().Y()); + reportPose.mutable_position()->set_z(artifact.pose.Pos().Z() - origin.pose.Pos().Z()); return this->ReportArtifact(artifact.type, reportPose); @@ -229,8 +232,15 @@ bool ArtifactValidatorPrivate::ReportArtifact(const ArtifactType _type, return false; } - this->client->SendTo(serializedData, subt::kBaseStationName); - return true; + if (this->client) + { + this->client->SendTo(serializedData, subt::kBaseStationName); + return true; + } + else + { + return false; + } } ///////////////////////////////////////////////// @@ -382,8 +392,6 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, req.set_data(true); this->dataPtr->node.Request("/subt/start", req, timeout, rep, result); } - - } ///////////////////////////////////////////////// @@ -394,7 +402,7 @@ void ArtifactValidator::PostUpdate( if (!this->dataPtr->client) { - this->dataPtr->client.reset(new subt::CommsClient("X2", false, true)); + this->dataPtr->client.reset(new subt::CommsClient("validator", false, true)); this->dataPtr->client->Bind(&ArtifactValidatorPrivate::OnArtifactAck, this->dataPtr.get()); } From 306b68eb21e54e6f0684007e04eef80fbdfee4f0 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 1 Apr 2021 18:11:24 -0500 Subject: [PATCH 3/3] Add a retry mechanism for comms client If the comms client doesn't connect, it will retry every 100 ms of sim time. Signed-off-by: Michael Carroll --- subt_ign/src/ArtifactValidator.cc | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/subt_ign/src/ArtifactValidator.cc b/subt_ign/src/ArtifactValidator.cc index 73012a09..c301bf56 100644 --- a/subt_ign/src/ArtifactValidator.cc +++ b/subt_ign/src/ArtifactValidator.cc @@ -141,6 +141,9 @@ class subt::ArtifactValidatorPrivate /// \brief World name of current world. public: std::string worldName; + /// \brief Next time to check if client is nullptr + public: std::chrono::steady_clock::duration clientCheck{0}; + /// \brief Communication client. public: std::unique_ptr client {nullptr}; }; @@ -188,7 +191,7 @@ bool ArtifactValidatorPrivate::MoveToString(const std::string &_name, ///////////////////////////////////////////////// bool ArtifactValidatorPrivate::OnScore(const ignition::msgs::StringMsg& _req, - ignition::msgs::StringMsg& _rep) + ignition::msgs::StringMsg& /*_rep*/) { // Report the artifact that we are currently at // @@ -396,15 +399,20 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, ///////////////////////////////////////////////// void ArtifactValidator::PostUpdate( - const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - - if (!this->dataPtr->client) + if (!this->dataPtr->client && + _info.simTime > this->dataPtr->clientCheck) { this->dataPtr->client.reset(new subt::CommsClient("validator", false, true)); - this->dataPtr->client->Bind(&ArtifactValidatorPrivate::OnArtifactAck, + auto bound = this->dataPtr->client->Bind(&ArtifactValidatorPrivate::OnArtifactAck, this->dataPtr.get()); + + if (!bound) { + this->dataPtr->client.reset(nullptr); + this->dataPtr->clientCheck = _info.simTime + std::chrono::milliseconds(100); + } } if (this->dataPtr->artifactsIter == this->dataPtr->artifacts.end())