From c28095971ff7f7f7560e8b7b72fd7742c4cda39f Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 19 Jan 2021 04:32:30 +0100 Subject: [PATCH] Publish static transforms describing the staging area in world frame. --- subt_ign/src/GameLogicPlugin.cc | 54 +++++++++++++++++++++++++++++++++ subt_ros/src/SubtRosRelay.cc | 46 ++++++++++++++++++++++++++++ 2 files changed, 100 insertions(+) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 5ffd3864..948d8d48 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -205,6 +205,14 @@ class subt::GameLogicPluginPrivate const ignition::msgs::StringMsg &_req, ignition::msgs::Pose &_res); + /// \brief Ignition service callback triggered when the service is called. + /// \param[in] _req The message containing the object name. + /// \param[out] _res The response message. + /// \return true on success. + public: bool OnPoseFromWorld( + const ignition::msgs::StringMsg &_req, + ignition::msgs::Pose &_res); + /// \brief Update the score.yml and summary.yml files. This function also /// returns the time point used to calculate the elapsed real time. By /// returning this time point, we can make sure that the ::Finish function @@ -750,6 +758,9 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, this->dataPtr->node.Advertise("/subt/pose_from_artifact_origin", &GameLogicPluginPrivate::OnPoseFromArtifact, this->dataPtr.get()); + this->dataPtr->node.Advertise("/subt/pose_from_world", + &GameLogicPluginPrivate::OnPoseFromWorld, this->dataPtr.get()); + this->dataPtr->node.Advertise("/subt/start", &GameLogicPluginPrivate::OnStartCall, this->dataPtr.get()); @@ -2700,6 +2711,49 @@ bool GameLogicPluginPrivate::PoseFromArtifactHelper(const std::string &_robot, return true; } +///////////////////////////////////////////////// +bool GameLogicPluginPrivate::OnPoseFromWorld( + const ignition::msgs::StringMsg &_req, + ignition::msgs::Pose &_res) +{ + if (_req.data() != subt::kArtifactOriginName && + _req.data() != "staging_area" && + _req.data() != subt::kBaseStationName) + { + ignerr << "[GameLogicPlugin]: Querying the pose of [" << _req.data() + << "] is not allowed. Ignoring PoseFromWorld request" << std::endl; + return false; + } + + if (_req.data() == subt::kArtifactOriginName) + { + if (this->artifactOriginPose == ignition::math::Pose3d::Zero) + return false; + ignition::msgs::Set(&_res, this->artifactOriginPose); + } + else + { + std::lock_guard lock(this->posesMutex); + + auto iter = this->poses.find(_req.data()); + if (iter == this->poses.end()) + { + ignerr << "[GameLogicPlugin]: Unable to find object [" << _req.data() + << "]. Ignoring PoseFromWorld request" << std::endl; + return false; + } + + ignition::msgs::Set(&_res, iter->second); + } + + _res.mutable_header()->mutable_stamp()->CopyFrom(this->simTime); + ignition::msgs::Header::Map *frame = _res.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value("world"); + + return true; +} + ///////////////////////////////////////////////// bool GameLogicPluginPrivate::OnPoseFromArtifact( const ignition::msgs::StringMsg &_req, diff --git a/subt_ros/src/SubtRosRelay.cc b/subt_ros/src/SubtRosRelay.cc index e8cc7657..6886c4a5 100644 --- a/subt_ros/src/SubtRosRelay.cc +++ b/subt_ros/src/SubtRosRelay.cc @@ -32,6 +32,7 @@ #include #include #include +#include #include @@ -149,6 +150,9 @@ class SubtRosRelay /// the origin artifact. public: ros::ServiceServer poseFromArtifactService; + /// \brief Static TF broadcaster to publish world->artifact_origin transform. + public: tf2_ros::StaticTransformBroadcaster staticTfBroadcaster; + /// \brief The set of bound address. This is bookkeeping that helps /// to reduce erroneous error output in the ::Bind function. public: std::set boundAddresses; @@ -173,6 +177,9 @@ class SubtRosRelay /// \brief Pointer to the ROS bag recorder. public: std::unique_ptr rosRecorder; + + /// \bried Timer that tries to read the position of artifact origin in world. + public: ros::Timer artifactOriginPoseTimer; }; ////////////////////////////////////////////////// @@ -249,6 +256,45 @@ SubtRosRelay::SubtRosRelay() this->bagThread.reset(new std::thread([&](){ this->rosRecorder->run(); })); + + // publish static transformation from world to artifact origin and other + // staging area objects + auto cb = [this] (const ros::TimerEvent&) { + ignition::msgs::StringMsg req; + const int timeout = 300; // ms + ignition::msgs::Pose pose; + bool successAll = true; + + for (const auto& frame : {"artifact_origin", "base_station", "staging_area"}) + { + req.set_data(frame); + bool success = false; + + this->node.Request("/subt/pose_from_world", req, timeout, pose, success); + + successAll = successAll && success; + if (!success) + continue; + + geometry_msgs::TransformStamped t; + t.header.frame_id = pose.header().data(0).value(0); + t.child_frame_id = frame; + t.header.stamp.sec = pose.header().stamp().sec(); + t.header.stamp.nsec = pose.header().stamp().nsec(); + t.transform.translation.x = pose.position().x(); + t.transform.translation.y = pose.position().y(); + t.transform.translation.z = pose.position().z(); + t.transform.rotation.x = pose.orientation().x(); + t.transform.rotation.y = pose.orientation().y(); + t.transform.rotation.z = pose.orientation().z(); + t.transform.rotation.w = pose.orientation().w(); + this->staticTfBroadcaster.sendTransform(t); + } + + if (successAll) + this->artifactOriginPoseTimer.stop(); + }; + this->artifactOriginPoseTimer = n.createTimer(ros::Duration(1), cb); } //////////////////////////////////////////////////