Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
yigitboracagiran opened this issue Oct 24, 2024 · 20 comments
Open

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

yigitboracagiran opened this issue Oct 24, 2024 · 20 comments

Comments

@yigitboracagiran
Copy link

yigitboracagiran commented Oct 24, 2024 •
edited
Loading

Bug report

Required Info:

  • Operating System:
    • Ubuntu 22.04
  • ROS2 Version:
    • Humble
  • DDS implementation:
    • CycloneDDS

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

378824118-3af2c407-5152-4e3d-9bc0-53be97746100.mp4
@SteveMacenski
Copy link
Member

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. #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
Copy link
Author

yigitboracagiran commented Oct 25, 2024 •
edited
Loading

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
Copy link
Member

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
Copy link

I think this behavior is different than #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 de4fd78 to get it to work. I see you are using ros2 humble version, you may miss the commit above.

@SteveMacenski
Copy link
Member

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
Copy link
Author

yigitboracagiran commented Oct 30, 2024 •
edited
Loading

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...

new.example.mp4

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
Copy link
Member

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
Copy link
Author

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

@SteveMacenski
Copy link
Member

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

@yigitboracagiran
Copy link
Author

Any progress?

@padhupradheep
Copy link
Member

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

@padhupradheep
Copy link
Member

padhupradheep commented Nov 22, 2024 •
edited
Loading

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.. 🤯

@yigitboracagiran
Copy link
Author

yigitboracagiran commented Nov 22, 2024 •
edited
Loading

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

@padhupradheep
Copy link
Member

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

@yigitboracagiran
Copy link
Author

yigitboracagiran commented Nov 22, 2024 •
edited
Loading

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

@yigitboracagiran
Copy link
Author

yigitboracagiran commented Nov 22, 2024 •
edited
Loading

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

new_video.online-video-cutter.com.mp4

@padhupradheep
Copy link
Member

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

@padhupradheep padhupradheep mentioned this issue Nov 22, 2024
Open
@padhupradheep
Copy link
Member

Thanks for the help @yigitboracagiran

@padhupradheep
Copy link
Member

For humble, we have to do this back port as suggested by @chanhhoang99 for sure: #4729 (comment)

@SteveMacenski
Copy link
Member

SteveMacenski commented Nov 22, 2024 •
edited
Loading

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
4 participants
@SteveMacenski @padhupradheep @chanhhoang99 @yigitboracagiran