Conversation
There is still a bug that the publish_trajectories_with_individual_cost doesn't update on reconfigure calls
Use robot_speed from feedback, as it better represents the actual direction of motion. This results in more consistent critic application and longer motions in each direction
Not sure if useful
Summary of ChangesHello, I'm Gemini Code Assist1! 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 comprehensive overhaul of the MPPI controller's visualization framework, providing users with much finer control over debugging and analysis outputs. Alongside these visualization enhancements, a new critic has been integrated to promote smoother robot motion by penalizing sudden direction changes. Significant performance gains are also achieved through the introduction of OpenMP parallelization in the core cost evaluation, making the controller more efficient and robust. Highlights
Using Gemini Code AssistThe 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
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 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
|
![gemini-code-assist[bot]](https://avatars.githubusercontent.com/in/956858?s=60&v=4){kind=small}
There was a problem hiding this comment.
Code Review
This pull request significantly enhances the nav2_mppi_controller by refactoring and expanding its visualization capabilities, introducing a new DirectionChangeCritic, and implementing performance optimizations. The TrajectoryVisualizer now supports publishing various data including candidate trajectories with total and individual critic costs, optimal trajectories, paths, and footprints, controlled by new parameters. OpenMP parallelization has been integrated into the CostCritic for improved performance, along with optimizations to collision checking and Eigen matrix operations. A new DirectionChangeCritic is added to penalize changes in driving direction. A review comment highlights an unimplemented feature related to publish_transformed_path that is being tested, suggesting either implementation or test removal. Another comment points out an unused variable in the DirectionChangeCritic implementation that should be removed for code cleanliness.
After looking into adding break early when computing the path's arc-length
The clamp (min(80 * temperature)) caps all exp() arguments at -80, where the value is already ~1e-35, slightly above float32's smallest fraction of ~1.2e-38. This eliminates potential underflows &= slow paths in the exp implementation. This reduces computation by ~6x, from 2000-300us to ~400us
Let's Eigen know there is no aliasing, very small optimization
Reduces computation time from 18-20ms to 5ms
This reverts commit 9d721b9.
1 participant
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers:
backport-*.