0
$\begingroup$

ROS Version: Humble

I'm trying to import a new .xml file to my bt_navigator in Turtlebot3 Simulation. I'm following this tutorial.

I've tried to copy the same .xml file from the tutorial but I had to remove error_code_id's because Humble Nav2 doesn't support error_code_id ports.

You can check the providedPorts() of the main and Humble branches from these links:

Main Branch

Humble Branch

The rest of the file is the same... I named my file nav_to_pose_and_pause_near_goal_obstacle.xml.

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="3.0" length_factor="2.0">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="5.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>

My problem is that 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.

You can watch the video from this link.

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
Improve this question
$\endgroup$
0

1 Answer 1

Reset to default
0
$\begingroup$

It was related to a commit which hasn't done yet on the Humble version. Not it's fixed. You can see the details in the link.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.