use tf robot pose and return succeeded

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);

nav2_motion_primitives/src/back_up.cpp

@@ -82,8 +82,10 @@ Status BackUp::onCycleUpdate()
   pose2d.theta = tf2::getYaw(current_odom_pose->pose.pose.orientation);
 
   if (!collision_checker_->isCollisionFree(pose2d)) {
-    RCLCPP_ERROR(node_->get_logger(), "Cannot safely execute spin without collision.");
-    return Status::FAILED;
+    cmd_vel.linear.x = 0;
+    robot_->sendVelocity(cmd_vel);
+    RCLCPP_ERROR(node_->get_logger(), "Cannot safely execute back up without collision.");
+    return Status::SUCCEEDED;
   }
 
   robot_->sendVelocity(cmd_vel);

nav2_motion_primitives/src/spin.cpp

@@ -81,21 +81,23 @@ Status Spin::timedSpin()
   cmd_vel.linear.y = 0.0;
   cmd_vel.angular.z = 0.5;
 
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose;
+  geometry_msgs::msg::PoseStamped current_pose;
 
-  if (!robot_->getCurrentPose(current_pose)) {
+  if (!collision_checker_->getRobotPose(current_pose)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
 
   geometry_msgs::msg::Pose2D pose2d;
-  pose2d.x = current_pose->pose.pose.position.x;
-  pose2d.y = current_pose->pose.pose.position.y;
-  pose2d.theta = tf2::getYaw(current_pose->pose.pose.orientation) + cmd_vel.angular.z * (1/cycle_frequency_);
+  pose2d.x = current_pose.pose.position.x;
+  pose2d.y = current_pose.pose.position.y;
+  pose2d.theta = tf2::getYaw(current_pose.pose.orientation) + cmd_vel.angular.z * (1/cycle_frequency_);
 
   if (!collision_checker_->isCollisionFree(pose2d)) {
+    cmd_vel.angular.z = 0.0;
+    robot_->sendVelocity(cmd_vel);    
     RCLCPP_ERROR(node_->get_logger(), "Cannot safely execute spin without collision.");
-    return Status::FAILED;    
+    return Status::SUCCEEDED;
   }
 
   robot_->sendVelocity(cmd_vel);