diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
index 93b373337747cabc90a7daa0867ba4dab59efe95..7449b3a5f51ac943bb30cc8b910f449602743d34 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
+++ b/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 @@ public:
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 @@ public:
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_;
};
diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp
index 3bf0206a1f6a31bca5b77b29757596191558f1fc..0494bad43d0818af22a5638d3b66e19ad66f7a86 100644
--- a/nav2_costmap_2d/src/collision_checker.cpp
+++ b/nav2_costmap_2d/src/collision_checker.cpp
@@ -14,9 +14,11 @@
#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
{
@@ -24,16 +26,11 @@ 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,32 +150,15 @@ 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;
}
@@ -186,14 +166,14 @@ 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);
diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
index 3c8cb40fdc6ef905a13f6a599f23a0cee4c93355..22bf83223b4006ff921e36c146d12f67f818797d 100644
--- a/nav2_costmap_2d/src/costmap_subscriber.cpp
+++ b/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));
}
diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
index 8d824786007716c6a8d7cd3e352daa7902d1d792..46b623ccac5947fed2aef3ee82a214f980789246 100644
--- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp
+++ b/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 @@ public:
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 @@ protected:
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 @@ protected:
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_;
+
};