From 7add0babd4270ad9567f3c67d1968c96e746cdcb Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 25 Jun 2019 18:16:36 -0700 Subject: [PATCH] use GetRobotPose service --- .../nav2_costmap_2d/collision_checker.hpp | 19 ++----- nav2_costmap_2d/src/collision_checker.cpp | 48 +++++------------ nav2_costmap_2d/src/costmap_subscriber.cpp | 2 +- .../integration/test_collision_checker.cpp | 51 ++++++++++--------- 4 files changed, 47 insertions(+), 73 deletions(-) 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 93b37333774..7449b3a5f51 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 @@ class CollisionChecker CollisionChecker( std::shared_ptr costmap_sub, std::shared_ptr 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 costmap_; - rclcpp::Node::SharedPtr node_; + std::string name_; + nav2_util::GetRobotPoseClient get_robot_pose_client_{"collision_checker"}; std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; }; diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 3bf0206a1f6..0494bad43d0 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 costmap_sub, std::shared_ptr 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(); - 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 & oriented_footprint, std::vector & 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 3c8cb40fdc6..22bf83223b4 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(node_topics_, topic_name, + costmap_sub_ = rclcpp::create_subscription(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 8d824786007..46b623ccac5 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode footprint_topic); collision_checker_ = std::make_unique( - 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( + "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/*request_header*/, + const std::shared_ptr/*request*/, + const std::shared_ptr response) + { + response->is_pose_valid = true; + response->pose = current_pose_; + } + std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + + rclcpp::Service::SharedPtr get_robot_pose_service_; std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::unique_ptr 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 footprint_; + };