From ba6d75e31ee95f01624ee7403aaa248a3c45177f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 28 Mar 2022 20:53:40 -0700 Subject: [PATCH] Support validating target reports through image stream (#59) * implemented target validation, streaming start/stop/report Signed-off-by: Ian Chen * add test Signed-off-by: Ian Chen * update test Signed-off-by: Ian Chen * double to int for img pos Signed-off-by: Ian Chen * doc Signed-off-by: Ian Chen * Stop robot motion when it moves out of 2nd geofence (#60) * disable robot when it moves out of outer geofence Signed-off-by: Ian Chen * make robot static instead of halting its motion Signed-off-by: Ian Chen * fix test Signed-off-by: Ian Chen * update comments Signed-off-by: Ian Chen * Bridge competition topics (#62) * publish phase, add competition launch file Signed-off-by: Ian Chen * fix and add test Signed-off-by: Ian Chen --- mbzirc_ign/src/GameLogicPlugin.cc | 991 ++++++++++++++++-- mbzirc_ign/test/test_game_logic.cc | 826 ++++++++++++++- mbzirc_ign/worlds/empty_platform.sdf | 7 + mbzirc_ign/worlds/faster_than_realtime.sdf | 157 ++- mbzirc_ros/CMakeLists.txt | 5 + mbzirc_ros/launch/competition.launch.py | 70 ++ mbzirc_ros/package.xml | 1 + mbzirc_ros/test/launch/test_ros_api.launch.py | 4 +- mbzirc_ros/test/ros_api/test_ros_api.cc | 21 + 9 files changed, 1969 insertions(+), 113 deletions(-) create mode 100644 mbzirc_ros/launch/competition.launch.py diff --git a/mbzirc_ign/src/GameLogicPlugin.cc b/mbzirc_ign/src/GameLogicPlugin.cc index a2ed7d30..0017464b 100644 --- a/mbzirc_ign/src/GameLogicPlugin.cc +++ b/mbzirc_ign/src/GameLogicPlugin.cc @@ -25,14 +25,28 @@ #include +#include +#include +#include +#include + +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/rendering/Events.hh" + +#include +#include +#include #include #include #include #include #include +#include #include +#include #include #include +#include #include #include @@ -79,6 +93,19 @@ class mbzirc::GameLogicPluginPrivate public: std::set largeObjectsReported; }; + /// \brief Information of target in stream + public: class TargetInStream + { + /// \brief Type of target: vessel, small, large + public: std::string type; + + /// \brief Image x position + public: unsigned int x = 0; + + /// \brief Image y position + public: unsigned int y = 0; + }; + public: enum PenaltyType { /// \brief Failure to ID target vessel @@ -150,6 +177,10 @@ class mbzirc::GameLogicPluginPrivate public: std::chrono::steady_clock::time_point UpdateScoreFiles( const ignition::msgs::Time &_simTime); + /// \brief Get the sim time msg + /// \return Sim time msg + public: ignition::msgs::Time SimTime(); + /// \brief Log an event to the eventStream. /// \param[in] _event The event to log. /// \param[in] _data Additional data for the event. Optional @@ -174,16 +205,46 @@ class mbzirc::GameLogicPluginPrivate /// \return True if the run was started. public: bool Start(const ignition::msgs::Time &_simTime); - /// \brief Ignition service callback triggered when the service is called. + /// \brief Ignition service callback triggered when finish is called. /// \param[in] _req The message containing a flag telling if the game is to /// be finished. /// \param[out] _res The response message. public: bool OnFinishCall(const ignition::msgs::Boolean &_req, ignition::msgs::Boolean &_res); + /// \brief Ignition service callback triggered when targets are reported. + /// \param[in] _req Report msg string in the following format: + /// [vessel name, small obj name. large obj name] + /// \param[out] _res The response message. public: bool OnReportTargets(const ignition::msgs::StringMsg_V &_req, ignition::msgs::Boolean &_res); + /// \brief Ignition service callback triggered when video stream is to be + /// started + /// \param[in] _req Report msg string in the following format: + /// [vehicle name, sensor_name] + /// where vehicle and sensor name are the vehicle carrying + /// the sensor with the image stream + /// \param[out] _res The response message. + public: bool OnTargetStreamStart(const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &_res); + + /// \brief Ignition service callback triggered when video stream is to be + /// stopped. + /// \param[in] _req Empty msg + /// \param[out] _res The response message. + public: bool OnTargetStreamStop(const ignition::msgs::Empty &_req, + ignition::msgs::Boolean &_res); + + /// \brief Ignition service callback triggered when targets are reported + /// with image position in the video stream + /// \param[in] _req Report msg string in the following format: + /// [type, image pos x, image pos y] + /// where type can be "vessel", "small", or "large" + /// \param[out] _res The response message. + public: bool OnTargetStreamReport(const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &_res); + /// \brief Check if an entity's position is within the input boundary //// \param[in] _ecm Entity component manager //// \param[in] _entity Entity id @@ -195,6 +256,57 @@ class mbzirc::GameLogicPluginPrivate /// \brief Valid target reports, add time penalties, and update score public: void ValidateTargetReports(); + /// \brief Check if robots are inside geofence boundary. Time penalties are + /// given if the robots exceed the first geofence two times, and the + /// is terminated on the third occurence. If a robot moves outside of the + /// outer geofence, the robot is disabled + /// \param[in] _ecm Mutable reference to Entity Component Manager + public: void CheckRobotsInGeofenceBoundary( + EntityComponentManager &_ecm); + + /// \brief Make an entity static + /// \param[in] _entity Entity to make static + /// \param[in] _ecm Mutable reference to Entity Component Manager + /// \return True if the entity is made static + public: bool MakeStatic(Entity _entity, EntityComponentManager &_ecm); + + /// \brief Callback invoked in the rendering thread after a render update + public: void OnPostRender(); + + /// \brief Find the visual of potential target at the specified image pos + /// \param[in] _visual Target visual + /// \param[in] _imagePos Image position to check for target + /// \param[in] _type Type of target: vessel, small, or large + /// \param[out] _objectAtImgPos Object found at image pos. + /// \return True if a valid target is found + public: bool FindTargetVisual( + const rendering::VisualPtr &_visual, + const math::Vector2i &_imagePos, + const std::string &_type, std::string &_objectAtImgPos) const; + + /// \brief Check if visual is in camera view + /// \param[in] _visual Visual to check + /// \param[out] _imagePos 2D position of visual in the image + /// \return True if visual is in view, false otherwise + public: bool VisualInView(const rendering::VisualPtr &_visual, + math::Vector2i &_imagePos) const; + + /// \brief Get visual at specified image position + /// \param[in] _x X image position + /// \param[in] _y Y image position + /// \param[in] _type Type of target: vessel, small, or large + /// \return Visual at the specified image position + public: rendering::VisualPtr VisualAt( + unsigned int _x, unsigned int _y, const std::string &_type) const; + + /// \brief Set the competition phase + /// \param[in] _phase Competition phase string + public: void SetPhase(const std::string &_phase); + + /// \brief Get the competition phase + /// \return Competition phase string + public: std::string Phase(); + /// \brief Ignition Transport node. public: transport::Node node; @@ -223,6 +335,9 @@ class mbzirc::GameLogicPluginPrivate /// \brief Mutex to protect log stream. public: std::mutex logMutex; + /// \brief Mutex to protect sim time + public: std::mutex simTimeMutex; + /// \brief Log file output stream. public: std::ofstream logStream; @@ -235,12 +350,21 @@ class mbzirc::GameLogicPluginPrivate /// \brief Mutex to protect reports public: std::mutex reportMutex; + /// \brief Mutex to protect target stream. + public: std::mutex streamMutex; + + /// \brief Mutex to protect phase. + public: std::mutex phaseMutex; + /// \brief Target reports public: std::vector reports; /// \brief Ignition transport competition clock publisher. public: transport::Node::Publisher competitionClockPub; + /// \brief Ignition transport competition phase publisher. + public: transport::Node::Publisher competitionPhasePub; + /// \brief Logpath. public: std::string logPath{"/dev/null"}; @@ -284,13 +408,33 @@ class mbzirc::GameLogicPluginPrivate /// This is geofenceBoundary - geofenceBoundaryBuffer public: math::AxisAlignedBox geofenceBoundaryInner; + /// \brief Outer / hard boundary of competition area + /// This is geofenceBoundary + geofenceBoundaryBufferOuter. + /// If any vehicles moves outside of this boundary, it will be disabled + public: math::AxisAlignedBox geofenceBoundaryOuter; + /// \brief Inner boundary of competition area public: double geofenceBoundaryBuffer = 5.0; + /// \brief Outer / hard boundary of competition area + public: double geofenceBoundaryBufferOuter = 25.0; + /// \brief Map of robot entity id and bool var to indicate if they are inside /// the competition boundary public: std::map robotsInBoundary; + /// \brief SDF DOM of a static model with empty link + public: sdf::Model staticModelToSpawn; + + /// \brief Creator interface + public: std::unique_ptr creator{nullptr}; + + /// \brief World entity + public: Entity worldEntity{kNullEntity}; + + /// \brief A list of robots that have been disabled + public: std::set disabledRobots; + /// \brief Number of times robot moved beyond competition boundary public: unsigned int geofenceBoundaryPenaltyCount = 0u; @@ -310,6 +454,54 @@ class mbzirc::GameLogicPluginPrivate /// \brief A map of target vessel name and targets. public: std::map targets; + + /// \brief Sensor entity on vehicle currently streaming video of target + public: Entity targetStreamSensorEntity; + + /// \brief Name of topic that contains the video stream of target + public: std::string targetStreamTopic; + + /// \brief Sensor entity on vehicle currently streaming video of target + public: std::set cameraSensors; + + /// \brief Connection to the post-render event. + public: ignition::common::ConnectionPtr postRenderConn; + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene; + + /// \brief Pointer to the rendering camera associated with the sensor + /// providing the image stream + public: rendering::CameraPtr camera; + + /// \brief Report of target in stream + public: TargetInStream targetInStreamReport; + + /// \brief Current valid target vessel that has been successfully ID'ed. + /// This variable is used later when validating reports of target objects. + public: std::string currentTargetVessel; + + /// \brief A set of target vessel visuals + public: std::set targetVesselVisuals; + + /// \brief A map of target vessel name and the small target object visuals + /// on the vessel + public: std::map> + targetSmallObjectVisuals; + + /// \brief A map of target vessel name and the large target object visuals + /// on the vessel + public: std::map> + targetLargeObjectVisuals; + + /// \brief Max allowed error (%) for reported target vessel image position + public: const double kTargetVesselInImageTol = 0.03; + + /// \brief Max allowed error (%) for reported target object image position + public: const double kTargetObjInImageTol = 0.008; + + /// \brief Compeition phase. + public: std::string phase{"setup"}; }; ////////////////////////////////////////////////// @@ -333,9 +525,11 @@ GameLogicPlugin::~GameLogicPlugin() ////////////////////////////////////////////////// void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager & /*_ecm*/, + ignition::gazebo::EntityComponentManager & _ecm, ignition::gazebo::EventManager & _eventMgr) { + this->dataPtr->creator = std::make_unique(_ecm, _eventMgr); + this->dataPtr->worldEntity = _ecm.EntityByComponents(components::World()); this->dataPtr->eventManager = &_eventMgr; // Check if the game logic plugin has a element. @@ -411,6 +605,9 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, this->dataPtr->geofenceBoundaryInner = math::AxisAlignedBox(min + this->dataPtr->geofenceBoundaryBuffer, max - this->dataPtr->geofenceBoundaryBuffer); + this->dataPtr->geofenceBoundaryOuter = + math::AxisAlignedBox(min - this->dataPtr->geofenceBoundaryBufferOuter, + max + this->dataPtr->geofenceBoundaryBufferOuter); ignmsg << "Geofence boundary min: " << min << ", max: " << max << std::endl; } else @@ -499,6 +696,19 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, this->dataPtr->competitionClockPub = this->dataPtr->node.Advertise("/mbzirc/run_clock"); + this->dataPtr->competitionPhasePub = + this->dataPtr->node.Advertise("/mbzirc/phase"); + + this->dataPtr->node.Advertise("/mbzirc/target/stream/start", + &GameLogicPluginPrivate::OnTargetStreamStart, this->dataPtr.get()); + + this->dataPtr->node.Advertise("/mbzirc/target/stream/stop", + &GameLogicPluginPrivate::OnTargetStreamStop, this->dataPtr.get()); + + this->dataPtr->node.Advertise("/mbzirc/target/stream/report", + &GameLogicPluginPrivate::OnTargetStreamReport, this->dataPtr.get()); + + ignmsg << "Starting MBZIRC" << std::endl; @@ -511,7 +721,8 @@ void GameLogicPluginPrivate::OnBatteryMsg( const ignition::msgs::BatteryState &_msg, const transport::MessageInfo &_info) { - ignition::msgs::Time localSimTime(this->simTime); + auto simT = this->SimTime(); + ignition::msgs::Time localSimTime(simT); if (_msg.percentage() <= 0) { std::vector topicParts = common::split(_info.Topic(), "/"); @@ -537,6 +748,13 @@ void GameLogicPluginPrivate::OnBatteryMsg( void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { + if (!this->dataPtr->started) + return; + + // check if any robots moved outside of geofence + // give time penalties or disable robots if they exceeded + // boundaries + this->dataPtr->CheckRobotsInGeofenceBoundary(_ecm); } ////////////////////////////////////////////////// @@ -547,8 +765,11 @@ void GameLogicPlugin::PostUpdate( // Store sim time int64_t s, ns; std::tie(s, ns) = ignition::math::durationToSecNsec(_info.simTime); - this->dataPtr->simTime.set_sec(s); - this->dataPtr->simTime.set_nsec(ns); + { + std::lock_guard lock(this->dataPtr->simTimeMutex); + this->dataPtr->simTime.set_sec(s); + this->dataPtr->simTime.set_nsec(ns); + } // Capture the names of the robots. We only do this until the team // triggers the start signal. @@ -556,7 +777,7 @@ void GameLogicPlugin::PostUpdate( { _ecm.Each( - [&](const gazebo::Entity &, + [&](const gazebo::Entity &_entity, const gazebo::components::Sensor *, const gazebo::components::ParentEntity *_parent) -> bool { @@ -598,6 +819,19 @@ void GameLogicPlugin::PostUpdate( this->dataPtr->node.Subscribe(batteryTopic, &GameLogicPluginPrivate::OnBatteryMsg, this->dataPtr.get()); } + + // store camera / rgbd camera sensor + // later user for target confirmation in image stream + auto camComp = _ecm.Component(_entity); + if (camComp) + { + this->dataPtr->cameraSensors.insert(_entity); + } + auto rgbdComp = _ecm.Component(_entity); + if (rgbdComp) + { + this->dataPtr->cameraSensors.insert(_entity); + } } return true; }); @@ -631,55 +865,22 @@ void GameLogicPlugin::PostUpdate( } } - // check if robots are inside the competition boundary - for (const auto &it : this->dataPtr->robotNames) + // find the sensor associated with the input stream + // used for validating target reports { - Entity robotEnt = it.first; - std::string robotName = it.second; - bool wasInBounds = true; - auto bIt = this->dataPtr->robotsInBoundary.find(robotEnt); - if (bIt == this->dataPtr->robotsInBoundary.end()) - this->dataPtr->robotsInBoundary[robotEnt] = wasInBounds; - else - wasInBounds = bIt->second; - - // If robot was outside of boundary, allow some buffer distance before - // counting it as returning to the inside boundary. - // This is so that we don't immediately trigger another exceed_boundary - // event if the robot oscillates slightly at the boundary line, - // e.g. USV motion due to waves - auto boundary = wasInBounds ? this->dataPtr->geofenceBoundary : - this->dataPtr->geofenceBoundaryInner; - - bool isInBounds = this->dataPtr->CheckEntityInBoundary(_ecm, - robotEnt, boundary); - - if (isInBounds != wasInBounds) + std::lock_guard lock(this->dataPtr->streamMutex); + if (!this->dataPtr->targetStreamTopic.empty()) { - // robot moved outside the boundary! - if (!isInBounds) + for (auto sensorEntity : this->dataPtr->cameraSensors) { - this->dataPtr->geofenceBoundaryPenaltyCount++; - if (this->dataPtr->geofenceBoundaryPenaltyCount == 1) + std::string topic = scopedName(sensorEntity, _ecm); + if (this->dataPtr->targetStreamTopic.find(topic) != std::string::npos) { - this->dataPtr->timePenalty += - this->dataPtr->kTimePenalties.at( - GameLogicPluginPrivate::BOUNDARY_1); - this->dataPtr->LogEvent("exceed_boundary_1", robotName); - this->dataPtr->UpdateScoreFiles(this->dataPtr->simTime); - } - else if (this->dataPtr->geofenceBoundaryPenaltyCount >= 2) - { - this->dataPtr->timePenalty = - this->dataPtr->kTimePenalties.at( - GameLogicPluginPrivate::BOUNDARY_2); - this->dataPtr->LogEvent("exceed_boundary_2", robotName); - // terminate run - this->dataPtr->Finish(this->dataPtr->simTime); + this->dataPtr->targetStreamSensorEntity = sensorEntity; } } - this->dataPtr->robotsInBoundary[robotEnt] = isInBounds; } + this->dataPtr->targetStreamTopic.clear(); } // validate target reports @@ -714,30 +915,23 @@ void GameLogicPlugin::PostUpdate( if (currentTime - this->dataPtr->lastStatusPubTime > std::chrono::seconds(1)) { ignition::msgs::Clock competitionClockMsg; - ignition::msgs::Header::Map *mapData = - competitionClockMsg.mutable_header()->add_data(); - mapData->set_key("phase"); - if (this->dataPtr->started) + ignition::msgs::StringMsg phaseMsg; + std::string p = this->dataPtr->Phase(); + phaseMsg.set_data(p); + if (p == "setup") { - mapData->add_value(this->dataPtr->finished ? "finished" : "run"); - auto secondsRemaining = std::chrono::duration_cast( - remainingCompetitionTime); - competitionClockMsg.mutable_sim()->set_sec(secondsRemaining.count()); - } - else if (!this->dataPtr->finished) - { - mapData->add_value("setup"); competitionClockMsg.mutable_sim()->set_sec( this->dataPtr->setupTimeSec - this->dataPtr->simTime.sec()); } else { - // It's possible for a team to call Finish before starting. - mapData->add_value("finished"); + auto secondsRemaining = std::chrono::duration_cast( + remainingCompetitionTime); + competitionClockMsg.mutable_sim()->set_sec(secondsRemaining.count()); } this->dataPtr->competitionClockPub.Publish(competitionClockMsg); - + this->dataPtr->competitionPhasePub.Publish(phaseMsg); this->dataPtr->lastStatusPubTime = currentTime; } @@ -749,6 +943,134 @@ void GameLogicPlugin::PostUpdate( } } + +///////////////////////////////////////////////// +void GameLogicPluginPrivate::CheckRobotsInGeofenceBoundary( + EntityComponentManager &_ecm) +{ + // check if robots are inside the competition boundary + for (const auto &it : this->robotNames) + { + Entity robotEnt = it.first; + std::string robotName = it.second; + + if (this->disabledRobots.find(robotEnt) != this->disabledRobots.end()) + { + continue; + } + + // update list of robot and whether or not it is inside boundary + bool wasInBounds = true; + auto bIt = this->robotsInBoundary.find(robotEnt); + if (bIt == this->robotsInBoundary.end()) + this->robotsInBoundary[robotEnt] = wasInBounds; + else + wasInBounds = bIt->second; + + // check if it exceeded the outer / hard boundary + // if so, disable the robot + if (!wasInBounds && !this->CheckEntityInBoundary(_ecm, + robotEnt, this->geofenceBoundaryOuter)) + { + this->MakeStatic(robotEnt, _ecm); + + this->disabledRobots.insert(robotEnt); + this->LogEvent("exceed_boundary_outer", robotName); + continue; + } + + // If robot was outside of boundary, allow some buffer distance before + // counting it as returning to the inside boundary. + // This is so that we don't immediately trigger another exceed_boundary + // event if the robot oscillates slightly at the boundary line, + // e.g. USV motion due to waves + auto boundary = wasInBounds ? this->geofenceBoundary : + this->geofenceBoundaryInner; + + bool isInBounds = this->CheckEntityInBoundary(_ecm, + robotEnt, boundary); + + if (isInBounds != wasInBounds) + { + // robot moved outside the boundary! + if (!isInBounds) + { + this->geofenceBoundaryPenaltyCount++; + if (this->geofenceBoundaryPenaltyCount == 1) + { + this->timePenalty += + this->kTimePenalties.at( + GameLogicPluginPrivate::BOUNDARY_1); + this->LogEvent("exceed_boundary_1", robotName); + this->UpdateScoreFiles(this->simTime); + } + else if (this->geofenceBoundaryPenaltyCount >= 2) + { + this->timePenalty = + this->kTimePenalties.at( + GameLogicPluginPrivate::BOUNDARY_2); + this->LogEvent("exceed_boundary_2", robotName); + // terminate run + this->Finish(this->simTime); + } + } + this->robotsInBoundary[robotEnt] = isInBounds; + } + } +} + +////////////////////////////////////////////////// +bool GameLogicPluginPrivate::MakeStatic(Entity _entity, + EntityComponentManager &_ecm) +{ + // make entity static by spawning a static model and attaching the + // entity to the static model + // todo(anyone) Add a feature in ign-physics to support making a model + // static + if (this->staticModelToSpawn.LinkCount() == 0u) + { + sdf::ElementPtr staticModelSDF(new sdf::Element); + sdf::initFile("model.sdf", staticModelSDF); + staticModelSDF->GetAttribute("name")->Set("static_model"); + staticModelSDF->GetElement("static")->Set(true); + sdf::ElementPtr linkElem = staticModelSDF->AddElement("link"); + linkElem->GetAttribute("name")->Set("static_link"); + this->staticModelToSpawn.Load(staticModelSDF); + } + + auto poseComp = _ecm.Component(_entity); + if (!poseComp) + return false; + math::Pose3d p = poseComp->Data(); + this->staticModelToSpawn.SetRawPose(p); + + auto nameComp = _ecm.Component(_entity); + this->staticModelToSpawn.SetName(nameComp->Data() + "__static__"); + + Entity staticEntity = this->creator->CreateEntities(&staticModelToSpawn); + this->creator->SetParent(staticEntity, this->worldEntity); + + Entity parentLinkEntity = _ecm.EntityByComponents( + components::Link(), components::ParentEntity(staticEntity), + components::Name("static_link")); + + if (parentLinkEntity == kNullEntity) + return false; + + Entity childLinkEntity = _ecm.EntityByComponents( + components::CanonicalLink(), components::ParentEntity(_entity)); + + if (childLinkEntity == kNullEntity) + return false; + + Entity detachableJointEntity = _ecm.CreateEntity(); + _ecm.CreateComponent(detachableJointEntity, + components::DetachableJoint( + {parentLinkEntity, childLinkEntity, "fixed"})); + + return true; +} + ///////////////////////////////////////////////// bool GameLogicPluginPrivate::CheckEntityInBoundary( const EntityComponentManager &_ecm, Entity _entity, @@ -767,7 +1089,6 @@ bool GameLogicPluginPrivate::CheckEntityInBoundary( return _boundary.Contains(pose.Pos()); } - ///////////////////////////////////////////////// void GameLogicPluginPrivate::PublishScore() { @@ -791,7 +1112,8 @@ void GameLogicPluginPrivate::PublishScore() bool GameLogicPluginPrivate::OnFinishCall(const ignition::msgs::Boolean &_req, ignition::msgs::Boolean &_res) { - ignition::msgs::Time localSimTime(this->simTime); + auto simT = this->SimTime(); + ignition::msgs::Time localSimTime(simT); if (this->started && _req.data() && !this->finished) { ignmsg << "User triggered OnFinishCall." << std::endl; @@ -811,7 +1133,8 @@ bool GameLogicPluginPrivate::OnFinishCall(const ignition::msgs::Boolean &_req, bool GameLogicPluginPrivate::OnStartCall(const ignition::msgs::Boolean &_req, ignition::msgs::Boolean &_res) { - ignition::msgs::Time localSimTime(this->simTime); + auto simT = this->SimTime(); + ignition::msgs::Time localSimTime(simT); if (_req.data()) _res.set_data(this->Start(localSimTime)); else @@ -834,6 +1157,7 @@ bool GameLogicPluginPrivate::Start(const ignition::msgs::Time &_simTime) ignmsg << "Scoring has Started" << std::endl; this->Log(_simTime) << "scoring_started" << std::endl; this->LogEvent("started"); + this->SetPhase("started"); } // Update files when scoring has started. @@ -891,6 +1215,7 @@ void GameLogicPluginPrivate::Finish(const ignition::msgs::Time &_simTime) this->logStream.flush(); this->LogEvent("finished"); + this->SetPhase("finished"); } this->finished = true; @@ -907,6 +1232,138 @@ bool GameLogicPluginPrivate::OnReportTargets( return true; } +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::OnTargetStreamStart( + const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &_res) +{ + if (_req.data_size() < 2) + { + ignwarn << "Target stream start request: Missing arguments " << std::endl; + this->LogEvent("stream_start_request", "missing_arg"); + _res.set_data(false); + return true; + } + std::string vehicleName = _req.data(0); + std::string vehicleSensor = _req.data(1); + + if (vehicleName.empty() || vehicleSensor.empty()) + { + ignwarn << "Target stream start request: Empty vehicle name or sensor" + << std::endl; + this->LogEvent("stream_start_request", "empty_vehicle_or_sensor"); + _res.set_data(false); + return true; + } + + std::string vehicleTopic; + + // verify that there is a image topic associated with the specified robot + // and sensor + std::vector allTopics; + this->node.TopicList(allTopics); + for (auto topic : allTopics) + { + std::vector publishers; + this->node.TopicInfo(topic, publishers); + for (auto pub : publishers) + { + if (pub.MsgTypeName() == "ignition.msgs.Image") + { + if (topic.find("/model/" + vehicleName + "/") != std::string::npos && + topic.find("/" + vehicleSensor +"/") != std::string::npos) + { + vehicleTopic = topic; + break; + } + } + } + } + + if (!vehicleTopic.empty()) + { + _res.set_data(true); + std::lock_guard lock(this->streamMutex); + this->targetStreamTopic = vehicleTopic; + + ignwarn << "Target stream start request: success" << std::endl; + this->LogEvent("stream_start_request", "success"); + } + else + { + ignwarn << "Target stream start request: Unable to find image topic for " + << "vehicle: " << vehicleName << ", and sensor: " << vehicleSensor + << std::endl; + _res.set_data(false); + this->LogEvent("stream_start_request", "no_image_topic_found"); + std::lock_guard lock(this->streamMutex); + this->targetStreamTopic.clear(); + } + this->targetStreamSensorEntity = kNullEntity; + return true; +} + +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::OnTargetStreamStop( + const ignition::msgs::Empty &_req, + ignition::msgs::Boolean &_res) +{ + _res.set_data(true); + + std::lock_guard lock(this->streamMutex); + this->targetStreamTopic.clear(); + + ignmsg << "Target stream stop request: success" << std::endl; + this->LogEvent("stream_stopped", this->targetStreamTopic); + return true; +} + +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::OnTargetStreamReport( + const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &_res) +{ + if (!this->started || this->finished) + { + ignwarn << "Unable to report target in stream. Run not active" + << std::endl; + this->LogEvent("target_reported_in_stream", "run_not_active"); + _res.set_data(false); + return true; + } + + std::string type = _req.data(0); + unsigned int x = std::stoul(_req.data(1)); + unsigned int y = std::stoul(_req.data(2)); + + TargetInStream tis; + tis.type = type; + tis.x = x; + tis.y = y; + + std::lock_guard lock(this->streamMutex); + // there can only be one target report at a time + this->targetInStreamReport = tis; + + // Set up the render connection so we can validate target in the rendering + // thread + if (!this->postRenderConn) + { + this->postRenderConn = + this->eventManager->Connect( + std::bind(&GameLogicPluginPrivate::OnPostRender, this)); + } + + std::string eventData = type + "::" + std::to_string(x) + "::" + + std::to_string(y); + ignmsg << "Target report in stream: " << type << " " << x << " " << y + << std::endl; + this->LogEvent("target_reported_in_stream", eventData); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// void GameLogicPluginPrivate::ValidateTargetReports() { @@ -926,6 +1383,8 @@ void GameLogicPluginPrivate::ValidateTargetReports() continue; } + auto simT = this->SimTime(); + auto it = this->targets.find(vessel); if (it != this->targets.end()) { @@ -937,6 +1396,8 @@ void GameLogicPluginPrivate::ValidateTargetReports() target.vesselReported = true; this->targets[vessel] = target; this->LogEvent("target_reported", "vessel_id_success"); + this->currentTargetVessel = vessel; + this->SetPhase("vessel_id_success"); continue; } // target has already been reported @@ -957,6 +1418,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() target.smallObjectsReported.insert(smallObj); this->targets[vessel] = target; this->LogEvent("target_reported", "small_object_id_success"); + this->SetPhase("small_object_id_success"); continue; } else @@ -978,7 +1440,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() { this->timePenalty += this->kTimePenalties.at(LARGE_OBJECT_ID_1); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); logData += "_1"; this->LogEvent("target_reported", logData); } @@ -986,7 +1448,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() { this->timePenalty += this->kTimePenalties.at(LARGE_OBJECT_ID_2); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); logData += "_2"; this->LogEvent("target_reported", logData); } @@ -997,7 +1459,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() logData += "_3"; this->LogEvent("target_reported", logData); // terminate run - this->Finish(this->simTime); + this->Finish(simT); break; } } @@ -1018,6 +1480,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() target.largeObjectsReported.insert(largeObj); this->targets[vessel] = target; this->LogEvent("target_reported", "large_object_id_success"); + this->SetPhase("large_object_id_success"); continue; } else @@ -1039,14 +1502,14 @@ void GameLogicPluginPrivate::ValidateTargetReports() { this->timePenalty += this->kTimePenalties.at(LARGE_OBJECT_ID_1); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); this->LogEvent("target_reported", logData + "_1"); } else if (count == 2u) { this->timePenalty += this->kTimePenalties.at(LARGE_OBJECT_ID_2); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); this->LogEvent("target_reported", logData + "_2"); } else @@ -1055,7 +1518,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() this->kTimePenalties.at(LARGE_OBJECT_ID_3); this->LogEvent("target_reported", logData + "_3"); // terminate run - this->Finish(this->simTime); + this->Finish(simT); break; } } @@ -1076,14 +1539,14 @@ void GameLogicPluginPrivate::ValidateTargetReports() { this->timePenalty += this->kTimePenalties.at(TARGET_VESSEL_ID_1); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); this->LogEvent("target_reported", logData + "_1"); } else if (this->vesselPenaltyCount == 2u) { this->timePenalty += this->kTimePenalties.at(TARGET_VESSEL_ID_2); - this->UpdateScoreFiles(this->simTime); + this->UpdateScoreFiles(simT); this->LogEvent("target_reported", logData + "_2"); } else @@ -1092,7 +1555,7 @@ void GameLogicPluginPrivate::ValidateTargetReports() this->kTimePenalties.at(TARGET_VESSEL_ID_3); this->LogEvent("target_reported", logData + "_3"); // terminate run - this->Finish(this->simTime); + this->Finish(simT); break; } } @@ -1100,6 +1563,13 @@ void GameLogicPluginPrivate::ValidateTargetReports() this->reports.clear(); } +///////////////////////////////////////////////// +ignition::msgs::Time GameLogicPluginPrivate::SimTime() +{ + std::lock_guard lock(this->simTimeMutex); + return this->simTime; +} + ///////////////////////////////////////////////// std::chrono::steady_clock::time_point GameLogicPluginPrivate::UpdateScoreFiles( const ignition::msgs::Time &_simTime) @@ -1153,11 +1623,14 @@ void GameLogicPluginPrivate::LogEvent(const std::string &_type, int simElapsed = 0; std::chrono::steady_clock::time_point currTime = std::chrono::steady_clock::now(); + + auto simT = this->SimTime(); + if (this->started) { realElapsed = std::chrono::duration_cast( currTime - this->startTime).count(); - simElapsed = this->simTime.sec() - this->startSimTime.sec(); + simElapsed = simT.sec() - this->startSimTime.sec(); } std::lock_guard lock(this->eventCounterMutex); @@ -1167,7 +1640,7 @@ void GameLogicPluginPrivate::LogEvent(const std::string &_type, << "- event:\n" << " id: " << this->eventCounter << "\n" << " type: " << _type << "\n" - << " time_sec: " << this->simTime.sec() << "\n" + << " time_sec: " << simT.sec() << "\n" << " elapsed_real_time: " << realElapsed << "\n" << " elapsed_sim_time: " << simElapsed << "\n" << " total_score: " << this->totalScore << std::endl; @@ -1178,7 +1651,7 @@ void GameLogicPluginPrivate::LogEvent(const std::string &_type, this->eventStream.flush(); this->eventCounter++; - this->Log(this->simTime) << "Logged Event:\n" << stream.str() << std::endl; + this->Log(simT) << "Logged Event:\n" << stream.str() << std::endl; } ///////////////////////////////////////////////// @@ -1189,3 +1662,365 @@ std::ofstream &GameLogicPluginPrivate::Log( << " " << _simTime.nsec() << " "; return this->logStream; } + +///////////////////////////////////////////////// +void GameLogicPluginPrivate::OnPostRender() +{ + // get scene + if (!this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + } + + // return if scene not ready or no sensors available. + if (!this->scene->IsInitialized() || + this->scene->SensorCount() == 0) + { + return; + } + + // get and store visuals of targets + if (this->targetVesselVisuals.empty() || + this->targetSmallObjectVisuals.empty() || + this->targetLargeObjectVisuals.empty()) + { + for (const auto &it : this->targets) + { + rendering::VisualPtr vesselVisual = + this->scene->VisualByName(it.first); + if (!vesselVisual) + { + ignerr << "Unable to find visual for vessel: " << it.first << std::endl; + } + else + { + this->targetVesselVisuals.insert(vesselVisual); + } + + + for (const auto &obj : it.second.smallObjects) + { + rendering::VisualPtr objVisual = + this->scene->VisualByName(obj); + if (!objVisual) + { + ignerr << "Unable to find visual for small object: " << it.first + << std::endl; + } + else + { + this->targetSmallObjectVisuals[it.first].insert(objVisual); + } + } + for (const auto &obj : it.second.largeObjects) + { + rendering::VisualPtr objVisual = + this->scene->VisualByName(obj); + if (!objVisual) + { + ignerr << "Unable to find visual for large object: " << it.first + << std::endl; + } + else + { + this->targetLargeObjectVisuals[it.first].insert(objVisual); + } + } + } + } + + // get camera currently streaming video + std::lock_guard lock(this->streamMutex); + if (!this->camera && this->targetStreamSensorEntity != kNullEntity) + { + for (unsigned int i = 0; i < this->scene->SensorCount(); ++i) + { + auto sensor = this->scene->SensorByIndex(i); + if (!sensor) + continue; + + if (sensor->HasUserData("gazebo-entity")) + { + // RenderUtil stores gazebo-entity user data as int + // \todo(anyone) Change this to uint64_t in Ignition H + auto variant = sensor->UserData("gazebo-entity"); + const int *value = std::get_if(&variant); + + if (value && *value == static_cast(this->targetStreamSensorEntity)) + { + this->camera = std::dynamic_pointer_cast(sensor); + break; + } + } + } + } + // reset camera pointer if stream sensor entity is null + // this could be the stream stopped + else if (this->camera && this->targetStreamSensorEntity == kNullEntity) + { + this->camera.reset(); + } + + // validate target + if (this->camera && !this->targetInStreamReport.type.empty()) + { + // check if the specified img pos contains the target visual + // using the FindTargetVisual function below + // It uses a 2 phase process for identifying target + // First it checks if target is at the exact img pos + // If not, it checks to see if any target is nearby with some tol + ignition::msgs::StringMsg_V req; + ignition::msgs::Boolean res; + std::string target; + // verify vesel + if (this->targetInStreamReport.type == "vessel") + { + for (const auto &v : this->targetVesselVisuals) + { + bool found = this->FindTargetVisual(v, + math::Vector2i(this->targetInStreamReport.x, + this->targetInStreamReport.y), + this->targetInStreamReport.type, target); + if (found) + { + break; + } + } + req.add_data(target); + this->OnReportTargets(req, res); + } + // verify small object + else if (this->targetInStreamReport.type == "small") + { + auto it = this->targetSmallObjectVisuals.find(this->currentTargetVessel); + if (this->currentTargetVessel.empty() || + it == this->targetSmallObjectVisuals.end()) + { + ignerr << "Target vessel has not been identified yet" << std::endl; + this->targetInStreamReport.type.clear(); + return; + } + + for (const auto &v : it->second) + { + bool found = this->FindTargetVisual(v, + math::Vector2i(this->targetInStreamReport.x, + this->targetInStreamReport.y), + this->targetInStreamReport.type, target); + if (found) + { + break; + } + } + + req.add_data(this->currentTargetVessel); + req.add_data(target); + this->OnReportTargets(req, res); + } + // verify large object + else if (this->targetInStreamReport.type == "large") + { + auto it = this->targetLargeObjectVisuals.find(this->currentTargetVessel); + if (this->currentTargetVessel.empty() || + it == this->targetLargeObjectVisuals.end()) + { + ignerr << "Target vessel has not been identified yet" << std::endl; + this->targetInStreamReport.type.clear(); + return; + } + + for (const auto &v : it->second) + { + bool found = this->FindTargetVisual(v, + math::Vector2i(this->targetInStreamReport.x, + this->targetInStreamReport.y), + this->targetInStreamReport.type, target); + if (found) + { + break; + } + } + + req.add_data(this->currentTargetVessel); + req.add_data(""); + req.add_data(target); + this->OnReportTargets(req, res); + } + + this->targetInStreamReport.type.clear(); + } +} + +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::FindTargetVisual( + const rendering::VisualPtr &_targetVis, + const math::Vector2i &_imagePos, const std::string &_type, + std::string &_objectAtImgPos) const +{ + // first check if object at specified image pos is the target visual + std::string target; + auto vis = this->VisualAt(_imagePos.X(), _imagePos.Y(), _type); + if (!vis || vis != _targetVis) + { + ignmsg << "No valid target at (" << _imagePos << ")." << std::endl; + ignmsg << "Checking nearby pixels." << std::endl; + // check if target vessel is in camera view + math::Vector2i pos; + if (this->VisualInView(_targetVis, pos)) + { + ignmsg << "Target is in view: " << _targetVis->Name() << std::endl; + // check image pos of target is within tolernace + auto diff = pos - _imagePos; + + // todo(anyone) make tol a function of distance from camera to + // target as well? + double tol = this->kTargetVesselInImageTol; + if (_type != "vessel") + tol = this->kTargetObjInImageTol; + + double pxTol = this->camera->ImageWidth() * tol; + if (diff.Length() < pxTol) + { + _objectAtImgPos = _targetVis->Name(); + ignmsg << " Found target: " << _targetVis->Name() << " " + << "within the allowed tolerance. Valid report." + << std::endl; + return true; + } + else + { + ignmsg << " Target is not within the allowed tolerance: " + << "diff: " << diff.Length() << ", tol: " << pxTol + << std::endl; + } + } + else + { + ignmsg << "Target is not in view: " << _targetVis->Name() << std::endl; + } + } + else + { + ignmsg << "Found target: " << vis->Name() << ". Valid report." + << std::endl; + _objectAtImgPos = vis->Name(); + return true; + } + + // set object at img pos even if it is not the target + if (vis) + _objectAtImgPos = vis->Name(); + else + _objectAtImgPos = "none"; + + return false; +} + +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::VisualInView( + const rendering::VisualPtr &_visual, math::Vector2i &_imagePos) const +{ + // transfrom visual to camera frame + auto poseInCameraFrame = + this->camera->WorldPose().Inverse() * _visual->WorldPose(); + + // check if visual is in front of camera and within frustum far clip distance + if (poseInCameraFrame.X() > 0 && + poseInCameraFrame.X() < this->camera->FarClipPlane()) + { + // check if projected pos of visual is within image width and height + auto projectedImgPos = this->camera->Project(_visual->WorldPosition()); + if (projectedImgPos.X() >= 0 && + projectedImgPos.X() < this->camera->ImageWidth() && + projectedImgPos.Y() >= 0 && + projectedImgPos.Y() < this->camera->ImageHeight()) + { + _imagePos = projectedImgPos; + return true; + } + } + return false; +} + +///////////////////////////////////////////////// +rendering::VisualPtr GameLogicPluginPrivate::VisualAt( + unsigned int _x, unsigned int _y, const std::string &_type) const +{ + math::Vector2i imagePos(_x, _y); + + // hide other target visuals when doing a visual check + // Why do we do this? For example, when checking a target vessel, + // we do not want the Camera::VisualAt check below to return the visual + // of a target object sitting on top of the vessel. + for (auto &v : this->targetVesselVisuals) + { + if (_type == "small" || _type == "large") + v->SetVisible(false); + } + for (auto &it : this->targetSmallObjectVisuals) + { + for (auto &v : it.second) + { + if (_type == "vessel" || _type == "large") + v->SetVisible(false); + } + } + for (auto &it : this->targetLargeObjectVisuals) + { + for (auto &v : it.second) + { + if (_type == "vessel" || _type == "small") + v->SetVisible(false); + } + } + + // visual check - this returns the first visible visual at specified + // image pos + rendering::VisualPtr visual = this->camera->VisualAt(imagePos); + + // restore visual visibility after doing a visual check + for (auto &v : this->targetVesselVisuals) + { + v->SetVisible(true); + } + for (auto &it : this->targetSmallObjectVisuals) + { + for (auto &v : it.second) + v->SetVisible(true); + } + for (auto &it : this->targetLargeObjectVisuals) + { + for (auto &v : it.second) + v->SetVisible(true); + } + + // check if any visual is found + // if so, get the top level visual (model) name + std::string target; + if (visual) + { + auto target = visual; + while (target->Parent() && + target->Parent() != this->scene->RootVisual()) + { + target = std::dynamic_pointer_cast( + target->Parent()); + } + return target; + } + return nullptr; +} + +///////////////////////////////////////////////// +void GameLogicPluginPrivate::SetPhase(const std::string &_phase) +{ + std::lock_guard lock(this->phaseMutex); + this->phase = _phase; +} + +///////////////////////////////////////////////// +std::string GameLogicPluginPrivate::Phase() +{ + std::lock_guard lock(this->phaseMutex); + return this->phase; +} diff --git a/mbzirc_ign/test/test_game_logic.cc b/mbzirc_ign/test/test_game_logic.cc index 904698d5..146a7f28 100644 --- a/mbzirc_ign/test/test_game_logic.cc +++ b/mbzirc_ign/test/test_game_logic.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -195,19 +196,15 @@ TEST_F(MBZIRCTestFixture, GameLogicPhase) ignition::transport::Node node; // Run clock callback to keep track of current competition state - std::mutex runClockMutex; + std::mutex phaseMutex; std::string phaseData; - std::function runClockCb = - [&](const ignition::msgs::Clock &_msg) + std::function phaseCb = + [&](const ignition::msgs::StringMsg &_msg) { - std::lock_guard lock(runClockMutex); - EXPECT_EQ(1, _msg.header().data_size()); - auto data = _msg.header().data(0); - EXPECT_EQ(1, data.value_size()); - EXPECT_EQ("phase", data.key()); - phaseData = data.value(0); + std::lock_guard lock(phaseMutex); + phaseData = _msg.data(); }; - node.Subscribe("/mbzirc/run_clock", runClockCb); + node.Subscribe("/mbzirc/phase", phaseCb); // verify that initial phase is "setup" int sleep = 0; @@ -217,7 +214,7 @@ TEST_F(MBZIRCTestFixture, GameLogicPhase) { std::this_thread::sleep_for(1000ms); Step(10); - std::lock_guard lock(runClockMutex); + std::lock_guard lock(phaseMutex); reachedPhase = phaseData == "setup"; } EXPECT_EQ("setup", phaseData); @@ -235,17 +232,17 @@ TEST_F(MBZIRCTestFixture, GameLogicPhase) EXPECT_TRUE(rep.data()); } - // check that phase is now "run" + // check that phase is now "started" sleep = 0; reachedPhase = false; while(!reachedPhase && sleep++ < maxSleep) { std::this_thread::sleep_for(1000ms); Step(10); - std::lock_guard lock(runClockMutex); - reachedPhase = phaseData == "run"; + std::lock_guard lock(phaseMutex); + reachedPhase = phaseData == "started"; } - EXPECT_EQ("run", phaseData); + EXPECT_EQ("started", phaseData); // publish finish event { @@ -267,7 +264,7 @@ TEST_F(MBZIRCTestFixture, GameLogicPhase) { std::this_thread::sleep_for(1000ms); Step(10); - std::lock_guard lock(runClockMutex); + std::lock_guard lock(phaseMutex); reachedPhase = phaseData == "finished"; } EXPECT_EQ("finished", phaseData); @@ -491,6 +488,7 @@ TEST_F(MBZIRCTestFixture, GameLogicBoundaryPenalty) != std::string::npos && eventsBuffer.str().find("type: exceed_boundary_2") == std::string::npos; + Step(1); } EXPECT_TRUE(logged); @@ -525,6 +523,7 @@ TEST_F(MBZIRCTestFixture, GameLogicBoundaryPenalty) != std::string::npos && eventsBuffer.str().find("type: exceed_boundary_2") == std::string::npos; + Step(1); } EXPECT_TRUE(logged); @@ -537,7 +536,6 @@ TEST_F(MBZIRCTestFixture, GameLogicBoundaryPenalty) EXPECT_LT(300, std::stoi(scoreBuffer.str())); } - // move robot back out of bounds again moveOutOfBounds2 = true; Step(1); @@ -559,6 +557,7 @@ TEST_F(MBZIRCTestFixture, GameLogicBoundaryPenalty) != std::string::npos && eventsBuffer.str().find("type: exceed_boundary_2") != std::string::npos; + Step(1); } EXPECT_TRUE(logged); @@ -577,3 +576,796 @@ TEST_F(MBZIRCTestFixture, GameLogicBoundaryPenalty) ignition::common::removeAll(logPath); return; } + +TEST_F(MBZIRCTestFixture, GameLogicTargetReport) +{ + using namespace std::literals::chrono_literals; + + /// This test checks reporting targets over video stream + std::vector> params{ + {"name", "quadrotor"}, + {"world", "faster_than_realtime"}, + {"model", "mbzirc_quadrotor"}, + {"type", "uav"}, + {"x", "1"}, + {"y", "2"}, + {"z", "1.05"}, + {"slot3", "mbzirc_hd_camera"}, + {"flightTime", "10"} + }; + + bool spawnedSuccessfully = false; + bool moveAboveTargetVessel = false; + bool moveAboveTargetVesselDone = false; + bool moveAboveTargetSmallObject = false; + bool moveAboveTargetSmallObjectDone = false; + bool moveAboveTargetLargeObject = false; + bool moveAboveTargetLargeObjectDone = false; + + SetMaxIter(1000); + + LoadWorld("faster_than_realtime.sdf"); + + int simTimeSec = 0; + OnPreupdate([&](const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) + { + int64_t s, ns; + std::tie(s, ns) = ignition::math::durationToSecNsec(_info.simTime); + simTimeSec = s; + + auto worldEntity = ignition::gazebo::worldEntity(_ecm); + ignition::gazebo::World world(worldEntity); + auto modelEntity = world.ModelByName(_ecm, "quadrotor"); + ignition::gazebo::Model model(modelEntity); + + if (moveAboveTargetVessel && !moveAboveTargetVesselDone) + { + auto vesselEntity = world.ModelByName(_ecm, "Vessel A"); + auto poseComp = + _ecm.Component(vesselEntity); + ASSERT_TRUE(poseComp != nullptr); + ignition::math::Pose3d pose = poseComp->Data(); + ignition::gazebo::Model vessel(vesselEntity); + model.SetWorldPoseCmd(_ecm, ignition::math::Pose3d( + pose.Pos() + ignition::math::Vector3d(0, 0, 80), + ignition::math::Quaterniond::Identity)); + static bool output = false; + if (!output) + { + ignmsg << "Moving UAV above target vessel." << std::endl; + output = true; + } + } + else if (moveAboveTargetSmallObject && !moveAboveTargetSmallObjectDone) + { + auto objectEntity = world.ModelByName(_ecm, "small_box_a"); + auto poseComp = + _ecm.Component(objectEntity); + ASSERT_TRUE(poseComp != nullptr); + ignition::math::Pose3d pose = poseComp->Data(); + ignition::gazebo::Model object(objectEntity); + model.SetWorldPoseCmd(_ecm, ignition::math::Pose3d( + pose.Pos() + ignition::math::Vector3d(0, 0, 1), + ignition::math::Quaterniond::Identity)); + static bool output = false; + if (!output) + { + ignmsg << "Moving UAV above target small object." << std::endl; + output = true; + } + } + else if (moveAboveTargetLargeObject && !moveAboveTargetLargeObjectDone) + { + auto objectEntity = world.ModelByName(_ecm, "large_box_a"); + auto poseComp = + _ecm.Component(objectEntity); + ASSERT_TRUE(poseComp != nullptr); + ignition::math::Pose3d pose = poseComp->Data(); + ignition::gazebo::Model object(objectEntity); + model.SetWorldPoseCmd(_ecm, ignition::math::Pose3d( + pose.Pos() + ignition::math::Vector3d(0, 0, 1), + ignition::math::Quaterniond::Identity)); + static bool output = false; + if (!output) + { + ignmsg << "Moving UAV above target large object." << std::endl; + output = true; + } + } + }); + + OnPostupdate([&](const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto worldEntity = ignition::gazebo::worldEntity(_ecm); + ignition::gazebo::World world(worldEntity); + + /// Check for model + auto modelEntity = world.ModelByName(_ecm, "quadrotor"); + if (modelEntity != ignition::gazebo::kNullEntity) + { + spawnedSuccessfully = true; + } + + if (Iter() % 1000 == 0) + ignmsg << Iter() < void + { + std::lock_guard lock(phaseMutex); + phase = _msg.data(); + }; + + // Subscribe to a topic by registering a callback. + auto cbFunc = std::function(cb); + EXPECT_TRUE(node.Subscribe("/mbzirc/phase", cbFunc)); + + std::string logPath = "mbzirc_logs"; + std::string eventsLogPath = + ignition::common::joinPaths(logPath, "events.yml"); + EXPECT_TRUE(ignition::common::isFile(eventsLogPath)); + std::string scoreLogPath = + ignition::common::joinPaths(logPath, "score.yml"); + EXPECT_TRUE(ignition::common::isFile(scoreLogPath)); + + // verify initial state of logs + int score = 0; + { + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + // events log should be empty + EXPECT_TRUE(eventsBuffer.str().empty()); + + // score should be 0 + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + EXPECT_EQ(0, score); + } + + // move quadrotor above target vessel + moveAboveTargetVessel = true; + Step(100); + + // verify that competition is started + int sleep = 0; + int maxSleep = 10; + bool logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + // there should be a started event + logged = eventsBuffer.str().find("type: started") + != std::string::npos; + } + EXPECT_TRUE(logged); + + sleep = 0; + bool phaseReached = false; + while(!phaseReached && sleep++ < maxSleep) + { + { + std::lock_guard lock(phaseMutex); + phaseReached = phase == "started"; + } + Step(1); + std::this_thread::sleep_for(1000ms); + } + EXPECT_TRUE(phaseReached); + + int startTime = simTimeSec; + + // start stream + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("quadrotor"); + req.add_data("sensor_3"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/start", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(10); + + // variables for counting score + // score = sim time + time penlty + // so keep track of sim time in OnPreUpdate and add penalties to it + // to get expected score + int vesselFirstPenalty = 180; + int vesselSecondPenalty = 240; + int smallFirstPenalty = 180; + int smallSecondPenalty = 240; + int largeFirstPenalty = 180; + int largeSecondPenalty = 240; + + // verify that start stream request is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + // there should be a stream_start_request event + // but nothing should be reported yet + logged = eventsBuffer.str().find("type: stream_start_request") + != std::string::npos && + eventsBuffer.str().find("type: target_reported") + == std::string::npos; + + } + EXPECT_TRUE(logged); + + // get current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime; + EXPECT_NEAR(expectedScore, score, 1); + } + + // test report target but give incorrect image position + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("vessel"); + req.add_data("240"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(100); + + // verify that a vessel report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("type: target_reported_in_stream") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_failure_2") + == std::string::npos && + eventsBuffer.str().find("data: vessel_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // advance time + Step(100); + + // test report target vessel but give incorrect image position again + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("vessel"); + req.add_data("340"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(100); + + // verify that another vessel report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: vessel_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // advance time + Step(100); + + // test report correct target vessel + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("vessel"); + req.add_data("640"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(100); + + // verify that a vessel report success event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: vessel_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: vessel_id_success") + != std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + sleep = 0; + phaseReached = false; + while(!phaseReached && sleep++ < maxSleep) + { + { + std::lock_guard lock(phaseMutex); + phaseReached = phase == "vessel_id_success"; + } + Step(1); + std::this_thread::sleep_for(1000ms); + } + EXPECT_TRUE(phaseReached); + + // advance time + Step(100); + + moveAboveTargetVesselDone = true; + moveAboveTargetSmallObject = true; + Step(20); + + // test report target small object but give incorrect image position + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("small"); + req.add_data("340"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(20); + + // verify that a small object report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: small_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: small_object_id_failure_2") + == std::string::npos && + eventsBuffer.str().find("data: small_object_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // test report target small object but give incorrect image position again + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("small"); + req.add_data("640"); + req.add_data("280"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(20); + + // verify that another small object report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: small_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: small_object_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: small_object_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty + smallSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // test report correct target small object + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("small"); + req.add_data("640"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(20); + + // verify that a small object report success event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: small_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: small_object_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: small_object_id_success") + != std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty + smallSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // advance time + Step(100); + + sleep = 0; + phaseReached = false; + while(!phaseReached && sleep++ < maxSleep) + { + { + std::lock_guard lock(phaseMutex); + phaseReached = phase == "small_object_id_success"; + } + Step(1); + std::this_thread::sleep_for(1000ms); + } + EXPECT_TRUE(phaseReached); + + moveAboveTargetSmallObjectDone = true; + moveAboveTargetLargeObject = true; + Step(20); + + // test report target large object but give incorrect image position + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("large"); + req.add_data("0"); + req.add_data("0"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(20); + + // verify that a large object report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: large_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: large_object_id_failure_2") + == std::string::npos; + eventsBuffer.str().find("data: large_object_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty + smallSecondPenalty + + largeFirstPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // test report target large object but give incorrect image position again + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("large"); + req.add_data("1280"); + req.add_data("960"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + Step(20); + + // verify that another large object report failure event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: large_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: large_object_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: large_object_id_success") + == std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty + smallSecondPenalty + + largeFirstPenalty + largeSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + // test report correct large object + { + ignition::msgs::Boolean rep; + ignition::msgs::StringMsg_V req; + req.add_data("large"); + req.add_data("640"); + req.add_data("480"); + bool result = false; + const unsigned int timeout = 5000; + bool executed = node.Request("/mbzirc/target/stream/report", + req, timeout, rep, result); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + } + + Step(20); + + // verify that a large object report success event is logged + sleep = 0; + logged = false; + while(!logged && sleep++ < maxSleep) + { + std::this_thread::sleep_for(1000ms); + + std::ifstream eventsLog; + eventsLog.open(eventsLogPath); + std::stringstream eventsBuffer; + eventsBuffer << eventsLog.rdbuf(); + + logged = eventsBuffer.str().find("data: large_object_id_failure_1") + != std::string::npos && + eventsBuffer.str().find("data: large_object_id_failure_2") + != std::string::npos && + eventsBuffer.str().find("data: large_object_id_success") + != std::string::npos; + } + EXPECT_TRUE(logged); + + // check current score + { + std::ifstream scoreLog; + scoreLog.open(scoreLogPath); + std::stringstream scoreBuffer; + scoreBuffer << scoreLog.rdbuf(); + score = std::stoi(scoreBuffer.str()); + int expectedScore = simTimeSec - startTime + + vesselFirstPenalty + vesselSecondPenalty + + smallFirstPenalty + smallSecondPenalty + + largeFirstPenalty + largeSecondPenalty; + EXPECT_NEAR(expectedScore, score, 1); + } + + moveAboveTargetLargeObjectDone = true; + + sleep = 0; + phaseReached = false; + while(!phaseReached && sleep++ < maxSleep) + { + { + std::lock_guard lock(phaseMutex); + phaseReached = phase == "large_object_id_success"; + } + Step(1); + std::this_thread::sleep_for(1000ms); + } + EXPECT_TRUE(phaseReached); + + + StopLaunchFile(launchHandle); + ignition::common::removeAll(logPath); +} diff --git a/mbzirc_ign/worlds/empty_platform.sdf b/mbzirc_ign/worlds/empty_platform.sdf index 7e9ecc57..6e9057a4 100644 --- a/mbzirc_ign/worlds/empty_platform.sdf +++ b/mbzirc_ign/worlds/empty_platform.sdf @@ -93,5 +93,12 @@ + + + 60 + 30 + + diff --git a/mbzirc_ign/worlds/faster_than_realtime.sdf b/mbzirc_ign/worlds/faster_than_realtime.sdf index 8ad987ce..0b78804e 100644 --- a/mbzirc_ign/worlds/faster_than_realtime.sdf +++ b/mbzirc_ign/worlds/faster_than_realtime.sdf @@ -4,16 +4,29 @@ --> + + 0.004 + 0.0 + + - 0.02 - 0 + + ogre2 + + + + false @@ -56,25 +69,11 @@ 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Albedo.jpg - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Normal.jpg - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Roughness.jpg - - - - 0 0 5 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap - - - true @@ -129,6 +128,127 @@ + + Vessel A + 25 25 0.3 0 0.0 -1.0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Vessel A + + link + 6 + 3.3 + 0.27 + 0.45 + + + + 1000 1000 + 50 50 + + PMS + 5 + 3 + 1.5 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + link + + 0.0 + 0.0 + 0.0 + + 51.3 + 72.4 + 40.0 + 0.0 + 500.0 + 50.0 + 50.0 + 400.0 + 0.0 + + + + + 24 26.5 0.5 0 0 0 + + + 1 + + 0.0066667 + 0.0 + 0.0 + 0.0066667 + 0.0 + 0.0066667 + + + + + + 0.2 0.2 0.2 + + + + 0 0 1 + + + + + + 0.2 0.2 0.2 + + + + + + + + 24.9 24.85 0.65 0 0 0 + + + 10 + + 0.26667 + 0.0 + 0.0 + 0.26667 + 0.0 + 0.26667 + + + + + + 0.4 0.4 0.4 + + + + 0 1 0 + + + + + + 0.4 0.4 0.4 + + + + + + @@ -141,6 +261,11 @@
0 0 48.46
140 140 146.92 + + Vessel A + small_box_a + large_box_a + diff --git a/mbzirc_ros/CMakeLists.txt b/mbzirc_ros/CMakeLists.txt index fcff7a34..c2579261 100644 --- a/mbzirc_ros/CMakeLists.txt +++ b/mbzirc_ros/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rosgraph_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) @@ -34,6 +35,9 @@ install(TARGETS optical_frame_publisher pose_tf_broadcaster DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) @@ -48,6 +52,7 @@ if(BUILD_TESTING) ament_target_dependencies(test_ros_api geometry_msgs rclcpp + rosgraph_msgs sensor_msgs std_msgs tf2_msgs diff --git a/mbzirc_ros/launch/competition.launch.py b/mbzirc_ros/launch/competition.launch.py new file mode 100644 index 00000000..c9194cff --- /dev/null +++ b/mbzirc_ros/launch/competition.launch.py @@ -0,0 +1,70 @@ +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import os + +def launch(context, *args, **kwargs): + + ign_args = LaunchConfiguration('ign_args').perform(context) + + ign_gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('ros_ign_gazebo'), 'launch'), + '/ign_gazebo.launch.py']), + launch_arguments = {'ign_args': ign_args}.items()) + + ros2_ign_score_bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + output='screen', + arguments=['/mbzirc/score@std_msgs/msg/Float32@ignition.msgs.Float'], + ) + + ros2_ign_run_clock_bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + output='screen', + arguments=['/mbzirc/run_clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock'], + ) + + ros2_ign_phase_bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + output='screen', + arguments=['/mbzirc/phase@std_msgs/msg/String@ignition.msgs.StringMsg'], + ) + + return [ign_gazebo, + ros2_ign_score_bridge, + ros2_ign_run_clock_bridge, + ros2_ign_phase_bridge] + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('ign_args', default_value='', + description='Arguments to be passed to Ignition Gazebo'), + OpaqueFunction(function = launch) + ]) + diff --git a/mbzirc_ros/package.xml b/mbzirc_ros/package.xml index 1e748ff4..4861aae9 100644 --- a/mbzirc_ros/package.xml +++ b/mbzirc_ros/package.xml @@ -14,6 +14,7 @@ geometry_msgs rclcpp + rosgraph_msgs sensor_msgs tf2 tf2_ros diff --git a/mbzirc_ros/test/launch/test_ros_api.launch.py b/mbzirc_ros/test/launch/test_ros_api.launch.py index 5a74c790..9d93ff87 100644 --- a/mbzirc_ros/test/launch/test_ros_api.launch.py +++ b/mbzirc_ros/test/launch/test_ros_api.launch.py @@ -45,8 +45,8 @@ def generate_test_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - get_package_share_directory('ros_ign_gazebo'), - 'launch/ign_gazebo.launch.py') + get_package_share_directory('mbzirc_ros'), + 'launch/competition.launch.py') ), launch_arguments=arguments.items()) diff --git a/mbzirc_ros/test/ros_api/test_ros_api.cc b/mbzirc_ros/test/ros_api/test_ros_api.cc index 0ecffd70..c8a13ae2 100644 --- a/mbzirc_ros/test/ros_api/test_ros_api.cc +++ b/mbzirc_ros/test/ros_api/test_ros_api.cc @@ -18,12 +18,15 @@ #include #include +#include #include #include #include #include #include #include +#include +#include #include using std::placeholders::_1; @@ -105,6 +108,24 @@ TEST(RosApiTest, SimTopics) waitUntilBoolVarAndSpin( node, tfStatic.callbackExecuted, 10ms, 3500); EXPECT_TRUE(tfStatic.callbackExecuted); + + // score + MyTestClass score("/mbzirc/score"); + waitUntilBoolVarAndSpin( + node, score.callbackExecuted, 10ms, 500); + EXPECT_TRUE(score.callbackExecuted); + + // phase + MyTestClass phase("/mbzirc/phase"); + waitUntilBoolVarAndSpin( + node, phase.callbackExecuted, 10ms, 500); + EXPECT_TRUE(phase.callbackExecuted); + + // run_clock + MyTestClass runClock("/mbzirc/run_clock"); + waitUntilBoolVarAndSpin( + node, runClock.callbackExecuted, 10ms, 500); + EXPECT_TRUE(runClock.callbackExecuted); } /////////////////////////////////////////////////