Added promixity BT node and BT tree

[Jakubach]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(Gazebo simulation of custom robot)
Does this PR contain AI generated software?	(No)

---

Description of contribution in a few bullet points

BT Node that return if the robot is in the goal proximity
PR related to the Steve advice on robotics stack:
https://robotics.stackexchange.com/questions/112576/maneuvers-on-paths-end-point-with-navigation2-smac-lattice-planner/112577?noredirect=1#comment48615_112577

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

Otherwise, needs a PR to add this new BT node to the migration guide and a configuration guide page to the BT XML nodes for this node. Looks good to start with! Make sure to check out the failing CI jobs for linting and other errors

[SteveMacenski]

EOF line

[Jakubach]

What should I correct in this case?

[SteveMacenski]

This needs test coverage

[SteveMacenski]

Need to add this to the nav2 node index

[SteveMacenski]

That's checking the last path length, but not the robot's proximity. If you replan every 10 seconds (or only on events) then this wouldn't tell you much about the robot's proximity to the goal based on the last path marker.

I think this either needs to

• find the path's closest point like we do in the Controller plugins [1] which means we need to track the last path index to know where to search starting from on each call up to some maximum distance
• just be based on a distance check from the robot's current pose -- which has problems if the path have overlapping segments from Navigate Through Poses.

[1] https://github.com/ros-navigation/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp#L61-L79

[Jakubach]

I am trying to implement the first idea but I am not sure if I've understood it correctly. It's should return a value to the closest point in the path (in case of straight line it would be a next point on the line, and in case of complex path not necessarily)?

Here is my current fragment of the code with that idea:

BT::NodeStatus IsGoalNearbyCondition::tick()
{
    nav_msgs::msg::Path path;
    double prox_thr;
    getInput("path", path);
    getInput("proximity_threshold", prox_thr);

    geometry_msgs::msg::PoseStamped pose; // robot pose in map frame
    nav2_util::getCurrentPose(
    pose, *tf_buffer_,
    "map", "base_link", 0.05);
    
    geometry_msgs::msg::PoseStamped robot_pose; // robot_pose in path frame
    if (!transformPose(path.header.frame_id, pose, robot_pose)) {
        return BT::NodeStatus::FAILURE;
    }

    auto closest_pose_upper_bound =
    nav2_util::geometry_utils::first_after_integrated_distance(
    path.poses.begin(), path.poses.end(), max_robot_pose_search_dist_);


    // First find the closest pose on the path to the robot
    // bounded by when the path turns around (if it does) so we don't get a pose from a later
    // portion of the path
    auto closest_pose_it =
    nav2_util::geometry_utils::min_by(
    path.poses.begin() + last_closest_index_, closest_pose_upper_bound,
    [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
      return nav2_util::geometry_utils::euclidean_distance(robot_pose, ps);
    });

    last_closest_index_ = std::distance(path.poses.begin(), closest_pose_it);

    double distance_to_closest_point = nav2_util::geometry_utils::euclidean_distance(robot_pose, *closest_pose_it);
    RCLCPP_INFO(node_->get_logger(), "Distance to closest point: %f", distance_to_closest_point);

    if(distance_to_closest_point < prox_thr){
         return BT::NodeStatus::SUCCESS;
    }
    return BT::NodeStatus::FAILURE;
}


bool IsGoalNearbyCondition::transformPose(
  const std::string frame,
  const geometry_msgs::msg::PoseStamped & in_pose,
  geometry_msgs::msg::PoseStamped & out_pose) const
{
  if (in_pose.header.frame_id == frame) {
    out_pose = in_pose;
    return true;
  }

  try {
    tf_buffer_->transform(in_pose, out_pose, frame);
    out_pose.header.frame_id = frame;
    return true;
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(node_->get_logger(), "Exception in transformPose: %s", ex.what());
  }
  return false;
}

I though it's even working but after few attempts I've got segmentation fault so still working on that

[SteveMacenski]

EOF line

[SteveMacenski]

Did you test this?

[SteveMacenski]

@Jakubach following up here - have you had a chance to work on this?