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 75e410eb559cca1c9073e7de76f5f1d1b0ef63b1..d9eb6f2902948bae79e2ddf097639da2f098d92f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp @@ -47,11 +47,11 @@ public: // Returns the obstacle footprint score for a particular pose double scorePose(const geometry_msgs::msg::Pose2D & pose); bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); - bool getRobotPose(geometry_msgs::msg::Pose & current_pose); protected: double lineCost(int x0, int x1, int y0, int y1) const; double pointCost(int x, int y) const; + bool getRobotPose(geometry_msgs::msg::Pose & current_pose); void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp index 50aa085ae461df61a507f6f0a65987fe4d06aadd..bb6831996211ec6c07077156e14699dcbdbce299 100644 --- a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp +++ b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp @@ -80,6 +80,7 @@ protected: std::unique_ptr<ActionServer> action_server_; std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_; std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_; + nav2_util::GetRobotPoseClient get_robot_pose_client_{"motion_primitive"}; std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_; double cycle_frequency_; @@ -109,7 +110,7 @@ protected: node_, footprint_topic); collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>( - *costmap_sub_, *footprint_sub_); + *costmap_sub_, *footprint_sub_, get_robot_pose_client_); } void cleanup() @@ -184,6 +185,18 @@ protected: robot_->sendVelocity(cmd_vel); } + + bool getRobotPose(geometry_msgs::msg::Pose & current_pose) + { + auto request = std::make_shared<nav2_util::GetRobotPoseClient::GetRobotPoseRequest>(); + + auto result = get_robot_pose_client_.invoke(request, 1s); + if (!result->is_pose_valid) { + return false; + } + current_pose = result->pose.pose; + return true; + } }; } // namespace nav2_motion_primitives diff --git a/nav2_motion_primitives/src/back_up.cpp b/nav2_motion_primitives/src/back_up.cpp index a49838b96e739a468bbb4729af6e76aebac77009..dc2c28e587a9ceda3b91334e5c89fc4d5396ed25 100644 --- a/nav2_motion_primitives/src/back_up.cpp +++ b/nav2_motion_primitives/src/back_up.cpp @@ -45,7 +45,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) command_x_ = command->target.x; - if (!collision_checker_->getRobotPose(initial_pose_)) { + if (!getRobotPose(initial_pose_)) { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); return Status::FAILED; } @@ -56,7 +56,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) Status BackUp::onCycleUpdate() { geometry_msgs::msg::Pose current_pose; - if (!collision_checker_->getRobotPose(current_pose)) { + if (!getRobotPose(current_pose)) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } diff --git a/nav2_motion_primitives/src/spin.cpp b/nav2_motion_primitives/src/spin.cpp index b6756ee816256517c807835c9b119b5dd10c14ea..9b61b346ba479b6269652dea52c4ebbb8d8ea310 100644 --- a/nav2_motion_primitives/src/spin.cpp +++ b/nav2_motion_primitives/src/spin.cpp @@ -89,7 +89,7 @@ Status Spin::timedSpin() cmd_vel.angular.z = 0.5; geometry_msgs::msg::Pose current_pose; - if (!collision_checker_->getRobotPose(current_pose)) { + if (!getRobotPose(current_pose)) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -102,7 +102,7 @@ Status Spin::timedSpin() if (!collision_checker_->isCollisionFree(pose2d)) { stopRobot(); - RCLCPP_WARN(node_->get_logger(), "Collision Ahead -Exiting Spin "); + RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin "); return Status::SUCCEEDED; }