behavior tree plugin PathLongerOnApproach bug report

[StetroF]

Bug report

Required Info:

• Operating System: -Ubuntu 22.04
• ROS2 Version: -Humble
• Version or commit hash: -1.1.14
• DDS implementation: -CycloneDDS

Steps to reproduce issue

bool PathLongerOnApproach::isPathUpdated(
  nav_msgs::msg::Path & new_path,
  nav_msgs::msg::Path & 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();
}

Expected behavior

Return true if path is update

Actual behavior

return false because in this line: old_path.poses.back() == new_path.poses.back(); It compare two different path's PoseStamped.But The PoseStamped's comparison contain time's compare and position's compare:

  // comparison operators
  bool operator==(const PoseStamped_ & other) const
  {
    if (this->header != other.header) {
      return false;
    }
    if (this->pose != other.pose) {
      return false;
    }
    return true;
  }

Old path's last point's position is same as the new path's last point's position. Meanwhile, their timestamped is obviously different as i observed. So that even if the new path has updated. It will still return false.Which cause PathLongerOnApproach plugin out of action. By the way. I tested it in the situation that use_sim_time : false

[padhupradheep]

Hey @StetroF

Thanks for reporting the issue. I guess you are using one of the smac planners if I'm not wrong. This plugin was always tested with the NavFn planner while I developed it. I oversaw this issue, since NavFn planner for some reason has the same timestamps.

The solution for this is simply straightforward, would you be interested in sending us a PR? I will be happy to review it for you.

[SteveMacenski]

That seems sensible - just change to old_path.poses.back().pose == new_path.poses.back().pose

[SteveMacenski]

@StetroF are you open to submitting the PR to resolve :smile: I'd love to have you contribute your own patch in!

[StetroF]

@StetroF are you open to submitting the PR to resolve 😄 I'd love to have you contribute your own patch in!

Sure! It's my first time to pull request. I will have try https://github.com/ros-navigation/navigation2/pull/4344

[SteveMacenski]

Thanks for the fix @StetroF :-)