diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 0b5f3b0972e..c2194116f8a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -84,7 +84,7 @@ class ComputePathThroughPosesAction BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), - BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( "error_code_id", "The compute path through poses error code"), }); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 829fd044282..a1b2998399b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -18,7 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" -#include "nav_msgs/msg/path.h" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" namespace nav2_behavior_tree @@ -84,7 +84,7 @@ class ComputePathToPoseAction : public BtActionNode( "planner_id", "", "Mapped name to the planner plugin type to use"), - BT::OutputPort("path", "Path created by ComputePathToPose node"), + BT::OutputPort("path", "Path created by ComputePathToPose node"), BT::OutputPort( "error_code_id", "The compute path to pose error code"), }); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 4460b0e9a41..2ba06394a03 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -81,7 +81,7 @@ class FollowPathAction : public BtActionNode { return providedBasicPorts( { - BT::InputPort("path", "Path to follow"), + BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index 4bf6dbb9344..cdd4b537b09 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -19,7 +19,7 @@ #include #include "nav2_msgs/action/smooth_path.hpp" -#include "nav_msgs/msg/path.h" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" namespace nav2_behavior_tree @@ -74,13 +74,13 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode("unsmoothed_path", "Path to be smoothed"), + BT::InputPort("unsmoothed_path", "Path to be smoothed"), BT::InputPort("max_smoothing_duration", 3.0, "Maximum smoothing duration"), BT::InputPort( "check_for_collisions", false, "If true collision check will be performed after smoothing"), BT::InputPort("smoother_id", ""), - BT::OutputPort( + BT::OutputPort( "smoothed_path", "Path smoothed by SmootherServer node"), BT::OutputPort("smoothing_duration", "Time taken to smooth path"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index 2604bcfea13..76d67dc3649 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/action_node.h" @@ -48,8 +48,8 @@ class TruncatePath : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_path", "Original Path"), - BT::OutputPort("output_path", "Path truncated to a certain distance"), + BT::InputPort("input_path", "Original Path"), + BT::OutputPort("output_path", "Path truncated to a certain distance"), BT::InputPort("distance", 1.0, "distance"), }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 113e0d83e72..9026e471ee6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -20,7 +20,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/action_node.h" #include "tf2_ros/buffer.h" @@ -50,8 +50,8 @@ class TruncatePathLocal : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_path", "Original Path"), - BT::OutputPort( + BT::InputPort("input_path", "Original Path"), + BT::OutputPort( "output_path", "Path truncated to a certain distance around robot"), BT::InputPort( "distance_forward", 8.0, @@ -115,8 +115,8 @@ class TruncatePathLocal : public BT::ActionNodeBase std::shared_ptr tf_buffer_; - nav_msgs::msg::Path path_; - nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_; + nav2_msgs::msg::PathWithCost path_; + nav2_msgs::msg::PathWithCost::_poses_type::iterator closest_pose_detection_begin_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a04b1263f4b..5cf2d242ffe 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -57,8 +57,9 @@ class IsPathValidCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("path", "Path to Check"), - BT::InputPort("server_timeout") + BT::InputPort("path", "Path to Check"), + BT::InputPort("server_timeout"), + BT::InputPort("consider_cost_change", true, "Consider cost changes") }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index fb2f42f3d4d..cef7eff98a3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/condition_node.h" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_behavior_tree { @@ -56,14 +56,14 @@ class PathExpiringTimerCondition : public BT::ConditionNode { return { BT::InputPort("seconds", 1.0, "Seconds"), - BT::InputPort("path") + BT::InputPort("path") }; } private: rclcpp::Node::SharedPtr node_; rclcpp::Time start_; - nav_msgs::msg::Path prev_path_; + nav2_msgs::msg::PathWithCost prev_path_; double period_; bool first_time_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp index 6e41516434c..508df776b28 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -20,7 +20,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/decorator_node.h" #include "rclcpp/rclcpp.hpp" @@ -50,7 +50,7 @@ class PathLongerOnApproach : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort("path", "Planned Path"), + BT::InputPort("path", "Planned Path"), BT::InputPort( "prox_len", 3.0, "Proximity length (m) for the path to be longer on approach"), @@ -74,8 +74,8 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the path is updated for the current goal */ bool isPathUpdated( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path); + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path); /** * @brief Checks if the robot is in the goal proximity @@ -84,7 +84,7 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the robot is in the goal proximity */ bool isRobotInGoalProximity( - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & old_path, double & prox_leng); /** @@ -95,13 +95,13 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the new path is longer */ bool isNewPathLonger( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path, double & length_factor); private: - nav_msgs::msg::Path new_path_; - nav_msgs::msg::Path old_path_; + nav2_msgs::msg::PathWithCost new_path_; + nav2_msgs::msg::PathWithCost old_path_; double prox_len_ = std::numeric_limits::max(); double length_factor_ = std::numeric_limits::max(); rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 6b6ac08eecc..bbf115490e5 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -245,6 +245,7 @@ Path to validate Server timeout + Check for significant cost change diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 5fede84f2cf..e70415c11fc 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -48,7 +48,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; @@ -56,7 +56,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionGoal::NONE); diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 0619a41ee06..5d928e3aef9 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -47,7 +47,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() BT::NodeStatus ComputePathToPoseAction::on_aborted() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; @@ -55,7 +55,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() BT::NodeStatus ComputePathToPoseAction::on_cancelled() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionGoal::NONE); @@ -64,7 +64,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() void ComputePathToPoseAction::halt() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); BtActionNode::halt(); } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 3649fad8a1b..7d6f7df30df 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -58,7 +58,7 @@ void FollowPathAction::on_wait_for_result( std::shared_ptr/*feedback*/) { // Grab the new path - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; getInput("path", new_path); // Check if it is not same with the current one diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 99568f933f9..c9fb94089eb 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -16,7 +16,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index 7f919a30f3a..6cd0e81d259 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -15,9 +15,8 @@ #include #include -#include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "behaviortree_cpp_v3/decorator_node.h" @@ -40,7 +39,7 @@ inline BT::NodeStatus TruncatePath::tick() { setStatus(BT::NodeStatus::RUNNING); - nav_msgs::msg::Path input_path; + nav2_msgs::msg::PathWithCost input_path; getInput("input_path", input_path); diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index 934f2b86bb8..da8d1aaae35 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "tf2_ros/create_timer_ros.h" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" @@ -54,7 +54,7 @@ inline BT::NodeStatus TruncatePathLocal::tick() getInput("max_robot_pose_search_dist", max_robot_pose_search_dist); bool path_pruning = std::isfinite(max_robot_pose_search_dist); - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; getInput("input_path", new_path); if (!path_pruning || new_path != path_) { path_ = new_path; @@ -96,7 +96,7 @@ inline BT::NodeStatus TruncatePathLocal::tick() auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance( std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward); - nav_msgs::msg::Path output_path; + nav2_msgs::msg::PathWithCost output_path; output_path.header = path_.header; output_path.poses = std::vector( backward_pose_it.base(), forward_pose_it); diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 4a02dede9aa..a9abe347a5e 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -34,12 +34,15 @@ IsPathValidCondition::IsPathValidCondition( BT::NodeStatus IsPathValidCondition::tick() { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; getInput("path", path); + bool consider_cost_change; + getInput("consider_cost_change", consider_cost_change); auto request = std::make_shared(); request->path = path; + request->consider_cost_change = consider_cost_change; auto result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) == diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index e018e025352..93509013ada 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -43,7 +43,7 @@ BT::NodeStatus PathExpiringTimerCondition::tick() } // Grab the new path - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; getInput("path", path); // Reset timer if the path has been updated diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964e..b17d8455603 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -31,8 +31,8 @@ PathLongerOnApproach::PathLongerOnApproach( } bool PathLongerOnApproach::isPathUpdated( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path) + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path) { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && @@ -40,15 +40,15 @@ bool PathLongerOnApproach::isPathUpdated( } bool PathLongerOnApproach::isRobotInGoalProximity( - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & old_path, double & prox_leng) { return nav2_util::geometry_utils::calculate_path_length(old_path, 0) < prox_leng; } bool PathLongerOnApproach::isNewPathLonger( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path, double & length_factor) { return nav2_util::geometry_utils::calculate_path_length(new_path, 0) > diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 122c7646212..81906344408 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -152,8 +152,8 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -173,7 +173,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -218,8 +218,8 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -241,7 +241,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 5e35a1981de..ed395ccb4ce 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -149,8 +149,8 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -170,7 +170,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -215,8 +215,8 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -238,7 +238,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 64ce7138887..487f2ae9ca7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class ControllerSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 1325070e000..a71b164a152 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -124,10 +124,10 @@ TEST_F(FollowPathActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set new path on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(1); path.poses[0].pose.position.x = 1.0; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -147,7 +147,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // set new goal path.poses[0].pose.position.x = -2.5; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index cc0b574ff98..861ea15dd22 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class GoalCheckerSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index baf856de64e..7dad38cb332 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class PlannerSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 4dc971a2caa..ff3ec067550 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 5bbf108c8f4..378032bb7dd 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -123,7 +123,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -143,13 +143,13 @@ TEST_F(SmoothPathActionTestFixture, test_tick) pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("unsmoothed_path", path); + config_->blackboard->set("unsmoothed_path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path path_empty; + nav2_msgs::msg::PathWithCost path_empty; EXPECT_NE(path_empty, path); EXPECT_EQ(action_server_->getCurrentGoal()->path, path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index c59cac1b2cf..7bcccaf95a6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class SmootherSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 11c56ef082d..fbea4b76f94 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" @@ -96,7 +96,7 @@ TEST_F(TruncatePathTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); geometry_msgs::msg::PoseStamped pose; @@ -126,7 +126,7 @@ TEST_F(TruncatePathTestFixture, test_tick) tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_NE(path, truncated_path); diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 4947c6bc8c9..76778ab0429 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" @@ -64,9 +64,9 @@ class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTest return pose; } - nav_msgs::msg::Path createLoopCrossingTestPath() + nav2_msgs::msg::PathWithCost createLoopCrossingTestPath() { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -125,7 +125,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); EXPECT_EQ(path.poses.size(), 12u); config_->blackboard->set("path", path); @@ -137,7 +137,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -220,7 +220,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -232,7 +232,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -263,7 +263,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -275,7 +275,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); @@ -305,7 +305,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); EXPECT_EQ(path.poses.size(), 12u); config_->blackboard->set("path", path); @@ -316,7 +316,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); @@ -347,8 +347,8 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 385b1cfed8e..cae182b3a50 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -73,12 +73,12 @@ TEST_F(PathExpiringTimerConditionTestFixture, test_behavior) } // place a new path on the blackboard to reset the timer - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); rclcpp::sleep_for(1500ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 341192757fb..6e4417c0cea 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 7a71f0a74b0..1cb3a5030fe 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "../../test_behavior_tree_fixture.hpp" #include "../../test_dummy_tree_node.hpp" @@ -100,13 +100,13 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set new path on blackboard - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; new_path.poses.resize(10); for (unsigned int i = 0; i < new_path.poses.size(); i++) { // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -126,13 +126,13 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set old path on blackboard - nav_msgs::msg::Path old_path; + nav2_msgs::msg::PathWithCost old_path; old_path.poses.resize(5); for (unsigned int i = 1; i <= old_path.poses.size(); i++) { // Assuming distance between waypoints to be 3.0m old_path.poses[i - 1].pose.position.x = 3.0 * i; } - config_->blackboard->set("path", old_path); + config_->blackboard->set("path", old_path); tree_->rootNode()->executeTick(); // set new path on blackboard @@ -141,7 +141,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 69a1f2507e3..594bf60c3e2 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -270,6 +270,7 @@ planner_server: tolerance: 0.5 use_astar: false allow_unknown: true + z_score: 2.55 smoother_server: ros__parameters: diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index d2a4b5e136d..b39d7516124 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -272,7 +272,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz_default_plugins/Path + Class: nav2_rviz_plugins/PathWithCost Color: 255; 0; 0 Enabled: true Head Diameter: 0.019999999552965164 @@ -372,7 +372,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz_default_plugins/Path + Class: nav2_rviz_plugins/PathWithCost Color: 0; 12; 255 Enabled: true Head Diameter: 0.30000001192092896 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index d6e218d860f..653391eb755 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_bt_navigator/navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/odometry_utils.hpp" diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 20230aa10e4..88d7fe98d37 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -25,7 +25,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/odometry_utils.hpp" namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 34a3f429a49..29aedb41553 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -120,8 +120,8 @@ NavigateThroughPosesNavigator::onLoop() try { // Get current path points - nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + nav2_msgs::msg::PathWithCost current_path; + blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 226a6735123..71f55b5291f 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -66,7 +66,7 @@ NavigateToPoseNavigator::getDefaultBTFilepath( node->declare_parameter( "default_nav_to_pose_bt_xml", pkg_share_dir + - "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); + "/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml"); } node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); @@ -123,8 +123,8 @@ NavigateToPoseNavigator::onLoop() try { // Get current path points - nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + nav2_msgs::msg::PathWithCost current_path; + blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp index bfd564772eb..e4729e11e7b 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp @@ -85,7 +85,7 @@ class ConstrainedSmoother : public nav2_core::Smoother * @return Smoothed path */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3c..e945645ab54 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -86,7 +86,7 @@ void ConstrainedSmoother::deactivate() plugin_name_.c_str()); } -bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Duration & max_time) +bool ConstrainedSmoother::smooth(nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { if (path.poses.size() < 2) { return true; diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index f50d2fb780b..60563269d07 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -187,7 +187,7 @@ class SmootherTest : public ::testing::Test const std::vector & input, std::vector & output, bool publish = false, bool cmp = false) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.reserve(input.size()); for (auto & xya : input) { geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index d2e70d67d32..6818095f9a7 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -146,7 +146,7 @@ class ControllerServer : public nav2_util::LifecycleNode * @brief Assigns path to controller * @param path Path received from action server */ - void setPlannerPath(const nav_msgs::msg::Path & path); + void setPlannerPath(const nav2_msgs::msg::PathWithCost & path); /** * @brief Calculates velocity and publishes to "cmd_vel" topic */ @@ -262,7 +262,7 @@ class ControllerServer : public nav2_util::LifecycleNode rclcpp::Time last_valid_cmd_time_; // Current path container - nav_msgs::msg::Path current_path_; + nav2_msgs::msg::PathWithCost current_path_; private: /** diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 9d341394a62..9a85b35a6dc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -462,7 +462,7 @@ void ControllerServer::computeControl() action_server_->succeeded_current(); } -void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) +void ControllerServer::setPlannerPath(const nav2_msgs::msg::PathWithCost & path) { RCLCPP_DEBUG( get_logger(), @@ -533,7 +533,7 @@ void ControllerServer::computeAndPublishVelocity() feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); // Find the closest pose to current pose on global path - nav_msgs::msg::Path & current_path = current_path_; + nav2_msgs::msg::PathWithCost & current_path = current_path_; auto find_closest_pose_idx = [&pose, ¤t_path]() { size_t closest_pose_idx = 0; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 4e89e0b1177..8f48724876d 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -46,7 +46,7 @@ #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_core/goal_checker.hpp" @@ -96,7 +96,7 @@ class Controller * @brief local setPlan - Sets the global plan * @param path The global plan */ - virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + virtual void setPlan(const nav2_msgs::msg::PathWithCost & path) = 0; /** * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index dff1207b88f..4001cb66fae 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/buffer.h" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -73,7 +73,7 @@ class GlobalPlanner * @param goal The goal pose of the robot * @return The sequence of poses to get from start to goal, if any */ - virtual nav_msgs::msg::Path createPlan( + virtual nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) = 0; }; diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp index e58b4a5ec62..ee0c9996396 100644 --- a/nav2_core/include/nav2_core/smoother.hpp +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -25,7 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_core @@ -74,7 +74,7 @@ class Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ virtual bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) = 0; }; diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index ba2c87d67b4..ae20fd289f8 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -92,7 +92,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index bd2b210e033..3e02b9e0025 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -42,7 +42,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "dwb_core/trajectory_critic.hpp" #include "dwb_msgs/msg/local_plan_evaluation.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -104,7 +104,7 @@ class DWBPublisher // Helper function for publishing other plans void publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, - rclcpp::Publisher & pub, bool flag); + rclcpp::Publisher & pub, bool flag); // Flags for turning on/off publishing specific components bool publish_evaluation_; @@ -120,9 +120,9 @@ class DWBPublisher // Publisher Objects std::shared_ptr> eval_pub_; - std::shared_ptr> global_pub_; - std::shared_ptr> transformed_pub_; - std::shared_ptr> local_pub_; + std::shared_ptr> global_pub_; + std::shared_ptr> transformed_pub_; + std::shared_ptr> local_pub_; std::shared_ptr> marker_pub_; std::shared_ptr> cost_grid_pc_pub_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index da8110a1a53..c2407dab0af 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -50,7 +50,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_core/controller_exceptions.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" using nav2_util::declare_parameter_if_not_declared; @@ -225,7 +225,7 @@ DWBLocalPlanner::loadCritics() } void -DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) +DWBLocalPlanner::setPlan(const nav2_msgs::msg::PathWithCost & path) { auto path2d = nav_2d_utils::pathToPath2D(path); for (TrajectoryCritic::Ptr & critic : critics_) { diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 7b860fef65a..f951fa968bb 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -102,9 +102,9 @@ DWBPublisher::on_configure() node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); eval_pub_ = node->create_publisher("evaluation", 1); - global_pub_ = node->create_publisher("received_global_plan", 1); - transformed_pub_ = node->create_publisher("transformed_global_plan", 1); - local_pub_ = node->create_publisher("local_plan", 1); + global_pub_ = node->create_publisher("received_global_plan", 1); + transformed_pub_ = node->create_publisher("transformed_global_plan", 1); + local_pub_ = node->create_publisher("local_plan", 1); marker_pub_ = node->create_publisher("marker", 1); cost_grid_pc_pub_ = node->create_publisher("cost_cloud", 1); @@ -238,7 +238,7 @@ DWBPublisher::publishLocalPlan( if (!publish_local_plan_) {return;} auto path = - std::make_unique( + std::make_unique( nav_2d_utils::poses2DToPath( traj.poses, header.frame_id, header.stamp)); @@ -358,11 +358,11 @@ DWBPublisher::publishLocalPlan(const nav_2d_msgs::msg::Path2D plan) void DWBPublisher::publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, - rclcpp::Publisher & pub, bool flag) + rclcpp::Publisher & pub, bool flag) { if (pub.get_subscription_count() < 1) {return;} if (!flag) {return;} - auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); + auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); pub.publish(std::move(path)); } diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp index 910361615ef..fc09f501c87 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp @@ -42,7 +42,7 @@ #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_msgs/msg/path2_d.hpp" #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/convert.h" @@ -59,12 +59,12 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped( geometry_msgs::msg::PoseStamped pose2DToPoseStamped( const geometry_msgs::msg::Pose2D & pose2d, const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path posesToPath(const std::vector & poses); -nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path); -nav_msgs::msg::Path poses2DToPath( +nav2_msgs::msg::PathWithCost posesToPath(const std::vector & poses); +nav_2d_msgs::msg::Path2D pathToPath2D(const nav2_msgs::msg::PathWithCost & path); +nav2_msgs::msg::PathWithCost poses2DToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d); +nav2_msgs::msg::PathWithCost pathToPath(const nav_2d_msgs::msg::Path2D & path2d); } // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 077707b3947..6ce0dc7f203 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -132,9 +132,9 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped( return pose; } -nav_msgs::msg::Path posesToPath(const std::vector & poses) +nav2_msgs::msg::PathWithCost posesToPath(const std::vector & poses) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; if (poses.empty()) { return path; } @@ -147,7 +147,7 @@ nav_msgs::msg::Path posesToPath(const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(poses.size()); path.header.frame_id = frame; path.header.stamp = stamp; @@ -172,9 +172,9 @@ nav_msgs::msg::Path poses2DToPath( return path; } -nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d) +nav2_msgs::msg::PathWithCost pathToPath(const nav_2d_msgs::msg::Path2D & path2d) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header = path2d.header; path.poses.resize(path2d.poses.size()); for (unsigned int i = 0; i < path.poses.size(); i++) { diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp index 8f2bd171b2d..6c59594ed46 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp +++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp @@ -47,7 +47,7 @@ using nav_2d_utils::pathToPath; TEST(nav_2d_utils, PosesToPathEmpty) { std::vector poses; - nav_msgs::msg::Path path = posesToPath(poses); + nav2_msgs::msg::PathWithCost path = posesToPath(poses); EXPECT_EQ(path.poses.size(), 0ul); } @@ -87,7 +87,7 @@ TEST(nav_2d_utils, PosesToPathNonEmpty) poses.push_back(pose1); poses.push_back(pose2); - nav_msgs::msg::Path path = posesToPath(poses); + nav2_msgs::msg::PathWithCost path = posesToPath(poses); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); @@ -104,7 +104,7 @@ TEST(nav_2d_utils, PosesToPathNonEmpty) TEST(nav_2d_utils, PathToPathEmpty) { nav_2d_msgs::msg::Path2D path2d; - nav_msgs::msg::Path path = pathToPath(path2d); + nav2_msgs::msg::PathWithCost path = pathToPath(path2d); EXPECT_EQ(path.poses.size(), 0ul); } @@ -125,7 +125,7 @@ TEST(nav_2d_utils, PathToPathNoNEmpty) path2d.poses.push_back(pose1); path2d.poses.push_back(pose2); - nav_msgs::msg::Path path = pathToPath(path2d); + nav2_msgs::msg::PathWithCost path = pathToPath(path2d); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); EXPECT_EQ(path.poses[0].pose.position.y, 2.0); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 09fbc082268..a9800ac76fe 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/PathWithCost.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 38c7f70a5da..c29ec0caca6 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -19,7 +19,7 @@ string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration planning_time uint16 error_code --- diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 5abf2e6826b..3c6ec3fcbdc 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -18,7 +18,7 @@ string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration planning_time uint16 error_code --- diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index c06b918c54a..65c5c116a86 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -10,7 +10,7 @@ uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 #goal definition -nav_msgs/Path path +PathWithCost path string controller_id string goal_checker_id --- diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index d7443fce5aa..96c279df1b8 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -9,13 +9,13 @@ uint16 FAILED_TO_SMOOTH_PATH=504 uint16 INVALID_PATH=505 #goal definition -nav_msgs/Path path +PathWithCost path string smoother_id builtin_interfaces/Duration max_smoothing_duration bool check_for_collisions --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration smoothing_duration bool was_completed uint16 error_code diff --git a/nav2_msgs/msg/PathWithCost.msg b/nav2_msgs/msg/PathWithCost.msg new file mode 100644 index 00000000000..6118c8cd9c6 --- /dev/null +++ b/nav2_msgs/msg/PathWithCost.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +geometry_msgs/PoseStamped[] poses +float32[] costs diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 8d6dc3b38dd..50cf30e5608 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,6 +1,7 @@ #Determine if the current path is still valid -nav_msgs/Path path +PathWithCost path +bool consider_cost_change --- bool is_valid -int32[] invalid_pose_indices +int32[] invalid_pose_indices diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 2330a5165b9..588e004a24c 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -26,7 +26,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_core/planner_exceptions.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -83,7 +83,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -99,7 +99,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Compute the navigation function given a seed point in the world to start from @@ -116,7 +116,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Remove artifacts at the end of the path - originated from planning on a discretized world @@ -125,7 +125,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Compute the potential, or navigation cost, at a given point in the world diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 53d72760364..148aae9f7b0 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -127,7 +127,7 @@ NavfnPlanner::cleanup() planner_.reset(); } -nav_msgs::msg::Path NavfnPlanner::createPlan( +nav2_msgs::msg::PathWithCost NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -166,7 +166,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( costmap_->getSizeInCellsY()); } - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // Corner case of the start(x,y) = goal(x,y) if (start.pose.position.x == goal.pose.position.x && @@ -185,6 +185,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) { pose.pose.orientation = goal.pose.orientation; } + path.costs.push_back(costmap_->getCost(mx_start, my_start)); path.poses.push_back(pose); return path; } @@ -194,6 +195,23 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } + for ([[maybe_unused]] const auto & pose : path.poses) { + unsigned int mx, my; + costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my); + auto cost = costmap_->getCost(mx, my); + path.costs.push_back(cost); +// if ([[maybe_unused]] auto cost = costmap_->getCost(mx, my)) { +// path.costs.push_back(50); +// } else { +// path.costs.push_back(0); +// } + } + + if (path.poses.size() != path.costs.size()) + { + RCLCPP_INFO_STREAM(logger_, "Poses and costs do not match!"); + } + #ifdef BENCHMARK_TESTING steady_clock::time_point b = steady_clock::now(); @@ -220,7 +238,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // clear the plan, just in case plan.poses.clear(); @@ -354,7 +372,7 @@ NavfnPlanner::makePlan( void NavfnPlanner::smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. @@ -377,7 +395,7 @@ NavfnPlanner::smoothApproachToGoal( bool NavfnPlanner::getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // clear the plan, just in case plan.poses.clear(); diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 197150837c9..5736e3fb845 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -27,6 +27,7 @@ include_directories( set(executable_name planner_server) set(library_name ${executable_name}_core) +set(path_validator path_validator_lib) set(dependencies rclcpp @@ -54,6 +55,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) +add_library(${path_validator} SHARED + src/path_validator.cpp) + +ament_target_dependencies(${path_validator} + ${dependencies} +) + add_executable(${executable_name} src/main.cpp ) diff --git a/nav2_planner/include/nav2_planner/path_validator.hpp b/nav2_planner/include/nav2_planner/path_validator.hpp new file mode 100644 index 00000000000..d654a193b82 --- /dev/null +++ b/nav2_planner/include/nav2_planner/path_validator.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef NAV2_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ +#define NAV2_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ + +#include "nav2_msgs/msg/path_with_cost.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_planner +{ +using Path = nav2_msgs::msg::PathWithCost; +using Pose = geometry_msgs::msg::PoseStamped; +using PathIndex = unsigned int; +using Point = geometry_msgs::msg::Point; +using Costs = std::vector; + +class [[maybe_unused]] PathValidator +{ +public: + PathValidator() = default; + /** + * @brief check if the path is valid + * \param path the path to check + * \param cost_change_z_score + * \param consider_cost_change true, change in cost in considered + * \return true path is valid + */ + bool check(const Path & path, float cost_change_z_score, bool consider_cost_change); + + /** + * @brief determine if the path is lethal + * @param path the path to check + * @return true, if path is lethal + */ + bool isThisALethal(const Path & path); + + /** + * @brief find the path index closest to the robot + * @param path the path to evalutate + * \param pose the current pose of the robotic platform + * \return PathIndex, the index of the pose closest to the robot + */ + PathIndex truncate(const Path &path, const Pose &pose); + + bool isCostChangeSignifant(const Costs &original_costs, + const Costs ¤t_costs, + float z_score); +}; +} + +#endif //NAV2_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index fee711f974a..40facb0e3fc 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -24,7 +24,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" @@ -69,7 +69,7 @@ class PlannerServer : public nav2_util::LifecycleNode * @param goal goal request * @return Path */ - nav_msgs::msg::Path getPlan( + nav2_msgs::msg::PathWithCost getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, const std::string & planner_id); @@ -180,7 +180,7 @@ class PlannerServer : public nav2_util::LifecycleNode template bool validatePath( const geometry_msgs::msg::PoseStamped & curr_goal, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const std::string & planner_id); /** @@ -208,7 +208,7 @@ class PlannerServer : public nav2_util::LifecycleNode * @brief Publish a path for visualization purposes * @param path Reference to Global Path */ - void publishPlan(const nav_msgs::msg::Path & path); + void publishPlan(const nav2_msgs::msg::PathWithCost & path); void exceptionWarning( const geometry_msgs::msg::PoseStamped & start, @@ -239,6 +239,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; + double cost_change_z_score_; std::string planner_ids_concat_; // Clock @@ -253,7 +254,7 @@ class PlannerServer : public nav2_util::LifecycleNode nav2_costmap_2d::Costmap2D * costmap_; // Publishers for the path - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Service to determine if the path is valid rclcpp::Service::SharedPtr is_path_valid_service_; diff --git a/nav2_planner/src/path_validator.cpp b/nav2_planner/src/path_validator.cpp new file mode 100644 index 00000000000..7df2adbcacb --- /dev/null +++ b/nav2_planner/src/path_validator.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "nav2_planner/path_validator.hpp" +#include "nav2_util/geometry_utils.hpp" + + +namespace nav2_planner +{ + +bool PathValidator::check(const Path &, float, bool) +{ + return true; +} + +PathIndex PathValidator::truncate(const Path & path, const Pose & current_pose) +{ + PathIndex closest_point_index = 0; + float current_distance = std::numeric_limits::max(); + float closest_distance = current_distance; + geometry_msgs::msg::Point current_point = current_pose.pose.position; + for (unsigned int i = 0; i != path.poses.size(); ++i) { + Point path_point = path.poses[i].pose.position; + + current_distance = nav2_util::geometry_utils::euclidean_distance( + current_point, + path_point); + + if (current_distance < closest_distance) { + closest_point_index = i; + closest_distance = current_distance; + } + } + return closest_point_index; +} +bool PathValidator::isCostChangeSignifant( + const Costs & original_costs, + const Costs & current_costs, + float z_score) +{ + if (original_costs.size() != current_costs.size()) { + return true; + } + + float original_costs_sum = std::accumulate(original_costs.begin(), original_costs.end(), 0.0f); + float original_costs_mean = original_costs_sum / static_cast(original_costs.size()); + + std::cout << "Original Mean: " << original_costs_mean << std::endl; + float current_costs_sum = std::accumulate(current_costs.begin(), current_costs.end(), 0.0f); + float current_costs_mean = current_costs_sum / static_cast(current_costs.size()); + + std::cout << "Current Mean: " << current_costs_mean << std::endl; + float variance_original = 0; + for (const auto & cost : original_costs) { + variance_original += (cost - original_costs_mean) * (cost - original_costs_mean); + } + variance_original /= static_cast(original_costs.size() - 1); + + + float variance_current = 0; + for (const auto & cost : current_costs) { + variance_current += (cost - current_costs_mean) * (cost - current_costs_mean); + } + variance_current /= static_cast(current_costs.size() - 1); + + // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples + // come from the same distribution (e.g. there is not a statistically significant change) + // Thus, the difference in population mean is 0 and the sample sizes are the same + float numerator = current_costs_mean - original_costs_mean; + float denominator = std::sqrt((variance_current + variance_original) / + static_cast(current_costs.size())); + + // if the variance of the current and original costs are 0 + if (denominator <= 0) { + if ( std::fabs(current_costs_mean - original_costs_mean) > 10) { + return true; + } else { + return false; + } + } + + float cost_change_z_score = numerator / denominator; + + std::cout << "Cost Change z score: " << cost_change_z_score << std::endl; + if (cost_change_z_score > z_score) { + return false; + } + return true; +} + +bool isThisALethal(const Path) +{ + return true; +} +} diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f58bdac31fd..da49f0d6130 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "builtin_interfaces/msg/duration.hpp" #include "nav2_util/costmap.hpp" @@ -52,6 +53,12 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); + nav2_util::declare_parameter_if_not_declared( + this, "cost_change_z_score", rclcpp::ParameterValue(2.55)); + + + get_parameter("cost_change_z_score", cost_change_z_score_); + get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { @@ -131,7 +138,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Initialize pubs & subs - plan_publisher_ = create_publisher("plan", 1); + plan_publisher_ = create_publisher("plan", 1); // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( @@ -310,7 +317,7 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( const geometry_msgs::msg::PoseStamped & goal, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const std::string & planner_id) { if (path.poses.empty()) { @@ -339,7 +346,7 @@ void PlannerServer::computePlanThroughPoses() // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); - nav_msgs::msg::Path concat_path; + nav2_msgs::msg::PathWithCost concat_path; geometry_msgs::msg::PoseStamped curr_start, curr_goal; @@ -378,7 +385,7 @@ void PlannerServer::computePlanThroughPoses() } // Get plan from start -> goal - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav2_msgs::msg::PathWithCost curr_path = getPlan(curr_start, curr_goal, goal->planner_id); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -539,7 +546,7 @@ PlannerServer::computePlan() } } -nav_msgs::msg::Path +nav2_msgs::msg::PathWithCost PlannerServer::getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, @@ -568,13 +575,13 @@ PlannerServer::getPlan( } } - return nav_msgs::msg::Path(); + return nav2_msgs::msg::PathWithCost(); } void -PlannerServer::publishPlan(const nav_msgs::msg::Path & path) +PlannerServer::publishPlan(const nav2_msgs::msg::PathWithCost & path) { - auto msg = std::make_unique(path); + auto msg = std::make_unique(path); if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) { plan_publisher_->publish(std::move(msg)); } @@ -591,13 +598,18 @@ void PlannerServer::isPathValid( return; } + RCLCPP_INFO_STREAM(get_logger(), "Path and cost" << request->path.poses.size() << " " << + request->path.costs.size()); + + // Find the closest point to the robot to evaluate from + // TODO add orientation element like `truncate_path_local_action` BT node for looping paths geometry_msgs::msg::PoseStamped current_pose; unsigned int closest_point_index = 0; if (costmap_ros_->getRobotPose(current_pose)) { float current_distance = std::numeric_limits::max(); float closest_distance = current_distance; geometry_msgs::msg::Point current_point = current_pose.pose.position; - for (unsigned int i = 0; i < request->path.poses.size(); ++i) { + for (unsigned int i = 0; i != request->path.poses.size(); ++i) { geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position; current_distance = nav2_util::geometry_utils::euclidean_distance( @@ -609,26 +621,68 @@ void PlannerServer::isPathValid( closest_distance = current_distance; } } + } - /** - * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied - */ - unsigned int mx = 0; - unsigned int my = 0; - for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { - costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); - unsigned int cost = costmap_->getCost(mx, my); - - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - { - response->is_valid = false; - } + // Checking path for collisions starting at the closest point to avoid those already passed + std::vector current_costs; + current_costs.reserve(request->path.costs.size() - closest_point_index - 1); + + unsigned int mx = 0; + unsigned int my = 0; + for (unsigned int i = closest_point_index; i != request->path.poses.size(); ++i) { + costmap_->worldToMap( + request->path.poses[i].pose.position.x, + request->path.poses[i].pose.position.y, mx, my); + unsigned int cost = costmap_->getCost(mx, my); + current_costs.push_back(cost); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + response->is_valid = false; + return; } } + + // Check using a statistical test if the cost changes are 'significant' enough + // to warrant replanning, due to changes in cost within the environment, + // when larger than the minimum sample size for the test + if (!request->consider_cost_change || request->path.costs.size() < 30) { + return; + } + + std::vector original_costs( + request->path.costs.begin() + closest_point_index, request->path.costs.end()); + + float mean_orginal = 0; + float mean_current = 0; + for (unsigned int i = 0; i != original_costs.size(); ++i) { + mean_orginal += static_cast(original_costs[i]); + mean_current += static_cast(current_costs[i]); + } + mean_orginal /= static_cast(original_costs.size()); + mean_current /= static_cast(current_costs.size()); + + float var_orginal = 0; + float var_current = 0; + for (unsigned int i = 0; i != original_costs.size(); ++i) { + var_orginal += std::pow(static_cast(original_costs[i]) - static_cast(mean_orginal), 2); + var_current += std::pow(static_cast(current_costs[i]) - static_cast(mean_current), 2); + } + var_orginal /= static_cast(original_costs.size() - 1); + var_current /= static_cast(current_costs.size() - 1); + + // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples + // come from the same distribution (e.g. there is not a statistically significant change) + // Thus, the difference in population mean is 0 and the sample sizes are the same + float z = (mean_current - mean_orginal) / std::sqrt((var_current + var_orginal) / current_costs.size()); + + // TODO try single tail or tune strictness? Parameterize? + // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. + if (z > cost_change_z_score_) { // critical z score for 95% level + RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << cost_change_z_score_); + response->is_valid = false; + } } rcl_interfaces::msg::SetParametersResult diff --git a/nav2_planner/test/CMakeLists.txt b/nav2_planner/test/CMakeLists.txt index d415d906ef5..200feb22748 100644 --- a/nav2_planner/test/CMakeLists.txt +++ b/nav2_planner/test/CMakeLists.txt @@ -8,3 +8,14 @@ ament_target_dependencies(test_dynamic_parameters target_link_libraries(test_dynamic_parameters ${library_name} ) + +#Test path validator +ament_add_gtest(test_path_validator + test_path_validator.cpp +) +ament_target_dependencies(test_path_validator + ${dependencies} +) +target_link_libraries(test_path_validator + ${path_validator} +) \ No newline at end of file diff --git a/nav2_planner/test/test_path_validator.cpp b/nav2_planner/test/test_path_validator.cpp new file mode 100644 index 00000000000..908c78106e3 --- /dev/null +++ b/nav2_planner/test/test_path_validator.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_planner/path_validator.hpp" + +using PathValidator = nav2_planner::PathValidator; +using Costs = nav2_planner::Costs; + +class RclCppFixture +{ + public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(PathValidatorTest, is_cost_change_signifant) +{ + PathValidator path_validator; + + // free space test + Costs original_costs(30); + std::fill(original_costs.begin(), original_costs.end(), 0); + + Costs current_costs(30); + std::fill( current_costs.begin(), current_costs.end(), 0); + float z_score = 2.55; + + bool is_cost_change_significant = + path_validator.isCostChangeSignifant(original_costs, current_costs, z_score); + + EXPECT_TRUE(is_cost_change_significant); +} diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6f84b05382d..96d88564f39 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -28,6 +29,7 @@ set(dependencies nav_msgs nav2_util nav2_core + nav2_msgs tf2 tf2_geometry_msgs ) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp index baccc6ca20b..7508cab9296 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -96,7 +96,7 @@ class CollisionChecker std::unique_ptr> footprint_collision_checker_; Parameters * params_; - std::shared_ptr> carrot_arc_pub_; + std::shared_ptr> carrot_arc_pub_; rclcpp::Clock::SharedPtr clock_; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 22bc0266a66..00338b4380c 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -60,7 +60,7 @@ class PathHandler * @param max_robot_pose_search_dist Distance to search for matching nearest path point * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan( + nav2_msgs::msg::PathWithCost transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist); @@ -76,9 +76,9 @@ 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 nav2_msgs::msg::PathWithCost & path) {global_plan_ = path;} - nav_msgs::msg::Path getPlan() {return global_plan_;} + nav2_msgs::msg::PathWithCost getPlan() {return global_plan_;} protected: /** @@ -91,7 +91,7 @@ class PathHandler tf2::Duration transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; - nav_msgs::msg::Path global_plan_; + nav2_msgs::msg::PathWithCost global_plan_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7b28ca9720e..78d783fa438 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -100,7 +100,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief Limits the maximum linear speed of the robot. @@ -164,7 +164,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & speed, - const double & pose_cost, const nav_msgs::msg::Path & path, + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign); /** @@ -187,14 +187,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param path Current global path * @return Lookahead point */ - geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav2_msgs::msg::PathWithCost &); /** * @brief checks for the cusp position * @param pose Pose input to determine the cusp position * @return robot distance from the cusp */ - double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); + double findVelocitySignChange(const nav2_msgs::msg::PathWithCost & transformed_plan); rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; @@ -207,10 +207,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller double goal_dist_tol_; double control_duration_; - std::shared_ptr> global_path_pub_; + std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; - std::shared_ptr> carrot_arc_pub_; + std::shared_ptr> carrot_arc_pub_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; std::unique_ptr collision_checker_; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp index 61dca3487ea..9c39151b181 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp @@ -89,7 +89,7 @@ inline double costConstraint( * @return A scale from 0.0-1.0 of the distance to goal scaled by minimum distance */ inline double approachVelocityScalingFactor( - const nav_msgs::msg::Path & transformed_path, + const nav2_msgs::msg::PathWithCost & transformed_path, const double approach_velocity_scaling_dist) { using namespace nav2_util::geometry_utils; // NOLINT @@ -117,7 +117,7 @@ inline double approachVelocityScalingFactor( */ inline double approachVelocityConstraint( const double constrained_linear_vel, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const double min_approach_velocity, const double approach_velocity_scaling_dist) { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index cffd2c1f499..638ce161737 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -16,7 +16,7 @@ nav2_costmap_2d rclcpp geometry_msgs - nav2_msgs + pluginlib tf2 tf2_geometry_msgs diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab76..f949f2e7b3f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -41,7 +41,7 @@ CollisionChecker::CollisionChecker( FootprintCollisionChecker>(costmap_); footprint_collision_checker_->setCostmap(costmap_); - carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); carrot_arc_pub_->on_activate(); } @@ -62,7 +62,7 @@ bool CollisionChecker::isCollisionImminent( } // visualization messages - nav_msgs::msg::Path arc_pts_msg; + nav2_msgs::msg::PathWithCost arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); arc_pts_msg.header.stamp = robot_pose.header.stamp; geometry_msgs::msg::PoseStamped pose_msg; diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index f4ceec759d2..46b5e79d193 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -45,7 +45,7 @@ double PathHandler::getCostmapMaxExtent() const return max_costmap_dim_meters / 2.0; } -nav_msgs::msg::Path PathHandler::transformGlobalPlan( +nav2_msgs::msg::PathWithCost PathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist) { @@ -95,7 +95,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( }; // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; + nav2_msgs::msg::PathWithCost transformed_plan; std::transform( transformation_begin, transformation_end, std::back_inserter(transformed_plan.poses), diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 674a456c57f..5f748a18634 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -71,7 +71,7 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; - global_path_pub_ = node->create_publisher("received_global_plan", 1); + global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); } @@ -297,7 +297,7 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( @@ -330,7 +330,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, - const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign) { double curvature_vel = linear_vel, cost_vel = linear_vel; @@ -359,7 +359,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +void RegulatedPurePursuitController::setPlan(const nav2_msgs::msg::PathWithCost & path) { path_handler_->setPlan(path); } @@ -385,7 +385,7 @@ void RegulatedPurePursuitController::setSpeedLimit( } double RegulatedPurePursuitController::findVelocitySignChange( - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { // Iterating through the transformed global path to determine the position of the cusp for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index 3e8d4c5b0a0..f69ba2266c0 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -12,4 +12,4 @@ target_link_libraries(test_regulated_pp # Path utils test ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) -ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs) +ament_target_dependencies(test_path_utils nav2_msgs geometry_msgs tf2_geometry_msgs) diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp index 4b516ec1f3b..d5740d33b82 100644 --- a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp @@ -22,7 +22,7 @@ namespace path_utils { void append_transform_to_path( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, tf2::Transform & relative_transform) { // Add a new empty pose @@ -42,7 +42,7 @@ void append_transform_to_path( new_pose.header.frame_id = previous_pose.header.frame_id; } -void Straight::append(nav_msgs::msg::Path & path, double spacing) const +void Straight::append(nav2_msgs::msg::PathWithCost & path, double spacing) const { auto num_points = std::floor(length_ / spacing); path.poses.reserve(path.poses.size() + num_points); @@ -57,7 +57,7 @@ double chord_length(double radius, double radians) return 2 * radius * sin(radians / 2); } -void Arc::append(nav_msgs::msg::Path & path, double spacing) const +void Arc::append(nav2_msgs::msg::PathWithCost & path, double spacing) const { double length = radius_ * std::abs(radians_); size_t num_points = std::floor(length / spacing); @@ -71,12 +71,12 @@ void Arc::append(nav_msgs::msg::Path & path, double spacing) const } } -nav_msgs::msg::Path generate_path( +nav2_msgs::msg::PathWithCost generate_path( geometry_msgs::msg::PoseStamped start, double spacing, std::initializer_list> segments) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header = start.header; path.poses.push_back(start); for (const auto & segment : segments) { diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp index c079e21614f..57d4c247227 100644 --- a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace path_utils { @@ -32,7 +32,7 @@ namespace path_utils class PathSegment { public: - virtual void append(nav_msgs::msg::Path & path, double spacing) const = 0; + virtual void append(nav2_msgs::msg::PathWithCost & path, double spacing) const = 0; virtual ~PathSegment() {} }; @@ -41,7 +41,7 @@ class Arc : public PathSegment public: explicit Arc(double radius, double radians) : radius_(radius), radians_(radians) {} - void append(nav_msgs::msg::Path & path, double spacing) const override; + void append(nav2_msgs::msg::PathWithCost & path, double spacing) const override; private: double radius_; @@ -53,7 +53,7 @@ class Straight : public PathSegment public: explicit Straight(double length) : length_(length) {} - void append(nav_msgs::msg::Path & path, double spacing) const override; + void append(nav2_msgs::msg::PathWithCost & path, double spacing) const override; private: double length_; @@ -101,7 +101,7 @@ class RightCircle : public Arc : Arc(radius, -2.0 * M_PI) {} }; -nav_msgs::msg::Path generate_path( +nav2_msgs::msg::PathWithCost generate_path( geometry_msgs::msg::PoseStamped start, double spacing, std::initializer_list> segments); diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 09892755d93..945ca2af06c 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -41,7 +41,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} + nav2_msgs::msg::PathWithCost getPlan() {return path_handler_->getPlan();} double getSpeed() {return params_->desired_linear_vel;} @@ -69,7 +69,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( - const double & dist, const nav_msgs::msg::Path & path) + const double & dist, const nav2_msgs::msg::PathWithCost & path) { return getLookAheadPoint(dist, path); } @@ -94,7 +94,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure void applyConstraintsWrapper( const double & curvature, const geometry_msgs::msg::Twist & curr_speed, - const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign) { return applyConstraints( curvature, curr_speed, pose_cost, path, @@ -102,12 +102,12 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } double findVelocitySignChangeWrapper( - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { return findVelocitySignChange(transformed_plan); } - nav_msgs::msg::Path transformGlobalPlanWrapper( + nav2_msgs::msg::PathWithCost transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); @@ -130,7 +130,7 @@ TEST(RegulatedPurePursuitTest, basicAPI) ctrl->cleanup(); // setPlan and get plan - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(2); path.poses[0].header.frame_id = "fake_frame"; ctrl->setPlan(path); @@ -185,7 +185,7 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) pose.pose.position.x = 1.0; pose.pose.position.y = 0.0; - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(3); path.header.frame_id = "smb"; path.header.stamp = pose.header.stamp; @@ -369,7 +369,7 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) // test getLookAheadPoint double dist = 1.0; - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(10); for (uint i = 0; i != path.poses.size(); i++) { path.poses[i].pose.position.x = static_cast(i); diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index c3812ec1d3c..98b77bff199 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -95,7 +95,7 @@ class RotationShimController : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief Limits the maximum linear speed of the robot. @@ -164,7 +164,7 @@ class RotationShimController : public nav2_core::Controller pluginlib::ClassLoader lp_loader_; nav2_core::Controller::Ptr primary_controller_; bool path_updated_; - nav_msgs::msg::Path current_path_; + nav2_msgs::msg::PathWithCost current_path_; double forward_sampling_distance_, angular_dist_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 7d32283db7c..792910aec24 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -268,7 +268,7 @@ void RotationShimController::isCollisionFree( } } -void RotationShimController::setPlan(const nav_msgs::msg::Path & path) +void RotationShimController::setPlan(const nav2_msgs::msg::PathWithCost & path) { path_updated_ = true; current_path_ = path; diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index e1bc30f80ec..e2a6020387c 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -47,7 +47,7 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return primary_controller_; } - nav_msgs::msg::Path getPath() + nav2_msgs::msg::PathWithCost getPath() { return current_path_; } @@ -135,7 +135,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "hi mate!"; path.poses.resize(10); path.poses[1].pose.position.x = 0.1; @@ -155,11 +155,11 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) EXPECT_EQ(pose.pose.position.x, 1.0); // default forward sampling is 0.5 EXPECT_EQ(pose.pose.position.y, 1.0); - nav_msgs::msg::Path path_invalid_leng; + nav2_msgs::msg::PathWithCost path_invalid_leng; controller->setPlan(path_invalid_leng); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); - nav_msgs::msg::Path path_invalid_dists; + nav2_msgs::msg::PathWithCost path_invalid_dists; path.poses.resize(10); controller->setPlan(path_invalid_dists); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); @@ -185,7 +185,7 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "fake_frame"; path.poses.resize(10); path.poses[1].pose.position.x = 0.1; @@ -255,7 +255,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "fake_frame"; path.poses.resize(10); diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 39bdbabae49..fcb685328e3 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -39,6 +39,7 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp + include/nav2_rviz_plugins/path_with_cost_display.hpp ) include_directories( @@ -52,6 +53,7 @@ add_library(${library_name} SHARED src/nav2_panel.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp + src/path_with_cost_display.cpp ${nav2_rviz_plugins_headers_to_moc} ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp new file mode 100644 index 00000000000..d792005f87d --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp @@ -0,0 +1,170 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ +#define NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ + +#include + +#include "nav2_msgs/msg/path_with_cost.hpp" + +#include "rviz_common/message_filter_display.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz_common +{ +namespace properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +class EnumProperty; +class VectorProperty; +} +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace displays +{ +/** + * \class PathWithCostDisplay + * \brief Displays a nav2_msgs::msg::PathWithCost message + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public + rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + // TODO(Martin-Idel-SI): Constructor for testing, remove once ros_nodes can be mocked and call + // initialize() instead + explicit PathWithCostDisplay(rviz_common::DisplayContext * context); + PathWithCostDisplay(); + ~PathWithCostDisplay() override; + + /** @brief Overridden from Display. */ + void reset() override; + + /** @brief Overridden from MessageFilterDisplay. */ + void processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) override; + +protected: + /** @brief Overridden from Display. */ + void onInitialize() override; + +private Q_SLOTS: + void updateBufferLength(); + void updateStyle(); + void updateLineWidth(); + void updateOffset(); + void updatePoseStyle(); + void updatePoseAxisGeometry(); + void updatePoseArrowColor(); + void updatePoseArrowGeometry(); + +private: + void destroyObjects(); + void allocateArrowVector(std::vector & arrow_vect, size_t num); + void allocateAxesVector(std::vector & axes_vect, size_t num); + void destroyPoseAxesChain(); + void destroyPoseArrowChain(); + void updateManualObject( + Ogre::ManualObject * manual_object, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updateBillBoardLine( + rviz_rendering::BillboardLine * billboard_line, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updatePoseMarkers( + size_t buffer_index, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updateAxesMarkers( + std::vector & axes_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updateArrowMarkers( + std::vector & arrow_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + + std::vector manual_objects_; + std::vector billboard_lines_; + std::vector> axes_chain_; + std::vector> arrow_chain_; + Ogre::MaterialPtr lines_material_; + + rviz_common::properties::EnumProperty * style_property_; + rviz_common::properties::ColorProperty * color_property_; + rviz_common::properties::FloatProperty * alpha_property_; + rviz_common::properties::FloatProperty * line_width_property_; + rviz_common::properties::IntProperty * buffer_length_property_; + rviz_common::properties::VectorProperty * offset_property_; + + enum LineStyle + { + LINES, + BILLBOARDS + }; + + // pose marker property + rviz_common::properties::EnumProperty * pose_style_property_; + rviz_common::properties::FloatProperty * pose_axes_length_property_; + rviz_common::properties::FloatProperty * pose_axes_radius_property_; + rviz_common::properties::ColorProperty * pose_arrow_color_property_; + rviz_common::properties::FloatProperty * pose_arrow_shaft_length_property_; + rviz_common::properties::FloatProperty * pose_arrow_head_length_property_; + rviz_common::properties::FloatProperty * pose_arrow_shaft_diameter_property_; + rviz_common::properties::FloatProperty * pose_arrow_head_diameter_property_; + + enum PoseStyle + { + NONE, + AXES, + ARROWS, + }; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 197a9a750d7..fe89ba96c57 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -18,4 +18,12 @@ The Particle Cloud rviz display. + + Displays data from a data from nav2_msgs::msg::PathWithCost. + nav2_msgs/msg/PathWithCost + + + diff --git a/nav2_rviz_plugins/src/path_with_cost_display.cpp b/nav2_rviz_plugins/src/path_with_cost_display.cpp new file mode 100644 index 00000000000..bc77f8ff947 --- /dev/null +++ b/nav2_rviz_plugins/src/path_with_cost_display.cpp @@ -0,0 +1,602 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_rviz_plugins/path_with_cost_display.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/int_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/uniform_string_stream.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_rendering/material_manager.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +PathWithCostDisplay::PathWithCostDisplay(rviz_common::DisplayContext * context) +: PathWithCostDisplay() +{ + context_ = context; + scene_manager_ = context->getSceneManager(); + scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + updateBufferLength(); +} + +PathWithCostDisplay::PathWithCostDisplay() +{ + style_property_ = new rviz_common::properties::EnumProperty( + "Line Style", "Lines", + "The rendering operation to use to draw the grid lines.", + this, SLOT(updateStyle())); + + style_property_->addOption("Lines", LINES); + style_property_->addOption("Billboards", BILLBOARDS); + + line_width_property_ = new rviz_common::properties::FloatProperty( + "Line Width", 0.03f, + "The width, in meters, of each path line." + "Only works with the 'Billboards' style.", + this, SLOT(updateLineWidth()), this); + line_width_property_->setMin(0.001f); + line_width_property_->hide(); + + color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 255, 0), + "Color to draw the path.", this); + + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, + "Amount of transparency to apply to the path.", this); + + buffer_length_property_ = new rviz_common::properties::IntProperty( + "Buffer Length", 1, + "Number of paths to display.", + this, SLOT(updateBufferLength())); + buffer_length_property_->setMin(1); + + offset_property_ = new rviz_common::properties::VectorProperty( + "Offset", Ogre::Vector3::ZERO, + "Allows you to offset the path from the origin of the reference frame. In meters.", + this, SLOT(updateOffset())); + + pose_style_property_ = new rviz_common::properties::EnumProperty( + "Pose Style", "None", + "Shape to display the pose as.", + this, SLOT(updatePoseStyle())); + pose_style_property_->addOption("None", NONE); + pose_style_property_->addOption("Axes", AXES); + pose_style_property_->addOption("Arrows", ARROWS); + + pose_axes_length_property_ = new rviz_common::properties::FloatProperty( + "Length", 0.3f, + "Length of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); + pose_axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Radius", 0.03f, + "Radius of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); + + pose_arrow_color_property_ = new rviz_common::properties::ColorProperty( + "Pose Color", + QColor(255, 85, 255), + "Color to draw the poses.", + this, SLOT(updatePoseArrowColor())); + pose_arrow_shaft_length_property_ = new rviz_common::properties::FloatProperty( + "Shaft Length", + 0.1f, + "Length of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_head_length_property_ = new rviz_common::properties::FloatProperty( + "Head Length", 0.2f, + "Length of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_shaft_diameter_property_ = new rviz_common::properties::FloatProperty( + "Shaft Diameter", + 0.1f, + "Diameter of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_head_diameter_property_ = new rviz_common::properties::FloatProperty( + "Head Diameter", + 0.3f, + "Diameter of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + + static int count = 0; + std::string material_name = "LinesMaterialPathWithCost" + std::to_string(count++); + lines_material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name); +} + +PathWithCostDisplay::~PathWithCostDisplay() +{ + destroyObjects(); + destroyPoseAxesChain(); + destroyPoseArrowChain(); +} + +void PathWithCostDisplay::onInitialize() +{ + MFDClass::onInitialize(); + updateBufferLength(); +} + +void PathWithCostDisplay::reset() +{ + MFDClass::reset(); + updateBufferLength(); +} + + +void PathWithCostDisplay::allocateAxesVector( + std::vector & axes_vect, + size_t num) +{ + auto vector_size = axes_vect.size(); + if (num > vector_size) { + axes_vect.reserve(num); + for (auto i = vector_size; i < num; ++i) { + axes_vect.push_back( + new rviz_rendering::Axes( + scene_manager_, scene_node_, + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat())); + } + } else if (num < vector_size) { + for (auto i = num; i < vector_size; ++i) { + delete axes_vect[i]; + } + axes_vect.resize(num); + } +} + +void PathWithCostDisplay::allocateArrowVector( + std::vector & arrow_vect, + size_t num) +{ + auto vector_size = arrow_vect.size(); + if (num > vector_size) { + arrow_vect.reserve(num); + for (auto i = vector_size; i < num; ++i) { + arrow_vect.push_back(new rviz_rendering::Arrow(scene_manager_, scene_node_)); + } + } else if (num < vector_size) { + for (auto i = num; i < vector_size; ++i) { + delete arrow_vect[i]; + } + arrow_vect.resize(num); + } +} + +void PathWithCostDisplay::destroyPoseAxesChain() +{ + for (auto & axes_vect : axes_chain_) { + allocateAxesVector(axes_vect, 0); + } + axes_chain_.clear(); +} + +void PathWithCostDisplay::destroyPoseArrowChain() +{ + for (auto & arrow_vect : arrow_chain_) { + allocateArrowVector(arrow_vect, 0); + } + arrow_chain_.clear(); +} + +void PathWithCostDisplay::updateStyle() +{ + auto style = static_cast(style_property_->getOptionInt()); + + if (style == BILLBOARDS) { + line_width_property_->show(); + } else { + line_width_property_->hide(); + } + + updateBufferLength(); +} + +void PathWithCostDisplay::updateLineWidth() +{ + auto style = static_cast(style_property_->getOptionInt()); + float line_width = line_width_property_->getFloat(); + + if (style == BILLBOARDS) { + for (auto billboard_line : billboard_lines_) { + if (billboard_line) { + billboard_line->setLineWidth(line_width); + } + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updateOffset() +{ + scene_node_->setPosition(offset_property_->getVector() ); + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseStyle() +{ + auto pose_style = static_cast(pose_style_property_->getOptionInt()); + switch (pose_style) { + case AXES: + pose_axes_length_property_->show(); + pose_axes_radius_property_->show(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + break; + case ARROWS: + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->show(); + pose_arrow_shaft_length_property_->show(); + pose_arrow_head_length_property_->show(); + pose_arrow_shaft_diameter_property_->show(); + pose_arrow_head_diameter_property_->show(); + break; + default: + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + } + updateBufferLength(); +} + +void PathWithCostDisplay::updatePoseAxisGeometry() +{ + for (auto & axes_vect : axes_chain_) { + for (auto & axes : axes_vect) { + axes->set( + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat() ); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseArrowColor() +{ + QColor color = pose_arrow_color_property_->getColor(); + + for (auto & arrow_vect : arrow_chain_) { + for (auto & arrow : arrow_vect) { + arrow->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseArrowGeometry() +{ + for (auto & arrow_vect : arrow_chain_) { + for (auto & arrow : arrow_vect) { + arrow->set( + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::destroyObjects() +{ + // Destroy all simple lines, if any + for (auto manual_object : manual_objects_) { + manual_object->clear(); + scene_manager_->destroyManualObject(manual_object); + } + manual_objects_.clear(); + + // Destroy all billboards, if any + for (auto billboard_line : billboard_lines_) { + delete billboard_line; // also destroys the corresponding scene node + } + billboard_lines_.clear(); +} + +void PathWithCostDisplay::updateBufferLength() +{ + // Destroy all path objects + destroyObjects(); + + // Destroy all axes and arrows + destroyPoseAxesChain(); + destroyPoseArrowChain(); + + // Read options + auto buffer_length = static_cast(buffer_length_property_->getInt()); + auto style = static_cast(style_property_->getOptionInt()); + + // Create new path objects + switch (style) { + case LINES: // simple lines with fixed width of 1px + manual_objects_.reserve(buffer_length); + for (size_t i = 0; i < buffer_length; i++) { + auto manual_object = scene_manager_->createManualObject(); + manual_object->setDynamic(true); + scene_node_->attachObject(manual_object); + + manual_objects_.push_back(manual_object); + } + break; + + case BILLBOARDS: // billboards with configurable width + billboard_lines_.reserve(buffer_length); + for (size_t i = 0; i < buffer_length; i++) { + auto billboard_line = new rviz_rendering::BillboardLine(scene_manager_, scene_node_); + billboard_lines_.push_back(billboard_line); + } + break; + } + axes_chain_.resize(buffer_length); + arrow_chain_.resize(buffer_length); +} + +bool validateFloats(const nav2_msgs::msg::PathWithCost & msg) +{ + bool valid = true; + valid = valid && rviz_common::validateFloats(msg.poses); + return valid; +} + +void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) +{ +// std::cout << "ProcessMessage: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; + // Calculate index of oldest element in cyclic buffer + size_t bufferIndex = messages_received_ % buffer_length_property_->getInt(); + + auto style = static_cast(style_property_->getOptionInt()); + Ogre::ManualObject * manual_object = nullptr; + rviz_rendering::BillboardLine * billboard_line = nullptr; + + // Delete oldest element + switch (style) { + case LINES: + manual_object = manual_objects_[bufferIndex]; + manual_object->clear(); + break; + + case BILLBOARDS: + billboard_line = billboard_lines_[bufferIndex]; + billboard_line->clear(); + break; + } + + // Check if path contains invalid coordinate values + if (!validateFloats(*msg)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid " + "floating point " + "values (nans or infs)"); + return; + } + + // Lookup transform into fixed frame + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) { + setMissingTransformToFixedFrame(msg->header.frame_id); + return; + } + setTransformOk(); + + Ogre::Matrix4 transform(orientation); + transform.setTrans(position); + + switch (style) { + case LINES: +// updateManualObject(manual_object, msg, transform); + break; + + case BILLBOARDS: + updateBillBoardLine(billboard_line, msg, transform); + break; + } + +// updateManualObject(manual_object, msg, transform); + updatePoseMarkers(bufferIndex, msg, transform); + + context_->queueRender(); +} + +void PathWithCostDisplay::updateManualObject( + Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ +// auto color = color_property_->getOgreColor(); +// color.a = alpha_property_->getFloat(); +// std::cout << "Manual Object: Costs Size: " << msg->costs.size() << "Path Size: " << msg->poses +// .size() +// <estimateVertexCount(msg->poses.size()); + manual_object->begin( + lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP, "rviz_rendering"); + + for (size_t i = 0; i < (msg->poses.size() && msg->costs.size()); ++i) { + manual_object->position(transform * rviz_common::pointMsgToOgre(msg->poses[i].pose.position)); + +// int r = i * 5; +// +// while ( r > 255) { +// r = 255 - r; +// } +// +// Ogre::ColourValue color(r, 0, 0); + + [[maybe_unused]] auto r = msg->costs[i]; + + if ( r > 255 ) { + r = 255; + } + if (r < 0 ) { + r = 0; + } + + Ogre::ColourValue color(r, 255 - r, 0); +// Ogre::ColourValue color(10, 10, 10); + rviz_rendering::MaterialManager::enableAlphaBlending(lines_material_, color.a); + manual_object->colour(color); + } + + manual_object->end(); +} + +void PathWithCostDisplay::updateBillBoardLine( + rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ +// auto color = color_property_->getOgreColor(); +// color.a = alpha_property_->getFloat(); + + billboard_line->setNumLines(1); + billboard_line->setMaxPointsPerLine(static_cast(msg->poses.size())); + billboard_line->setLineWidth(line_width_property_->getFloat()); + + for (auto pose_stamped : msg->poses) { + Ogre::Vector3 xpos = transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position); + Ogre::ColourValue color(10, 10, 10); + billboard_line->addPoint(xpos, color); + } +} + +void PathWithCostDisplay::updatePoseMarkers( + size_t buffer_index, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) +{ +// std::cout << "UpdatePoseMarkers: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; + auto pose_style = static_cast(pose_style_property_->getOptionInt()); + auto & arrow_vect = arrow_chain_[buffer_index]; + auto & axes_vect = axes_chain_[buffer_index]; + + if (pose_style == AXES) { + updateAxesMarkers(axes_vect, msg, transform); + } + if (pose_style == ARROWS) { +// std::cout << "UpdatePoseMarkersCase: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; + updateArrowMarkers(arrow_vect, msg, transform); + } +} + +void PathWithCostDisplay::updateAxesMarkers( + std::vector & axes_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ + auto num_points = msg->poses.size(); + allocateAxesVector(axes_vect, num_points); + for (size_t i = 0; i < num_points; ++i) { + const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; + axes_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); + Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); + + // Extract the rotation part of the transformation matrix as a quaternion + Ogre::Quaternion transform_orientation = transform.linear(); + + axes_vect[i]->setOrientation(transform_orientation * orientation); + } +} + +void PathWithCostDisplay::updateArrowMarkers( + std::vector & arrow_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ +// std::cout << "Arrow Markers: Costs Size: " << msg->costs.size() << "Path Size: " << msg->poses +// .size() << std::endl; + auto num_points = msg->poses.size(); + allocateArrowVector(arrow_vect, num_points); + for (size_t i = 0; i < num_points; ++i) { + QColor color(msg->costs[i], 255-msg->costs[i], 0); + arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); + + arrow_vect[i]->set( + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); + const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; + arrow_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); + Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); + + // Extract the rotation part of the transformation matrix as a quaternion + Ogre::Quaternion transform_orientation = transform.linear(); + + Ogre::Vector3 dir(1, 0, 0); + dir = transform_orientation * orientation * dir; + arrow_vect[i]->setDirection(dir); + } +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::PathWithCostDisplay, rviz_common::Display) diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index c499d0796fb..d57db52e684 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -26,7 +26,7 @@ #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -84,7 +84,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -107,7 +107,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner float _tolerance; int _downsampling_factor; bool _downsample_costmap; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; bool _allow_unknown; int _max_iterations; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index c782fe41e08..f4c57d010a6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -25,7 +25,7 @@ #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -82,7 +82,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -118,7 +118,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner double _minimum_turning_radius_global_coords; std::string _motion_model_for_search; MotionModel _motion_model; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index f5207cd45c5..e39904995e6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -24,7 +24,7 @@ #include "nav2_smac_planner/utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -81,7 +81,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -108,7 +108,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner int _max_iterations; int _max_on_approach_iterations; float _tolerance; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index d896cd90b5f..530e1485afc 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -26,7 +26,7 @@ #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" #include "ompl/base/StateSpace.h" @@ -113,7 +113,7 @@ class Smoother * @return If smoothing was successful */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); @@ -127,7 +127,7 @@ class Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); @@ -158,7 +158,7 @@ class Smoother * @param path Path in which to look for cusps * @return Set of index pairs for each segment of the path in a given direction */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); + std::vector findDirectionalPathSegments(const nav2_msgs::msg::PathWithCost & path); /** * @brief Enforced minimum curvature boundary conditions on plan output @@ -170,7 +170,7 @@ class Smoother */ void enforceStartBoundaryConditions( const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment); @@ -184,7 +184,7 @@ class Smoother */ void enforceEndBoundaryConditions( const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment); @@ -228,7 +228,7 @@ class Smoother * @param reversing_segment Return if this is a reversing segment */ inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment); double min_turning_rad_, tolerance_, data_w_, smooth_w_; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 46dfa98fb25..af58f7df649 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -139,7 +139,7 @@ void SmacPlanner2D::configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " @@ -190,7 +190,7 @@ void SmacPlanner2D::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlanner2D::createPlan( +nav2_msgs::msg::PathWithCost SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -227,7 +227,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setGoal(mx_goal, my_goal, 0); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 2d0351c5b9d..13c99405dc4 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -214,7 +214,7 @@ void SmacPlannerHybrid::configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " @@ -266,7 +266,7 @@ void SmacPlannerHybrid::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlannerHybrid::createPlan( +nav2_msgs::msg::PathWithCost SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -322,7 +322,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setGoal(mx, my, orientation_bin_id); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 6d00a3479f5..424930430fe 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -57,7 +57,7 @@ void SmacPlannerLattice::configure( _costmap_ros = costmap_ros; _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str()); @@ -233,7 +233,7 @@ void SmacPlannerLattice::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlannerLattice::createPlan( +nav2_msgs::msg::PathWithCost SmacPlannerLattice::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -267,7 +267,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index a69e14011f3..c52e0d087d2 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -41,7 +41,7 @@ void Smoother::initialize(const double & min_turning_radius) } bool Smoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) { @@ -53,7 +53,7 @@ bool Smoother::smooth( steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time; bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -96,7 +96,7 @@ bool Smoother::smooth( } bool Smoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) @@ -110,8 +110,8 @@ bool Smoother::smoothImpl( double x_i, y_i, y_m1, y_ip1, y_i_org; unsigned int mx, my; - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; + nav2_msgs::msg::PathWithCost new_path = path; + nav2_msgs::msg::PathWithCost last_path = path; while (change >= tolerance_) { its += 1; @@ -214,7 +214,7 @@ void Smoother::setFieldByDim( } } -std::vector Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path) +std::vector Smoother::findDirectionalPathSegments(const nav2_msgs::msg::PathWithCost & path) { std::vector segments; PathSegment curr_segment; @@ -265,7 +265,7 @@ std::vector Smoother::findDirectionalPathSegments(const nav_msgs::m } void Smoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { double dx, dy, theta, pt_yaw; @@ -421,7 +421,7 @@ BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, It void Smoother::enforceStartBoundaryConditions( const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment) { @@ -467,7 +467,7 @@ void Smoother::enforceStartBoundaryConditions( void Smoother::enforceEndBoundaryConditions( const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment) { diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index d7d27f1d20d..e8f4dc7290d 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -46,7 +46,7 @@ class SmootherWrapper : public nav2_smac_planner::Smoother : nav2_smac_planner::Smoother(params) {} - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + std::vector findDirectionalPathSegmentsWrapper(nav2_msgs::msg::PathWithCost path) { return findDirectionalPathSegments(path); } @@ -106,7 +106,7 @@ TEST(SmootherTest, test_full_smoother) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // Convert to world coordinates and get length to compare to smoothed length - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = node->now(); plan.header.frame_id = "map"; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index b52ebf65219..22abbd84021 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -141,7 +141,7 @@ class SmootherServer : public nav2_util::LifecycleNode * @param path current path * return bool if the path is valid */ - bool validate(const nav_msgs::msg::Path & path); + bool validate(const nav2_msgs::msg::PathWithCost & path); // Our action server implements the SmoothPath action std::unique_ptr action_server_; @@ -151,7 +151,7 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr transform_listener_; // Publishers and subscribers - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Smoother Plugins pluginlib::ClassLoader lp_loader_; diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 090aa07af85..80eaee529fd 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -29,7 +29,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -82,7 +82,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: @@ -95,7 +95,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment); bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 2e8fea4a945..bd5bbaa97e4 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -29,7 +29,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -82,7 +82,7 @@ class SimpleSmoother : public nav2_core::Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: @@ -95,7 +95,7 @@ class SimpleSmoother : public nav2_core::Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp index 67d7296353f..3772c374181 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -28,7 +28,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -49,7 +49,7 @@ typedef std::vector::iterator PathIterator; typedef std::vector::reverse_iterator ReversePathIterator; inline std::vector findDirectionalPathSegments( - const nav_msgs::msg::Path & path) + const nav2_msgs::msg::PathWithCost & path) { std::vector segments; PathSegment curr_segment; @@ -92,7 +92,7 @@ inline std::vector findDirectionalPathSegments( } inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { double dx, dy, theta, pt_yaw; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 4e3588436a7..31fad2a45f6 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -102,7 +102,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) } // Initialize pubs & subs - plan_publisher_ = create_publisher("plan_smoothed", 1); + plan_publisher_ = create_publisher("plan_smoothed", 1); // Create the action server that we implement with our smoothPath method action_server_ = std::make_unique( @@ -352,7 +352,7 @@ void SmootherServer::smoothPlan() } } -bool SmootherServer::validate(const nav_msgs::msg::Path & path) +bool SmootherServer::validate(const nav2_msgs::msg::PathWithCost & path) { if (path.poses.empty()) { RCLCPP_WARN(get_logger(), "Requested path to smooth is empty"); diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 17fe63a926b..b812fb99b52 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -43,14 +43,14 @@ void SavitzkyGolaySmoother::configure( } bool SavitzkyGolaySmoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time.seconds(); bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -90,7 +90,7 @@ bool SavitzkyGolaySmoother::smooth( } bool SavitzkyGolaySmoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { // Must be at least 10 in length to enter function diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a828..2cec692a938 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -57,7 +57,7 @@ void SimpleSmoother::configure( } bool SimpleSmoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { auto costmap = costmap_sub_->getCostmap(); @@ -67,7 +67,7 @@ bool SimpleSmoother::smooth( bool success = true, reversing_segment; unsigned int segments_smoothed = 0; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -112,7 +112,7 @@ bool SimpleSmoother::smooth( } bool SimpleSmoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) @@ -126,8 +126,8 @@ bool SimpleSmoother::smoothImpl( double x_i, y_i, y_m1, y_ip1, y_i_org; unsigned int mx, my; - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; + nav2_msgs::msg::PathWithCost new_path = path; + nav2_msgs::msg::PathWithCost last_path = path; while (change >= tolerance_) { its += 1; diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index 89df4c3a4fa..624a29ef191 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -71,7 +71,7 @@ TEST(SmootherTest, test_sg_smoother_basics) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Test regular path, should see no effective change - nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + nav2_msgs::msg::PathWithCost straight_regular_path, straight_regular_path_baseline; straight_regular_path.header.frame_id = "map"; straight_regular_path.header.stamp = node->now(); straight_regular_path.poses.resize(11); @@ -144,7 +144,7 @@ TEST(SmootherTest, test_sg_smoother_noisey_path) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Given nominal irregular/noisey path, test that the output is shorter and smoother - nav_msgs::msg::Path noisey_path, noisey_path_baseline; + nav2_msgs::msg::PathWithCost noisey_path, noisey_path_baseline; noisey_path.header.frame_id = "map"; noisey_path.header.stamp = node->now(); noisey_path.poses.resize(11); @@ -202,7 +202,7 @@ TEST(SmootherTest, test_sg_smoother_noisey_path) // Test again with refinement, even shorter and smoother node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); - nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + nav2_msgs::msg::PathWithCost noisey_path_refined = noisey_path_baseline; EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); length = 0; @@ -249,7 +249,7 @@ TEST(SmootherTest, test_sg_smoother_reversing) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Test reversing / multiple segments via a cusp - nav_msgs::msg::Path cusp_path, cusp_path_baseline; + nav2_msgs::msg::PathWithCost cusp_path, cusp_path_baseline; cusp_path.header.frame_id = "map"; cusp_path.header.stamp = node->now(); cusp_path.poses.resize(22); diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index c8f7da8e871..9639451a518 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -44,7 +44,7 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother { } - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + std::vector findDirectionalPathSegmentsWrapper(nav2_msgs::msg::PathWithCost path) { return findDirectionalPathSegments(path); } @@ -88,7 +88,7 @@ TEST(SmootherTest, test_simple_smoother) smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); // Test that an irregular distributed path becomes more distributed - nav_msgs::msg::Path straight_irregular_path; + nav2_msgs::msg::PathWithCost straight_irregular_path; straight_irregular_path.header.frame_id = "map"; straight_irregular_path.header.stamp = node->now(); straight_irregular_path.poses.resize(11); @@ -128,7 +128,7 @@ TEST(SmootherTest, test_simple_smoother) } // Test regular path, should see no effective change - nav_msgs::msg::Path straight_regular_path; + nav2_msgs::msg::PathWithCost straight_regular_path; straight_regular_path.header = straight_irregular_path.header; straight_regular_path.poses.resize(11); straight_regular_path.poses[0].pose.position.x = 0.5; @@ -164,7 +164,7 @@ TEST(SmootherTest, test_simple_smoother) } // test shorter and curved if given a right angle - nav_msgs::msg::Path right_angle_path; + nav2_msgs::msg::PathWithCost right_angle_path; right_angle_path = straight_regular_path; straight_regular_path.poses[6].pose.position.x = 0.6; straight_regular_path.poses[6].pose.position.y = 0.5; @@ -181,7 +181,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01); // Test that collisions are rejected - nav_msgs::msg::Path collision_path; + nav2_msgs::msg::PathWithCost collision_path; collision_path.poses.resize(11); collision_path.poses[0].pose.position.x = 0.0; collision_path.poses[0].pose.position.y = 0.0; @@ -208,7 +208,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath); // test cusp / reversing segments - nav_msgs::msg::Path reversing_path; + nav2_msgs::msg::PathWithCost reversing_path; reversing_path.poses.resize(11); reversing_path.poses[0].pose.position.x = 0.5; reversing_path.poses[0].pose.position.y = 0.0; @@ -248,7 +248,7 @@ TEST(SmootherTest, test_simple_smoother) // test max iterations smoother->setMaxItsToInvalid(); - nav_msgs::msg::Path max_its_path; + nav2_msgs::msg::PathWithCost max_its_path; max_its_path.poses.resize(11); max_its_path.poses[0].pose.position.x = 0.5; max_its_path.poses[0].pose.position.y = 0.0; diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index d7afe9b55d9..ffa1035dfe9 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -56,7 +56,7 @@ class DummySmoother : public nav2_core::Smoother virtual void deactivate() {} virtual bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { assert(path.poses.size() == 2); diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index ba194b195d8..3a2c3862ad8 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -41,7 +41,7 @@ class UnknownErrorController : public nav2_core::Controller void deactivate() {} - void setPlan(const nav_msgs::msg::Path &) {} + void setPlan(const nav2_msgs::msg::PathWithCost &) {} virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d3..77b72d22e56 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -49,7 +49,7 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner void deactivate() override {} - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -59,7 +59,7 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner class StartOccupiedErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -69,7 +69,7 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -79,7 +79,7 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -89,7 +89,7 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -99,18 +99,18 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner class NoValidPathErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { - return nav_msgs::msg::Path(); + return nav2_msgs::msg::PathWithCost(); } }; class TimedOutErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -120,7 +120,7 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner class TFErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -130,7 +130,7 @@ class TFErrorPlanner : public UnknownErrorPlanner class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp index 45ad7f469e2..5bb5b740a3b 100644 --- a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp @@ -40,7 +40,7 @@ class UnknownErrorSmoother : public nav2_core::Smoother void deactivate() override {} bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) override { throw nav2_core::SmootherException("Unknown Smoother exception"); @@ -50,7 +50,7 @@ class UnknownErrorSmoother : public nav2_core::Smoother class TimeOutErrorSmoother : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::SmootherTimedOut("Smoother timedOut"); @@ -60,7 +60,7 @@ class TimeOutErrorSmoother : public UnknownErrorSmoother class SmoothedPathInCollision : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::SmoothedPathInCollision("Smoother path in collision"); @@ -70,7 +70,7 @@ class SmoothedPathInCollision : public UnknownErrorSmoother class FailedToSmoothPath : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::FailedToSmoothPath("Failed to smooth path"); @@ -80,7 +80,7 @@ class FailedToSmoothPath : public UnknownErrorSmoother class InvalidPath : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::InvalidPath("Invalid path"); diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 0a2368bab6b..298bd9b60b9 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -397,7 +397,7 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } -bool PlannerTester::isPathValid(nav_msgs::msg::Path & path) +bool PlannerTester::isPathValid(nav2_msgs::msg::PathWithCost & path) { planner_tester_->setCostmap(costmap_.get()); // create a fake service request diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index b849e899087..cb46104557e 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -77,7 +77,7 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer bool createPath( const geometry_msgs::msg::PoseStamped & goal, - nav_msgs::msg::Path & path) + nav2_msgs::msg::PathWithCost & path) { geometry_msgs::msg::PoseStamped start; if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) { @@ -129,7 +129,7 @@ class PlannerTester : public rclcpp::Node { public: using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; - using ComputePathToPoseResult = nav_msgs::msg::Path; + using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; PlannerTester(); ~PlannerTester(); @@ -157,7 +157,7 @@ class PlannerTester : public rclcpp::Node const unsigned int number_tests, const float acceptable_fail_ratio); - bool isPathValid(nav_msgs::msg::Path & path); + bool isPathValid(nav2_msgs::msg::PathWithCost & path); private: void setCostmap(); diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp index 27b953efcf9..a68d7b10a91 100644 --- a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; TEST(testSimpleCostmaps, testSimpleCostmaps) { diff --git a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp index 958891e02ad..12c9bafd0bc 100644 --- a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp +++ b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp @@ -30,7 +30,7 @@ TEST(testIsPathValid, testIsPathValid) planner_tester->activate(); planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // empty path bool is_path_valid = planner_tester->isPathValid(path); diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3a..3475eb1dcdb 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -29,9 +29,9 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; -void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) +void callback(const nav2_msgs::msg::PathWithCost::ConstSharedPtr /*grid*/) { } @@ -120,7 +120,7 @@ TEST(testPluginMap, Failures) rclcpp_lifecycle::State state; obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0)); obj->onConfigure(state); - obj->create_subscription( + obj->create_subscription( "plan", rclcpp::SystemDefaultsQoS(), callback); geometry_msgs::msg::PoseStamped start; diff --git a/nav2_system_tests/src/planning/test_planner_random_node.cpp b/nav2_system_tests/src/planning/test_planner_random_node.cpp index 3e072414c78..6a51a0abe4f 100644 --- a/nav2_system_tests/src/planning/test_planner_random_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_random_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; TEST(testWithHundredRandomEndPoints, testWithHundredRandomEndPoints) { diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index bcbff2ae7c0..598637e6301 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -26,7 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_core/planner_exceptions.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -54,7 +54,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner void deactivate() override; - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -78,7 +78,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner * @brief the function responsible for calling the algorithm and retrieving a path from it * @return global_path is the planned path to be taken */ - void getPlan(nav_msgs::msg::Path & global_path); + void getPlan(nav2_msgs::msg::PathWithCost & global_path); /** * @brief interpolates points between the consecutive waypoints of the path @@ -86,7 +86,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner * @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution) * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other */ - static nav_msgs::msg::Path linearInterpolation( + static nav2_msgs::msg::PathWithCost linearInterpolation( const std::vector & raw_path, const double & dist_bw_points); diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index c509afb065c..65cd5be08a8 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -84,11 +84,11 @@ void ThetaStarPlanner::deactivate() RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); } -nav_msgs::msg::Path ThetaStarPlanner::createPlan( +nav2_msgs::msg::PathWithCost ThetaStarPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { - nav_msgs::msg::Path global_path; + nav2_msgs::msg::PathWithCost global_path; auto start_time = std::chrono::steady_clock::now(); // Corner case of start and goal beeing on the same cell @@ -176,7 +176,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } -void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +void ThetaStarPlanner::getPlan(nav2_msgs::msg::PathWithCost & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { @@ -192,11 +192,11 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) global_path.header.frame_id = global_frame_; } -nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( +nav2_msgs::msg::PathWithCost ThetaStarPlanner::linearInterpolation( const std::vector & raw_path, const double & dist_bw_points) { - nav_msgs::msg::Path pa; + nav2_msgs::msg::PathWithCost pa; geometry_msgs::msg::PoseStamped p1; for (unsigned int j = 0; j < raw_path.size() - 1; j++) { diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index c2360d9f871..a50ccd27487 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -163,7 +163,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { planner_2d->configure(life_node, "test", nullptr, costmap_ros); planner_2d->activate(); - nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + nav2_msgs::msg::PathWithCost path = planner_2d->createPlan(start, goal); EXPECT_GT(static_cast(path.poses.size()), 0); // test if the goal is unsafe diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 884b7b366ed..8215d678392 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_util { @@ -167,7 +167,7 @@ inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getComp * subset of the path. * @return double Path length */ -inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0) +inline double calculate_path_length(const nav2_msgs::msg::PathWithCost & path, size_t start_index = 0) { if (start_index + 1 >= path.poses.size()) { return 0.0; diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 38f27ad3044..e8de718c241 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -16,7 +16,7 @@ #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "gtest/gtest.h" using nav2_util::geometry_utils::euclidean_distance; @@ -84,7 +84,7 @@ TEST(GeometryUtils, euclidean_distance_pose_2d) TEST(GeometryUtils, calculate_path_length) { - nav_msgs::msg::Path straight_line_path; + nav2_msgs::msg::PathWithCost straight_line_path; size_t nb_path_points = 10; float distance_between_poses = 2.0; float current_x_loc = 0.0; @@ -106,7 +106,7 @@ TEST(GeometryUtils, calculate_path_length) calculate_path_length(straight_line_path, straight_line_path.poses.size()), 0.0, 1e-5); - nav_msgs::msg::Path circle_path; + nav2_msgs::msg::PathWithCost circle_path; float polar_distance = 2.0; uint32_t current_polar_angle_deg = 0; constexpr float pi = 3.14159265358979; diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index e5c59aa20b6..2d62af0410d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -24,7 +24,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav2_msgs/msg/missed_waypoint.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index aa9a5c83fe9..3c3d1a950ca 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -55,7 +55,7 @@ index c7a90bcb..6f93edbf 100644 // Get plan from start -> goal + auto planning_start = steady_clock_.now(); - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav2_msgs::msg::PathWithCost curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + auto planning_duration = steady_clock_.now() - planning_start; + result->planning_time = planning_duration;