-
Notifications
You must be signed in to change notification settings - Fork 1.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
SmacHybrid planner can crash to segmentation fault #4141
Comments
The C++ docs that I usually reference have some remark if that would be a problem that I see aren't listed there. But just being a sane person knowing STL, yeah your expectation there would align with mine. I think the documentation just had that case missing. Can you adjust that to:
Does it go away? If so, submit a PR to
You can add that too! Why not both, I practice defensive programming. |
Thanks for quick response and the feedback! I will do this once I am back from my trip on Monday. |
ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
Merging imminent |
* Prevent a possible segmentation fault #4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
* Prevent a possible segmentation fault #4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> (cherry picked from commit 654e7e0) # Conflicts: # nav2_smac_planner/src/analytic_expansion.cpp
* Prevent a possible segmentation fault #4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> (cherry picked from commit 654e7e0) # Conflicts: # nav2_smac_planner/src/analytic_expansion.cpp
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> [behavior_tree] support both ports and blackboard (ros-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove unused header Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add description Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * updated tree XML Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> adding new Friends of Nav2 Sponsors! Update README.md (ros-navigation#4114) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> CI green P3 (ros-navigation#4117) Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (ros-navigation#4113). (ros-navigation#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> Adding context to MPPI readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> [behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * revert change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Fix compilation with clang (ros-navigation#4131) Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix goal handle was not freed correctly (ros-navigation#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Adding Nvidia as Gold Sponsor! (ros-navigation#4144) Prevent a possible segmentation fault (ros-navigation#4141) (ros-navigation#4147) * Prevent a possible segmentation fault Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Updating stereolabs logo (ros-navigation#4158) Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> behavior_tree: migration to BT.CPP 4.5.x (ros-navigation#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix lint warning Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * handle SKIPPED state Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix unit tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fit test Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix compilation Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> bug fix (ros-navigation#4160) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> Stop planner if the goal is cancelled (ros-navigation#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add system tests for cancel Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix BT.CPP import (ros-navigation#4165) Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> improvement based on feedback Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Signed-off-by: enricosutera <enricosutera@outlook.com>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
…gation#4147) * Prevent a possible segmentation fault ros-navigation#4141 Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> (cherry picked from commit 654e7e0)
Bug report
Required Info:
Steps to reproduce issue
This part I am still uncertain. The issue keep happening seemingly randomly during navigation. Have not been able to pinpoint the exact reason/case as I do not yet fully understand what is going on in the SmacHybridPlanner. Sometimes the segmentation fault happens quite soon after starting navigating. Sometimes it takes many many navigations. (Had my simulation giving consecutive goals for 95 minutes before I got the latest seg fault for the log to include in this issue.)
Expected behavior
Planner keeps running 😁
Actual behavior
Planner server crashes to a segmentation fault.
Log
// Logs I have added
[planner_server-2] D: 1.22173 is smaller than sqrt(2)
[planner_server-2] From x: 603.254 From y: 180.122 From yaw: 5.67232
[planner_server-2] To x: 604 To y: 180 To yaw: 0
// My logs end
[planner_server-2] Stack trace (most recent call last) in thread 1430:
[planner_server-2] #31 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ffaaf4aaee7, in
[planner_server-2] #30 Source "/usr/include/c++/11/mutex", line 712, in _Prepare_execution<std::call_once<void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool>(std::once_flag&, void (std::__future_base::_State_baseV2::&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>&&, bool&&)::<lambda()> > [0x7ffaafe2eb2b]
[planner_server-2] 709: // Store address in thread-local pointer:
[planner_server-2] 710: __once_callable = std::__addressof(__c);
[planner_server-2] 711: // Trampoline function to invoke the closure via thread-local pointer:
[planner_server-2] > 712: __once_call = [] { (static_cast<_Callable>(__once_callable))(); };
[planner_server-2] 713: }
[planner_server-2] 714:
[planner_server-2] 715: ~_Prepare_execution()
[planner_server-2] #29 Source "/usr/include/c++/11/mutex", line 712, in operator() [0x7ffaafe2eb16]
[planner_server-2] 709: // Store address in thread-local pointer:
[planner_server-2] 710: __once_callable = std::__addressof(__c);
[planner_server-2] 711: // Trampoline function to invoke the closure via thread-local pointer:
[planner_server-2] > 712: __once_call = [] { (static_cast<_Callable>(__once_callable))(); };
[planner_server-2] 713: }
[planner_server-2] 714:
[planner_server-2] 715: ~_Prepare_execution()
[planner_server-2] #28 Source "/usr/include/c++/11/mutex", line 776, in operator() [0x7ffaafe22ddd]
[planner_server-2] 773: {
[planner_server-2] 774: // Closure type that runs the function
[planner_server-2] 775: auto __callable = [&] {
[planner_server-2] > 776: std::__invoke(std::forward<_Callable>(__f),
[planner_server-2] 777: std::forward<_Args>(__args)...);
[planner_server-2] 778: };
[planner_server-2] #27 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool> [0x7ffaafe2eade]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #26 Source "/usr/include/c++/11/bits/invoke.h", line 74, in __invoke_impl<void, void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool> [0x7ffaafe3abcc]
[planner_server-2] 71: __invoke_impl(__invoke_memfun_deref, _MemFun&& __f, _Tp&& __t,
[planner_server-2] 72: _Args&&... __args)
[planner_server-2] 73: {
[planner_server-2] > 74: return ((std::forward<_Tp>(__t)).__f)(std::forward<_Args>(__args)...);
[planner_server-2] 75: }
[planner_server-2] 76:
[planner_server-2] 77: template<typename _Res, typename _MemPtr, typename _Tp>
[planner_server-2] #25 Source "/usr/include/c++/11/future", line 571, in _M_do_set [0x7ffaafe1bc15]
[planner_server-2] 568: void
[planner_server-2] 569: _M_do_set(function<_Ptr_type()>* __f, bool* __did_set)
[planner_server-2] 570: {
[planner_server-2] > 571: _Ptr_type __res = (__f)();
[planner_server-2] 572: // Notify the caller that we did try to set; if we do not throw an
[planner_server-2] 573: // exception, the caller will be aware that it did set (e.g., see
[planner_server-2] 574: // _M_set_result).
[planner_server-2] #24 Source "/usr/include/c++/11/bits/std_function.h", line 590, in operator() [0x7ffaafe23081]
[planner_server-2] 587: {
[planner_server-2] 588: if (_M_empty())
[planner_server-2] 589: __throw_bad_function_call();
[planner_server-2] > 590: return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[planner_server-2] 591: }
[planner_server-2] 592:
[planner_server-2] 593: #if __cpp_rtti
[planner_server-2] #23 Source "/usr/include/c++/11/bits/std_function.h", line 291, in _M_invoke [0x7ffaafea9ea1]
[planner_server-2] 288: _M_invoke(const _Any_data& __functor, _ArgTypes&&... __args)
[planner_server-2] 289: {
[planner_server-2] 290: return std::__invoke_r<_Res>(_Base::_M_get_pointer(__functor),
[planner_server-2] > 291: std::forward<_ArgTypes>(__args)...);
[planner_server-2] 292: }
[planner_server-2] 293:
[planner_server-2] 294: template
[planner_server-2] #22 Source "/usr/include/c++/11/bits/invoke.h", line 116, in __invoke_r<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>, std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > >, void>&> [0x7ffaafeabd6a]
[planner_server-2] 113: else
[planner_server-2] 114: return std::__invoke_impl<__type>(__tag{},
[planner_server-2] 115: std::forward<_Callable>(__fn),
[planner_server-2] > 116: std::forward<_Args>(__args)...);
[planner_server-2] 117: }
[planner_server-2] 118: #else // C++11
[planner_server-2] 119: template<typename _Res, typename _Callable, typename... _Args>
[planner_server-2] #21 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<std::unique_ptr<std::__future_base::_Result, std::__future_base::_Result_base::_Deleter>, std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > >, void>&> [0x7ffaafead04a]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[planner_server-2] 64: constexpr _Res
[planner_server-2] #20 Source "/usr/include/c++/11/future", line 1409, in operator() [0x7ffaafeae77d]
[planner_server-2] 1406: {
[planner_server-2] 1407: __try
[planner_server-2] 1408: {
[planner_server-2] >1409: (_M_fn)();
[planner_server-2] 1410: }
[planner_server-2] 1411: __catch(const __cxxabiv1::__forced_unwind&)
[planner_server-2] 1412: {
[planner_server-2] #19 Source "/usr/include/c++/11/bits/std_thread.h", line 266, in operator() [0x7ffaafeafe23]
[planner_server-2] 263: {
[planner_server-2] 264: using _Indices
[planner_server-2] 265: = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
[planner_server-2] > 266: return _M_invoke(_Indices());
[planner_server-2] 267: }
[planner_server-2] 268: };
[planner_server-2] #18 Source "/usr/include/c++/11/bits/std_thread.h", line 259, in _M_invoke<0> [0x7ffaafeb0ce3]
[planner_server-2] 256: template<size_t... _Ind>
[planner_server-2] 257: typename __result<_Tuple>::type
[planner_server-2] 258: _M_invoke(_Index_tuple<_Ind...>)
[planner_server-2] > 259: { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
[planner_server-2] 260:
[planner_server-2] 261: typename __result<_Tuple>::type
[planner_server-2] 262: operator()()
[planner_server-2] #17 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > [0x7ffaafeb1b04]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #16 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > [0x7ffaafeb2c76]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename Res, typename MemFun, typename Tp, typename... Args>
[planner_server-2] 64: constexpr Res
[planner_server-2] #15 Source "/nav2_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 228, in handle_accepted [0x7ffaafe4d64d]
[planner_server-2] 225: execution_future = std::async(
[planner_server-2] 226: std::launch::async, this {
[planner_server-2] 227: setSoftRealTimePriority();
[planner_server-2] > 228: work();
[planner_server-2] 229: });
[planner_server-2] 230: }
[planner_server-2] 231: }
[planner_server-2] #14 Source "/nav2_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 241, in work [0x7ffaafe59d73]
[planner_server-2] 238: while (rclcpp::ok() && !stop_execution && is_active(current_handle)) {
[planner_server-2] 239: debug_msg("Executing the goal...");
[planner_server-2] 240: try {
[planner_server-2] > 241: execute_callback();
[planner_server-2] 242: } catch (std::exception & ex) {
[planner_server-2] 243: RCLCPP_ERROR(
[planner_server-2] 244: node_logging_interface->get_logger(),
[planner_server-2] #13 Source "/usr/include/c++/11/bits/std_function.h", line 590, in operator() [0x7ffaafe258df]
[planner_server-2] 587: {
[planner_server-2] 588: if (_M_empty())
[planner_server-2] 589: __throw_bad_function_call();
[planner_server-2] > 590: return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[planner_server-2] 591: }
[planner_server-2] 592:
[planner_server-2] 593: #if __cpp_rtti
[planner_server-2] #12 Source "/usr/include/c++/11/bits/std_function.h", line 290, in _M_invoke [0x7ffaafe415ce]
[planner_server-2] 287: static _Res
[planner_server-2] 288: _M_invoke(const _Any_data& __functor, _ArgTypes&&... __args)
[planner_server-2] 289: {
[planner_server-2] > 290: return std::__invoke_r<_Res>(_Base::_M_get_pointer(__functor),
[planner_server-2] 291: std::forward<_ArgTypes>(__args)...);
[planner_server-2] 292: }
[planner_server-2] #11 Source "/usr/include/c++/11/bits/invoke.h", line 111, in __invoke_r<void, std::_Bind<void (nav2_planner::PlannerServer::(nav2_planner::PlannerServer))()>&> [0x7ffaafe4d10b]
[planner_server-2] 108: using __type = typename __result::type;
[planner_server-2] 109: using __tag = typename __result::__invoke_type;
[planner_server-2] 110: if constexpr (is_void_v<_Res>)
[planner_server-2] > 111: std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 112: std::forward<_Args>(__args)...);
[planner_server-2] 113: else
[planner_server-2] 114: return std::__invoke_impl<__type>(__tag{},
[planner_server-2] #10 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<void, std::_Bind<void (nav2_planner::PlannerServer::(nav2_planner::PlannerServer))()>&> [0x7ffaafe5995d]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[planner_server-2] 64: constexpr _Res
[planner_server-2] #9 Source "/usr/include/c++/11/functional", line 503, in operator()<> [0x7ffaafe657d2]
[planner_server-2] 500: _Result
[planner_server-2] 501: operator()(_Args&&... __args)
[planner_server-2] 502: {
[planner_server-2] > 503: return this->__call<_Result>(
[planner_server-2] 504: std::forward_as_tuple(std::forward<_Args>(__args)...),
[planner_server-2] 505: _Bound_indexes());
[planner_server-2] 506: }
[planner_server-2] #8 Source "/usr/include/c++/11/functional", line 420, in __call<void, 0> [0x7ffaafe6dff8]
[planner_server-2] 417: _Result
[planner_server-2] 418: __call(tuple<_Args...>&& __args, _Index_tuple<_Indexes...>)
[planner_server-2] 419: {
[planner_server-2] > 420: return std::__invoke(_M_f,
[planner_server-2] 421: _Mu<_Bound_args>()(std::get<_Indexes>(_M_bound_args), __args)...
[planner_server-2] 422: );
[planner_server-2] 423: }
[planner_server-2] #7 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<void (nav2_planner::PlannerServer::&)(), nav2_planner::PlannerServer&> [0x7ffaafe7695a]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #6 Source "/usr/include/c++/11/bits/invoke.h", line 74, in __invoke_impl<void, void (nav2_planner::PlannerServer::&)(), nav2_planner::PlannerServer&> [0x7ffaafe80046]
[planner_server-2] 71: __invoke_impl(__invoke_memfun_deref, _MemFun&& __f, _Tp&& __t,
[planner_server-2] 72: _Args&&... __args)
[planner_server-2] 73: {
[planner_server-2] > 74: return ((std::forward<_Tp>(__t)).__f)(std::forward<_Args>(_args)...);
[planner_server-2] 75: }
[planner_server-2] 76:
[planner_server-2] 77: template<typename Res, typename MemPtr, typename Tp>
[planner_server-2] #5 Source "/nav2_ws/src/navigation2/nav2_planner/src/planner_server.cpp", line 519, in computePlan [0x7ffaafe178dc]
[planner_server-2] 516: throw nav2_core::PlannerTFError("Unable to transform poses to global frame");
[planner_server-2] 517: }
[planner_server-2] 518:
[planner_server-2] > 519: result->path = getPlan(start, goal_pose, goal->planner_id);
[planner_server-2] 520:
[planner_server-2] 521: if (!validatePath(goal_pose, result->path, goal->planner_id)) {
[planner_server-2] 522: throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
[planner_server-2] #4 Source "/karelics_workspace/karelics_brain/nav2_ws/src/navigation2/nav2_planner/src/planner_server.cpp", line 589, in getPlan [0x7ffaafe18b8f]
[planner_server-2] 586: goal.pose.position.x, goal.pose.position.y);
[planner_server-2] 587:
[planner_server-2] 588: if (planners.find(planner_id) != planners.end()) {
[planner_server-2] > 589: return planners[planner_id]->createPlan(start, goal);
[planner_server-2] 590: } else {
[planner_server-2] 591: if (planners.size() == 1 && planner_id.empty()) {
[planner_server-2] 592: RCLCPP_WARN_ONCE(
[planner_server-2] #3 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/smac_planner_hybrid.cpp", line 395, in createPlan [0x7ffa79bc36a4]
[planner_server-2] 392: expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
[planner_server-2] 393: }
[planner_server-2] 394: // Note: All exceptions thrown are handled by the planner server and returned to the action
[planner_server-2] > 395: if (!_a_star->createPath(
[planner_server-2] 396: path, num_iterations,
[planner_server-2] 397: _tolerance / static_cast(costmap->getResolution()), expansions.get()))
[planner_server-2] 398: {
[planner_server-2] #2 Source "/karelics_workspace/karelics_brain/nav2_ws/src/navigation2/nav2_smac_planner/src/a_star.cpp", line 318, in createPath [0x7ffa79be4fba]
[planner_server-2] 316: // 2.1) Use an analytic expansion (if available) to generate a path
[planner_server-2] 317: expansion_result = nullptr;
[planner_server-2] > 318: expansion_result = _expander->tryAnalyticExpansion(
[planner_server-2] 319: current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
[planner_server-2] 320: if (expansion_result != nullptr) {
[planner_server-2] 321: current_node = expansion_result;
[planner_server-2] #1 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/analytic_expansion.cpp", line 83, in tryAnalyticExpansion [0x7ffa79bf9627]
[planner_server-2] 80: if (analytic_iterations <= 0) {
[planner_server-2] 81: // Reset the counter and try the analytic path expansion
[planner_server-2] 82: analytic_iterations = desired_iterations;
[planner_server-2] > 83: AnalyticExpansionNodes analytic_nodes =
[planner_server-2] 84: getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space);
[planner_server-2] 85: if (!analytic_nodes.empty()) {
[planner_server-2] 86: // If we have a valid path, attempt to refine it
[planner_server-2] #0 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/analytic_expansion.cpp", line 259, in getAnalyticPath [0x7ffa79bf8915]
[planner_server-2] 256: const float max_cost = _search_info.analytic_expansion_max_cost;
[planner_server-2] 257: if (*std::max_element(node_costs.begin(), node_costs.end()) > max_cost) {
[planner_server-2] 258: // If any element is above the comfortable cost limit, check edge cases:
[planner_server-2] > 259: // (1) Check if goal is in greater than max_cost space requiring
[planner_server-2] 260: // entering it, but only entering it on final approach, not in-and-out
[planner_server-2] 261: // (2) Checks if goal is in normal space, but enters costed space unnecessarily
[planner_server-2] 262: // mid-way through, skirting obstacle or in non-globally confined space
[planner_server-2] Segmentation fault (Address not mapped to object [(nil)])
[ERROR] [planner_server-2]: process has died [pid 205, exit code -11, cmd '/karelics_workspace/karelics_brain/nav2_ws/install/nav2_planner/lib/nav2_planner/planner_server --ros-args --log-level info --ros-args -r __node:=planner_server -p use_sim_time:=True --params-file /tmp/tmp3_qleyov --params-file /tmp/tmp6ufp_bh7 -r /tf:=tf -r /tf_static:=tf_static'].
Note: On the last part of the trace the line with
>
seems to point to wrong line. On earlier logs it was the line 257 the if clause with dereferencing the max_element. Not sure why. Could be backward_ros and changing the file on runtime combination or something.Additional information
I have been investigating this now for a bit. According to StackOverflow comment dereferencing the iterator returned by
std::max_element
when the input array is empty, can lead into segmentation fault. Runningstd::max_element(EmptyArray)
returnsEmptyArray.end()
. I believe that is what happens in this case.To have the
node_costs
array to be empty thenum_iterations
has to be 0. There is a comment on line 196 about the possibility but I fail to notice any action taken for that. Basically if distance between from and to poses is less than sqrt(2) we get the zero.For myself I fixed this by returning early in similar fashion as on line 186. Simple
if (d < sqrt_2) return AnalyticExpansionNodes();
and that seems to be working fine for me.Feature request
Feature description
Implementation considerations
The text was updated successfully, but these errors were encountered: