PathAlignCritic: Improve occupancy check, Add DirectionChangeCritic

[adivardi]

Additions & Changes

PathAlign Critic

Add a minimum distance for the occupancy check.
Previously, the occupancy check was done up to a distance = furthest_reached_path_point along the path,
Now, it is done over a distance = min(min_dist, furthest_reached_path_point).

The effect of this change is that when approaching an obstacle, even if the robot slows down (and thus furthest_reached_path_point distance becomes smaller and smaller), the check would still trigger and disable the critic. This solves the issue where the robot slows down when approaching an obstacle instead of overtaking it, until it gets stuck when the obstacle is just in front of the robot.

Results of this change are susbtiantal:
Previously, robot was not able to overtake a box of 1.5m width places in front of it.
With the change, the robot can overtake obstacles up to the width of a car and even more.

Remaining issues

When approaching a car parked perpendicular to the robot, the robot correctly starts going around it. However, when the robot is perpendicular to the path, MPPI finds no good solution and "gets lost"

DirectionChange Critic

Add a critic for penalizing changes in driving direction. This reduces MPPI switching direction too quickly, leading to robot to get stuck as the motors cannot follow.

Applying the critic on the current robot's speed (and not last command published) leads to much better results

Added a new state.robot_speed field as state.speed is either current speed or last command published, depends on open_loop setting.

Remaining issues

DirectionChange: Also penalize wz to avoid quick steering in place when stuck

Checklist (tick off when done or remove when not relevant):

• Set good PR title for changelog
• Perform a self-review if the PR has more than 50 lines of changes

[gemini-code-assist]

Summary of Changes

Hello, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request introduces a new critic to the MPPI controller to enhance robot motion by penalizing abrupt changes in driving direction. It also provides a comprehensive overhaul of the MPPI controller's visualization framework, offering detailed and configurable output for better debugging and analysis. Additionally, a key behavior tree component has been refactored for improved code organization and reusability.

Highlights

• New DirectionChangeCritic: Introduced a new critic function that penalizes changes in the robot's driving direction, improving smooth motion.
• Enhanced MPPI Visualization: Significantly expanded visualization capabilities for the MPPI controller, allowing granular control over publishing total trajectory costs, individual critic costs, optimal trajectory messages, robot footprints, and transformed paths. Visualization parameters are now grouped under a dedicated 'Visualization' namespace.
• Improved PathAlignCritic: The PathAlignCritic now includes a configurable 'occupancy_check_min_distance' parameter and supports visualization of the furthest reached path point and occupancy check end point.
• Accurate Robot Speed Tracking: Added a 'robot_speed' field to the 'State' struct to store actual odometry-based robot speed, enabling more precise critic calculations.
• Behavior Tree Refactoring: The 'FollowPath' node in the 'navigate_through_poses_w_replanning_and_recovery.xml' behavior tree has been refactored into a reusable 'SubTree' for improved modularity.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for GitHub and other Google products, sign up here.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

This pull request introduces a new DirectionChangeCritic to penalize changes in driving direction, requiring the addition of robot_speed to the State struct for accurate feedback. The trajectory visualization system has been significantly refactored and expanded, moving all visualization logic into a dedicated TrajectoryVisualizer class with new parameters for publishing optimal trajectories, footprints, and individual critic costs. The CriticData structure now includes optional storage for per-critic costs to support this enhanced visualization. Additionally, PathAlignCritic, PathAngleCritic, and PathFollowCritic have been updated with new visualization options and refined logic for path occupancy checks. A review comment highlights that the VisPathRepub test is likely to fail due to a missing implementation for the publish_transformed_path feature in TrajectoryVisualizer. Another comment suggests refactoring duplicated cost calculation code in DirectionChangeCritic for improved conciseness and maintainability. Finally, two patch files (subtree.patch and subtree_first.patch) were accidentally committed and should be removed.

[adivardi]

self review