From e2282f735add84ac5e568c7e75a097f6bc2979ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Wed, 24 Apr 2024 08:51:08 -0500 Subject: [PATCH] Plugin to accumulate segmentation confidence (#12) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * bump to 1.2.0 for iron release * Iron sync June 9: 1.2.1 (#3615) * Improve usability of PWAs in Dev Containers (#3576) * Add WIP icons * Add WIP icons for gzweb * Add WIP icons for glances * Set cross origin to use credentials ensuring auth cookie is included in request header when requesting for web app manifest file thus avoiding CORS policy violations in browser when accessing forwarded codespaces ports from the web > The request for the manifest is made without credentials (even if it's on the same domain), thus if the manifest requires credentials, you must include `crossorigin="use-credentials"` in the manifest tag. - https://web.dev/add-manifest/ - https://stackoverflow.com/a/57184506/2577586 * Use ReqHost variable in templates to account for X-Forwarded-Host value in header * Delete duplicate manifest * Set id property in app manifests so we can address them independently from their start_url - https://developer.chrome.com/blog/pwa-manifest-id/ * Ensure apps are uniquely identifies by adding trailing slash to id and thus different URI directories * Refactor root landing page into nav2 app by moving page file into nav2 sub folder adding root redirect pointing to /nav2/ and updating html, markdown, manifest files respectively * Fix https detection for Caddy reverse proxies by also checking X-Forwarded-Proto in request header * Remove unnecessary files * Prune smaller images * Prune duplicate icon * Clean up html tags * Update manifest icons * Rename icons * Revert "Prune duplicate icon" This reverts commit 571040173ca83716dfd2f6d5db4b351389a557a8. * Add back favicon for shortcut * Add self index for completeness and bookmarking * Simplify icon linking * Delete binary files * Fix hyperlink path * Include image files using gitattributes to track these binary files via git LFS * Add icons using git lfs * Standardized all icon paths * Use external links for icons to avoid the need for using git LFS although this is a bit of a hack * Stage any and maskable icons * Use any and masked icons * Set colors to match maskable icon colors * Update icon * Use lossless compression without removing background - https://shortpixel.com/online-image-compression * Use WebP instead of PNG for smaller file sizes - https://en.wikipedia.org/wiki/WebP * Move icons into icons folder * Use _SRV environment variables for service paths * Download media files from github during docker image build to avoid adding always online dependencies when creating or starting dev containers * Delete media icons from git repo now that we download media from anonymized URLs on github - https://docs.github.com/en/get-started/writing-on-github/working-with-advanced-formatting/attaching-files * Add comments * Enable file browsing for non app paths for remote debugging of media and asset files * Consolidate assets into single folder * Add links for file browser paths to Server Diagnostics * Delete unused symlink * Update landing page to match manifest by including same shortcuts and start url * Patch gzweb to disable modelList avoiding 404s for thumbnails as they are hardcoded into js * Update comments * Simplify Caddyfile by reverting to symlinking but add ROOT_SRV env for custom overriding * Loop over nav2 srv folders when symlinking to generalize over folder names * Add matcher for file browsing root directory while still redirecting to nav2 app by default * Use placeholders for root variable to consolidate env default fallback settings e.g `:/srv` * Promote file browser in Nav2 app shortcuts * Fix and update SRV envs * Postpone symlinking for Nav2 web app to when post-create-command script then runs given full repo is not copied into builder stage in Dockerfile. While this could be postponed to update-content-command leaving it here avoids blowing user changes after the container has been created or modified. * Add guard to check if srv folder exists * Add refresh rate shortcuts to glances * Add file browser shortcut to nav2 * Set scope for nav2 PWA to root to allow for opening child apps inside nav2 app * Display child apps in fullscreen mode by default as users can still open them in standalone via nav2 app given the nav2 app's scope is the parent root path * Update shortcuts and landing page * Document PWA scope and installation order when using Nav2 PWA scoped as root * Revert setting scope for nav2 PWA to root path as adding file browser shortcut to nav2 PWA is not worth the trouble of having to explain installation order caveats and URL launch behavior. File browser shortcut is still accessible from inside nav2 pwa launcher but merely displays in browser preview given root / is out of scope for /nav2/ * Update server diagnostics for troubleshooting * Verify checksum of archive before extraction incase anonymized URL changes expected archive * Fix the condition in ackerman motion model constraints (#3581) * Fix the condition in ackerman motion model constraints * Fix ackerman motion model tests * Fix another ackerman motion model test * Add viz_expansions parameter for debug (#3577) * viz_expansions * lint * switch to unique_ptr * readme update --------- Co-authored-by: Guillaume Doisy * Fix broken symlink for gzweb (#3585) to load world models * (collision monitor) add limit polygon type (#3519) * add LIMIT polygon type * fix unit tests * Fix MIN_POINT doesn't exist * Fix Action type enum * FIX velocity used * FIX unit test point distance increase point distance to not be in limit field * Update collision_monitor_node_test.cpp * fix status name not updated * [MOD] only single linear limit * Apply review comments * Update nav2_collision_monitor/include/nav2_collision_monitor/types.hpp --------- Co-authored-by: Steve Macenski * Fix broken link to contributing guidelines (#3587) The original URL (https://navigation.ros.org/contribute/index.html) seems not to exist, returning an HTTP 404. Hence, I've replaced the link with a page that seems most relevant. * Adding Our Sponsors - May 2023 (#3593) * adding our sponsors - may 2023 * adding blurb * adding links * adding links * adding links * adding Open Nav * Fix dynamic polygons vertices not updated for different frame (#3591) * Fix dynamic polygons vertices not updated for different frame * Update nav2_collision_monitor/src/collision_monitor_node.cpp --------- Co-authored-by: Steve Macenski * Fixing 3586: rviz panel operating in multiple non-standard frames (#3595) * Fixes #3586 on rviz time stamping * removing timing from rviz testing panel * Add CostmapFilterInfoServer as a component (#3596) * adding iron to the readme table statuses (#3598) * adding iron to the readme table statuses * collapsing table * adding citation RPP * Resolve #3532: reset i (#3597) * [MPPI] empty path_follow_critic proper fix (#3599) * [MPPI] empty path_follow_critic proper fix * fix linting issue --------- Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski * Refactor sim_time and composable node usage in Collision Monitor (#3604) * Refactor sim_time and composable node usage in Collision Monitor launch script * Added namespace support * Fix comment * Fix costmap 2D test failures (#3611) * costmap test without action * test2 * try just printing first thing * Update test_costmap_2d_publisher.cpp * Compile flags non-virtual-destructor (#3609) * added compile flags * cleanup * cleanup * cleanup * add virtual destructors * uncrusify * updating to 1.2.1 for release --------- Co-authored-by: Ruffin Co-authored-by: Alexandr Buyval Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Hyung-Taik Choi Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Filipe Cerveira Co-authored-by: Joshua Wallace * remove non-virtual-dtor flag from nav2_package() * WIP: Merge segmentation layer and GPS wpf * WIP: remove wrong whitespace * WIP: Bring back ffollow gps waypoints * WIP: error codes to gps waypoint follwer * Fix GPS waypoint follower error codes * Fixing build warning (#3667) (#3672) (cherry picked from commit 7d4b1992811ccc9d36566d29251fdc8eaee66efc) Co-authored-by: Steve Macenski * Install nav2_behavior_tree utils outside of BUILD_TESTING (#3692) (#3697) (cherry picked from commit abf3e85d05ac1dbb3a10bfda4afe37cf780184f4) Co-authored-by: Aaditya Ravindran * Fix segfault on relaunch of controller server * Fix the velocity smoother being stuck when the deadband is too high (#3690) (#3714) * Move last_cmd update before deadband * fix lint (cherry picked from commit cb34d0ce1d24c1c437f548834a31a2ee8c4d9889) Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> * Iron sync 2, Aug 4, 1.2.2 (#3740) * Only apply Wnon-virtual-dtor if the compile language is CXX (#3614) * https://stackoverflow.com/questions/25525047/cmake-generator-expression-differentiate-c-c-code Signed-off-by: Ryan Friedman * Fix map not showing on rviz when navigation is launched with namespace (#3620) * Fix Wshadow errors and enforce it (#3617) * Fix Wshadow errors and enforce it Signed-off-by: Ryan Friedman * Remove workaround for pluginlib * This was only needed because it was included transitively * By finding and linking properly, the compiler flags get propogated as SYSTEM correctly Signed-off-by: Ryan Friedman --------- Signed-off-by: Ryan Friedman * add-Wnull-dereference and fix warnings (#3622) Signed-off-by: Ryan Friedman * updating mppi's path angle critic for optional bidirectionality (#3624) * updating mppi's path angle critic for optional bidirectionality * Update README.md * correct error message (#3631) * correct error message * clean up * cleanup * remove header * Let Navigators have different error codes (#3642) * Change ERROR to DEBUG * INFO message on init * format code * Replace newlines with spaces * fixing path angle critic's non-directional bias (#3632) * fixing path angle critic's non-directional bias * adding reformat * adapting goal critic for speed to goal (#3641) * adapting goal critic for speed to goal * retuning goal critic * add readme entries * Update critics_tests.cpp * Fix uninitialized value (#3651) * In NAV2, this warning is treated as an error Signed-off-by: Ryan Friedman * Fix rviz panel node arguments (#3655) Signed-off-by: Nick Lamprianidis * Reduce out-of-range log to DEBUG (#3656) * Adding nan twist rejection for velocity smoother and collision monitor (#3658) * adding nan twist rejection for velocity smoother and collision monitor * deref * Ceres exposes a namespaced export and recommends it in their docs (#3652) Signed-off-by: Ryan Friedman * Enable multiple MPPI path angle modes depending on preferences in behavior (#3650) * fixing path angle critic's non-directional bias * adding reformat * handle linting * add utility unit tests * adding unit tests for path angle * MPPI: Support Exact Path Following For Feasible Plans (#3659) * alternative to path align critic for inversion control * fix default behavior (enforce_path_inversion: false) (#3643) Co-authored-by: Guillaume Doisy * adding dyaw option for path alignment to incentivize following the path's intent where necessary * add docs for use path orientations * fix typo --------- Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX (#3662) * Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX * Update CMakeLists.txt * Fix smoother server tests (#3663) * Fix smoother server tests * Update test_smoother_server.cpp * fix some new build warnings from sync (#3674) * fix some new build warnings * fixing last issue * Update navigate_through_poses_action.hpp * adding unsigned int to tests * all to unsigned shorts * test new warning resolution * Update * convert unsigned shorts to uint16_t for linter * Fix costmap publisher test (#3679) * added printouts * ignore system tests * fix * cleanup * Update test_costmap_2d_publisher.cpp remove space * remove empty message (#3691) * Collision Monitor fixups (#3696) * Fix max_points -> min_points in parameters * Move robot_utils.hpp include to source where it actually using * Remove double-description of getTransform() * Use ParameterFile (allow_substs) (#3706) Signed-off-by: ymd-stella * nav2_bt_navigator: log current location on navigate_to_pose action initialization (#3720) It is very useful to know the current location considered by the bt_navigator for debug purposes. * nav2_behaviors: export all available plugins (#3716) It allows external packages to include those headers and create child classes through inheritance. * changing costmap layers private to protected (#3722) * Update costmap_2d_ros.cpp (#3687) * updated nav2_behavior_tree test util install path (#3718) * launch linting (#3729) * adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (#3728) * adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially * fix test failures * Update RewrittenYaml to support list rewrites (#3727) * allowing leaf key rewrites that aren't dcits (#3730) * adding checks on config and dynamic parameters for proper velocity and acceleration limits (#3731) * Fix Goal updater QoS (#3719) * Fix GoalUpdater QoS * Fixes * adding tolerance back in for smac lattice and hybrid-A* planners (#3734) * Completing Hybrid-A* visualization of expansion footprints PR (#3733) * smach_planner_hybrid: add support visualization for hybrid Astar * smac_planner_hyrid: revert some * smach_planner_hybrid: improving code quality * utils: add some useful functions * utils: fix mistake * nav2_smac_planner: fix format problem * utils: fix format and revise functions * smach_planner_hybrid: delete _viz_expansion parameter * smac_planner_hybrid: fix format * README: update parameter * utils: corrct mistake return * utils: make timestamp a const reference * nav2_smac_planner: correct format problem * add unit test functions * further detection of element equality * test_utils: add non-trival translation and rotation * smac_planner_hybrid: pass value instead of references * completing hybrid A* visualization --------- Co-authored-by: xianglunkai <1322099375@qq.com> * Update README.md (#3736) * Update README.md * Update README.md * sync iron to 1.2.2 to release --------- Signed-off-by: Ryan Friedman Signed-off-by: Nick Lamprianidis Signed-off-by: ymd-stella Co-authored-by: Ryan Co-authored-by: Filipe Cerveira Co-authored-by: Joshua Wallace Co-authored-by: RBT22 <43723477+RBT22@users.noreply.github.com> Co-authored-by: Nick Lamprianidis Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Aaditya Ravindran Co-authored-by: gyaanantia <78275262+gyaanantia@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: xianglunkai <1322099375@qq.com> * nav2_collision_monitor: collision detector (#3500) (Iron backport) (#3758) * nav2_collision_monitor: collision detector (#3500) * fix * mppi: return NO_INFORMATION when the checked point is outside the costmap (#3816) (#3817) otherwise the controller crashes at ObstaclesCritic::costAtPose because x_i and y_i isn't initialized. (cherry picked from commit 6b250a7c57536ee43a402c9820ac2a2acdb8bc13) Co-authored-by: Chuanhong Guo * Iron Sync 3 - Sept 25 (#3837) * Same orientation of coordinate frames in rviz ang gazebo (#3751) * rviz view straight in default xy orientation Signed-off-by: Christian Henkel * gazebo orientation to match rviz Signed-off-by: Christian Henkel * rotating in direction of view --------- Signed-off-by: Christian Henkel * Fix flaky costmap filters tests: (#3754) 1. Set forward_prune_distance to 1.0 to robot not getting lost 2. Correct map name for costmap filter tests * Update smac_planner_hybrid.cpp (#3760) * Fix missing mutex in PlannerServer::isPathValid (#3756) Signed-off-by: ymd-stella * Rename PushRosNamespace to PushROSNamespace (#3763) * Rewrite the scan topic costmap plugins for multi-robot(namespace) before launch navigation. (#3572) * Make it possible to launch namspaced robot which rewrites `` to namespace. - It allows to apply namespace automatically on specific target topic path in costmap plugins. Add new nav2 params file for multi-robot(rewriting ``) as an example. - nav2_multirobot_params_all.yaml Modify nav2_common.ReplaceString - add condition argument * Update nav2_bringup/launch/bringup_launch.py Co-authored-by: Steve Macenski * Add new luanch script for multi-robot bringup Rename luanch script for multi-robot simulation bringup Add new nav2_common script - Parse argument - Parse multirobot pose Update README.md * Update README.md Apply suggestions from code review Fix pep257 erors Co-authored-by: Steve Macenski --------- Co-authored-by: Steve Macenski * use ros clock for wait (#3782) * use ROS clock for wait * fix backport issue --------- Co-authored-by: Guillaume Doisy * fixing external users of the BT action node template (#3792) * fixing external users of the BT action node template * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Guillaume Doisy --------- Co-authored-by: Guillaume Doisy * Fixing typo in compute path through poses error codes (#3799) Signed-off-by: Mannucci, Anna (Bosch (CR)) Co-authored-by: Mannucci, Anna (Bosch (CR)) * Fixes for flaky WPF test (#3785) * Fixes for flaky WPF test: * New RewrittenYaml ability to add non-existing parameters * Prune distance fix for WPF test * Treat UNKNOWN status as error in WPF * Clear error codes after BT run * Remove unnecessary setInitialPose() from WPF test * Update nav2_waypoint_follower/src/waypoint_follower.cpp Co-authored-by: Steve Macenski * Clean error code in any situation * Fix UNKNOWN WPF status handling --------- Co-authored-by: Steve Macenski * Fix `min_points` comparison check (#3795) * Fix min_points checking * Expose action server result timeout as a parameter in bt navigator servers (#3787) * Expose action server default timeout in bt navigator servers * typo * duplicated comment * Expose result timeout in other actions * Proper timeout in bt node * Change default timeouts and remove comments * Remove comment in params file * uncrustify controller server * Using Simple Commander API for multi robot systems (#3803) * support multirobot namespaces * add docs * adding copy all params primitive for BT navigator (to ingest into rclcpp) (#3804) * adding copy all params primitive * fix linting * lint * I swear to god, this better be the last linting issue * allowing params to be declared from yaml * Update bt_navigator.cpp * Fix CD configuration link reference (#3811) * Fix CD configuration page reference * Add CM work on 6th ROS Developers Day reference * some minor optimizations (#3821) * fix broken behaviortree doc link (#3822) Signed-off-by: Anton Kesy * [MPPI] complete minor optimaization with floating point calculations (#3827) * floating point calculations * Update optimizer_unit_tests.cpp * Update critics_tests.cpp * Update critics_tests.cpp * 25% speed up of goal critic; 1% speed up from vy striding when not in use * Add nav2_gps_waypoint_follower (#2814) * Add nav2_gps_waypoint_follower * use correct client node while calling it to spin * changed after 1'st review * apply requested changes * nav2_util::ServiceClient instead of CallbackGroup * another iteration to adress issues * update poses with function in the follower logic * add deps of robot_localization: diagnostics * fix typo in underlay.repo * add deps of robot_localization: geographic_info * minor clean-ups * bond_core version has been updated * rotation should also be considered in GPS WPFing * use better namings related to gps wpf orientation * handle cpplint errors * tf_listener needs to be initialized * apply requested changes * apply requested changes 3.0/3.0 * fix misplaced ";" * use run time param for gps transform timeout * change timeout var name * make use of stop_on_failure for GPS too * passing emptywaypont vectors are seen as failure * update warning for empty requests * consider utm -> map yaw offset * fix missed RCLCPP info * reorrect action;s name * waypoint stamps need to be updated * Fix segmentation fault on waypoint follower * Parametric frames and matrix multiplications * Replace oriented navsatfix for geographic_msgs/geopose * Remove deprecated oriented navsatfix message * Update branch name on robot_localization dependency * Fix parametric frames logic * Rename functions and adress comments * fix style in underlay.repos * remove duplicate word in underlay.repos * update dependency version of ompl * Template ServiceClient class to accept lifecycle node * Remove link to stackoverflow answer * Remove yaw offset compensation * Fix API change * Fix styling * Minor docs fixes * Fix style divergences * Style fixes * Style fixes v2 * Style fixes v3 * Remove unused variables and timestam overrides * restore goal timestamp override * WIP: Add follow gps waypoints test * Style fixes and gazebo world inertia fix * Reduce velocity smoother timeout * empty commit to rerun tests * Increment circle ci cache idx * Remove extra space in cmakelists.txt * Fix wrong usage of the global action server * update follow gps waypoints action definition * Fix action definition and looping * update params for the unit testing * WIP: update tests * fix tests * fixes to nav2 simple commander * add robot_localization localizer * Bring back from LL client * Update nav2_simple_commander/nav2_simple_commander/robot_navigator.py Co-authored-by: Steve Macenski * missing argument in test function * small test error * style fixes nav2 simple commander * rename cartesian action server --------- Co-authored-by: jediofgever Co-authored-by: Steve Macenski * bumping iron from 1.2.2 to 1.2.3 for release * Update waypoint_follower.cpp --------- Signed-off-by: Christian Henkel Signed-off-by: ymd-stella Signed-off-by: Mannucci, Anna (Bosch (CR)) Signed-off-by: Anton Kesy Co-authored-by: Christian Henkel <6976069+ct2034@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: Hyunseok Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Anna Mannucci <19239559+annamannucci@users.noreply.github.com> Co-authored-by: Mannucci, Anna (Bosch (CR)) Co-authored-by: Tony Najjar Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Anton Kesy Co-authored-by: jediofgever * Update CMakeLists.txt (#3843) (#3844) (cherry picked from commit 2d6e9a96354c0ea763e70eedd81225635f7b9db5) Co-authored-by: Steve Macenski * bump to 1.2.4 for AVX512 binary fix * add option for sse4 and avs512 (#3853) (#3854) (cherry picked from commit 7274811c5cb512a05b87523183e29e75ace77f4a) Co-authored-by: Steve Macenski * Bumping to 1.2.5 for binary release of AVX512 patches * Start planner in the end of previous path * [MPPI Optimization] adding regenerate noise param + adding docs (#3868) (#3869) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp (cherry picked from commit 924f167382080f3ccdd000ffc34b921cb64bcf95) Co-authored-by: Steve Macenski * [MPPI] Reworked Path Align Critic; 70% faster + Tracks Paths Better! Edit: strike that, now 80% (#3872) (#3881) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp * adding refactored path alignment critic * fix visualization bug * speed up another 30% * remove a little jitter * a few more small optimizaitons * fixing unit tests * retain legacy critic * adding tests for legacy (cherry picked from commit 7009ffba5f85c50ac97fd0057924b0f1447c5e85) Co-authored-by: Steve Macenski * revert back to no gps waypoint follower * Use mutex to protect costmap reads. (#3877) (#3896) * Use mutex to protect costmap reads. Otherwise costmap can be read during a map update and return 0. * Revert "Use mutex to protect costmap reads." This reverts commit e16a44c65ee7064e2271118894b92bb6e24ce28d. * Lock costmap before running MPPI controller. * Fix typo. * Protect against costmap updates in MPP and RotationShim controllers. --------- Co-authored-by: Leif Terry (cherry picked from commit a1c9fd5ad29bb00e40ce6e696d899a2bcd50cde5) Co-authored-by: LeifHookedWireless * Adjust the Variable types in Nav2_costmap_2d pkg in [nav2_humble] #3891 (#3900) (#3901) * image.hpp #3891 * Update image.hpp (cherry picked from commit 7a7c6da59a923f06f51ca98ad0b0cc412801ad12) Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> * Log if BT rate is exceeded (#3909) (#3912) (cherry picked from commit a11cdd80665238628be9eef4b439067d4b675b7b) Co-authored-by: Steve Macenski * Fixing subtree issues with blackboard shared resources (3640) (#3911) (#3915) * fixing subtree issues * Update bt_action_server_impl.hpp (cherry picked from commit 4b4465dfc9427b95e98aef70620bedd933cfbe56) Co-authored-by: Steve Macenski * Update theta_star_planner.cpp (#3918) (#3921) (cherry picked from commit 0629ff36e36ecc135b9b13fc78f213cfe0a361ef) Co-authored-by: Steve Macenski * Change custom semantic segmentation message for image * Handle NaNs in AMCL beam sensor model (#3929) (#3936) * Handle NaNs in AMCL beam sensor model Signed-off-by: Michel Hidalgo * Use proper isnan check Signed-off-by: Michel Hidalgo --------- Signed-off-by: Michel Hidalgo (cherry picked from commit 06c35504e461d8c3233ce76ca35d63e61f95429a) Co-authored-by: Michel Hidalgo * Fix NaN in Updated PathAlign (#3943) (#3944) (cherry picked from commit 3d14d98e4a69de546036ceeb3919592c8f5646f4) Co-authored-by: Steve Macenski * Fix for robot footprint collision in obstacles critic (#3878) (#3946) * Inscribed/Circumscribed costs must be updated to take into account the current shape of the robot. Was previous only being called once in initialize(). * Add early return to avoid calculations if footprint has not changed. * Only update radius if using footprint. Add perf timers. * Remove perf timers. * Update comments. --------- Co-authored-by: Leif Terry (cherry picked from commit 98af3b9a28d9180ba22b17573f01cccaf7cbe04b) Co-authored-by: LeifHookedWireless * Enabling soft realtime prioritization to the Controller Server (#3914) (#3975) * Enabling soft realtime prioritization to the controller server * abstracting to another function * changing default priorities * linting (cherry picked from commit fbe8f56042a07513630f50b563f5690a30b1346a) Co-authored-by: Steve Macenski * Integration of mapping api with dynamic velocities changes * simplify logic * eliminate un-necessary lines of code * style * style2 * remove duplicate line from merge * remove blank line * restore nav2_utils copy_all_paramteres function. * Add segmentation confidence plugin * Remove deprecated segmentation file * Docstrings and remove repeated struct * Add docstrings & minor changes --------- Signed-off-by: Ryan Friedman Signed-off-by: Nick Lamprianidis Signed-off-by: ymd-stella Signed-off-by: Christian Henkel Signed-off-by: Mannucci, Anna (Bosch (CR)) Signed-off-by: Anton Kesy Co-authored-by: stevemacenski Co-authored-by: Ruffin Co-authored-by: Alexandr Buyval Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Hyung-Taik Choi Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Filipe Cerveira Co-authored-by: Joshua Wallace Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> Co-authored-by: Aaditya Ravindran Co-authored-by: Ryan Co-authored-by: RBT22 <43723477+RBT22@users.noreply.github.com> Co-authored-by: Nick Lamprianidis Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Aaditya Ravindran Co-authored-by: gyaanantia <78275262+gyaanantia@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: xianglunkai <1322099375@qq.com> Co-authored-by: Chuanhong Guo Co-authored-by: Christian Henkel <6976069+ct2034@users.noreply.github.com> Co-authored-by: Hyunseok Co-authored-by: Anna Mannucci <19239559+annamannucci@users.noreply.github.com> Co-authored-by: Mannucci, Anna (Bosch (CR)) Co-authored-by: Tony Najjar Co-authored-by: Anton Kesy Co-authored-by: jediofgever Co-authored-by: LeifHookedWireless Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: Michel Hidalgo Co-authored-by: Sebastian Solarte --- .../bt_action_server_impl.hpp | 2 +- nav2_costmap_2d/CMakeLists.txt | 4 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../nav2_costmap_2d/observation_buffer.hpp | 19 + .../nav2_costmap_2d/obstacle_layer.hpp | 1 + .../nav2_costmap_2d/segmentation_buffer.hpp | 628 ++++++++++++++++++ .../semantic_segmentation_layer.hpp | 175 +++++ nav2_costmap_2d/package.xml | 1 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 27 +- .../plugins/semantic_segmentation_layer.cpp | 534 +++++++++++++++ nav2_costmap_2d/src/observation_buffer.cpp | 2 + nav2_costmap_2d/src/segmentation_buffer.cpp | 238 +++++++ nav2_util/include/nav2_util/node_utils.hpp | 19 + nav2_util/test/test_node_utils.cpp | 32 + 14 files changed, 1677 insertions(+), 8 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp create mode 100644 nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp create mode 100644 nav2_costmap_2d/src/segmentation_buffer.cpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index deaeea1bdc2..3f0af303a4e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -141,7 +141,7 @@ bool BtActionServer::on_configure() node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); nav2_util::declare_parameter_if_not_declared( node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - rclcpp::copy_all_parameter_values(node, client_node_); + nav2_util::copy_all_parameters(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished double action_server_result_timeout; diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 723f81f0f9b..97dade4efc6 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rmw REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(vision_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -40,6 +41,7 @@ add_library(nav2_costmap_2d_core SHARED src/footprint.cpp src/costmap_layer.cpp src/observation_buffer.cpp + src/segmentation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp @@ -66,6 +68,7 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs std_srvs tf2 tf2_geometry_msgs @@ -88,6 +91,7 @@ add_library(layers SHARED plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp + plugins/semantic_segmentation_layer.cpp ) add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 6748cd5fcae..ccc9766e691 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -18,6 +18,9 @@ Filters noise-induced freestanding obstacles or small obstacles groups + + A plugin for semantic segmentation data produced by RGBD cameras + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index 2ff834a41e7..57d2b6a1196 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -62,6 +62,7 @@ class ObservationBuffer /** * @brief Constructs an observation buffer * @param topic_name The topic of the observations, used as an identifier for error and warning messages + * @param source_name The name of the source as declared in the parameters, used to change source specific params * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit * @param min_obstacle_height The minimum height of a hitpoint to be considered legal @@ -78,6 +79,7 @@ class ObservationBuffer ObservationBuffer( const nav2_util::LifecycleNode::WeakPtr & parent, std::string topic_name, + std::string source_name, double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, @@ -132,6 +134,22 @@ class ObservationBuffer */ void resetLastUpdated(); + /** + * @brief Set max obstacle distance + */ + void setMaxObstacleDistance(double max_obstacle_distance) + { + obstacle_max_range_ = max_obstacle_distance; + } + + /** + * @brief Get source name + */ + std::string getSourceName() + { + return source_name_; + } + private: /** * @brief Removes any stale observations from the buffer list @@ -148,6 +166,7 @@ class ObservationBuffer std::string sensor_frame_; std::list observation_list_; std::string topic_name_; + std::string source_name_; double min_obstacle_height_, max_obstacle_height_; std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 4b8409f4678..6b1a9723444 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -253,6 +253,7 @@ class ObstacleLayer : public CostmapLayer bool rolling_window_; bool was_reset_; nav2_costmap_2d::CombinationMethod combination_method_; + std::string _topics_string; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp new file mode 100644 index 00000000000..07281ce7314 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -0,0 +1,628 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#ifndef NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ +#define NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ + +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/time.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#include "vision_msgs/msg/label_info.hpp" + +/** + * @brief Represents the parameters associated with the cost calculation for a given class + */ +struct CostHeuristicParams +{ + uint8_t base_cost, max_cost, mark_confidence; + int samples_to_max_cost; +}; + +/** + * @brief Represents a 2D grid index with equality comparison. Supports negative indexes + */ +struct TileIndex { + int x, y; + + bool operator==(const TileIndex& other) const { + return x == other.x && y == other.y; + } +}; + +namespace std { + /** + * @brief Custom hash function for TileIndex to enable its use as a key in unordered_map. + */ + template<> + struct hash { + size_t operator()(const TileIndex& coord) const { + // Compute individual hash values for two integers + // and combine them using bitwise XOR + // and bit shifting: + return std::hash()(coord.x) ^ (std::hash()(coord.y) << 1); + } + }; +} + + +/** + * @brief Represents the world coordinates of a tile. + */ +struct TileWorldXY +{ + double x, y; +}; + +/** + * @brief Encapsulates the observation data for a tile, including class ID, cost, confidence, and timestamp. + */ +struct TileObservation { + using UniquePtr = std::unique_ptr; + + uint8_t class_id; + float confidence; + double timestamp; +}; + +/** + * @brief Manages temporal observations with a decay mechanism, maintaining a sum of confidences. + * Wraps a std::deque to store observations, allowing for efficient insertion and removal. + */ +class TemporalObservationQueue { +private: + + std::deque queue_; + float confidence_sum_ = 0.0f; + double decay_time_; + +public: + TemporalObservationQueue(){} + + /** + * @brief Adds an observation to the queue, resets the queue if class ID changes. + * @param tile_obs The observation to add. + */ + void push(TileObservation tile_obs) { + if(tile_obs.class_id != getClassId()) + { + std::deque emptyQueue; + std::swap(queue_, emptyQueue); + confidence_sum_ = 0.0; + } + confidence_sum_ += tile_obs.confidence; + queue_.push_back(tile_obs); + } + + /** + * @brief Removes the oldest observation from the queue. + */ + void pop() { + if (!queue_.empty()) { + confidence_sum_ -= queue_.front().confidence; + queue_.pop_front(); + } + } + + /** + * @brief Checks if the queue is empty. + * @return True if empty, false otherwise. + */ + bool empty() const {return queue_.empty();} + + /** + * @brief Gets the size of the queue. + * @return The number of observations in the queue. + */ + int size() const { return queue_.size(); } + + /** + * @brief Sets the decay time for observations. + * @param decay_time The decay time in seconds. + */ + void setDecayTime(float decay_time) + { + decay_time_ = decay_time; + } + + /** + * @brief Gets the current sum of confidence values of all observations. + * @return The sum of confidences. + */ + float getConfidenceSum() const { + return confidence_sum_; + } + + /** + * @brief Gets the class ID of the most recent observation. + * @return The class ID, or 0 if queue is empty. + */ + uint8_t getClassId() const + { + if(!queue_.empty()) + { + return queue_.back().class_id; + } + return 0; + } + + /** + * @brief Returns a copy of the deque object. Will have overhead + * due to the copy operation but avoids race conditions since + * the object in the class is not made editable by others + * @return The class cost, or 0 if queue is empty. + */ + std::deque getQueue() + { + return queue_; + } + + /** + * @brief Removes observations older than the decay time. + * @param current_time The current time for comparison. + */ + void purgeOld(double current_time) { + while (!queue_.empty()) { + double age = current_time - queue_.front().timestamp; + if (age > decay_time_) { + pop(); + } else { + break; + } + } + } +}; + +/** + * @brief Manages a map of tile observations, allowing for spatial and temporal querying. + * Utilizes an unordered_map to efficiently index observations by tile and supports locking for thread safety. + */ +class SegmentationTileMap { + private: + std::unordered_map tile_map_; + float resolution_; + float decay_time_; + std::recursive_mutex lock_; + + + public: + using SharedPtr = std::shared_ptr; + + // Define iterator types + using Iterator = typename std::unordered_map::iterator; + using ConstIterator = typename std::unordered_map::const_iterator; + + SegmentationTileMap(float resolution, float decay_time) : resolution_(resolution), decay_time_(decay_time) { + // 10k observations seemed to be a good estimate of the amount of data to be held for a decay time of ~5s + tile_map_.reserve(1e4); + } + SegmentationTileMap(){} + + // Return iterator to the beginning of the tile_map_ + Iterator begin() { return tile_map_.begin(); } + ConstIterator begin() const { return tile_map_.begin(); } + + // Return iterator to the end of the tile_map_ + Iterator end() { return tile_map_.end(); } + ConstIterator end() const { return tile_map_.end(); } + + /** + * @brief Locks the map for exclusive access. + */ + inline void lock() { lock_.lock(); } + + /** + * @brief Unlocks the map. + */ + inline void unlock() { lock_.unlock(); } + + /** + * @brief Returns the number of elements in the map. + * @return The size of the map. + */ + int size() + { + return tile_map_.size(); + } + + /** + * @brief Converts world coordinates to a TileIndex. + * @param x X coordinate in world space. + * @param y Y coordinate in world space. + * @return The corresponding TileIndex. + */ + TileIndex worldToIndex(double x, double y) const { + // Convert world coordinates to grid indices + int ix = static_cast(std::floor(x / resolution_)); + int iy = static_cast(std::floor(y / resolution_)); + return TileIndex{ix, iy}; + } + + /** + * @brief Converts a TileIndex to world coordinates. + * @param idx The index to convert. + * @return The world coordinates of the tile's center. + */ + TileWorldXY indexToWorld(int x, int y) const { + // Calculate the world coordinates of the center of the grid cell + double x_world = (static_cast(x) + 0.5) * resolution_; + double y_world = (static_cast(y) + 0.5) * resolution_; + return TileWorldXY{x_world, y_world}; + } + + /** + * @brief Adds an observation to the specified tile. + * @param obs The observation to add. + * @param idx The index of the tile. + */ + void pushObservation(TileObservation& obs, TileIndex& idx) + { + auto it = tile_map_.find(idx); + if (it != tile_map_.end()) { + // TileIndex exists, push the observation + it->second.push(obs); + } else { + // TileIndex does not exist, create a new TemporalObservationQueue with decay time + TemporalObservationQueue& queue = tile_map_[idx]; + queue.setDecayTime(decay_time_); + queue.push(obs); + } + } + + /** + * @brief Removes observations older than the decay time from all tiles. + * @param current_time The current time for comparison. + */ + void purgeOldObservations(double current_time) + { + std::vector tiles_to_remove; + for (auto& tile : tile_map_) + { + tile.second.purgeOld(current_time); + if(tile.second.empty()) + { + tiles_to_remove.emplace_back(tile.first); + } + } + if(tile_map_.size() > 0) + for (auto& tile : tiles_to_remove) + { + tile_map_.erase(tile); + } + } +}; + +/** + * @brief Struct for holding the relevant data of any observation. Includes + * its position, its confidence, the confidence sum of the tile and the + * class to which it belongs + */ +struct PointData { + float x, y, z; + float confidence, confidence_sum; + uint8_t class_id; +}; + +/** + * @brief Creates a PointCloud2 message that contains a visual representation of + * a temporal tile map. There's a "column" of points on each tile, each point represents + * a segmentation observation over that tile and they are all stacked together. Each observation + * Has a channel for the class, for the confidence, and the confidence sum of the observations + * over that tile + * @param tileMap The segmentation tile map + */ +sensor_msgs::msg::PointCloud2 visualizeTemporalTileMap(SegmentationTileMap& tileMap) { + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = "map"; // Set appropriate frame_id + cloud.header.stamp = rclcpp::Clock().now(); // Set current time as timestamp + + // Define fields for PointCloud2 + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2Fields(6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "confidence", 1, sensor_msgs::msg::PointField::FLOAT32, + "confidence_sum", 1, sensor_msgs::msg::PointField::FLOAT32, + "class", 1, sensor_msgs::msg::PointField::UINT8); + + // Reserve space for points + std::vector points; + for (auto& tile : tileMap) { + TileIndex idx = tile.first; + TileWorldXY worldXY = tileMap.indexToWorld(idx.x, idx.y); + double z = 0.0; + for (auto& obs : tile.second.getQueue()) { + PointData point; + point.x = worldXY.x; + point.y = worldXY.y; + point.z = z; + point.confidence = obs.confidence; + point.confidence_sum = tile.second.getConfidenceSum() / tile.second.size(); + point.class_id = static_cast(obs.class_id); + points.push_back(point); + z += 0.02; // Increment Z by 0.02m for each observation + } + } + + // Set data in PointCloud2 + modifier.resize(points.size()); // Number of points + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + sensor_msgs::PointCloud2Iterator iter_confidence(cloud, "confidence"); + sensor_msgs::PointCloud2Iterator iter_confidence_sum(cloud, "confidence_sum"); + sensor_msgs::PointCloud2Iterator iter_class(cloud, "class"); + + for (const auto& point : points) { + *iter_x = point.x; + *iter_y = point.y; + *iter_z = point.z; + *iter_confidence = point.confidence; + *iter_confidence_sum = point.confidence_sum; + *iter_class = point.class_id; + ++iter_x; ++iter_y; ++iter_z; ++iter_confidence;++iter_confidence_sum; ++iter_class; + } + + return cloud; +} + +/** + * Manages segmentation class information, including mapping between class names and IDs, + * as well as managing the cost heuristic parameters associated with each class. + */ +class SegmentationCostMultimap { +public: + SegmentationCostMultimap(){} + /** + * Constructs the SegmentationCostMultimap. + * + * @param nameToIdMap A map from class names to class IDs. + * @param nameToCostMap A map from class names to CostHeuristicParams. + */ + SegmentationCostMultimap(const std::unordered_map& nameToIdMap, + const std::unordered_map& nameToCostMap) { + name_to_id_ = nameToIdMap; + for (const auto& pair : nameToIdMap) { + const auto& name = pair.first; + uint8_t id = pair.second; + CostHeuristicParams cost = nameToCostMap.at(name); + id_to_cost_[id] = cost; + } + } + + /** + * Updates the cost heuristic parameters associated with a class ID. + * + * @param id The class ID. + * @param cost The new CostHeuristicParams to associate with the class. + */ + void updateCostById(uint8_t id, const CostHeuristicParams& cost) { + id_to_cost_[id] = cost; + } + + /** + * Retrieves the cost heuristic parameters associated with a class ID. + * + * @param id The class ID. + * @return The CostHeuristicParams associated with the class. + */ + CostHeuristicParams getCostById(uint8_t id) const { + return id_to_cost_.at(id); + } + + /** + * Updates the cost heuristic parameters associated with a class name. + * + * @param name The class name. + * @param cost The new CostHeuristicParams to associate with the class. + */ + void updateCostByName(const std::string& name, const CostHeuristicParams& cost) { + uint8_t id = name_to_id_.at(name); + id_to_cost_[id] = cost; + } + + /** + * Retrieves the cost heuristic parameters associated with a class name. + * + * @param name The class name. + * @return The CostHeuristicParams associated with the class. + */ + CostHeuristicParams getCostByName(const std::string& name) const { + uint8_t id = name_to_id_.at(name); + return id_to_cost_.at(id); + } + + bool empty() + { + return name_to_id_.empty() || id_to_cost_.empty(); + } + +private: + std::unordered_map name_to_id_; // Maps class names to class IDs + std::unordered_map id_to_cost_; // Maps class IDs to CostHeuristicParams +}; + +namespace nav2_costmap_2d { +/** + * @class SegmentationBuffer + * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them + */ +class SegmentationBuffer +{ + public: + using SharedPtr = std::shared_ptr; + /** + * @brief Constructs an segmentation buffer + * @param topic_name The topic of the segmentations, used as an identifier for error and warning + * messages + * @param observation_keep_time Defines the persistence of segmentations in seconds, 0 means only + * keep the latest + * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is + * no limit + * @param min_obstacle_height The minimum height of a hitpoint to be considered legal + * @param max_obstacle_height The minimum height of a hitpoint to be considered legal + * @param obstacle_max_range The range to which the sensor should be trusted for inserting + * obstacles + * @param obstacle_min_range The range from which the sensor should be trusted for inserting + * obstacles + * @param raytrace_max_range The range to which the sensor should be trusted for raytracing to + * clear out space + * @param raytrace_min_range The range from which the sensor should be trusted for raytracing to + * clear out space + * @param tf2_buffer A reference to a tf2 Buffer + * @param global_frame The frame to transform PointClouds into + * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from + * the messages + * @param tf_tolerance The amount of time to wait for a transform to be available when setting a + * new global frame + */ + SegmentationBuffer(const nav2_util::LifecycleNode::WeakPtr& parent, std::string buffer_source, + std::vector class_types, + std::unordered_map class_names_cost_map, double observation_keep_time, + double expected_update_rate, double max_lookahead_distance, double min_lookahead_distance, + tf2_ros::Buffer& tf2_buffer, std::string global_frame, std::string sensor_frame, + tf2::Duration tf_tolerance, double costmap_resolution, double tile_map_decay_time, bool visualize_tile_map = false); + + /** + * @brief Destructor... cleans up + */ + ~SegmentationBuffer(); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * Note: The burden is on the user to make sure the transform is available... ie they should + * use a MessageNotifier + * @param cloud The cloud to be buffered + */ + void bufferSegmentation(const sensor_msgs::msg::PointCloud2& cloud, const sensor_msgs::msg::Image& segmentation, + const sensor_msgs::msg::Image& confidence); + + /** + * @brief gets the class map associated with the segmentations stored in the buffer + * @return the class map + */ + std::unordered_map getClassMap(); + + void createSegmentationCostMultimap(const vision_msgs::msg::LabelInfo& label_info); + + bool isClassIdCostMapEmpty() { return segmentation_cost_multimap_.empty(); } + + /** + * @brief Check if the segmentation buffer is being update at its expected rate + * @return True if it is being updated at the expected rate, false otherwise + */ + bool isCurrent() const; + + /** + * @brief Lock the segmentation buffer + */ + inline void lock() { lock_.lock(); } + + /** + * @brief Lock the segmentation buffer + */ + inline void unlock() { lock_.unlock(); } + + /** + * @brief Reset last updated timestamp + */ + void resetLastUpdated(); + + /** + * @brief Reset last updated timestamp + */ + std::string getBufferSource() { return buffer_source_; } + std::vector getClassTypes() { return class_types_; } + + void setMinObstacleDistance(double distance) { sq_min_lookahead_distance_ = pow(distance, 2); } + + void setMaxObstacleDistance(double distance) { sq_max_lookahead_distance_ = pow(distance, 2); } + + void updateClassMap(std::string new_class, CostHeuristicParams new_cost); + + SegmentationTileMap::SharedPtr getSegmentationTileMap() + { + return temporal_tile_map_; + } + + CostHeuristicParams getCostForClassId(uint8_t class_id) + { + return segmentation_cost_multimap_.getCostById(class_id); + } + + CostHeuristicParams getCostForClassName(std::string class_name) + { + return segmentation_cost_multimap_.getCostByName(class_name); + } + + private: + /** + * @brief Removes any stale segmentations from the buffer list + */ + void purgeStaleSegmentations(); + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; + tf2_ros::Buffer& tf2_buffer_; + std::vector class_types_; + std::unordered_map class_names_cost_map_; + const rclcpp::Duration observation_keep_time_; + const rclcpp::Duration expected_update_rate_; + rclcpp::Time last_updated_; + std::string global_frame_; + std::string sensor_frame_; + std::string buffer_source_; + std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely + double sq_max_lookahead_distance_; + double sq_min_lookahead_distance_; + tf2::Duration tf_tolerance_; + + SegmentationCostMultimap segmentation_cost_multimap_; + + SegmentationTileMap::SharedPtr temporal_tile_map_; + + bool visualize_tile_map_; + rclcpp::Publisher::SharedPtr tile_map_pub_; +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp new file mode 100644 index 00000000000..d04a9c84262 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -0,0 +1,175 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2020, Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Pedro Gonzalez + + *********************************************************************/ +#ifndef SEMANTIC_SEGMENTATION_LAYER_HPP_ +#define SEMANTIC_SEGMENTATION_LAYER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/segmentation_buffer.hpp" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2_ros/message_filter.h" +#include "vision_msgs/msg/label_info.hpp" + +namespace nav2_costmap_2d { +/** + * @class SemanticSegmentationLayer + * @brief Takes in semantic segmentation messages and aligned pointclouds to populate the 2D costmap + */ +class SemanticSegmentationLayer : public CostmapLayer +{ + public: + /** + * @brief A constructor + */ + SemanticSegmentationLayer(); + + /** + * @brief A destructor + */ + virtual ~SemanticSegmentationLayer() {} + + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + /** + * @brief Reset this costmap + */ + virtual void reset(); + + virtual void onFootprintChanged(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable() { return true; } + + /** + * @brief Get the buffers and the tile maps the plugin stores. one for each source. Takes a vector of tile maps + * as reference and fills it inside the function + * @param segmentation_tile_maps the vector of tile maps to be filled by the function + * @return whether the tile maps could be retrieved and filled successfully + */ + bool getSegmentationTileMaps(std::vector>& segmentation_tile_maps); + + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + + private: + void syncSegmPointcloudCb(const std::shared_ptr& segmentation, + const std::shared_ptr& pointcloud, + const std::shared_ptr& buffer); + + void syncSegmConfPointcloudCb(const std::shared_ptr& segmentation, + const std::shared_ptr& confidence, + const std::shared_ptr& pointcloud, + const std::shared_ptr& buffer); + + void labelinfoCb(const std::shared_ptr& label_info, + const std::shared_ptr& buffer); + + std::vector>> + semantic_segmentation_subs_; + std::vector>> + semantic_segmentation_confidence_subs_; + std::vector< + std::shared_ptr>> + label_info_subs_; + std::vector< + std::shared_ptr>> + pointcloud_subs_; + std::vector< + std::shared_ptr>> + segm_pc_notifiers_; + std::vector< + std::shared_ptr>> + segm_conf_pc_notifiers_; + std::vector>> pointcloud_tf_subs_; + + // debug publishers + std::map>> proc_pointcloud_pubs_map_; + + std::vector> segmentation_buffers_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + + std::string global_frame_; + std::string topics_string_; + + std::map class_map_; + + bool rolling_window_; + bool was_reset_; + int combination_method_; +}; + +} // namespace nav2_costmap_2d + +#endif // SEMANTIC_SEGMENTATION_LAYER_HPP_ \ No newline at end of file diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 77d020799bc..7ce21b875f5 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -32,6 +32,7 @@ rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs std_srvs tf2 tf2_geometry_msgs diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index cccae4d3938..15405d8d0e5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -73,9 +73,6 @@ void ObstacleLayer::onInitialize() bool track_unknown_space; double transform_tolerance; - // The topics that we'll subscribe to from the parameter server - std::string topics_string; - declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); @@ -94,7 +91,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); - node->get_parameter(name_ + "." + "observation_sources", topics_string); + node->get_parameter(name_ + "." + "observation_sources", _topics_string); int combination_method_param{}; node->get_parameter(name_ + "." + "combination_method", combination_method_param); @@ -108,7 +105,7 @@ void ObstacleLayer::onInitialize() RCLCPP_INFO( logger_, - "Subscribed to Topics: %s", topics_string.c_str()); + "Subscribed to Topics: %s", _topics_string.c_str()); rolling_window_ = layered_costmap_->isRolling(); @@ -128,7 +125,7 @@ void ObstacleLayer::onInitialize() sub_opt.callback_group = callback_group_; // now we need to split the topics based on whitespace which we can use a stringstream for - std::stringstream ss(topics_string); + std::stringstream ss(_topics_string); std::string source; while (ss >> source) { @@ -197,7 +194,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr( new ObservationBuffer( - node, topic, observation_keep_time, expected_update_rate, + node, topic, source, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range, raytrace_min_range, *tf_, @@ -317,6 +314,22 @@ ObstacleLayer::dynamicParametersCallback( combination_method_ = combination_method_from_int(parameter.as_int()); } } + std::stringstream ss(_topics_string); + std::string source; + while (ss >> source) + { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + source + "." + "obstacle_max_range") { + for (auto & buffer : observation_buffers_) { + if (buffer->getSourceName() == source) { + buffer->lock(); + buffer->setMaxObstacleDistance(parameter.as_double()); + buffer->unlock(); + } + } + } + } + } } result.successful = true; diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp new file mode 100644 index 00000000000..d31e72753e5 --- /dev/null +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -0,0 +1,534 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2020, Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + * Alexey Merzlyakov + * + * Reference tutorial: + * https://navigation.ros.org/tutorials/docs/writing_new_costmap2d_plugin.html + *********************************************************************/ +#include "nav2_costmap_2d/semantic_segmentation_layer.hpp" + +#include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +namespace nav2_costmap_2d { + +SemanticSegmentationLayer::SemanticSegmentationLayer() {} + +// This method is called at the end of plugin initialization. +// It contains ROS parameter(s) declaration and initialization +// of need_recalculation_ variable. +void SemanticSegmentationLayer::onInitialize() +{ + current_ = true; + was_reset_ = false; + auto node = node_.lock(); + if (!node) + { + throw std::runtime_error{"Failed to lock node"}; + } + std::string segmentation_topic, confidence_topic, pointcloud_topic, labels_topic, sensor_frame; + std::vector class_types_string; + double max_obstacle_distance, min_obstacle_distance, observation_keep_time, transform_tolerance, + expected_update_rate, tile_map_decay_time; + bool track_unknown_space, visualize_tile_map; + + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + declareParameter("publish_debug_topics", rclcpp::ParameterValue(false)); + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter("track_unknown_space", track_unknown_space); + node->get_parameter("transform_tolerance", transform_tolerance); + + global_frame_ = layered_costmap_->getGlobalFrameID(); + rolling_window_ = layered_costmap_->isRolling(); + + if (track_unknown_space) { + default_value_ = NO_INFORMATION; + } else { + default_value_ = FREE_SPACE; + } + + matchSize(); + + node->get_parameter(name_ + "." + "observation_sources", topics_string_); + + // now we need to split the topics based on whitespace which we can use a stringstream for + std::stringstream ss(topics_string_); + + std::string source; + + while (ss >> source) { + declareParameter(source + "." + "segmentation_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "confidence_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "labels_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "pointcloud_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue("")); + declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "class_types", rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + "." + "max_obstacle_distance", rclcpp::ParameterValue(5.0)); + declareParameter(source + "." + "min_obstacle_distance", rclcpp::ParameterValue(0.3)); + declareParameter(source + "." + "tile_map_decay_time", rclcpp::ParameterValue(5.0)); + declareParameter(source + "." + "visualize_tile_map", rclcpp::ParameterValue(false)); + + node->get_parameter(name_ + "." + source + "." + "segmentation_topic", segmentation_topic); + node->get_parameter(name_ + "." + source + "." + "confidence_topic", confidence_topic); + node->get_parameter(name_ + "." + source + "." + "labels_topic", labels_topic); + node->get_parameter(name_ + "." + source + "." + "pointcloud_topic", pointcloud_topic); + node->get_parameter(name_ + "." + source + "." + "observation_persistence", observation_keep_time); + node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); + node->get_parameter(name_ + "." + source + "." + "expected_update_rate", expected_update_rate); + node->get_parameter(name_ + "." + source + "." + "class_types", class_types_string); + node->get_parameter(name_ + "." + source + "." + "max_obstacle_distance", max_obstacle_distance); + node->get_parameter(name_ + "." + source + "." + "min_obstacle_distance", min_obstacle_distance); + node->get_parameter(name_ + "." + source + "." + "tile_map_decay_time", tile_map_decay_time); + node->get_parameter(name_ + "." + source + "." + "visualize_tile_map", visualize_tile_map); + if (class_types_string.empty()) + { + RCLCPP_ERROR(logger_, "no class types defined for source %s. Segmentation plugin cannot work this way", source.c_str()); + exit(-1); + } + + std::unordered_map class_map; + + for (auto& class_type : class_types_string) + { + std::vector classes_ids; + declareParameter(source + "." + class_type + ".classes", rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + "." + class_type + ".base_cost", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".max_cost", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".mark_confidence", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".samples_to_max_cost", rclcpp::ParameterValue(0)); + + node->get_parameter(name_ + "." + source + "." + class_type + ".classes", classes_ids); + if (classes_ids.empty()) + { + RCLCPP_ERROR(logger_, "no classes defined on type %s", class_type.c_str()); + continue; + } + CostHeuristicParams cost_params; + node->get_parameter(name_ + "." + source + "." + class_type + ".base_cost", cost_params.base_cost); + node->get_parameter(name_ + "." + source + "." + class_type + ".max_cost", cost_params.max_cost); + node->get_parameter(name_ + "." + source + "." + class_type + ".mark_confidence", cost_params.mark_confidence); + node->get_parameter(name_ + "." + source + "." + class_type + ".samples_to_max_cost", cost_params.samples_to_max_cost); + for (auto& class_id : classes_ids) + { + class_map.insert(std::pair(class_id, cost_params)); + } + } + + if (class_map.empty()) + { + RCLCPP_ERROR(logger_, "No classes defined for source %s. Segmentation plugin cannot work this way", source.c_str()); + exit(-1); + } + + //sensor data subscriptions + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; + + // label info subscription + rclcpp::SubscriptionOptionsWithAllocator> tl_sub_opt; + tl_sub_opt.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + tl_sub_opt.callback_group = callback_group_; + rmw_qos_profile_t tl_qos = rmw_qos_profile_system_default; + tl_qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + tl_qos.depth = 5; + tl_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + tl_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + + auto segmentation_buffer = std::make_shared( + node, source, class_types_string, class_map, observation_keep_time, expected_update_rate, max_obstacle_distance, + min_obstacle_distance, *tf_, global_frame_, sensor_frame, + tf2::durationFromSec(transform_tolerance), getResolution(), tile_map_decay_time, visualize_tile_map); + + segmentation_buffers_.push_back(segmentation_buffer); + + + auto semantic_segmentation_sub = + std::make_shared>( + node, segmentation_topic, custom_qos_profile, sub_opt); + semantic_segmentation_subs_.push_back(semantic_segmentation_sub); + + auto label_info_sub = std::make_shared>( + node, labels_topic, tl_qos, tl_sub_opt); + label_info_sub->registerCallback(std::bind(&SemanticSegmentationLayer::labelinfoCb, this, std::placeholders::_1, segmentation_buffers_.back())); + label_info_subs_.push_back(label_info_sub); + + auto pointcloud_sub = std::make_shared>( + node, pointcloud_topic, custom_qos_profile, sub_opt); + pointcloud_subs_.push_back(pointcloud_sub); + + auto pointcloud_tf_sub = std::make_shared>( + *pointcloud_subs_.back(), *tf_, global_frame_, 1000, node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); + pointcloud_tf_subs_.push_back(pointcloud_tf_sub); + + if(!confidence_topic.empty()) + { + auto semantic_segmentation_confidence_sub = + std::make_shared>( + node, confidence_topic, custom_qos_profile, sub_opt); + semantic_segmentation_confidence_subs_.push_back(semantic_segmentation_confidence_sub); + auto segm_conf_pc_sync = + std::make_shared>( + *semantic_segmentation_subs_.back(), *semantic_segmentation_confidence_subs_.back(), *pointcloud_tf_subs_.back(), 1000); + segm_conf_pc_sync->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmConfPointcloudCb, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, segmentation_buffers_.back())); + segm_conf_pc_notifiers_.push_back(segm_conf_pc_sync); + RCLCPP_INFO(logger_, "Confidence is enabled for source %s", source.c_str()); + } + else + { + RCLCPP_WARN(logger_, "Confidence topic was empty for source %s, not using segmentation confidence in that source", source.c_str()); + auto segm_pc_sync = + std::make_shared>( + *semantic_segmentation_subs_.back(), *pointcloud_tf_subs_.back(), 1000); + segm_pc_sync->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmPointcloudCb, this, + std::placeholders::_1, std::placeholders::_2, segmentation_buffers_.back())); + segm_pc_notifiers_.push_back(segm_pc_sync); + } + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &SemanticSegmentationLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); +} + +// The method is called to ask the plugin: which area of costmap it needs to update. +// Inside this method window bounds are re-calculated if need_recalculation_ is true +// and updated independently on its value. +void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, double /*robot_yaw*/, + double* min_x, double* min_y, double* max_x, + double* max_y) +{ + std::lock_guard guard(*getMutex()); + if (rolling_window_) + { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!enabled_) + { + return; + } + + std::vector> segmentation_tile_maps; + getSegmentationTileMaps(segmentation_tile_maps); + for (auto& tile_map : segmentation_tile_maps) + { + for(auto& tile: *tile_map.first) + { + TileWorldXY tile_world_coords = tile_map.first->indexToWorld(tile.first.x, tile.first.y); + // alias tile.second with a name for more readability + TemporalObservationQueue& obs_queue = tile.second; + unsigned int mx, my; + if (!worldToMap(tile_world_coords.x, tile_world_coords.y, mx, my)) + { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + unsigned int index = getIndex(mx, my); + CostHeuristicParams cost_params = tile_map.second->getCostForClassId(obs_queue.getClassId()); + if(obs_queue.size() >= cost_params.samples_to_max_cost && obs_queue.getConfidenceSum() / obs_queue.size() > cost_params.mark_confidence) + { + costmap_[index] = cost_params.max_cost; + } + else + { + costmap_[index] = cost_params.base_cost; + } + touch(tile_world_coords.x, tile_world_coords.y, min_x, min_y, max_x, max_y); + } + } + + current_ = true; +} + +// The method is called when footprint was changed. +// Here it just resets need_recalculation_ variable. +void SemanticSegmentationLayer::onFootprintChanged() +{ + RCLCPP_DEBUG(rclcpp::get_logger("nav2_costmap_2d"), + "SemanticSegmentationLayer::onFootprintChanged(): num footprint points: %lu", + layered_costmap_->getFootprint().size()); +} + +// The method is called when costmap recalculation is required. +// It updates the costmap within its window bounds. +// Inside this method the costmap gradient is generated and is writing directly +// to the resulting costmap master_grid without any merging with previous layers. +void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, + int min_j, int max_i, int max_j) +{ + std::lock_guard guard(*getMutex()); + if (!enabled_) + { + return; + } + + if (!current_ && was_reset_) + { + was_reset_ = false; + current_ = true; + } + if (!costmap_) + { + return; + } + // RCLCPP_INFO(logger_, "Updating costmap"); + switch (combination_method_) + { + case 0: // Overwrite + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case 1: // Maximum + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } +} + +void SemanticSegmentationLayer::labelinfoCb( + const std::shared_ptr& label_info, + const std::shared_ptr & buffer) + { + buffer->createSegmentationCostMultimap(*label_info); + } + +void SemanticSegmentationLayer::syncSegmPointcloudCb( + const std::shared_ptr& segmentation, + const std::shared_ptr& pointcloud, + const std::shared_ptr & buffer) +{ + if (segmentation->width * segmentation->height != pointcloud->width * pointcloud->height) + { + RCLCPP_WARN(logger_, + "Pointcloud and segmentation sizes are different, will not buffer message. " + "segmentation->width:%u, " + "segmentation->height:%u, pointcloud->width:%u, pointcloud->height:%u", + segmentation->width, segmentation->height, pointcloud->width, pointcloud->height); + return; + } + unsigned expected_array_size = segmentation->width * segmentation->height; + if (segmentation->data.size() < expected_array_size) + { + RCLCPP_WARN(logger_, + "segmentation arrays have wrong sizes: data->%lu, expected->%u. " + "Will not buffer message", + segmentation->data.size(), expected_array_size); + return; + } + if (buffer->isClassIdCostMapEmpty()) + { + RCLCPP_WARN(logger_, "Class map is empty because a labelinfo message has not been received for topic %s. Will not buffer message", buffer->getBufferSource().c_str()); + return; + } + // if no confidence available, create a mask with all elements having max confidence + // in this case the plugin thresholding will only work with the number of observations + // accumulated in a given tile + sensor_msgs::msg::Image conf_mask = *segmentation; + std::fill(conf_mask.data.begin(), conf_mask.data.end(), 255); + buffer->lock(); + buffer->bufferSegmentation(*pointcloud, *segmentation, conf_mask); + buffer->unlock(); +} + +void SemanticSegmentationLayer::syncSegmConfPointcloudCb(const std::shared_ptr& segmentation, + const std::shared_ptr& confidence, + const std::shared_ptr& pointcloud, + const std::shared_ptr& buffer) +{ + if (segmentation->width * segmentation->height != pointcloud->width * pointcloud->height) + { + RCLCPP_WARN(logger_, + "Pointcloud and segmentation sizes are different, will not buffer message. " + "segmentation->width:%u, " + "segmentation->height:%u, pointcloud->width:%u, pointcloud->height:%u", + segmentation->width, segmentation->height, pointcloud->width, pointcloud->height); + return; + } + unsigned expected_array_size = segmentation->width * segmentation->height; + if (segmentation->data.size() < expected_array_size) + { + RCLCPP_WARN(logger_, + "segmentation arrays have wrong sizes: data->%lu, expected->%u. " + "Will not buffer message", + segmentation->data.size(), expected_array_size); + return; + } + if (buffer->isClassIdCostMapEmpty()) + { + RCLCPP_WARN(logger_, "Class map is empty because a labelinfo message has not been received for topic %s. Will not buffer message", buffer->getBufferSource().c_str()); + return; + } + buffer->lock(); + buffer->bufferSegmentation(*pointcloud, *segmentation, *confidence); + buffer->unlock(); +} + +void SemanticSegmentationLayer::reset() +{ + resetMaps(); + current_ = false; + was_reset_ = true; +} + +bool SemanticSegmentationLayer::getSegmentationTileMaps( + std::vector>& segmentation_tile_maps) +{ + bool current = true; + // get the marking observations + for (unsigned int i = 0; i < segmentation_buffers_.size(); ++i) { + segmentation_buffers_[i]->lock(); + SegmentationTileMap::SharedPtr tile_map = segmentation_buffers_[i]->getSegmentationTileMap(); + segmentation_tile_maps.emplace_back(std::make_pair(tile_map, segmentation_buffers_[i])); + segmentation_buffers_[i]->unlock(); + } + return current; +} + + rcl_interfaces::msg::SetParametersResult +SemanticSegmentationLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + auto result = rcl_interfaces::msg::SetParametersResult(); + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == rclcpp::ParameterType::PARAMETER_BOOL) { + if (name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + + std::stringstream ss(topics_string_); + std::string source; + while (ss >> source) { + if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + "." + source + "." + "max_obstacle_distance") { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + buffer->setMaxObstacleDistance(parameter.as_double()); + } + } + } else if (name == name_ + "." + source + "." + "min_obstacle_distance") { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + buffer->setMinObstacleDistance(parameter.as_double()); + } + } + } + } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) { + for(auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + for(auto & class_type : buffer->getClassTypes()){ + if (name == name_ + "." + source + "." + class_type + "." + "base_cost") { + CostHeuristicParams cost_params; + std::vector class_names_for_type; + node_.lock()->get_parameter(name_ + "." + source + "." + class_type + ".classes", class_names_for_type); + for(auto & class_name : class_names_for_type){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.base_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "max_cost") { + CostHeuristicParams cost_params; + std::vector class_names_for_type; + node_.lock()->get_parameter(name_ + "." + source + "." + class_type + ".classes", class_names_for_type); + for(auto & class_name : class_names_for_type){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.max_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "mark_confidence") { + CostHeuristicParams cost_params; + std::vector class_names_for_type; + node_.lock()->get_parameter(name_ + "." + source + "." + class_type + ".classes", class_names_for_type); + for(auto & class_name : class_names_for_type){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.mark_confidence = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "samples_to_max_cost") { + CostHeuristicParams cost_params; + std::vector class_names_for_type; + node_.lock()->get_parameter(name_ + "." + source + "." + class_type + ".classes", class_names_for_type); + for(auto & class_name : class_names_for_type){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.samples_to_max_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + } + } + } + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_costmap_2d + +// This is the macro allowing a nav2_costmap_2d::SemanticSegmentationLayer class +// to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer. +// Usually places in the end of cpp-file where the loadable class written. +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::SemanticSegmentationLayer, nav2_costmap_2d::Layer) \ No newline at end of file diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 8a06823df38..6f39dcc5220 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -51,6 +51,7 @@ namespace nav2_costmap_2d ObservationBuffer::ObservationBuffer( const nav2_util::LifecycleNode::WeakPtr & parent, std::string topic_name, + std::string source_name, double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, @@ -65,6 +66,7 @@ ObservationBuffer::ObservationBuffer( global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), + source_name_(source_name), min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range), raytrace_max_range_(raytrace_max_range), raytrace_min_range_( diff --git a/nav2_costmap_2d/src/segmentation_buffer.cpp b/nav2_costmap_2d/src/segmentation_buffer.cpp new file mode 100644 index 00000000000..19843eb4310 --- /dev/null +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -0,0 +1,238 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#include "nav2_costmap_2d/segmentation_buffer.hpp" + +#include +#include +#include +#include +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/convert.h" +#include "rclcpp/rclcpp.hpp" +using namespace std::chrono_literals; + +namespace nav2_costmap_2d { +SegmentationBuffer::SegmentationBuffer(const nav2_util::LifecycleNode::WeakPtr& parent, + std::string buffer_source, std::vector class_types, std::unordered_map class_names_cost_map, double observation_keep_time, + double expected_update_rate, double max_lookahead_distance, + double min_lookahead_distance, tf2_ros::Buffer& tf2_buffer, + std::string global_frame, std::string sensor_frame, + tf2::Duration tf_tolerance, double costmap_resolution, double tile_map_decay_time, bool visualize_tile_map) + : tf2_buffer_(tf2_buffer) + , class_types_(class_types) + , class_names_cost_map_(class_names_cost_map) + , observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)) + , expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)) + , global_frame_(global_frame) + , sensor_frame_(sensor_frame) + , buffer_source_(buffer_source) + , sq_max_lookahead_distance_(std::pow(max_lookahead_distance, 2)) + , sq_min_lookahead_distance_(std::pow(min_lookahead_distance, 2)) + , tf_tolerance_(tf_tolerance) +{ + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + last_updated_ = node->now(); + temporal_tile_map_ = std::make_shared(costmap_resolution, tile_map_decay_time); + visualize_tile_map_ = visualize_tile_map; + if(visualize_tile_map_) + { + tile_map_pub_ = node->create_publisher(buffer_source + "/tile_map",1); + } +} + +SegmentationBuffer::~SegmentationBuffer() {} + +void SegmentationBuffer::createSegmentationCostMultimap(const vision_msgs::msg::LabelInfo& label_info) +{ + std::unordered_map class_to_id_map; + for (const auto& semantic_class : label_info.class_map) + { + class_to_id_map[semantic_class.class_name] = semantic_class.class_id; + } + segmentation_cost_multimap_ = SegmentationCostMultimap(class_to_id_map, class_names_cost_map_); +} + +void SegmentationBuffer::bufferSegmentation( + const sensor_msgs::msg::PointCloud2& cloud, + const sensor_msgs::msg::Image& segmentation, + const sensor_msgs::msg::Image& confidence) +{ + geometry_msgs::msg::PointStamped global_origin; + // check whether the origin frame has been set explicitly + // or whether we should get it from the cloud + std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; + + try + { + // given these segmentations come from sensors... + // we'll need to store the origin pt of the sensor + geometry_msgs::msg::PointStamped local_origin; + local_origin.header.stamp = cloud.header.stamp; + local_origin.header.frame_id = origin_frame; + local_origin.point.x = 0; + local_origin.point.y = 0; + local_origin.point.z = 0; + tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_); + + sensor_msgs::msg::PointCloud2 global_frame_cloud; + + // transform the point cloud + tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_); + global_frame_cloud.header.stamp = cloud.header.stamp; + + sensor_msgs::PointCloud2ConstIterator iter_x_global(global_frame_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y_global(global_frame_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z_global(global_frame_cloud, "z"); + std::unordered_map best_observations_idxs; + double cloud_time_seconds = rclcpp::Time(cloud.header.stamp.sec, cloud.header.stamp.nanosec).seconds(); + + // copy over the points that are within our segmentation range + for (size_t v = 0; v < segmentation.height; v++) + { + for (size_t u = 0; u < segmentation.width; u++) + { + int pixel_idx = v * segmentation.width + u; + // remove invalid points + if (!std::isfinite(*(iter_z_global))) + { + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + continue; + } + double sq_dist = + std::pow(*(iter_x_global) - global_origin.point.x, 2) + + std::pow(*(iter_y_global) - global_origin.point.y, 2) + + std::pow(*(iter_z_global) - global_origin.point.z, 2); + if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) + { + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + continue; + } + + TileIndex costmap_index = temporal_tile_map_->worldToIndex(*iter_x_global, *iter_y_global); + + // Find the observation with the greater confidence for each tile + auto it = best_observations_idxs.find(costmap_index); + // if an observation already exists in the index, compare its confidence to the current + // one and if its greater store this element as the new best confidence index for the tile + if (it != best_observations_idxs.end()) { + if(confidence.data[pixel_idx] > confidence.data[best_observations_idxs[costmap_index]]) + { + best_observations_idxs[costmap_index] = pixel_idx; + } + } + // if this is the first observation for the tile index just store its confidence as the best one + else + { + best_observations_idxs[costmap_index] = pixel_idx; + } + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + } + } + + // emplace the best observations in the mask into the tile map + temporal_tile_map_->lock(); + temporal_tile_map_->purgeOldObservations(cloud_time_seconds); + for (auto& idx : best_observations_idxs) + { + int img_idx_for_best_obs = idx.second; + TileIndex costmap_index = idx.first; + TileObservation best_obs{segmentation.data[img_idx_for_best_obs], static_cast(confidence.data[img_idx_for_best_obs]), cloud_time_seconds}; + temporal_tile_map_->pushObservation(best_obs, costmap_index); + } + temporal_tile_map_->unlock(); + + if(visualize_tile_map_) + { + sensor_msgs::msg::PointCloud2 tile_map_cloud = visualizeTemporalTileMap(*temporal_tile_map_); + tile_map_pub_->publish(tile_map_cloud); + } + + } catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(logger_, + "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", + sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); + return; + } + + // if the update was successful, we want to update the last updated time + last_updated_ = clock_->now(); +} + + +std::unordered_map SegmentationBuffer::getClassMap() +{ + return class_names_cost_map_; +} + + +void SegmentationBuffer::updateClassMap(std::string new_class, CostHeuristicParams new_cost) +{ + segmentation_cost_multimap_.updateCostByName(new_class, new_cost); +} + +bool SegmentationBuffer::isCurrent() const +{ + if (expected_update_rate_ == rclcpp::Duration(0.0s)) + { + return true; + } + + bool current = (clock_->now() - last_updated_) <= expected_update_rate_; + if (!current) + { + RCLCPP_WARN(logger_, + "The %s segmentation buffer has not been updated for %.2f seconds, " + "and it should be updated every %.2f seconds.", + buffer_source_.c_str(), (clock_->now() - last_updated_).seconds(), + expected_update_rate_.seconds()); + } + return current; +} + +void SegmentationBuffer::resetLastUpdated() { last_updated_ = clock_->now(); } +} // namespace nav2_costmap_2d diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 99570224add..a6a511a89c5 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -153,6 +153,25 @@ std::string get_plugin_type_param( return plugin_type; } +/** + * @brief A method to copy all parameters from one node (parent) to another (child). + * May throw parameter exceptions in error conditions + * @param parent Node to copy parameters from + * @param child Node to copy parameters to + */ +template +void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) +{ + using Parameters = std::vector; + std::vector param_names = parent->list_parameters({}, 0).names; + Parameters params = parent->get_parameters(param_names); + for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { + if (!child->has_parameter(iter->get_name())) { + child->declare_parameter(iter->get_name(), iter->get_parameter_value()); + } + } +} + /** * @brief Sets the caller thread to have a soft-realtime prioritization by * increasing the priority level of the host thread. diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index bf945276358..1ae944b46e4 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,3 +94,35 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } + +TEST(TestParamCopying, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + nav2_util::copy_all_parameters(node1, node2); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); +} \ No newline at end of file