Galactic to Humble

Moving from ROS 2 Galactic to Humble, a number of stability improvements were added that we will not specifically address here.

Major improvements to Smac Planners

The Smac Planner was significantly improved, of both the 2D and Hybrid-A* implementations, making the paths better, faster, and of higher quality.

  • Collision checker rejects collisions faster and queries the costmap for coordinates less often
  • Zero-copy collision checking object
  • precompute collision checking footprint orientations so no need for trig at runtime
  • Only checking full SE2 footprint when the robot is in the possibly inscribed zones
  • Computing the possibly inscribed zones, or the cost over which some part of the footprint may be in collision with a boundary to check the full footprint. Else, check center cost since promised to not be in a potential collision state
  • Renaming Hybrid-A* planner to SmacPlannerHybrid
  • Precomputing the Reedshepp and Dubin paths offline so at runtime its just a lookup table
  • Replacing the wavefront heuristic with a new, and novel, heuristic dubbed the obstacle heuristic. This computes a Dijkstra’s path taking into account the 8 connected space, as well as weights for the cost at the positions to guide the heuristic into the center of aisle ways. It also downsamples the costmap such that it can reduce the number of expansions by 75% and have a very small error introduced into the heuristic by being off by at most a partial fraction of a single cell distance
  • Improvements to the analytic expansion algorithm to remove the possibility of loops at the end of paths, whenever possible to remove
  • Improving analytic expansions to provide maximum path length to prevent skirting close to obstacles
  • 2D A* travel cost and heuristic improvements to speed up planning times and also increase the path quality significantly
  • Replaced smoother with a bespoke gradient descent implementation
  • Abstract out common utilities of planners into a utils file
  • tuned cost functions
  • precomputed obstacle heuristic using dynamic programming to expand only the minimum number of nodes
  • A caching heuristic setting to enable 25hz planning rates using cached obstacle heuristic values when the goal remains the same
  • Leveraging the symmetry in the dubin and reeds-sheep space to reduce cache size by 50% to increase the window size available for heuristic lookup.
  • Precompute primitives at all orientation bins
  • SmacPlanner2D parameters are now all reconfigurable
  • Both Hybrid-A* and State Lattice planners are now fully admissible
The tl;dr of these improvements is:
  • Plans are 2-3x as fast as they were before, well under 200ms for nearly all situations, making it as fast as NavFn and Global Planner (but now kinematically feasible). Typical planning times are sub-100ms without even making use of the caching or downsampling features.
  • Paths are of significantly higher quality via improved smoothers and a novel heuristic that steers the robot towards the center of aisleways implicitly. This makes smoother paths that are also further from obstacles whenever possible.
  • Using caching or downsampler parameterizations, can easily achieve path planning with sub-50ms in nearly any sized space.

Additional improvements were made to include a analytic_expansion_max_length parameter such that analytic expansions are limited in their potential length. If the length is too far, reject this expansion. This prevents unsafe shortcutting of paths into higher cost areas far out from the goal itself, let search to the work of getting close before the analytic expansion brings it home. This should never be smaller than 4-5x the minimum turning radius being used, or planning times will begin to spike.

Further, the traversal cost and heuristic cost computations were updated requiring retuning of your penalty functions if you have a previously existing configuration. Defaults of the algorithm were also retuned appropriately to the change for similar our of the box behavior as before (to use as a reference).

Simple (Python) Commander

This PR introduces a new package to Nav2, called the nav2_simple_commander. It is a set of functions in an object, BasicNavigator, which can be used to build Nav2-powered autonomy tasks in Python3 without concerning yourself with the Nav2, ROS 2, or Action server details. It contains a simple API taking common types (primarily PoseStamped) and handles all of the implementation details behind the hood. For example, this is a simple navigation task using this API:

def main():
    rclpy.init()
    navigator = BasicNavigator()

    # Set our demo's initial pose
    initial_pose = PoseStamped()
    ... populate pose ...
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to fully activate
    navigator.waitUntilNav2Active()

    # Go to our demos first goal pose
    goal_pose = PoseStamped()
    ... populate pose ...
    navigator.goToPose(goal_pose)

    while not navigator.isNavComplete():
        feedback = navigator.getFeedback()
        ... do something with feedback ...

        # Basic navigation timeout
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelNav()

    result = navigator.getResult()
    if result == NavigationResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == NavigationResult.CANCELED:
        print('Goal was canceled!')
    elif result == NavigationResult.FAILED:
        print('Goal failed!')

The full API can be found in the README of the package. A number of well commented examples and demos can also be found in the package’s source code at the link prior.

Reduce Nodes and Executors

In order for nav2 to make the best use of ROS 2, we need minimize the number of nodes and executors in nav2, which can improve performance.

This functionality has been discussed in the ticket #816, and carried out in

  • Remove client_node_ in class WaypointFollower : PR2441
  • Remove rclcpp_node_ in class MapSaver : PR2454
  • Remove bond_client_node_ in class LifecycleManager : PR2456
  • Remove node_ in class LifecycleManagerClient : PR2469
  • Remove rclcpp_node_ in class ControllerServer : PR2459, PR2479
  • Remove rclcpp_node_ in class PlannerServer : PR2459, PR2480

Extending the BtServiceNode to process Service-Results

This PR addresses this Ticket and adds a virtual on_completion() function to the BtServiceNode class (here). Similar to the already existing virtual on_wait_for_result() function, it can be overwritten in the child class to react to a respective event with some user-defined operation. The added on_completion() function will be called after the service interaction of the BtServiceNode has been successfully completed.

/**
* @brief Function to perform some user-defined operation upon successful
* completion of the service. Could put a value on the blackboard.
* @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value
*/
virtual BT::NodeStatus on_completion()
{
  return BT::NodeStatus::SUCCESS;
}

The returned BT::NodeStatus will set the current status of the BT-Node. Since the function has access to the results of the service, the returned node-status can depend on those service results, for example. The normal behavior of the BtServiceNode is not affected by introducing the on_completion() function, since the the default implementation still simply returns BT::NodeStatus::SUCCESS, if the service interaction completed successfully.

Including new Rotation Shim Controller Plugin

This PR introduces the new nav2_rotation_shim_controller. This controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the primary controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking.

The Rotation Shim Controller is suitable for:

  • Robots that can rotate in place, such as differential and omnidirectional robots.
  • Preference to rotate in place rather than ‘spiral out’ when starting to track a new path that is at a significantly different heading than the robot’s current heading.
  • Using planners that are non-kinematically feasible, such as NavFn, Theta*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot’s actual starting heading, requiring no rotation).

Spawning the robot in Gazebo

This PR deletes the pkg nav2_gazebo_spawner inside nav2_bringup directory. Instead of nav2_gazebo_spawner the Node spawn_entity.py of gazebo_ros is recomended to spawn the robot in gazebo. Note that

  • gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly.
  • spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. This problem was overcame by adding remapping argument <remapping>/tf:=tf</remapping> <remapping>/tf_static:=tf_static</remapping> under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf’s of the different robot.

Recovery Behavior Timeout

Recoveries in Nav2, spin and backup, now have time_allowance ports in their BT nodes and request fields in their actions to specify a timeout. This helps ensure that the robot can exit a backup or spin primitive behavior in case it gets stuck or otherwise is unable to backup the full distance over a reasonable block of time.

New parameter use_final_approach_orientation for the 3 2D planners

This PR adds a new parameter use_final_approach_orientation to the 3 2D planners (Theta*, SmacPlanner2D and NavFn), false by default. If true, the last pose of the path generated by the planner will have its orientation set to the approach orientation, i.e. the orientation of the vector connecting the last two points of the path. It allows sending the robot to a position (x,y) instead of a pose (x,y,theta) by effectively ignoring the goal orientation. For example, below, for the same goal with an orientaton pointed left of the screen, use_final_approach_orientation=false (left) and use_final_approach_orientation=true (right)

../_images/use_final_approach_orientation_false.gif ../_images/use_final_approach_orientation_true.gif

SmacPlanner2D and Theta*: fix goal orientation being ignored

This PR fixes the issue of the goal pose orientation being ignored (the end path pose orientation was always set to 0).

SmacPlanner2D, NavFn and Theta*: fix small path corner cases

This PR ensures the planners are not failing when the distance between the start and the goal is small (i.e. when they are on the same costmap cell), and in that case the output path is constructed with a single pose.

Change and fix behavior of dynamic parameter change detection

This and this PR modify the method used to catch the changes of dynamic parameters. The motivation was to fix the issue that void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) was called for every parameter change of every node leading to unwanted parameter changes if 2 different nodes had the same parameter name.

Dynamic Parameters

Newly added dynamic parameters to:

  • This PR makes most of the Costmap2DROS parameters dynamic
  • This PR makes most of the Regulated Pure Pursuit parameters dynamic
  • This PR makes most of the Theta * Planner parameters dynamic
  • This PR makes Waypoint Follower, Planner Server, and Controller Server’s params reconfigurable

BT Action Nodes Exception Changes

When BT action nodes throw exceptions due to networking or action server failures, they now return a status code of FAILURE to fail that particular action in the behavior tree to react to. This is in contrast to prior where the exceptions thrown from networking were sent to the root tree which would count as a task-level failure that the tree could not react to.

BT Navigator Groot Multiple Navigators

This PR creates separate parameters for groot monitoring for the NavToPose and NavThroughPoses navigator types so you can individually track the state of each behavior tree through the ZMQ publisher. This resolves a long-standing problem after we added multiple navigator types to BT Navigator that you could only view the nav to poses BT execution live. BT.CPP and Groot only support one static ZMQ stream at a time, so there is a bit of a quirk where you must locally reset Groot after switching trees in order to view the live stream of the Nav Through Poses BT, if in use. This is a state of the BT.CPP and Groot libraries and not something we can resolve withing Nav2.

There is some thought into the future regarding complete deprecation of live BT monitoring using Groot due to this quirk and the almost-certain infux of tickets on the topic. Groot will however always be supported for visualizing behavior tree XML files and modifications, simply not visualizing the BT execution live during robot navigation.

Removed Kinematic Limiting in RPP

The parameters max_linear_accel and max_linear_decel were removed along with the kinematic limiting in the controller causing instabilities. Instead, use a velocity smoother available in the ROS ecosystem if you would like similar behavior.

Removed Use Approach Velocity Scaling Param in RPP

The parameter use_approach_linear_velocity_scaling is removed in favor of always on to help in smooth transitions to the goal. This PR implements.

Refactored AMCL motion models as plugins

This PR creates plugins for the different motion models currently used in AMCL. This functionality enables users to use any custom motion model by creating it as a plugin and changing the robot_model_type parameter to the name of the plugin in nav2_params.yaml file. This helps to use custom motion models without the need to modify the AMCL source code.

Dropping Support for Live Groot Monitoring of Nav2

It was a great feature idea but never quite panned out, especially after we introduced multiple navigator types in the BT Navigator server. The issue we run into primarily is that Zero-MQ prevents users from producing multiple logger types in the same process. Since BT nav has multiple servers, the swapping between them for viewing has never had a clean hand off causing folks to file tickets or have nasty logs appear or ZMQ crashes in the background. The BT.CPP client for this doesn’t allow us to have a clean shutdown process so we’re left with hoping that ZMQ properly handles the situation, which it rarely does. Further, Groot only supports visualizing one type of tree at a time so for applications often switching between navigator types, its not possible to use a single groot client, causing great frustration.

So, what I propose here is to remove live monitoring of the BT from Nav2. We can still use Groot to modify, visualize, and generally work with behavior trees, the only thing being removed is to live view the executing behavior tree as Nav2 is currently executing it (it used to light up the boxes of the current nodes). This was of dubious value anyhow, since the tree ticks so fast its difficult to visualize and get meaningful insights into things as the system is moving so quickly.