ros-navigation · jwallace42 · Jun 15, 2022 · Jul 25, 2022 · Jul 25, 2022 · Jul 25, 2022
diff --git a/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -84,7 +84,7 @@ class ComputePathThroughPosesAction
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
-        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
+        BT::OutputPort<nav2_msgs::msg::PathWithCost>("path", "Path created by ComputePathThroughPoses node"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "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
@@ -18,7 +18,7 @@
 #include <string>
 
 #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<nav2_msgs::action::ComputePa
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
-        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+        BT::OutputPort<nav2_msgs::msg::PathWithCost>("path", "Path created by ComputePathToPose node"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "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
@@ -81,7 +81,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
   {
     return providedBasicPorts(
       {
-        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+        BT::InputPort<nav2_msgs::msg::PathWithCost>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::OutputPort<ActionResult::_error_code_type>(

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
@@ -19,7 +19,7 @@
 #include <string>
 
 #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<nav2_msgs::acti
   {
     return providedBasicPorts(
       {
-        BT::InputPort<nav_msgs::msg::Path>("unsmoothed_path", "Path to be smoothed"),
+        BT::InputPort<nav2_msgs::msg::PathWithCost>("unsmoothed_path", "Path to be smoothed"),
         BT::InputPort<double>("max_smoothing_duration", 3.0, "Maximum smoothing duration"),
         BT::InputPort<bool>(
           "check_for_collisions", false,
           "If true collision check will be performed after smoothing"),
         BT::InputPort<std::string>("smoother_id", ""),
-        BT::OutputPort<nav_msgs::msg::Path>(
+        BT::OutputPort<nav2_msgs::msg::PathWithCost>(
           "smoothed_path",
           "Path smoothed by SmootherServer node"),
         BT::OutputPort<double>("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
@@ -19,7 +19,7 @@
 #include <memory>
 #include <string>
 
-#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<nav_msgs::msg::Path>("input_path", "Original Path"),
-      BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
+      BT::InputPort<nav2_msgs::msg::PathWithCost>("input_path", "Original Path"),
+      BT::OutputPort<nav2_msgs::msg::PathWithCost>("output_path", "Path truncated to a certain distance"),
       BT::InputPort<double>("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
@@ -20,7 +20,7 @@
 #include <string>
 #include <limits>
 
-#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<nav_msgs::msg::Path>("input_path", "Original Path"),
-      BT::OutputPort<nav_msgs::msg::Path>(
+      BT::InputPort<nav2_msgs::msg::PathWithCost>("input_path", "Original Path"),
+      BT::OutputPort<nav2_msgs::msg::PathWithCost>(
         "output_path", "Path truncated to a certain distance around robot"),
       BT::InputPort<double>(
         "distance_forward", 8.0,
@@ -115,8 +115,8 @@ class TruncatePathLocal : public BT::ActionNodeBase
 
   std::shared_ptr<tf2_ros::Buffer> 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
@@ -57,8 +57,9 @@ class IsPathValidCondition : public BT::ConditionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+      BT::InputPort<nav2_msgs::msg::PathWithCost>("path", "Path to Check"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+      BT::InputPort<bool>("consider_cost_change", true, "Consider cost changes")
     };
   }
 

diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/...avior_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<double>("seconds", 1.0, "Seconds"),
-      BT::InputPort<nav_msgs::msg::Path>("path")
+      BT::InputPort<nav2_msgs::msg::PathWithCost>("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
@@ -20,7 +20,7 @@
 #include <limits>
 
 #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<nav_msgs::msg::Path>("path", "Planned Path"),
+      BT::InputPort<nav2_msgs::msg::PathWithCost>("path", "Planned Path"),
       BT::InputPort<double>(
         "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<double>::max();
   double length_factor_ = std::numeric_limits<double>::max();
   rclcpp::Node::SharedPtr node_;

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -245,6 +245,7 @@
     <Condition ID="IsPathValid">
       <input_port name="path"> Path to validate </input_port>
       <input_port name="server_timeout"> Server timeout </input_port>
+      <input_port name="consider_cost_change"> Check for significant cost change</input_port>
     </Condition>
 
     <!-- ############################### CONTROL NODES ################################ -->

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
@@ -48,15 +48,15 @@ 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;
 }
 
 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
@@ -47,15 +47,15 @@ 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;
 }
 
 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
@@ -58,7 +58,7 @@ void FollowPathAction::on_wait_for_result(
   std::shared_ptr<const Action::Feedback>/*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
@@ -16,7 +16,7 @@
 #include <memory>
 #include <limits>
 
-#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
@@ -15,9 +15,8 @@
 
 #include <string>
 #include <memory>
-#include <limits>
 
-#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
@@ -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<geometry_msgs::msg::PoseStamped>(
     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
@@ -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<nav2_msgs::srv::IsPathValid::Request>();
 
   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
@@ -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
@@ -31,24 +31,24 @@ 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 &&
          old_path.poses.back() == new_path.poses.back();
 }
 
 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) >