search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.63k stars 1.31k forks source link

Navigate To Pose and Pause Near Goal Obstacle Doesn't Work #4729

Closed yigitboracagiran closed 2 days ago

yigitboracagiran commented 1 month ago

Bug report

Required Info:

Steps to reproduce issue

1- I've changed the default_nav_to_pose_bt_xml from bt_navigator's parameters and set the value to <path_to>nav_to_pose_and_pause_near_goal_obstacle.xml

nav_to_pose_and_pause_near_goal_obstacle.xml:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <ReactiveSequence name="MonitorAndFollowPath">
          <PathLongerOnApproach path="{path}" prox_len="10.0" length_factor="2.0">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="10.0"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="{selected_controller}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </RecoveryNode>
        </ReactiveSequence>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5.0"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

2- Running ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

3- Running ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True

4- Give a NAV2 Goal to the robot.

5- Put an obstacle in the path.

Expected behavior

Robot should wait for wait_duration parameter inside the CancelingControlAndWait.

Actual behavior

I can't make the robot to wait. When I put an obstacle dynamically in Gazebo, it recalculates the path and it moves to the goal, instead of waiting for wait_duration parameter inside the CancelingControlAndWait.

Additional information

https://github.com/user-attachments/assets/27f89ed9-9ffd-40f1-9685-b78a68705abd

SteveMacenski commented 1 month ago

Do you see logging that the control is canceled and the with behavior action is being called? That section of the BT looks correct to me, so I’m wondering if that section of tree is being called. https://github.com/ros-navigation/navigation2/issues/4681 files a ticket that the path longer on approach has some problems that @padhupradheep is looking at. If you can see that these are not being called in your ros logs, then it’s the case that the condition isn’t working in your case and we should migrate the conversation over to that ticket to consolidate.

Or, I think my memory serves that there was another issue with that condition node, @padhupradheep is there something we (cough I cough) should have back ported but didn’t to humble?

yigitboracagiran commented 1 month ago

I added RCLCPP_INFO's but I can't get any log from the node...

inline BT::NodeStatus PathLongerOnApproach::tick()
{

  getInput("path", new_path_);
  getInput("prox_len", prox_len_);
  getInput("length_factor", length_factor_);

  if (status() == BT::NodeStatus::IDLE) {
    // Reset the starting point since we're starting a new iteration of
    // PathLongerOnApproach (moving from IDLE to RUNNING)
    first_time_ = true;
  }

  setStatus(BT::NodeStatus::RUNNING);

  // Check if the path is updated and valid, compare the old and the new path length,
  // given the goal proximity and check if the new path is longer
  if (isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_, prox_len_) &&
    isNewPathLonger(new_path_, old_path_, length_factor_) && !first_time_)
  {
    RCLCPP_INFO(node_->get_logger(), "isPathUpdated(new_path_, old_path_): %d", isPathUpdated(new_path_, old_path_));
    RCLCPP_INFO(node_->get_logger(), "isRobotInGoalProximity(old_path_, prox_len_): %d", isRobotInGoalProximity(old_path_, prox_len_));
    RCLCPP_INFO(node_->get_logger(), "isNewPathLonger(new_path_, old_path_, length_factor_): %d", isNewPathLonger(new_path_, old_path_, length_factor_));
    RCLCPP_INFO(node_->get_logger(), "!first_time_: %d", !first_time_);
    const BT::NodeStatus child_state = child_node_->executeTick();
    switch (child_state) {
      case BT::NodeStatus::RUNNING:
        return BT::NodeStatus::RUNNING;
      case BT::NodeStatus::SUCCESS:
        old_path_ = new_path_;
        return BT::NodeStatus::SUCCESS;
      case BT::NodeStatus::FAILURE:
        old_path_ = new_path_;
        return BT::NodeStatus::FAILURE;
      default:
        old_path_ = new_path_;
        return BT::NodeStatus::FAILURE;
    }
  }
  old_path_ = new_path_;
  first_time_ = false;
  return BT::NodeStatus::SUCCESS;
}

My Logs...

[rviz2-2] Start navigation
[rviz2-2] [INFO] [1729833615.215169490] [rviz_navigation_dialog_action_client]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[component_container_isolated-1] [INFO] [1729833615.223856384] [bt_navigator]: Received goal preemption request
[component_container_isolated-1] [INFO] [1729833615.224028176] [bt_navigator]: Begin navigating from current location (-1.58, -0.54) to (-1.60, -0.53)
[component_container_isolated-1] [INFO] [1729833615.764905618] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833615.872068510] [controller_server]: Reached the goal!
[component_container_isolated-1] [INFO] [1729833615.903716829] [bt_navigator]: Goal succeeded
[rviz2-2] Start navigation
[rviz2-2] [INFO] [1729833618.070878386] [rviz_navigation_dialog_action_client]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[component_container_isolated-1] [INFO] [1729833618.071428995] [bt_navigator]: Begin navigating from current location (-1.44, -0.61) to (1.72, -0.50)
[component_container_isolated-1] [INFO] [1729833618.092584848] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [INFO] [1729833619.192929624] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833620.192965232] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833621.192955122] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833622.292949293] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833623.292972074] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833624.292968832] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833625.392950488] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1729833625.602956268] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1729833625.602995757] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1729833625.603168948] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [INFO] [1729833626.392963703] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1729833626.902219404] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1729833626.902248553] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1729833626.902497489] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1729833627.005680683] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1729833627.005717964] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1729833627.006040783] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [INFO] [1729833627.392952896] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833628.392951132] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833629.492967161] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833630.492958709] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833631.492949578] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833632.592951968] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833633.592961333] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833634.592960129] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833635.692975286] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833636.692957272] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833637.692953681] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833638.692956073] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833639.792960641] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833640.792969005] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833641.792957142] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833642.892948724] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833643.892967444] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833644.892963180] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833645.992973924] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833646.992975565] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833647.992965296] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833648.992960271] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1729833649.904044240] [controller_server]: Reached the goal!
[component_container_isolated-1] [INFO] [1729833649.941624765] [bt_navigator]: Goal succeeded
SteveMacenski commented 1 month ago

That is inside of the if statement, so that doesn’t generally show what’s being outputted by each. I think this relates to the ticket I linked to above. Maybe we should consolidate the discussion there with your findings of the logging before the if statement?

chanhhoang99 commented 1 month ago

I think this behavior is different than https://github.com/ros-navigation/navigation2/issues/4681. The video shows that it does not wait when placing an obstacle, but in my case it is waiting as expected. If i remember correctly, I have to build my own nav2 rolling version to update 1 commit https://github.com/ros-navigation/navigation2/commit/de4fd7863610fb8aceb44893a9d2368596eaa2a1 to get it to work. I see you are using ros2 humble version, you may miss the commit above.

SteveMacenski commented 1 month ago

Got it! I just checked and that was not backported to Humble yet. I plan to do a sync once I’m back from Europe for ROSCon and ROS-I the week of Nov 3. If you’re in a rush, you can open a backport PR for that commit to Humble and I can merge it into the branch so that mainline source works for you!

yigitboracagiran commented 1 month ago

I changed my cpp file to this commit https://github.com/ros-navigation/navigation2/commit/de4fd7863610fb8aceb44893a9d2368596eaa2a1

My new cpp file:

#include <string>
#include <memory>
#include <vector>
#include "nav2_util/geometry_utils.hpp"

#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"

namespace nav2_behavior_tree
{

PathLongerOnApproach::PathLongerOnApproach(
  const std::string & name,
  const BT::NodeConfiguration & conf)
: BT::DecoratorNode(name, conf)
{
  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
}

bool PathLongerOnApproach::isPathUpdated(
  nav_msgs::msg::Path & new_path,
  nav_msgs::msg::Path & old_path)
{
  return old_path.poses.size() != 0 &&
         new_path.poses.size() != 0 &&
         new_path.poses.size() != old_path.poses.size() &&
         old_path.poses.back().pose.position == new_path.poses.back().pose.position;
}

bool PathLongerOnApproach::isRobotInGoalProximity(
  nav_msgs::msg::Path & 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,
  double & length_factor)
{
  return nav2_util::geometry_utils::calculate_path_length(new_path, 0) >
         length_factor * nav2_util::geometry_utils::calculate_path_length(
    old_path, 0);
}

inline BT::NodeStatus PathLongerOnApproach::tick()
{
  getInput("path", new_path_);
  getInput("prox_len", prox_len_);
  getInput("length_factor", length_factor_);

  if (first_time_ == false) {
    if (old_path_.poses.empty() || new_path_.poses.empty() ||
      old_path_.poses.back().pose != new_path_.poses.back().pose)
    {
      first_time_ = true;
    }
  }
  setStatus(BT::NodeStatus::RUNNING);

  // Check if the path is updated and valid, compare the old and the new path length,
  // given the goal proximity and check if the new path is longer
  if (isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_, prox_len_) &&
    isNewPathLonger(new_path_, old_path_, length_factor_) && !first_time_)
  {
    RCLCPP_INFO(node_->get_logger(), "isPathUpdated(new_path_, old_path_): %d", isPathUpdated(new_path_, old_path_));
    RCLCPP_INFO(node_->get_logger(), "isRobotInGoalProximity(old_path_, prox_len_): %d", isRobotInGoalProximity(old_path_, prox_len_));
    RCLCPP_INFO(node_->get_logger(), "isNewPathLonger(new_path_, old_path_, length_factor_): %d", isNewPathLonger(new_path_, old_path_, length_factor_));
    RCLCPP_INFO(node_->get_logger(), "!first_time_: %d", !first_time_);
    const BT::NodeStatus child_state = child_node_->executeTick();
    switch (child_state) {
      // case BT::NodeStatus::SKIPPED:
      case BT::NodeStatus::RUNNING:
        return child_state;
      case BT::NodeStatus::SUCCESS:
      case BT::NodeStatus::FAILURE:
        old_path_ = new_path_;
        resetChild();
        return child_state;
      default:
        old_path_ = new_path_;
        return BT::NodeStatus::FAILURE;
    }
  }
  old_path_ = new_path_;
  first_time_ = false;
  return BT::NodeStatus::SUCCESS;
}

}  // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<nav2_behavior_tree::PathLongerOnApproach>("PathLongerOnApproach");
}

But the problem is still the same...

https://github.com/user-attachments/assets/172bf4bf-c6fb-485f-affc-41fbeb7ec18e

Output:

[rviz2-2] Start navigation
[rviz2-2] [INFO] [1730266531.482031722] [rviz_navigation_dialog_action_client]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[component_container_isolated-1] [INFO] [1730266531.482504977] [bt_navigator]: Begin navigating from current location (-0.70, -0.53) to (1.86, 0.57)
[component_container_isolated-1] [INFO] [1730266531.503922533] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [INFO] [1730266532.604224916] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266533.604214157] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266534.604220703] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266535.704211535] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266536.704227690] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266537.704227056] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266538.804227549] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266539.804220321] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266540.716467309] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266540.716521388] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266540.716883328] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [WARN] [1730266540.754324102] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[component_container_isolated-1] [WARN] [1730266540.754426620] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (1.86, 0.57)
[component_container_isolated-1] [WARN] [1730266540.754488324] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266540.773031140] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [INFO] [1730266540.804227646] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266540.804526459] [controller_server]: Invalid path, Path is empty.
[component_container_isolated-1] [WARN] [1730266540.804592820] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266540.833006603] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1730266540.834982198] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [ERROR] [1730266540.835255175] [controller_server]: Invalid path, Path is empty.
[component_container_isolated-1] [WARN] [1730266540.835359572] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [ERROR] [1730266540.873347111] [bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!
[component_container_isolated-1] [INFO] [1730266540.873805445] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1730266540.874650136] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [WARN] [1730266540.876956371] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1730266540.886366689] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.1010 Hz
[component_container_isolated-1] [INFO] [1730266540.908247710] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [INFO] [1730266542.008804756] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266543.008820217] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266544.008805716] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266545.108802710] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266545.920594851] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266545.920649136] [DWBLocalPlanner]: 0.52: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [ERROR] [1730266545.920659046] [DWBLocalPlanner]: 0.48: Oscillation/Trajectory is oscillating.
[component_container_isolated-1] [WARN] [1730266545.921050941] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.019926119] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.019968430] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.020321885] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [INFO] [1730266546.108803110] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266546.118328800] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.118342902] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.118510338] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.220743076] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.220770851] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.221025936] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.221061829] [controller_server]: Controller patience exceeded
[component_container_isolated-1] [WARN] [1730266546.221083189] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266546.237132508] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1730266546.237823310] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [ERROR] [1730266546.293865347] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.294022063] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.294658224] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.348316175] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.348345457] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.348632277] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.446370461] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.446397070] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.446666437] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.550339290] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.550362414] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.550582704] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.645856881] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.645883341] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266546.646080030] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266546.646115949] [controller_server]: Controller patience exceeded
[component_container_isolated-1] [WARN] [1730266546.646135643] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266546.657392826] [behavior_server]: Running spin
[component_container_isolated-1] [INFO] [1730266546.657546892] [behavior_server]: Turning 1.57 for spin behavior.
[component_container_isolated-1] [WARN] [1730266547.857760804] [behavior_server]: Collision Ahead - Exiting Spin
[component_container_isolated-1] [WARN] [1730266547.857802571] [behavior_server]: spin failed
[component_container_isolated-1] [WARN] [1730266547.857832306] [behavior_server]: [spin] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266547.887353458] [behavior_server]: Running wait
[component_container_isolated-1] [INFO] [1730266552.887533730] [behavior_server]: wait completed successfully
[component_container_isolated-1] [INFO] [1730266552.937557523] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [INFO] [1730266554.037981997] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266555.037994848] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266556.037980742] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266557.137982118] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266558.137991514] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266559.137977963] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266560.237986032] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266561.237986948] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266562.237980216] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266563.237979916] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266564.337987160] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266565.337971134] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266566.337974600] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266567.437981185] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1730266567.944863530] [controller_server]: Reached the goal!
[component_container_isolated-1] [INFO] [1730266567.977014321] [bt_navigator]: Goal succeeded
padhupradheep commented 1 month ago

Thanks for reporting @yigitboracagiran and it is a good find.

I'd check it out and get back to you soon. Probably in few days.

@SteveMacenski I think we cannot just back port the changes made for Jazzy to humble because of the difference in BT versions and as well as the node functionalities. This needs to be checked carefully. I'll take care of both the issues related to the node and get back ASAP.

yigitboracagiran commented 1 month ago

Thank you, I'll be waiting for your answer...

SteveMacenski commented 3 weeks ago

Thanks @padhupradheep -- let me know how it goes! I'm just now back from Europe from ROSCon and associated events.

yigitboracagiran commented 1 week ago

Any progress?

padhupradheep commented 1 week ago

Sorry for the delay, I'm swamped for few more weeks. I'd try to get to the bottom of it ASAP.

padhupradheep commented 1 week ago

One quick point @yigitboracagiran would be great if you can test it out.. have you tried without composition? I vaguely remember having this problem with composition once I tried it with the turtlebot like an year back. However, when I did the deployment on a real robot(doesn't use composition), few months back, I never faced the issue.

And I don't remember why I didn't take care of it then.. :exploding_head:

yigitboracagiran commented 1 week ago

How can I try it without composition, can you help me

padhupradheep commented 1 week ago

ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True use_composition:=False

yigitboracagiran commented 1 week ago

I think now something has changed, I'm analyzing...

yigitboracagiran commented 6 days ago

Why has it changed? How does use_composition has changed the result?

https://github.com/user-attachments/assets/c0b053dd-588c-4c79-984a-100226c6f27c

padhupradheep commented 6 days ago

@SteveMacenski have you seen any other behaviors exhibiting this problem with composition ?

padhupradheep commented 6 days ago

Thanks for the help @yigitboracagiran

padhupradheep commented 6 days ago

For humble, we have to do this back port as suggested by @chanhhoang99 for sure: https://github.com/ros-navigation/navigation2/issues/4729#issuecomment-2439841384

SteveMacenski commented 6 days ago

I've never seen anything behaviorally change because of composition, no. Only computation use and latencies.

padhupradheep commented 4 days ago

Thanks @SteveMacenski, @yigitboracagiran @chanhhoang99 Please give me some more time like I told earlier for both the issues. I'd get back to you on this.

chanhhoang99 commented 3 days ago

Thanks @padhupradheep , in case you need more information, I can show real setup where the issue is fairly easy to reproduce.

padhupradheep commented 3 days ago 
[component_container_isolated-1] [INFO] [1730266539.804220321] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266540.716467309] [DWBLocalPlanner]: No valid trajectories out of 860! 
[component_container_isolated-1] [ERROR] [1730266540.716521388] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-1] [WARN] [1730266540.716883328] [controller_server]: No valid trajectories out of 860! 
[component_container_isolated-1] [WARN] [1730266540.754324102] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[component_container_isolated-1] [WARN] [1730266540.754426620] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (1.86, 0.57)
[component_container_isolated-1] [WARN] [1730266540.754488324] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266540.773031140] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [INFO] [1730266540.804227646] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1730266540.804526459] [controller_server]: Invalid path, Path is empty.
[component_container_isolated-1] [WARN] [1730266540.804592820] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1730266540.833006603] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap

Looking here at the logs closely, I can see DWB complains about the obstacle in front. Followed by that the global planner was not able to plan any further, triggering the request to clear the entire Global Costmap. Then on ticking the PathLongerOnApproach, it shouldn't have had any effect, since there is no proper valid path. Then on reaching the FollowPath the controller complains about the invalid path. I think this is also not an issue with the existing BT.

Let me analyze it further..

padhupradheep commented 2 days ago

@yigitboracagiran could you please try once again with composition and tell me if you can reproduce the issue?

Because, I cannot by no ways produce your issue even with composition enabled. This will be a great help

yigitboracagiran commented 2 days ago

Do you still need any help? I saw that you did a commit 9 hours ago... And with that commit, I couldn't reproduce the issue so far. I'll keep trying...

padhupradheep commented 2 days ago

Great, I'm closing the issue now. Feel free to write back if you have anything further