create getRobotPose client in motion_primitive

ros-navigation · Jun 29, 2019 · 328983d · 328983d
1 parent d1ecb61
commit 328983d
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 6 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
@@ -47,11 +47,11 @@ class CollisionChecker
   // 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
@@ -80,6 +80,7 @@ class MotionPrimitive
   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 @@ class MotionPrimitive
       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 @@ class MotionPrimitive
 
     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
@@ -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
@@ -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;
   }