Goal Validity Checker

During navigation, the Goal Validity Checker verifies the validity of goal positions. To be considered valid, a goal must be located at an unoccupied cell and meet a specific minimum distance from any obstacles. The required distance is determined by the min_range parameter. In the case that the goal position is not valid, it will calculate a new valid goal pose within a defined range from the original goal. The defined range is determined by the max_range_new_goal parameter.

The Goal Validity Checker has been tested on ROS 2 Galactic.

This feature was created by Jason Koubi while working as an intern for Karelics Oy.

Table of Contents

• Goal Validity Checker
• Table of Contents
• Usage Instructions
  • Using nav2
  • Using service without behavior trees
• Configuration
• How it works?
  • Example 1: No min_range
  • Example 2: min_range = 5cm
  • Example 3: min_range = 15cm
• Limitation
• License

Usage Instructions

Using nav2

In order to use the Goal Validity Checker for ROS 2 navigation purposes, we need to add the service into the behavior tree. This will require the following:

• Add included (goal_checker_bt_node) behavior tree node into your behavior tree.
• Add the goal_checker_bt_node to the bt_navigator parameter plugin_lib_names list.

Note: An example of a behavior tree can be found in the behavior_trees folder where the Goal Validity Checker is incorporated. This integration enables us to employ the Goal Validity Checker for nav2.

Using service without behavior trees

The Goal Validity Checker node comprises a service named goal_checker_validity which takes a geometry_msgs/PoseStamped message as a request. The response is composed of 3 messages: a bool which will inform us if it has successfully found a valid goal pose, a string message indicating the resulting status and finally, a geometry_msgs/PoseStamped corresponding to the goal pose.

geometry_msgs/PoseStamped goal_pose
---
bool success
string message
geometry_msgs/PoseStamped new_goal_pose

The service will report success:

• True - If the current goal is valid and within the defined range (also returns original goal)
• True - If a new valid goal was found within the defined range (returns new goal)
• False - If there was no valid goal found within the defined range (returns empty goal)

Configuration

The Goal Validity Checker node is composed of various parameters which can be found in the goal_validity_checker.yaml parameter file. Here is a description of those parameters:

Parameters	Description	Type	Default value
free_thresh	Threshold determining free occupancy cells in the global costmap	integer	65
max_range_new_goal	Maximum range allowed to search for a new goal pose	float	2.0
set_unknown_area_search	Enable or disable searching for valid goal pose if the original one is unknown	bool	True
min_range	Checks if the goal pose is a minimum distance away from any obstacles	float	0.1

Note: The unit of measurement for both the max_range_new_goal and the min_range is meters.

Can check parameters description using the following command:

ros2 param describe /goal_checker free_thresh
ros2 param describe /goal_checker max_range_new_goal
ros2 param describe /goal_checker set_unknown_area_search
ros2 param describe /goal_checker min_range

How it works?

In order to understand what the code does, a visual explanation is carried out. First, the map is represented as a list of cells containing occupancy data corresponding to the presence or no of an obstacle at that corresponding cell.

Here is a gif illustrating the goal pose search method that we use if the original goal is on an occupied cell (represented in black).

As we can see, we are looking for neighboring cells starting with the right cell, then left, upper, and bottom (in green) and finally, we search in diagonal (in blue). If the new goal pose is still not found, we extend our search loop in the same order till we find a valid unoccupied cell.

Example 1: No min_range

In this example, min_range = 0.0, we can see that a solution is found.

Example 2: min_range = 5cm

Here, for min_range = 0.05, a solution is found.

Example 3: min_range = 15cm

Finally, in this example, for min_range = 0.15, no solution is found.

Limitation

This Goal Validity Checker has a limitation related to the planner. It will not check if a path can be planned to this new goal pose. This means that it can find a valid goal pose but this might not guarantee that the robot will actually go to this position since it needs to calculate a plan to this valid goal pose in order to navigate.

License

This piece of software is released under the GPL 3.0 license.

Goal Validity Checker - ROS 2 Node checking valid goal poses during navigation.

Copyright (C) 2023 Karelics Oy

This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see https://www.gnu.org/licenses/.