diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 9837dd93c82..c66d32257e7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/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 getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); /** * @brief Prune global path to only interesting portions diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 9768eeccbf4..83ec415e172 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/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 +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; diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index e2427ac475d..3eb737ed36a 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/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 + 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( "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,34 @@ TEST(PathHandlerTests, TestBounds) handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5); + // Set tf between map odom and base_link + std::unique_ptr tf_broadcaster_ = + std::make_unique(node); + geometry_msgs::msg::TransformStamped t; + 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 +152,27 @@ TEST(PathHandlerTests, TestTransforms) // Test basic transformations and path handling handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); + // Set tf between map odom and base_link + std::unique_ptr tf_broadcaster_ = + std::make_unique(node); + geometry_msgs::msg::TransformStamped t; + 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 +182,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);