diff --git a/README.md b/README.md index 1b3bdfa8402..93c3a4e06f0 100644 --- a/README.md +++ b/README.md @@ -12,51 +12,39 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://navigation2.slack.com). -## Citation - -If you use the navigation framework, an algorithm from this repository, or ideas from it -please cite this work in your papers! - - - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. - - ```bibtex - @InProceedings{macenski2020marathon2, - title = {The Marathon 2: A Navigation System}, - author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan}, - year = {2020}, - booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, - url = {https://github.com/ros-planning/navigation2}, - pdf = {https://arxiv.org/abs/2003.00368} - } +# Pre-requisites: +* [Install ROS2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/) +* [Install CLOiSim](https://github.com/lge-ros2/cloisim) +* [Install sim_device](https://github.com/lge-ros2/sim_device) + +# Build foxy-devel +```bash +mkdir -p ~/navigation2_ws/src +cd ~/navigation2_ws/src +git clone https://github.com/lge-ros2/navigation2 --branch foxy-devel +cd ~/navigation2_ws +rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy +colcon build --symlink-install ``` -## Build Status - -| Service | Dashing | Eloquent | Foxy | Master | -| :---: | :---: | :---: | :---: | :---: | -| ROS Build Farm | [![Build Status](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | N/A | -| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/master.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/master) | - - -| Package | Dashing Source | Dashing Debian | Eloquent Source | Eloquent Debian | Foxy Source | Foxy Debian | -| :---: | :---: | :---: | :---: | :---: | :---: | :---: | -| Navigation2 | [![Build Status](http://build.ros2.org/job/Dsrc_uB__navigation2__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__navigation2__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__navigation2__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__navigation2__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__navigation2__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__navigation2__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__navigation2__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__navigation2__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | -| nav2_amcl | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_amcl__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_amcl__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_amcl__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_amcl__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_behavior_tree__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_behavior_tree__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_behavior_tree__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_behavior_tree__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | -| nav2_bringup | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_bringup__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_bringup__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_bringup__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_bringup__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_bt_navigator__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_bt_navigator__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_bt_navigator__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_bt_navigator__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | -| nav2_common | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_common__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_common__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_common__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_common__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_common__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_common__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_common__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_common__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | -| nav2_controller | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | -| nav2_core | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_core__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_core__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_core__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_core__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_costmap_2d__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_costmap_2d__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_costmap_2d__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_costmap_2d__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_dwb_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_dwb_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_dwb_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_dwb_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | -| nav2_map_server | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_map_server__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_map_server__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_map_server__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_map_server__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | -| nav2_msgs | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_msgs__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_msgs__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_msgs__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_msgs__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_navfn_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_navfn_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_navfn_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_navfn_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | -| nav2_planner | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | -| nav2_recoveries | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_recoveries__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_recoveries__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_recoveries__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_recoveries__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) -| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_system_tests__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_system_tests__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_system_tests__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_system_tests__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | -| nav2_util | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_util__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_util__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_util__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_util__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_util__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_util__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_util__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_util__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | -| nav2_waypoint_follower | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_waypoint_follower__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_waypoint_follower__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_waypoint_follower__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_waypoint_follower__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | +# Lanch with CLOiSim +## Terminal 1: Launch CLOiSim world with robot name 'cloi' +Example: See [CLOiSim](https://github.com/lge-ros2/cloisim) for details + +## Terminal 2: Run cloisim_ros + +Example: See [cloisim_ros](https://github.com/lge-ros2/cloisim_ros/tree/foxy) for details + +```bash +ros2 run cloisim_ros_bringup cloisim_ros_bringup +``` + +## Terminal 3: Launch navigation with params +```bash +ros2 launch nav2_bringup bringup_launch.py use_namespace:=true namespace:=cloi use_sim_time:=true map:=/home/zikprid/work/cloi_ws/seocho_tower_B1F.yaml +``` + +## Terminal 4: Launch rviz +```bash +ros2 launch nav2_bringup rviz_launch.py use_namespace:=true namespace:=cloi rviz_config:=../src/navigation2/nav2_bringup/bringup/rviz/nav2_cloi.rviz +``` diff --git a/nav2_bringup/bringup/launch/localization_launch.py b/nav2_bringup/bringup/launch/localization_launch.py index e058bba8c10..c807658f610 100644 --- a/nav2_bringup/bringup/launch/localization_launch.py +++ b/nav2_bringup/bringup/launch/localization_launch.py @@ -23,6 +23,24 @@ from nav2_common.launch import RewrittenYaml +def get_initial_pose_from_env(): + set_initial_pose = False + initial_pose_x = '0.0' + initial_pose_y = '0.0' + initial_pose_yaw = '0.0' + if 'INITIAL_POSE_X' in os.environ.keys(): + set_initial_pose = True + initial_pose_x = os.environ['INITIAL_POSE_X'] + if 'INITIAL_POSE_Y' in os.environ.keys(): + set_initial_pose = True + initial_pose_y = os.environ['INITIAL_POSE_Y'] + if 'INITIAL_POSE_YAW' in os.environ.keys(): + set_initial_pose = True + initial_pose_yaw = os.environ['INITIAL_POSE_YAW'] + result = [set_initial_pose, initial_pose_x, initial_pose_y, initial_pose_yaw] + return result + + def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') @@ -48,6 +66,18 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} + initial_pose_env = get_initial_pose_from_env() + if initial_pose_env[0]: + print('get_initial_pose_from_env initial_pose_env True') + set_initial_pose = LaunchConfiguration('set_initial_pose', default='true') + initial_pose_x = LaunchConfiguration('x', default=initial_pose_env[1]) + initial_pose_y = LaunchConfiguration('y', default=initial_pose_env[2]) + initial_pose_yaw = LaunchConfiguration('yaw', default=initial_pose_env[3]) + param_substitutions['set_initial_pose'] = set_initial_pose + param_substitutions['x'] = initial_pose_x + param_substitutions['y'] = initial_pose_y + param_substitutions['yaw'] = initial_pose_yaw + configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index c421d709731..c04e807c0eb 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -13,6 +13,8 @@ # limitations under the License. import os +import sys +from collections import OrderedDict from ament_index_python.packages import get_package_share_directory @@ -23,7 +25,23 @@ from nav2_common.launch import RewrittenYaml +def parse_launch_arguments(launch_arguments): + """Parse the given launch arguments from the command line, into list of tuples for launch.""" + parsed_launch_arguments = OrderedDict() # type: ignore + for argument in launch_arguments: + count = argument.count(':=') + if count == 0 or argument.startswith(':=') or (count == 1 and argument.endswith(':=')): + pass + else: + name, value = argument.split(':=', maxsplit=1) + parsed_launch_arguments[name] = value # last one wins is intentional + return parsed_launch_arguments + def generate_launch_description(): + parsed_args_dict = parse_launch_arguments(sys.argv) + remap_ns = parsed_args_dict['namespace'] + print('remap_ns : '+remap_ns) + # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') @@ -47,7 +65,8 @@ def generate_launch_description(): # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + ('/tf_static', 'tf_static'), + ('/scan', '/'+remap_ns+'/scan')] # Create our own temporary YAML files that include substitutions param_substitutions = { diff --git a/nav2_bringup/bringup/launch/rviz_launch.py b/nav2_bringup/bringup/launch/rviz_launch.py index 3126b113485..586870bbc19 100644 --- a/nav2_bringup/bringup/launch/rviz_launch.py +++ b/nav2_bringup/bringup/launch/rviz_launch.py @@ -61,17 +61,13 @@ def generate_launch_description(): arguments=['-d', rviz_config_file], output='screen') - namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}) - start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), package='rviz2', executable='rviz2', name='rviz2', namespace=namespace, - arguments=['-d', namespaced_rviz_config_file], + arguments=['-d', rviz_config_file], output='screen', remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'), diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index bae3aa36c03..0a080494770 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -1,6 +1,12 @@ amcl: ros__parameters: use_sim_time: True + set_initial_pose: True + initial_pose: + x: 62.0 + y: 28.0 + z: 0.0 + yaw: 0.0 alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -51,8 +57,8 @@ bt_navigator: ros__parameters: use_sim_time: True global_frame: map - robot_base_frame: base_link - odom_topic: /odom + robot_base_frame: base_footprint + odom_topic: odom enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 @@ -113,20 +119,20 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.26 + max_vel_x: 1.5 max_vel_y: 0.0 max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 + min_speed_xy: 0.01 + max_speed_xy: 1.5 + min_speed_theta: 0.1 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 - decel_lim_x: -2.5 + decel_lim_x: -5.0 decel_lim_y: 0.0 - decel_lim_theta: -3.2 + decel_lim_theta: -5.0 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 @@ -144,7 +150,7 @@ controller_server: PathAlign.forward_point_distance: 0.1 GoalAlign.scale: 24.0 GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 + PathDist.scale: 96.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 @@ -160,7 +166,7 @@ local_costmap: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_link + robot_base_frame: base_footprint use_sim_time: True rolling_window: true width: 3 @@ -201,21 +207,21 @@ local_costmap: global_costmap: global_costmap: ros__parameters: - update_frequency: 1.0 + update_frequency: 10.0 publish_frequency: 1.0 global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["static_layer", "obstacle_layer", "inflation_layer", "others_footprint_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: - topic: /scan + topic: "/scan" max_obstacle_height: 2.0 clearing: True marking: True @@ -227,6 +233,9 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 + others_footprint_layer: + plugin: "nav2_others_footprint_costmap_plugin::OthersFootprintLayer" + topic: "/cloi2/global_costmap/published_footprint" always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -276,7 +285,7 @@ recoveries_server: wait: plugin: "nav2_recoveries/Wait" global_frame: odom - robot_base_frame: base_link + robot_base_frame: base_footprint transform_timeout: 0.1 use_sim_time: true simulate_ahead_time: 2.0 diff --git a/nav2_bringup/bringup/rviz/nav2_cloi.rviz b/nav2_bringup/bringup/rviz/nav2_cloi.rviz new file mode 100644 index 00000000000..48d7250ef21 --- /dev/null +++ b/nav2_bringup/bringup/rviz/nav2_cloi.rviz @@ -0,0 +1,407 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + - /Controller1 + - /Controller1/Local Costmap1 + - /Controller1/Local Plan1 + Splitter Ratio: 0.4444444477558136 + Tree Height: 325 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 200 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: false + base_scan: + Value: false + imu_link: + Value: false + left_wheel_link: + Value: true + odom: + Value: false + right_wheel_link: + Value: true + Marker Scale: 2 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /cloi/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /cloi/mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /cloi/particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 20 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloi/initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: goal_pose + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 11.84328842163086 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -15.775951385498047 + Y: 1.1489958763122559 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 860 + Hide Left Dock: true + Hide Right Dock: true + Navigation 2: + collapsed: true + QMainWindow State: 000000ff00000000fd0000000400000000000001e800000302fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000245000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000288000000b7000000b700ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002e90000030200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 745 + X: 738 + Y: 477 diff --git a/nav2_others_footprint_costmap_plugin/CMakeLists.txt b/nav2_others_footprint_costmap_plugin/CMakeLists.txt new file mode 100644 index 00000000000..4b252418b72 --- /dev/null +++ b/nav2_others_footprint_costmap_plugin/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_others_footprint_costmap_plugin) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(lib_name ${PROJECT_NAME}_core) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) + +set(dep_pkgs + rclcpp + nav2_costmap_2d + pluginlib) + + +add_library(${lib_name} SHARED + src/others_footprint_layer.cpp) +include_directories(include) + + +install(TARGETS ${lib_name} + DESTINATION lib) + + +pluginlib_export_plugin_description_file(nav2_costmap_2d others_footprint_layer.xml) +ament_target_dependencies(${lib_name} ${dep_pkgs}) +ament_package() diff --git a/nav2_others_footprint_costmap_plugin/include/nav2_others_footprint_costmap_plugin/others_footprint_layer.hpp b/nav2_others_footprint_costmap_plugin/include/nav2_others_footprint_costmap_plugin/others_footprint_layer.hpp new file mode 100644 index 00000000000..93f448c7c1b --- /dev/null +++ b/nav2_others_footprint_costmap_plugin/include/nav2_others_footprint_costmap_plugin/others_footprint_layer.hpp @@ -0,0 +1,46 @@ +#ifndef NAV2_OTHERS_FOOTPRINT_LAYER_HPP_ +#define NAV2_OTHERS_FOOTPRINT_LAYER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + + +namespace nav2_others_footprint_costmap_plugin +{ + +class OthersFootprintLayer : public nav2_costmap_2d::Layer +{ +public: + OthersFootprintLayer(); + + virtual void onInitialize(); + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y); + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j); + + virtual void reset() + { + return; + } + + virtual void onFootprintChanged(); + + virtual bool isClearable() {return false;} +private: + std::string device_id_; + std::string topic_; + double time_tolerance_; + + std::map footprint_map_; + + rclcpp::Subscription::SharedPtr footprint_sub_; +}; + +} // namespace nav2_others_footprint_costmap_plugin + +#endif // NAV2_OTHERS_FOOTPRINT_LAYER_HPP_ diff --git a/nav2_others_footprint_costmap_plugin/others_footprint_layer.xml b/nav2_others_footprint_costmap_plugin/others_footprint_layer.xml new file mode 100644 index 00000000000..4fbe5a347a6 --- /dev/null +++ b/nav2_others_footprint_costmap_plugin/others_footprint_layer.xml @@ -0,0 +1,7 @@ + + + This is an example plugin which puts repeating costs gradients to costmap + + diff --git a/nav2_others_footprint_costmap_plugin/package.xml b/nav2_others_footprint_costmap_plugin/package.xml new file mode 100644 index 00000000000..5bf3617bd04 --- /dev/null +++ b/nav2_others_footprint_costmap_plugin/package.xml @@ -0,0 +1,23 @@ + + + + nav2_others_footprint_costmap_plugin + 1.0.0 + Run-time plugin for others footprint layer + Sungkyu Kang + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + nav2_costmap_2d + pluginlib + rclcpp + + + + ament_cmake + + diff --git a/nav2_others_footprint_costmap_plugin/src/others_footprint_layer.cpp b/nav2_others_footprint_costmap_plugin/src/others_footprint_layer.cpp new file mode 100644 index 00000000000..be594b9c1c7 --- /dev/null +++ b/nav2_others_footprint_costmap_plugin/src/others_footprint_layer.cpp @@ -0,0 +1,118 @@ +#include "nav2_others_footprint_costmap_plugin/others_footprint_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::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +namespace nav2_others_footprint_costmap_plugin +{ + +OthersFootprintLayer::OthersFootprintLayer() +{ +} + +void OthersFootprintLayer::onInitialize() +{ + declareParameter("enabled", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("topic", rclcpp::ParameterValue("/lge_seocho/B1F/others_footprint")); + node_->get_parameter(name_ + "." + "topic", topic_); + declareParameter("time_tolerance", rclcpp::ParameterValue(3.0)); + node_->get_parameter(name_ + "." + "time_tolerance", time_tolerance_); + + std::string full_ns = node_->get_namespace(); + size_t split = full_ns.find('/'); + if (split == std::string::npos) { + device_id_ = full_ns; + } else { + std::string without_first_slash = full_ns; + if(split == 0) { + without_first_slash = full_ns.substr(1); + } + split = without_first_slash.find('/'); + device_id_ = without_first_slash.substr(0, split); + } + + auto callback = [this](const geometry_msgs::msg::PolygonStamped::SharedPtr msg) -> void { + if(device_id_ == "cloi2") { + return; + } + std::string frame_id = msg->header.frame_id; + if(frame_id != device_id_ && frame_id != "") { + if(footprint_map_.find(frame_id) != footprint_map_.end()) { + footprint_map_[frame_id] = msg; + } else { + footprint_map_.insert(make_pair(frame_id, msg)); + } + } + }; + footprint_sub_ = node_->create_subscription( + topic_, + rclcpp::SensorDataQoS(), + callback); + + RCLCPP_INFO(rclcpp::get_logger( + "nav2_costmap_2d"), "OthersFootprintLayer enabled: %d", + enabled_); + RCLCPP_INFO(rclcpp::get_logger( + "nav2_costmap_2d"), "OthersFootprintLayer topic: %s", + topic_.c_str()); + RCLCPP_INFO(rclcpp::get_logger( + "nav2_costmap_2d"), "OthersFootprintLayer time_tolerance: %f", + time_tolerance_); +} + +void OthersFootprintLayer::updateBounds( + double /* robot_x */, double /* robot_y */, double /* robot_yaw */, + double * /* min_x */, double * /* min_y */, double * /* max_x */, double * /* max_y */) +{ +} + +void OthersFootprintLayer::onFootprintChanged() +{ + RCLCPP_DEBUG(rclcpp::get_logger( + "nav2_costmap_2d"), "OthersFootprintLayer::onFootprintChanged(): num footprint points: %lu", + layered_costmap_->getFootprint().size()); +} + +void OthersFootprintLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int /* min_i */, int /* min_j */, int /* max_i */, int /* max_j */) +{ + if (!enabled_) { + return; + } + unsigned char * master_array = master_grid.getCharMap(); + auto system_clock = node_->get_clock(); + bool ros_time_is_active = system_clock->ros_time_is_active(); + rclcpp::Time system_now = system_clock->now(); + rclcpp::Duration tolerance_duration = rclcpp::Duration(time_tolerance_, 0); + for(auto it=footprint_map_.begin(); it!=footprint_map_.end(); ++it) { + geometry_msgs::msg::PolygonStamped::SharedPtr footprint = it->second; + rclcpp::Time msg_time = footprint->header.stamp; + rclcpp::Duration time_sub = system_now - msg_time; + if(time_sub < tolerance_duration) { + for(unsigned int point_index = 0; point_index < footprint->polygon.points.size(); point_index++) { + double wx = (double)footprint->polygon.points[point_index].x; + double wy = (double)footprint->polygon.points[point_index].y; + int mx, my; + master_grid.worldToMapNoBounds(wx, wy, mx, my); + int index = master_grid.getIndex(mx, my); + master_array[index] = LETHAL_OBSTACLE; + } + } + } + +} + +} // namespace nav2_others_footprint_costmap_plugin + +// This is the macro allowing a nav2_others_footprint_costmap_plugin::OthersFootprintLayer 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_others_footprint_costmap_plugin::OthersFootprintLayer, nav2_costmap_2d::Layer)