use GetRobotPose service

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

@@ -23,11 +23,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
-#include "tf2/transform_datatypes.h"
-#include "tf2_ros/buffer.h"
-#include "tf2/convert.h"
-#include "tf2/LinearMath/Transform.h"
-#include "tf2_ros/transform_listener.h"
+#include "nav2_util/get_robot_pose_client.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
@@ -43,7 +39,6 @@ class CollisionChecker
   CollisionChecker(
     std::shared_ptr<CostmapSubscriber> costmap_sub,
     std::shared_ptr<FootprintSubscriber> footprint_sub,
-    tf2_ros::Buffer & tf_buffer,
     std::string name = "collision_checker");
 
   ~CollisionChecker();
@@ -56,19 +51,13 @@ class CollisionChecker
 protected:
   double lineCost(int x0, int x1, int y0, int y1);
   double pointCost(int x, int y);
-  bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const;
+  bool getRobotPose(geometry_msgs::msg::Pose & current_pose);
   void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
 
-  // Transform buffer
-  tf2_ros::Buffer & tf_buffer_;
-
-  // TF Frames
-  std::string global_frame_;
-  std::string robot_base_frame_;
-
   std::shared_ptr<Costmap2D> costmap_;
-  rclcpp::Node::SharedPtr node_;
+
   std::string name_;
+  nav2_util::GetRobotPoseClient get_robot_pose_client_{"collision_checker"};
   std::shared_ptr<CostmapSubscriber> costmap_sub_;
   std::shared_ptr<FootprintSubscriber> footprint_sub_;
 };

nav2_costmap_2d/src/collision_checker.cpp

@@ -14,26 +14,23 @@
 
 #include "nav2_costmap_2d/collision_checker.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
-#include "nav2_util/line_iterator.hpp"
 #include "nav2_costmap_2d/exceptions.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+using namespace std::chrono_literals;
 
 namespace nav2_costmap_2d
 {
 
 CollisionChecker::CollisionChecker(
   std::shared_ptr<CostmapSubscriber> costmap_sub,
   std::shared_ptr<FootprintSubscriber> footprint_sub,
-  tf2_ros::Buffer & tf_buffer,
   std::string name)
-: tf_buffer_(tf_buffer),
-  name_(name),
+: name_(name),
   costmap_sub_(costmap_sub),
   footprint_sub_(footprint_sub)
-{
-  global_frame_ = std::string("map");
-  robot_base_frame_ = std::string("base_link");
-}
+{}
 
 CollisionChecker::~CollisionChecker() {}
 
@@ -153,47 +150,30 @@ double CollisionChecker::pointCost(int x, int y)
 }
 
 bool
-CollisionChecker::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const
+CollisionChecker::getRobotPose(geometry_msgs::msg::Pose & current_pose)
 {
-  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
-  geometry_msgs::msg::PoseStamped robot_pose;
-  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
+  auto request = std::make_shared<nav2_util::GetRobotPoseClient::GetRobotPoseRequest>();
 
-  robot_pose.header.frame_id = robot_base_frame_;
-  robot_pose.header.stamp = rclcpp::Time();
-
-  // get the global pose of the robot
-  try {
-    tf_buffer_.transform(robot_pose, global_pose, global_frame_);
-  } catch (tf2::LookupException & ex) {
-    RCLCPP_ERROR(rclcpp::get_logger(name_),
-      "No Transform available Error looking up robot pose: %s\n", ex.what());
-    return false;
-  } catch (tf2::ConnectivityException & ex) {
-    RCLCPP_ERROR(rclcpp::get_logger(name_),
-      "Connectivity Error looking up robot pose: %s\n", ex.what());
-    return false;
-  } catch (tf2::ExtrapolationException & ex) {
-    RCLCPP_ERROR(rclcpp::get_logger(name_),
-      "Extrapolation Error looking up robot pose: %s\n", ex.what());
+  auto result = get_robot_pose_client_.invoke(request, 1s);
+  if (!result.get()->is_pose_valid) {
     return false;
   }
-
+  current_pose = result.get()->pose.pose;
   return true;
 }
 
 void CollisionChecker::unorientFootprint(
   const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
   std::vector<geometry_msgs::msg::Point> & reset_footprint)
 {
-  geometry_msgs::msg::PoseStamped current_pose;
+  geometry_msgs::msg::Pose current_pose;
   if (!getRobotPose(current_pose)) {
     throw CollisionCheckerException("Robot pose unavailable.");
   }
 
-  double x = current_pose.pose.position.x;
-  double y = current_pose.pose.position.y;
-  double theta = tf2::getYaw(current_pose.pose.orientation);
+  double x = current_pose.position.x;
+  double y = current_pose.position.y;
+  double theta = tf2::getYaw(current_pose.orientation);
 
   Footprint temp;
   transformFootprint(-x, -y, 0, oriented_footprint, temp);

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -38,7 +38,7 @@ CostmapSubscriber::CostmapSubscriber(
   node_logging_(node_logging),
   topic_name_(topic_name)
 {
-  costmap_sub_ = rclcpp::create_subscription<nav2_msgs::msg::Costmap>(node_topics_, topic_name,
+  costmap_sub_ = rclcpp::create_subscription<nav2_msgs::msg::Costmap>(node_topics_, topic_name_,
       rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
       std::bind(&CostmapSubscriber::costmap_callback, this, std::placeholders::_1));
 }

nav2_costmap_2d/test/integration/test_collision_checker.cpp

@@ -24,16 +24,18 @@
 #include "nav2_costmap_2d/inflation_layer.hpp"
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
 #include "nav2_costmap_2d/testing_helper.hpp"
+#include "nav2_msgs/srv/get_robot_pose.hpp"
 #include "nav2_util/node_utils.hpp"
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "tf2/transform_datatypes.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
 
 using namespace std::chrono_literals;
+using namespace std::placeholders;
 
 class RclCppFixture
 {
@@ -79,7 +81,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 {
 public:
   TestCollisionChecker(std::string name)
-  : LifecycleNode(name, "", false),
+  : LifecycleNode(name, "", true),
     global_frame_("map")
   {
     // Declare non-plugin specific costmap parameters
@@ -111,13 +113,10 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
       footprint_topic);
 
     collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
-      costmap_sub_, footprint_sub_, *tf_buffer_);
+      costmap_sub_, footprint_sub_, get_name());
 
-    base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
-    base_rel_map.child_frame_id = "base_link";
-    base_rel_map.header.frame_id = "map";
-    base_rel_map.header.stamp = now();
-    tf_buffer_->setTransform(base_rel_map, "collision_checker_test");
+    get_robot_pose_service_ = rclcpp_node_->create_service<nav2_msgs::srv::GetRobotPose>(
+      "GetRobotPose", std::bind(&TestCollisionChecker::get_robot_pose_callback, this, _1, _2, _3));
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("frame", false, false);
     // Add Static Layer
@@ -189,19 +188,12 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
     y_ = y;
     yaw_ = theta;
 
-    geometry_msgs::msg::Pose pose;
-    pose.position.x = x_;
-    pose.position.y = y_;
-    pose.position.z = 0;
+    current_pose_.pose.position.x = x_;
+    current_pose_.pose.position.y = y_;
+    current_pose_.pose.position.z = 0;
     tf2::Quaternion q;
     q.setRPY(0, 0, yaw_);
-    pose.orientation = tf2::toMsg(q);
-
-    tf2::Transform transform;
-    tf2::fromMsg(pose, transform);
-    base_rel_map.transform = tf2::toMsg(transform);
-    base_rel_map.header.stamp = now();
-    tf_buffer_->setTransform(base_rel_map, "collision_checker_test");
+    current_pose_.pose.orientation = tf2::toMsg(q);
   }
 
   void publishFootprint()
@@ -253,16 +245,29 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
     return costmap_msg;
   }
 
+  void get_robot_pose_callback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Request>/*request*/,
+    const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Response> response)
+  {
+    response->is_pose_valid = true;
+    response->pose = current_pose_;
+  }
+
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Service<nav2_msgs::srv::GetRobotPose>::SharedPtr get_robot_pose_service_;
   std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
   std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
   std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+
+  nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
   std::string global_frame_;
-  geometry_msgs::msg::TransformStamped base_rel_map;
   double x_, y_, yaw_;
-  nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
+  geometry_msgs::msg::PoseStamped current_pose_;
   std::vector<geometry_msgs::msg::Point> footprint_;
+
 };