Merge pull request #8 from botsandus/AUTO-604_fix_check_costmap_bounds

AUTO-604 Fix using different frame for global and local costmap

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -114,24 +114,14 @@ class PathHandler
   geometry_msgs::msg::PoseStamped
   transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
 
-  /**
-    * @brief Transform a plan to the costmap reference frame
-    * @param begin Start of path to transform
-    * @param end End of path to transform
-    * @param stamp Timestamp to use for transformation
-    * @return output path in costmap reference frame
-    */
-  nav_msgs::msg::Path
-  transformPlanPosesToCostmapFrame(
-    PathIterator begin, PathIterator end,
-    const builtin_interfaces::msg::Time & stamp);
-
   /**
     * @brief Get global plan within window of the local costmap size
     * @param global_pose Robot pose
-    * @return Range of path iterators belonging to the path within costmap window
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
     */
-  PathRange getGlobalPlanConsideringBounds(const geometry_msgs::msg::PoseStamped & global_pose);
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
 
   /**
     * @brief Prune global path to only interesting portions

nav2_mppi_controller/src/path_handler.cpp

@@ -37,10 +37,12 @@ void PathHandler::initialize(
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
 }
 
-PathRange PathHandler::getGlobalPlanConsideringBounds(
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
   const geometry_msgs::msg::PoseStamped & global_pose)
 {
   using nav2_util::geometry_utils::euclidean_distance;
+
   auto begin = global_plan_.poses.begin();
   auto end = global_plan_.poses.end();
 
@@ -55,19 +57,38 @@ PathRange PathHandler::getGlobalPlanConsideringBounds(
       return euclidean_distance(global_pose, ps);
     });
 
-  // Find the furthest relevent point on the path to consider within costmap
-  // bounds
-  const auto * costmap = costmap_->getCostmap();
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
   unsigned int mx, my;
-  auto last_point =
-    std::find_if(
-    closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) {
-      auto distance = euclidean_distance(global_pose, global_plan_pose);
-      return distance >= prune_distance_ || !costmap->worldToMap(
-        global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, mx, my);
-    });
+  // Find the furthest relevent pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != end; ++global_plan_pose) {
+    // Distance relative to robot pose check
+    auto distance = euclidean_distance(global_pose, *global_plan_pose);
+    if (distance >= prune_distance_) {
+      return {transformed_plan, closest_point};
+    }
+
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
 
-  return {closest_point, last_point};
+  return {transformed_plan, closest_point};
 }
 
 geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
@@ -92,12 +113,7 @@ nav_msgs::msg::Path PathHandler::transformPath(
   // Find relevent bounds of path to use
   geometry_msgs::msg::PoseStamped global_pose =
     transformToGlobalPlanFrame(robot_pose);
-  auto [lower_bound, upper_bound] = getGlobalPlanConsideringBounds(global_pose);
-
-  // Transform these bounds into the local costmap frame and prune older points
-  const auto & stamp = global_pose.header.stamp;
-  nav_msgs::msg::Path transformed_plan =
-    transformPlanPosesToCostmapFrame(lower_bound, upper_bound, stamp);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
 
   pruneGlobalPlan(lower_bound);
 
@@ -136,31 +152,6 @@ double PathHandler::getMaxCostmapDist()
          costmap->getResolution() / 2.0;
 }
 
-nav_msgs::msg::Path PathHandler::transformPlanPosesToCostmapFrame(
-  PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-{
-  std::string frame = costmap_->getGlobalFrameID();
-  auto transformToFrame = [&](const auto & global_plan_pose) {
-      geometry_msgs::msg::PoseStamped from_pose;
-      geometry_msgs::msg::PoseStamped to_pose;
-
-      from_pose.header.frame_id = global_plan_.header.frame_id;
-      from_pose.header.stamp = stamp;
-      from_pose.pose = global_plan_pose.pose;
-
-      transformPose(frame, from_pose, to_pose);
-      return to_pose;
-    };
-
-  nav_msgs::msg::Path plan;
-  plan.header.frame_id = frame;
-  plan.header.stamp = stamp;
-
-  std::transform(begin, end, std::back_inserter(plan.poses), transformToFrame);
-
-  return plan;
-}
-
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;

nav2_mppi_controller/test/path_handler_test.cpp

@@ -18,6 +18,7 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "tf2_ros/transform_broadcaster.h"
 
 // Tests path handling
 
@@ -47,9 +48,10 @@ class PathHandlerWrapper : public PathHandler
     return getMaxCostmapDist();
   }
 
-  PathRange getGlobalPlanConsideringBoundsWrapper(const geometry_msgs::msg::PoseStamped & pose)
+  std::pair<nav_msgs::msg::Path, PathIterator>
+  getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose)
   {
-    return getGlobalPlanConsideringBounds(pose);
+    return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
   }
 
   bool transformPoseWrapper(
@@ -59,12 +61,6 @@ class PathHandlerWrapper : public PathHandler
     return transformPose(frame, in_pose, out_pose);
   }
 
-  nav_msgs::msg::Path transformPlanPosesToCostmapFrameWrapper(
-    PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-  {
-    return transformPlanPosesToCostmapFrame(begin, end, stamp);
-  }
-
   geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
     const geometry_msgs::msg::PoseStamped & pose)
   {
@@ -98,6 +94,9 @@ TEST(PathHandlerTests, TestBounds)
   node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", "dummy_costmap", true);
+  auto results = costmap_ros->set_parameters_atomically(
+    {rclcpp::Parameter("global_frame", "odom"),
+      rclcpp::Parameter("robot_base_frame", "base_link")});
   ParametersHandler param_handler(node);
   rclcpp_lifecycle::State state;
   costmap_ros->on_configure(state);
@@ -106,22 +105,35 @@ TEST(PathHandlerTests, TestBounds)
   handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
   EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5);
 
+  // Set tf between map odom and base_link
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
+    std::make_unique<tf2_ros::TransformBroadcaster>(node);
+  geometry_msgs::msg::TransformStamped t;
+  //t.header.stamp = node->get_clock()->now();
+  t.header.frame_id = "map";
+  t.child_frame_id = "base_link";
+  tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "odom";
+  tf_broadcaster_->sendTransform(t);
+
   // Test getting the global plans within a bounds window
   nav_msgs::msg::Path path;
-  path.header.frame_id = "odom";
+  path.header.frame_id = "map";
   path.poses.resize(100);
   for (unsigned int i = 0; i != path.poses.size(); i++) {
     path.poses[i].pose.position.x = i;
+    path.poses[i].header.frame_id = "map";
   }
   geometry_msgs::msg::PoseStamped robot_pose;
   robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 25.0;
 
   handler.setPath(path);
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
+  auto [transformed_plan, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
   auto & path_in = handler.getPath();
   EXPECT_EQ(closest - path_in.poses.begin(), 25);
-  EXPECT_EQ(furthest - path_in.poses.begin(), 25);
   handler.pruneGlobalPlanWrapper(closest);
   auto & path_pruned = handler.getPath();
   EXPECT_EQ(path_pruned.poses.size(), 75u);
@@ -141,15 +153,28 @@ TEST(PathHandlerTests, TestTransforms)
   // Test basic transformations and path handling
   handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
 
+  // Set tf between map odom and base_link
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
+    std::make_unique<tf2_ros::TransformBroadcaster>(node);
+  geometry_msgs::msg::TransformStamped t;
+  //t.header.stamp = node->get_clock()->now();
+  t.header.frame_id = "map";
+  t.child_frame_id = "base_link";
+  tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "odom";
+  tf_broadcaster_->sendTransform(t);
+
   nav_msgs::msg::Path path;
   path.header.frame_id = "map";
   path.poses.resize(100);
   for (unsigned int i = 0; i != path.poses.size(); i++) {
     path.poses[i].pose.position.x = i;
+    path.poses[i].header.frame_id = "map";
   }
 
   geometry_msgs::msg::PoseStamped robot_pose, output_pose;
-  robot_pose.header.frame_id = "map";
+  robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 2.5;
 
   EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
@@ -159,10 +184,8 @@ TEST(PathHandlerTests, TestTransforms)
   handler.setPath(path);
   EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose));
 
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
-
-  builtin_interfaces::msg::Time stamp;
-  auto path_out = handler.transformPlanPosesToCostmapFrameWrapper(closest, furthest, stamp);
+  auto [path_out, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
 
   // Put it all together
   auto final_path = handler.transformPath(robot_pose);

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
+find_package(nav2_amcl REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
@@ -30,6 +31,7 @@ set(dependencies
   nav2_core
   tf2
   tf2_geometry_msgs
+  nav2_amcl
 )
 
 set(library_name nav2_regulated_pure_pursuit_controller)

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -50,6 +50,7 @@ struct Parameters
   double regulated_linear_scaling_min_radius;
   double regulated_linear_scaling_min_speed;
   bool use_rotate_to_heading;
+  bool use_rotate_to_path;
   double max_angular_accel;
   double rotate_to_heading_min_angle;
   bool allow_reversing;

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp

@@ -76,10 +76,18 @@ class PathHandler
     const geometry_msgs::msg::PoseStamped & in_pose,
     geometry_msgs::msg::PoseStamped & out_pose) const;
 
-  void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;}
+  void setPlan(const nav_msgs::msg::Path & path)
+  {
+    global_plan_ = path;
+    if (path.poses.size() > 1) {
+      previous_last_pose_ = *std::prev(path.poses.end(), 2);
+    }
+  }
 
   nav_msgs::msg::Path getPlan() {return global_plan_;}
 
+  geometry_msgs::msg::PoseStamped getBeforeLastPose() {return previous_last_pose_;}
+
 protected:
   /**
    * Get the greatest extent of the costmap in meters from the center.
@@ -92,6 +100,7 @@ class PathHandler
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;
+  geometry_msgs::msg::PoseStamped previous_last_pose_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -134,7 +134,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @return Whether should rotate to path heading
    */
   bool shouldRotateToPath(
-    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path);
+    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, double sign);
 
   /**
    * @brief Whether robot should rotate to final goal orientation
@@ -210,6 +210,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
   carrot_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
+  interpolated_carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::PathHandler> path_handler_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::ParameterHandler> param_handler_;

nav2_regulated_pure_pursuit_controller/package.xml

@@ -10,6 +10,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>nav2_amcl</depend>
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_util</depend>

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -77,6 +77,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_rotate_to_path", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
@@ -141,6 +143,7 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".regulated_linear_scaling_min_speed",
     params_.regulated_linear_scaling_min_speed);
   node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading);
+  node->get_parameter(plugin_name_ + ".use_rotate_to_path", params_.use_rotate_to_path);
   node->get_parameter(
     plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
   node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
@@ -240,6 +243,8 @@ ParameterHandler::dynamicParametersCallback(
         params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_rotate_to_path") {
+        params_.use_rotate_to_path = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
         if (parameter.as_bool() && params_.allow_reversing) {
           RCLCPP_WARN(