IsPathValid to Considering robot footprint

[aniket11bh]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • humble
• Version or commit hash:
  • ros-humble-navigation2 1.1.12-1jammy.20231005.224358
• DDS implementation:
  • cycloneDDS

Steps to reproduce issue

• Use navigate through poses bt with replanning_only_if_path_becomes_invalid.xml. I have to write one, since it was only present for navigate to pose.
• Setup a path and obstacle such that the
  • obstacle is not within the initial lidar range
  • obstacle is very close to the to be planned path between 2 waypoints. I.e, line touches the inflation layer, but not the inscribed radius as shown in the picture.
• When the obstacle comes within the range of the lidar.
  • Note that:

    • the path is not replanned even when the path passes through the high cost inflation layer.
    • As a result, when the vehicle passes by the obstacle the vehicle footprint touches not only the inscribed radius layer, but also the obstacle itself.

Expected behavior

• Planner should have planned a new path in this case.

Actual behavior

• IsPathValid is returning true, for the existing path, because of which new path is not being generated.

Additional information

• This behaviour was reproducible also for navigate to pose.
• costmap resolution : 1.0m
• navigate_through_poses bt xml file

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence>
        <RateController hz="2.0" name="RateControllerComputePathToPose">
          <RecoveryNode number_of_retries="1" name="RecoveryComputePathToPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{rem_goals}" output_goals="{rem_goals}" radius="10.0"/>
              <Fallback name="FallbackComputePathToPoses">
                <ReactiveSequence name="CheckIfNewPathNeeded">
                  <Inverter>
                    <GlobalUpdatedGoal/>
                  </Inverter>
                  <IsPathValid path="{path}"/>
                </ReactiveSequence>
                <Sequence>
                  <Fallback>
                    <Inverter>
                      <GlobalUpdatedGoal/>
                    </Inverter>
                    <CopyGoals input_key="goals" output_key="rem_goals" />
                  </Fallback>
                  <ComputePathThroughPoses goals="{rem_goals}" path="{path}" planner_id="GridBased"/>
                </Sequence>
              </Fallback>
            </ReactiveSequence>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="FallbackRecoveries">
        <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>
          <Wait name="WaitRecovery" wait_duration="5"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

[aniket11bh]

Tried with planner server changes submitted in #3756 and latest changes for smac planner from main branch. However, I am still able to reproduce the issue.

[SteveMacenski]

@aniket11bh that sounds like a great potential improvement that you can contribute! Have the function not just check point-costs but optionally check the footprint. Since the is path valid service is in the global costmap, it should have easy access to such information.

Is that something you'd be open to contribute? Else, this will probably be marked for good first issue until a student or someone with extra time has time to add this feature.

[aniket11bh]

@SteveMacenski Yes indeed, it would be a great improvement.
I would have loved to contribute, but my schedule is fully committed till December end. You can assign this to someone else as per availability.

[SteveMacenski]

That might actually still yet be the soonest anyone would address it, if you were able to work on it in the new year. That’s only 6 weeks out and this is not high priority for maintainer’s time so it would only be if a volunteer contributor was interested in it sooner

[SteveMacenski]

#4030 merging imminently