One critical aspect of robotics is navigating and interacting with the environment effectively. To achieve this, robotic systems use costmaps, representing the cost or difficulty associated with traversing different areas of the environment.
One key challenge in working with costmaps is ensuring that robot goals are valid and can be reached while avoiding obstacles and other hazards. To address this challenge, a robotics project was undertaken, and a Goal Validity Checker node was developed, which can quickly and accurately determine if a robot’s intended destination is safe and feasible.
The Goal Validity Checker project is the result of the work done by Jason Koubi while working as an intern at Karelics for three months under the supervision of Joni Pöllänen.
The code is open-sourced and available on github. For installation instructions, please check the repo.
Table of Contents
Goal Validity Checker
During navigation, the Goal Validity Checker verifies the validity of goal positions.
To be considered valid, a goal must be located in 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.
Below are examples of a valid and an invalid goal pose, keeping in mind that the min_range parameter mostly determines the validity. However, for the sake of simplicity, we will assume that the min_range parameter is set to zero in the following examples.
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.
The service will report the following feedback:
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 has various parameters that can be tuned to optimize the user’s needs. Here is a description of those parameters:
| Parameter | Description | Type | Default value |
free_thresh | Threshold determining free occupancy cells in the global costmap | integer | 65 |
max_range_new_goal | The 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 |
max_range_new_goal and the min_range is meters. How does it work?
To understand what the algorithm 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 absence of an obstacle at that corresponding cell.
Here is a gif illustrating our goal pose search method if the original goal is on an occupied cell (represented in black).
As we can see, we are looking for neighbouring 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, for min_range = 0.0, we can see that a solution is found at the green cell located at the edge of the occupied area.
As a side note, sensors used for navigation, such as lidars, will always have some noise that will affect the costmap, and some unoccupied cells may become occupied and vice versa. This can lead to recalculating a valid goal pose, especially if the new goal pose is located at the edge of an occupied area. This is where the min_range parameter can be useful by securing a minimum distance from any obstacles. This will be illustrated in the following example.
Example 2: min_range = 5cm
Here, for min_range = 0.05, a solution is found. It follows the same steps as in the previous example, except when it finds the first unoccupied cell (bright yellow cell), it will shift one cell + 5 centimeters away from this unoccupied cell to the new potential goal pose (purple cell). From here, it will check if there are no obstacles 5 centimeters away from this area. Once it checks that all its neighbor cells (blue cells) are unoccupied, we can confirm that this cell is valid.
Example 3: min_range = 15cm
In this last example, for min_range = 0.15, we observe that it fails to find a valid goal pose. This time the new potential goal pose (purple cell) is located one cell + 15 centimetres away from the first unoccupied cell encountered (bright yellow cell). It is now ready to check if there are no obstacles 15 centimetres away from this area. This is where it fails, an obstacle is encountered on the right side (orange cell).
Results
The Goal Validity Checker service has been added to the Behavior Tree for navigation. The solution has been tested on turtlebot3 in Gazebo. To check that the Goal Validity Checker service works properly, a Nav2 goal is sent to an occupied area:
We can see that the service node successfully calculates a new goal pose.
Limitations
This Goal Validity Checker has a limitation related to the planner. It will not check if a path can be planned for this new goal pose. This means it can find a valid goal pose, but this does not guarantee that the robot will go to this position since it needs to calculate a plan for this valid goal pose to navigate.
Conclusion
Overall, the development of the Goal Validity Checker represents a significant advance in the field of robotics, enabling robots to navigate and interact with their environment more effectively and efficiently. The proposed solution has been tested on the company’s SAMPO robot and showed promising results.
