Reorganize some of the BehaviorTree-related code #957
Conversation
mjeronimo
requested review from
SteveMacenski,
bpwilcox,
crdelsey,
orduno and
mhpanah
| @@ -69,6 +70,18 @@ class BehaviorTreeEngine | |||
| } | |||
|
|
|||
| protected: | |||
| // Methods used to register as (simple action) BT nodes | |||
| BT::NodeStatus globalLocalizationServiceRequest(); | |||
There was a problem hiding this comment.
globalLocalizationServiceRequest and initialPoseReceived arent used, that was part of my proposal to just delete them. They break important Navigation encapsulation concepts.
There was a problem hiding this comment.
I think they were used by the auto-localization version of the navigation XML. @mhpanah can confirm. We took out the XML files for the Dashing release, but didn't remove this functionality.
@SteveMacenski I'm thinking that we have any navigation-specific BT nodes defined in a plug-in that the BehaviorTreeEngine loads up. Are you OK with functions like this living here for now and then going into a plug-in?
There was a problem hiding this comment.
The navigator shouldn't be tightly coupled with the positioning system, hooks are OK but the core navigator code in my opinion shouldn't contain positioning specific items. Since this is the "core" of the BT (and in future loading of nodes) this seems like exactly the place this shouldn't be. Probably as a utility?
Yes the factory items are all good since we haven't gotten to plugin-izing things.
There was a problem hiding this comment.
How about the localization-related nodes as a separate plugin from the navigation-related nodes? For now, both would be registered in the BehaviorTreeEngine, but soon, they would be separated out so that the BehaviorTreeEngine is generic and each plug-in loads its own node types.
There was a problem hiding this comment.
I don't think localization-based nodes should exist except for checking-state to trigger something like "stop navigator, I'm delocalized an entering a localization-driven motion mode to re-establish localization". The navigator's job is to take positioning and go to a place according to some rule set that the BT defines. I think that breaks the encapsulation of a pure-navigator. The inputs to the Navigator are positioning and a Director (autonomy, brain, whatever, executor) who decides what the navigator should do as part of its higher level objective. I would like to make sure to keep these concepts separated.
It seems to me it would make most sense if the autolocalization mode was an action server that could be called from a node that checks localization quality and triggers, but it itself shouldn't really be in the Navigators behavior tree
There was a problem hiding this comment.
Ok, I understand your point about trying to keep the navigation BT "pure." However, if we made the autolocalization an action server and chose to implement this action server with a BT (similar to how the navigation is implemented using a BT), we'd still need to define the localization-related BT nodes (presumably using a plugin-model) for the auto-localization action server. We've then also introduced yet another node. How about instead using the XML include mechanism supported in the 3.0 library to move the auto-localization sub-tree into its own XML file. The net effect is that the navigation BT references only a single, external node/subtree. We have the benefit of encapsulation without the overhead of introducing another node and action server.
There was a problem hiding this comment.
Why would autolocalization need to be a behavior tree? The package is called nav2_bt_navigator and that has a set of expectations on it - trying to have a package that does everything is probably not a good way to go.
Auto-localization is using experimental tech that only works for 1 robot model in simulation (safely), I dont think that is mature enough to justify making compromises for in design. If there's some hacky-way we get auto-localization into the stack, I think that's probably more appropriate since its not something most users will be using at this point as it won't work on most people's configurations.
There was a problem hiding this comment.
Reading back over this: the 2 fns in question both just call services or topics. Why do they have to be in the navigator? The autolocalization module should be more than capable of creating it own service clients, correct @mjeronimo? @mhpanah
|
@SteveMacenski Sorry about the duplicated effort. I had some code from my other PR that I split off. Going forward, I created a meta-ticked: #958 for the BT work. Can you add the items that you think we should do. Then, we can assign names and split up the work. Thanks. |
|
@SteveMacenski How about we proceed with this PR and then you take care of the clear costmap change that you mention as a follow-up? |
|
No worries, it happens. Clearing costmap recovery as another PR is fine with me. Only thing to me that is worth blocking for discussion is removing those extra functions in the thread above |
|
If OK with @mhpanah, I can remove those functions and then we can introduce later when we're actually using them. Also, if they are localization-specific, they'd be contained in a separate plug-in from the navigation-specific plugin. |
|
@mjeronimo @SteveMacenski Very soon we will re-introduce the auto-localization functionality back to the stack. My vote is to keep it, but if we prefer to remove it now and reintroduce it later then I think that should happen in a separate PR. |
|
Ok, If its a research instrument, then I think it should be plugged into the Navigator via other means than some highly active support. But if I'm alone in that, maybe ignore me and do it anyhow. I believe that the defaults and support needs to be for stable and mature technologies people can rely on -- and then also have some experimental packages with instructions about how they should intentionally and deliberately install/configure for their application. That barrier to entry I think is important so someone doesn't do something stupid and run a jackal off their office building because it didnt see the glass Edit: This is something that needs some real discussion on other stakeholders than Intel and Simb-er-um Samsung. This should be a serious discussion point with a group of industrial and research people in the room to what extent we should bend production for frontier technology integration. Are we OK with default behavior that may be unstable? To what extent? How should we make sure things are safe for the first-time user on hardware? |
|
@SteveMacenski @mhpanah How about an offline discussion to resolve? I think it would be higher bandwidth and easier to explain in a call. |
|
Sure, but we need to make sure we have a paper trail for these types of conversations in tickets for posterity. |
|
Yes, I agree. Let's talk and then summarize here. |
|
I'll wait until the autolocalization stuff is done and I can see then what I might propose to make the bt navigator cleaner. I don't want to block this since some clear costmap recovery work I'm going to do next week needs this merged |
Eliminate the NavigateToPoseBehaviorTree class, merging it with the BehaviorTreeEngine. Put clear_entirely service client with the others in nav2_util. Move RecoveryNode to nav2_behavior_tree with the other BT nodes.
* remove nav2_robot package * remove CMakeList and package.xml dependence on nav2_robot * robot_util classes for publishing and states * changing all users of robot_ to their appropriate eq. * changing the controller to use the nav2_utils velocity publisher wrapper as well * adding shared locks properly to share get resources * correct namespace nav2_robot -> nav2_util * remove sharedptr references * resolve merge conflicts * Setting default to use AMCL topic to true * fix rebase/merge conflicts deleting robot package * removing old maintainers from navigation1 * Switch from map service to topic (#885) * Add parameter to disable map server * Move parameter check to getParameters * Uncrustify * Don't mark static layer as current until we have a map * Make sure static layer gets initialized Make sure static layer gets initialized, even with an empty layer, so it doesn't crash if you disable the map service and enable the obstacle layer * remove map service from static layer Only use map topic instead Subscribe to map topic with default transient local volatility Add parameter "map_subscribe_transient_local" (default=true) for backwards compatibility with volatile publishers * Change AMCL to use only the map topic, not map service * remove now-unused MapServiceClient * uncrustify * only set current_=true when first map received * Fix nav2_costmap inflation tests. * Fixing collision tester test. * Revert "uncrustify" This reverts commit 82f88a7. This uncrustify failure is enforced differently depending on whether we are on the dashing branch or on the master branch. This particular commit is only appropriate on the dashing branch, but is already present there. * change primitives to recoveries (#705) * change primitives to recoveries * changing all over new bringups with nav2_recoveries * renaming primitive_name -> recovery_name * stop robot on cancel in recovery * fixed recovery node halt bug * removing nav2_robot package * adding lifecycle publisher for dwb * Removing unneeded tf initialization calls * Remove dedicated thread from costmap * Add support for new createTimerInterface API * Fix system test launch file. * updated the build scripts for ROS2 Dashing (#924) * run build from anywhere in tree * renaming dependencies because its driving me up a wall autocomplete for nav doesnt work * remove extra files re-introduced in PR #704 * set align critics scale to 0 (#936) * fix typo in build tool updates * Nav2-rviz-plugin changes. (#937) * I added a timeout to spin_until_future_complete, so rviz doesn't get frozen if something goes wrong in the navigation stack. I did some changes in the start navigation; - changed the funtion type to boolean - added returns to the funtion, so it will return false if something goes wrong. I changed goal_tool added a if statement to check if start_navigation doesn't return false before calling any other functions in goal_tool * made the code cleaner and changed the error messages. Reduced the timeout value to 3 seconds. The timeouts will be parametrized in the future. * added timeout parameter * fix linter failures in Rviz plugin (#979) * I added a timeout to spin_until_future_complete, so rviz doesn't get frozen if something goes wrong in the navigation stack. I did some changes in the start navigation; - changed the funtion type to boolean - added returns to the funtion, so it will return false if something goes wrong. I changed goal_tool added a if statement to check if start_navigation doesn't return false before calling any other functions in goal_tool * made the code cleaner and changed the error messages. Reduced the timeout value to 3 seconds. The timeouts will be parametrized in the future. * added timeout parameter * Update navigation_dialog.cpp * Update navigation_dialog.hpp * Update navigation_dialog.cpp * Update navigation_dialog.hpp * Fixing typo in dependencies (#982) * Fixing typo in dependencies * Also remove some duplicate dependencies * Fixing robot_state_publisher dependency * Add support to change the local controller rate. (#970) * Add back parameter to configure DWA rate * Report out if we missed our control loop time. * Fix the dashing build on OSRF build farm. (#935) All our builds have boost installed due to other dependencies, so we've never run into this. The build farm must build with all other dependencies removed somehow. * bump for release to 0.2.2 * Fix for TEB local planner buildable (#981) Signed-off-by: vinnamkim <vinnam.kim@gmail.com> * adding rate loops where applicable (#743) * adding rate loops where applicable * throttle while loops with sleep_for * sleep_dir -> sleep_dur * enabling closed-loop control in spin (#983) * implemented controlled spin * remove files reintroduced (#991) * collision checkers in recoveries now look sufficiently far ahead in the local coordinate frame * removing whitespace * revert to global: need PR for TF positioning merged before can fix that * Creating a simple action server to be able to invoke Random Crawl from BT (#945) added random crawl action to bt * amcl particlecloud is best effort (#1003) Signed-off-by: Mikael Arguedas <mikael.arguedas@gmail.com> * Disable displaying RobotModel in Rviz by default (#1001) * returning failed if recovery cannot be completed due to collision * backing out collision detection into a new fuction * removing extraneous visualization from tests that have been removed * revert my own config * make ye old linter happy * Footprint subscriber const topic Signed-off-by: Shane Loretz <sloretz@osrfoundation.org> * Costmap subscriber const topic Signed-off-by: Shane Loretz <sloretz@osrfoundation.org> * fixing PR feedback to stopRobot() in better location and parameterization * removing unnecessary smart pointers that dont effect lifecycle * lifecycle executor doesnt need to be a pointer * removing extra PR template field in application no one uses * Reorganize some of the BehaviorTree-related code (#957) * Reorganize some of the BehaviorTree-related code. Eliminate the NavigateToPoseBehaviorTree class, merging it with the BehaviorTreeEngine. Put clear_entirely service client with the others in nav2_util. Move RecoveryNode to nav2_behavior_tree with the other BT nodes. * Remove method signature that isn't implemented yet * Add a missing dependency * Address a couple linter issues * Tf based positioning (#993) * remove topic based positioning for TF and removing extraneous build fields * robot utils complete for getting positioning from TF * readding the custom logger for getting current pose * remove extra TODO * fixing fun crustyness * pass by ref not shared ptr * wrapping costmap 2d's get pose with util * revert costmap 2d plugin initialization swap * fixing more uncrustified issues while I wait * adding transform listener to buffers * adding tf listeners and missing dependencies * editing test cases * trying no exception * first stab at unit tests with TF2 for costmap 2D integration test * adding the create timer interface * adding updated pose info for testing * com'n crusty code, why are you different in CI? * fixing merge conflicts p2 * remove unnecessary CMake code to remove warning (#1005) Signed-off-by: Mikael Arguedas <mikael.arguedas@gmail.com> * disabling by default the debug markers for local planner (#1007) * adding deprecation warning for topic-based goal navigation * fixing spin recovery * incorrect topic names * Provide correct path to default map (#987) * Extend the costmap's namespace with the hosting node's namespace (#999) * Parameter dumping utility (#902) * Add a parameter dumping utility to nav2_utils * Add some convenience functions to simplify declaring parameters * Add some examples for AMCL * Remove stray example file * Default to dumping all nodes; throw/catch exceptions * Address some review feedback * Fix node name collisions (#1020) * Fix node name collisions Make sure service clients have a unique node name (this was a problem in recoveries nodes) Delimit ros arguments with new `--ros-args`,`--` * Uncrustify * Fixing linter failures introduced in master (#1029) * Fixing linter failures introduced in master * Sort of fix test_occ_grid_cpp test but disable since it's flaky * Disable failing test_collision_checker test * Recovery using odometric poses and costmaps (#1023) * recovery behaviors using the odometric positioning for pose estimation * use local costmap information in recoveries * fixing costmap namespacing for collision detector * odometry based recoveries with collision checker global frame model * on download, rosdep all necessary items, calling on each build is redundant * doing rosdep install in a single call * removing extraneous cd * adding recoveries XML to simulation launch file (#1031) * adding recoveries XML to simulation launch file * fix typo from name vs frame odom * refactor goal handle into panel clean up remove extra log info * remove nav2 goal tool and address review feedback * fix lint issue * send pose via qt signal through goal tool * remove navigation dialog minor clean up * use enum for action states fix compile issues remove extra semicolons fix uncrustify * add QBasicTimer cleanup timer * remove more unnecessary headers * Add parallel testing jobs for each rmw vendor * Fix yaml * Fix yaml * Use custom version of nightly instead * Extend workflow for rmw testing on release * Fix typo * Try speeding up build by omitting job limit * Ensure checksum.txt exist for restoring cache * Set --load-average +1 from -j For a 2CPU/4096MB CI resource * Reorganized executor and add caching nonce Improve env DRY'ness by using separate executors Enable debug builds Add nonce to caching to help avoid caching key collisions between debug and release workflow paths * Fix awk to print env to checksum * Add .dockerhub build hooks and add FROM_IMAGE to arguments * Switch to custom docker repo * Use symlinks to place Dockerfile with hooks Tweak build hook to compensate for dockhub assumptions DockerHub repo build rules: source: master tag: master Dockerfile: /.dockerhub/nightly/Dockerfile source: master tag: master-rmw Dockerfile: /.dockerhub/nightly-rmw/Dockerfile source: /^.*-devel$ tag: {sourceref} Dockerfile: /.dockerhub/devel/Dockerfile * Have CI Dockerfile use ccache * Add .dockerignore to avoid braking build cache So tweaking unrelated files doesn't invalidate: COPY ./ navigation2/ * Word wrap long command * Combine ccache with Circle CI caching * Correct terms for underlay and overlay upstream -> underlay navigation2,build,source -> overlay * Change setup_underlay to setup_dependencies * Share ccache across overlay and underlay * Correct typo * Add new line * Always save ccache avoid unnecessary checking step * Hack to avoid breaking docker build cache for COPY when changing Dockerfile itself. This shouldn't be needed, but does not seem to work without ** ¯\_(ツ)_/¯ * Add adjustable build failure behavior When FAIL_ON_BUILD_FAILURE is unset failures from colcon build will be supressed allowing for docker build to succeed. This prevents DockerHub CI images from becoming stale when branch build brakes, previously blocking DockerHub CI image builds. * Enable nightly release tests now that docker CI builds unset FAIL_ON_BUILD_FAILURE thus its status is less indicative of master build breaking * Avoid cache collisions for PRs with same branch name by including unique PR number into cache key CircleCI branch name substitution using {{ .Branch }} does not prefix names with the repo name. Thus PRs from separate forks using the same branch name could collide. https://circleci.com/docs/2.0/env-vars/#built-in-environment-variables https://circleci.com/docs/2.0/configuration-reference/#save_cache * Do not share underlay cache across branchs/PRs * Abridge lengthening cache key names * Account for failed underlay builds if we don't happen to clean the underlay when we keep the underlay from the docker image Copy logic to overlay for consistency in behavior * Add epoch to cache key given caches are immutable and use spaceless string concatenation in yaml to split long key names into multiple lines. Given the immutability of caches, and that cache key matching is prefix based, {{ epoch }} must be the last part of the cache-save key. https://discuss.circleci.com/t/add-mechanism-to-update-existing-cache-key/9014/13 https://circleci.com/docs/2.0/caching/#restoring-cache https://circleci.com/docs/2.0/configuration-reference/#save_cache https://stackoverflow.com/questions/3790454/how-do-i-break-a-string-over-multiple-lines/21699210#comment65667554_21699210 * Clean workspace incase build_failed * Improve underlay build cache By only copying .repo file for vcs import of underlay and move overlay copy just before rosdep install for overlay * Remove duplicate copy of overlay .repos file given the dockerfile now copies this directly into the overlay wrokspace * Rename workspaces to underlay and overlay To complete the standardization of terminology ros_ws -> underlay_ws nav2_ws -> overlay_ws * Add on_checkout anchor to make config more DRY * Target nonfree tag to include connext * Add ccache stats command to print and zero ccache statistics as well as ccache version and config after restore of ccache cache and after each underlay/overlay build * Cleanup code coverage pipline Use offical Codecov orb for uploading make the coverage script more DRY * Remove unnecessary indents * Split debuild build and test and make release tests more DRY * Shorten config * Print out lcov command when runing script * Use to avoid infinite recursion * Add common_environment refrence and make jobs more DRY * Store ccache logs for CI debugging * Simplify code coverage script to prevent untented shell glob expansion * Rename fields * Store test result * Copy test results into expected directory structure See directory structure example in https://circleci.com/docs/2.0/configuration-reference/#store_test_results * Store test artifacts for debug uploaded results * Support parallel testing * Fix xargs line splitting * Run colcon test-results to fail on test errors Needed to use return status from test-results * Lookup historical timing data by classname for split-by E.g. "costmap_queue.CostmapQueue" in `costmap_queue/utest.gtest.xml` : <testsuites tests="5" failures="0" disabled="0" errors="0" timestamp="2019-08-12T06:57:26" time="0.499" name="AllTests"> <script/> <testsuite name="CostmapQueue" tests="5" failures="0" disabled="0" errors="0" time="0.499"> <testcase name="basicQueue" status="run" time="0" classname="costmap_queue.CostmapQueue"/> <testcase name="bigTest" status="run" time="0.496" classname="costmap_queue.CostmapQueue"/> <testcase name="linearQueue" status="run" time="0" classname="costmap_queue.CostmapQueue"/> <testcase name="crossQueue" status="run" time="0.001" classname="costmap_queue.CostmapQueue"/> <testcase name="limitedQueue" status="run" time="0.002" classname="costmap_queue.CostmapQueue"/> </testsuite> </testsuites> * Show split count and omit unneeded xargs * Simplify build using colcon mixins and add build args for underlay and overlay mixins * Override mixing in build hook * Rename build hooks and tags * Build overlay with same settings to maximize ccache hit rate * Correct mixin typo * Make checksum.txt more informative by piping message into directly into file for debugging and simplify digests inclusion for spotting hash diffs * Fixing up paths for codecov.io report (#1036) * Update .dockerignore Co-Authored-By: Carl Delsey <carl.r.delsey@intel.com> * Move rwm jobs back into nightly workflow * Set genhtml as default and add case for ci * Refactor config using commands * Use paths instead of env * Revert ccache anchor name * Fix install linking * Avoid using paths as keys prefix matching fails when key includes slashes * Rename yaml anchor * Make sure lcov is installed for CI * Fix the build due to access specifiers (#1040) No idea why this builds under GCC but these changes were needed to build under clang. * Mark timerEvent override (#1037) * Add dummy Dockerfiles for setting repository links on Docker Hub * Set exit and print trace in script * Tweek tag names * nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds (#1048) * nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds * Update nav2_costmap_2d/plugins/static_layer.cpp oops! Co-Authored-By: Carl Delsey <carl.r.delsey@intel.com> * Rewrite nav2_map_server (#989) Add GraphicsMagick API and use it to implement map_saver, map_loader Removes dependency on SDL, creates installation dependency on libgraphicsmagick++; apt package libgraphicsmagick++1-dev Allows saving in trinary, raw, or scale mode. Added configurable image format pgm, png, etc. Add warning when using scale mode with opaque format. Remove SDL from map server Extract repeated MapMode to shared enum class Switch writing YAML to same library that reads it (yaml-cpp) Add --help alias to map saver -h Extract map yaml loading to its own function, with better error messages. Also fixed crash due to misuse of std::string constructor. * Migrate image tag to rosplanning repo * Enable AMCL to initialize at the origin, under control of a parameter * Make sure everything shuts down cleanly * Some cleanup and linting * Address review feedback * Also have to update the YAML file * Remove the connection object for now. Needs further investigation * Find all uses of yaw to quaternion and use new function * Simplify a quaternion copy * Restore COLCON_IGNORE file * Oops. Missed a package dependency * Fixing a rebase error * Add missing dependencies to system tests (#1059) Signed-off-by: Louise Poubel <louise@openrobotics.org> * add tf2_geometry_msgs dependency * Fix the updown test. (#1061) * Fixing linter errors, accidentally commited. * Export envs for arg in build hook * Remove the setSimTime() method - not needed anymore * Remove ros2param package dependency * Fix indentation (#1065) * BT Service node & implementations (#1055) * generic ros service caller BT node * BT specialty service callers using BT node service * rename to reinitialize global localization * uncrustify/lint * changes per PR requests * changing pose topics to goal_pose * Removing deprecation warning * Removing an accidentally re-introduced file * use seconds in Duration * Enable global planner tester (#927) * use duration from seconds * Add ctest loop script to system tests * Set line buffering to 1 in system test launch * Update README * Update README for using ctest_loop script * Recombine AMCL library components with AMCL to have consistent license (#1074) * Move AMCL library files back to nav2_amcl package * Fixing up naming so things compile again * Transition copyright disable check to follow eig3 files. * Transition amcl files license to LGPL 2.1 * Adding missing copyright headers to nav2_bringup package * Adding missing copyright headers to nav2_common * Fixing up include guard to match coding style * Reformatting license to pass the copyright check The change here is only whitespace changes - line length and paragraph spacing, as well as the addition of the license title * Relicensing file to better match the existing package * Fixing readme to account for the new change * Begin updating nav2_behavior_tree README * Continue update of nav2_behavior_tree readme * Add link to behavior tree basic nodes to README Adding a link to the upstream documentation on behaviortree.dev * Clarify Licensing throughout the repo (#1080) * Add proper SPDX license IDs and repo LICENSE file * Fixing a minor linter typo * Use python yaml safe loader (#1085) * Null-terminate string to dirname function (#1093) possibly responsible for heisenbug #1079 (comment) * Update to latest v2 BT Library (#1088) * Revert CreateTimer change * Revert "nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds (#1048)" This reverts commit 7c473cd. * Fix dependency exports * Fixing missing boost dependency
Description
Future work that may be required in bullet points