Developing a “Goal Validity Checker” node for navigation goals

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.

Goal Validity Checker
Figure 1: Valid goal pose (left), Invalid goal pose (right).

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:

ParameterDescriptionTypeDefault value
free_threshThreshold determining free occupancy cells in the global costmapinteger65
max_range_new_goalThe maximum range allowed to search for a new goal posefloat2.0
set_unknown_area_searchEnable or disable searching for valid goal pose if the original one is unknownboolTrue
min_rangeChecks if the goal pose is a minimum distance away from any obstaclesfloat0.1
It should be noted that the unit of measurement for both the 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.

Facebook
Twitter
LinkedIn
Picture of Jason Koubi
Jason Koubi

I am a recent graduate student in Mechanical Engineering from California State University Los Angeles, with a strong focus on software robotics throughout my studies. Ever since my initial course in robotics two years ago, I have been deeply intrigued by the field. To further enhance my skills, I enrolled in The Construct's Robotics Developer Master Class, where I acquired expertise in programming robots using ROS. The concepts I learned directly influenced my thesis and played a pivotal role in securing an internship with Karelics.

All Posts

Pipe Inspection request

Photo Documentation
request