├── .gitignore ├── LICENSE ├── README.md ├── marathon.repos ├── marathon_log_nodes ├── CMakeLists.txt ├── package.xml └── src │ └── marathon_log_node.cpp ├── marathon_ros2_bringup ├── CMakeLists.txt ├── behavior_trees │ └── navigate_w_replanning_and_recovery.xml ├── launch │ ├── kobuki │ │ └── nav2_kobuki_launch.py │ ├── nav2_bringup_launch.py │ ├── nav2_contextual_launch.py │ ├── nav2_marathon_launch.py │ ├── nav2_navigation_launch.py │ ├── navigation_launch.py │ └── tiago │ │ ├── nav2_tiago_adapt_pp_launch.py │ │ ├── nav2_tiago_launch.py │ │ ├── nav2_tiago_pp_launch.py │ │ ├── nav2_tiago_regulated_pp_launch.py │ │ └── nav2_tiago_teb_launch.py ├── maps │ ├── aulario │ │ ├── map.pgm │ │ ├── map.yaml │ │ └── map_obstacles.pgm │ ├── map.pgm │ └── map.yaml ├── package.xml └── params │ ├── nav2_adapt_pp_tiago_params.yaml │ ├── nav2_contextual_tiago_params.yaml │ ├── nav2_dynamic_follow_params.yaml │ ├── nav2_marathon_kobuki_params.yaml │ ├── nav2_marathon_teb_kobuki_params.yaml │ ├── nav2_marathon_teb_tiago_params.yaml │ ├── nav2_marathon_tiago_params.yaml │ ├── nav2_pp_tiago_params.yaml │ └── nav2_regulated_pp_tiago_params.yaml ├── marathon_ros2_csv ├── marathon_ros2_csv │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-36.pyc │ │ └── topics_2_csv.cpython-36.pyc │ ├── plot_csv.py │ └── topics_2_csv.py ├── package.xml ├── resource │ └── marathon_ros2_csv ├── setup.cfg └── setup.py └── marathon_ros2_wp_manager ├── CMakeLists.txt ├── package.xml └── src └── wp_manager.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | teb/ 2 | marathon_ros2_csv/marathon_ros2_csv/__pycache__/* 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # marathon_ros2 2 | 3 | [![](https://img.youtube.com/vi/tXp8wFZr68M/0.jpg)](https://www.youtube.com/watch?v=tXp8wFZr68M&feature=youtu.be "Click to play on You Tube") 4 | 5 | ## Working components for the experiment 6 | 7 | - [x] Tiago drivers in ROS 8 | - [x] ros1_bridge 9 | - [x] [cmd_vel_mux](https://github.com/IntelligentRoboticsLabs/nav2-TIAGo-support/tree/master/cmd_vel_mux) and [tf_static_resender](https://github.com/IntelligentRoboticsLabs/nav2-TIAGo-support/tree/master/tf_static_resender) to correctly bridge all the required topics and tf ROS <-> ROS2 10 | - [x] Navigation2 11 | - [x] STVL 12 | - [x] TEB 13 | - [x] Adjust/optimize speeds and other parameters for Tiago 14 | - [x] Tool to monitorize experiment (distance and time) 15 | - [x] ROSbag configuration 16 | - [x] Launcher to launch experiment complete 17 | 18 | ## Instructions to reproduce this experiment 19 | 20 | 1. Create a new workspace with the ROS2 sources. Follow [this](https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Development-Setup/) instructions of the ROS2 wiki. 21 | Before compile the workspace, change the ros1_bridge for [this version](https://github.com/fmrico/ros1_bridge/tree/dedicated_bridges). 22 | - Is important to compile this workspace, compile first without the ros1_bridge: 23 | ``` 24 | colcon build --symlink-install --packages-skip ros1_bridge 25 | ``` 26 | - If the compilation conclude and everything is OK do: 27 | ``` 28 | source /opt/ros/melodic/setup.bash 29 | colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure 30 | ``` 31 | 2. Create other workspace with the marathon_ros2 and the dependencies. 32 | ``` 33 | vcs import src < marathon.repos 34 | ``` 35 | 3. When the two workspaces are compiled you can start with the marathon... 36 | 4. Open 6 differents terminals and run: 37 | 38 | ``` 39 | # Terminal 1 40 | source ~/ros2_sources_ws/install/setup.bash 41 | source /opt/ros/melodic/setup.bash 42 | ros2 run ros1_bridge tf_static_1_to_2 43 | ``` 44 | ``` 45 | # Terminal 2 46 | source ~/ros2_sources_ws/install/setup.bash 47 | source /opt/ros/melodic/setup.bash 48 | ros2 run ros1_bridge imu_1_to_2 49 | ``` 50 | ``` 51 | # Terminal 3 52 | source ~/ros2_sources_ws/install/setup.bash 53 | source /opt/ros/melodic/setup.bash 54 | ros2 run ros1_bridge scan_1_to_2 55 | ``` 56 | ``` 57 | # Terminal 4 58 | source ~/ros2_sources_ws/install/setup.bash 59 | source /opt/ros/melodic/setup.bash 60 | ros2 run ros1_bridge tf_1_to_2 61 | ``` 62 | ``` 63 | # Terminal 5 64 | source ~/ros2_sources_ws/install/setup.bash 65 | source /opt/ros/melodic/setup.bash 66 | ros2 run ros1_bridge pc2_1_to_2 67 | ``` 68 | ``` 69 | # Terminal 6 70 | source ~/ros2_sources_ws/install/setup.bash 71 | source /opt/ros/melodic/setup.bash 72 | ros2 run ros1_bridge twist_2_to_1 73 | ``` 74 | 75 | 5. Open two more terminals and run: 76 | 77 | ``` 78 | # Terminal 7 79 | source ~/ros2_sources_ws/install/setup.bash 80 | source ~/ros2_ws/install/setup.bash 81 | ros2 launch marathon_ros2_bringup nav2_tiago_launch.py 82 | ``` 83 | ``` 84 | # Terminal 8 85 | source ~/ros2_sources_ws/install/setup.bash 86 | source ~/ros2_ws/install/setup.bash 87 | ros2 topic pub /start_navigate std_msgs/msg/Empty {} 88 | ``` 89 | -------------------------------------------------------------------------------- /marathon.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | BehaviorTree.CPP: 3 | type: git 4 | url: https://github.com/BehaviorTree/BehaviorTree.CPP.git 5 | version: master 6 | navigation2: 7 | type: git 8 | url: https://github.com/MROS-RobMoSys-ITP/navigation2.git 9 | version: master 10 | pcl_msgs: 11 | type: git 12 | url: https://github.com/ros-perception/pcl_msgs.git 13 | version: ros2 14 | perception_pcl: 15 | type: git 16 | url: https://github.com/ros-perception/perception_pcl.git 17 | version: eloquent-devel 18 | spatio_temporal_voxel_layer: 19 | type: git 20 | url: https://github.com/SteveMacenski/spatio_temporal_voxel_layer.git 21 | version: eloquent-devel 22 | -------------------------------------------------------------------------------- /marathon_log_nodes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(marathon_log_nodes) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_lifecycle REQUIRED) 22 | find_package(ament_index_cpp REQUIRED) 23 | find_package(nav2_msgs REQUIRED) 24 | find_package(nav2_util REQUIRED) 25 | find_package(rclcpp_action REQUIRED) 26 | find_package(rcl_lifecycle REQUIRED) 27 | find_package(lifecycle_msgs REQUIRED) 28 | find_package(geometry_msgs REQUIRED) 29 | find_package(builtin_interfaces REQUIRED) 30 | find_package(tf2_ros REQUIRED) 31 | 32 | # uncomment the following section in order to fill in 33 | # further dependencies manually. 34 | # find_package( REQUIRED) 35 | 36 | include_directories( 37 | include 38 | ) 39 | 40 | set(dependencies 41 | rclcpp 42 | rclcpp_lifecycle 43 | ament_index_cpp 44 | nav2_msgs 45 | rclcpp_action 46 | lifecycle_msgs 47 | rcl_lifecycle 48 | geometry_msgs 49 | tf2_ros 50 | nav2_util 51 | ) 52 | 53 | if(BUILD_TESTING) 54 | find_package(ament_lint_auto REQUIRED) 55 | # the following line skips the linter which checks for copyrights 56 | # uncomment the line when a copyright and license is not present in all source files 57 | #set(ament_cmake_copyright_FOUND TRUE) 58 | # the following line skips cpplint (only works in a git repo) 59 | # uncomment the line when this package is not in a git repo 60 | #set(ament_cmake_cpplint_FOUND TRUE) 61 | ament_lint_auto_find_test_dependencies() 62 | endif() 63 | 64 | add_executable(marathon_log_node src/marathon_log_node.cpp) 65 | ament_target_dependencies(marathon_log_node ${dependencies}) 66 | 67 | #install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 68 | #install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 69 | 70 | install(TARGETS 71 | marathon_log_node 72 | DESTINATION lib/${PROJECT_NAME} 73 | ) 74 | 75 | ament_package() 76 | -------------------------------------------------------------------------------- /marathon_log_nodes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | marathon_log_nodes 5 | 0.0.1 6 | TODO: Package description 7 | Lorena Bajo Rebollo 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | ament_index_cpp 15 | nav2_msgs 16 | nav2_util 17 | rclcpp_action 18 | builtin_interfaces 19 | tf2_ros 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /marathon_log_nodes/src/marathon_log_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Intelligent Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * * Neither the name of Intelligent Robotics nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Lorena Bajo Rebollo lorena.bajo@urjc.es */ 34 | 35 | /* Mantainer: Lorena Bajo Rebollo lorena.bajo@urjc.es */ 36 | 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "rclcpp_action/rclcpp_action.hpp" 47 | #include "ament_index_cpp/get_package_share_directory.hpp" 48 | 49 | #include "geometry_msgs/msg/pose_stamped.hpp" 50 | #include "geometry_msgs/msg/pose.hpp" 51 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 52 | #include "geometry_msgs/msg/vector3.hpp" 53 | #include "geometry_msgs/msg/quaternion.hpp" 54 | #include "geometry_msgs/msg/point.hpp" 55 | 56 | #include "rclcpp/rclcpp.hpp" 57 | #include "std_msgs/msg/string.hpp" 58 | #include "std_msgs/msg/float64.hpp" 59 | #include "nav_msgs/msg/path.hpp" 60 | 61 | #include "boost/date_time/posix_time/posix_time.hpp" 62 | 63 | #include 64 | #include 65 | 66 | #include "tf2_ros/transform_listener.h" 67 | #include "tf2_ros/transform_broadcaster.h" 68 | #include "tf2_ros/message_filter.h" 69 | #include "nav2_util/geometry_utils.hpp" 70 | 71 | #include "builtin_interfaces/msg/time.hpp" 72 | 73 | #include 74 | 75 | using namespace std::chrono_literals; 76 | using std::placeholders::_1; 77 | 78 | using namespace std::placeholders; 79 | using namespace std::chrono_literals; 80 | 81 | class MarathonLogNode : public rclcpp::Node 82 | { 83 | public: 84 | MarathonLogNode() 85 | : Node("marathon_log_node"), path_() 86 | { 87 | amcl_pose_sub_ = create_subscription("/amcl_pose", rclcpp::QoS(10), std::bind(&MarathonLogNode::poseAMCLCallback, this, _1)); 88 | path_sub_ = create_subscription("/plan", rclcpp::QoS(10), std::bind(&MarathonLogNode::path_callback, this, _1)); 89 | distance_pub_ = create_publisher("/marathon_ros2/distance", rclcpp::QoS(10)); 90 | time_nav_pub_ = create_publisher("/marathon_ros2/time_nav", rclcpp::QoS(10)); 91 | time_pub_ = create_publisher("/marathon_ros2/clock", rclcpp::QoS(10)); 92 | path_distance_pub_ = create_publisher("/marathon_ros2/path_distance", rclcpp::QoS(10)); 93 | 94 | meters_ = 0.0; 95 | old_x = 0.0; 96 | old_y = 0.0; 97 | } 98 | 99 | 100 | void calculateTimeNavigating() 101 | { 102 | rclcpp::Time current_time_ = now(); 103 | rclcpp::Time time_navigating_; 104 | 105 | double secs = (current_time_ - started_time_).seconds(); 106 | double nanosecs = (current_time_ - started_time_).nanoseconds(); 107 | 108 | builtin_interfaces::msg::Time msg_time; 109 | 110 | msg_time.sec = secs; 111 | msg_time.nanosec = nanosecs; 112 | 113 | time_nav_pub_->publish(msg_time); 114 | 115 | builtin_interfaces::msg::Time msg_curr_time; 116 | 117 | msg_curr_time.sec = current_time_.seconds(); 118 | msg_curr_time.nanosec = current_time_.nanoseconds(); 119 | 120 | time_pub_->publish(msg_curr_time); 121 | 122 | } 123 | 124 | float calculateDistance(float current_x, float current_y, float old_x, float old_y) 125 | { 126 | return sqrt((pow(current_x - old_x, 2) + pow(current_y - old_y, 2))); 127 | } 128 | 129 | float metersToMiles(float meters) 130 | { 131 | return meters*0.000621371; 132 | } 133 | 134 | void setOldposition(int current_x, int current_y) 135 | { 136 | old_x = current_x; 137 | old_y = current_y; 138 | } 139 | 140 | void poseAMCLCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) //const 141 | { 142 | float current_x, current_y; 143 | 144 | current_x = msg->pose.pose.position.x; 145 | current_y = msg->pose.pose.position.y; 146 | float miles = 0.0; 147 | if (old_x != 0.0 && old_y != 0.0) 148 | { 149 | meters_ = calculateDistance(current_x, current_y, old_x, old_y); 150 | setOldposition(current_x, current_y); 151 | miles = metersToMiles(meters_); 152 | } 153 | else 154 | setOldposition(current_x, current_y); 155 | 156 | if (path_) 157 | { 158 | std_msgs::msg::Float64 path_dist; 159 | path_dist.data = get_path_distance(path_, msg->pose.pose.position); 160 | path_distance_pub_->publish(path_dist); 161 | } 162 | 163 | 164 | std_msgs::msg::Float64 dist; 165 | dist.data = miles; 166 | 167 | distance_pub_->publish(dist); 168 | 169 | meters_ = 0.0; 170 | 171 | } 172 | 173 | void path_callback(const nav_msgs::msg::Path::SharedPtr msg) 174 | { 175 | if (!path_) 176 | { 177 | path_ = msg; 178 | } 179 | } 180 | 181 | float get_path_distance(nav_msgs::msg::Path::SharedPtr path, geometry_msgs::msg::Point robot_position) 182 | { 183 | std::vector path_dis_v; 184 | for (auto pose : path->poses) 185 | { 186 | path_dis_v.push_back(nav2_util::geometry_utils::euclidean_distance(pose.pose.position, robot_position)); 187 | } 188 | return *std::min(path_dis_v.begin(), path_dis_v.end()); 189 | } 190 | 191 | void step() 192 | { 193 | calculateTimeNavigating(); 194 | } 195 | 196 | void firstTime(){ 197 | started_time_= now(); 198 | } 199 | 200 | 201 | protected: 202 | rclcpp::Publisher::SharedPtr distance_pub_, path_distance_pub_; 203 | rclcpp::Publisher::SharedPtr time_nav_pub_, time_pub_; 204 | rclcpp::Subscription::SharedPtr amcl_pose_sub_; 205 | rclcpp::Subscription::SharedPtr path_sub_; 206 | 207 | std::shared_ptr tf_listener_; 208 | std::shared_ptr tf_buffer_; 209 | rclcpp::TimerBase::SharedPtr timer_; 210 | std::shared_ptr path_; 211 | 212 | float old_x, old_y; 213 | float meters_; 214 | 215 | rclcpp::Time started_time_; 216 | 217 | }; 218 | 219 | int main(int argc, char * argv[]) 220 | { 221 | rclcpp::init(argc, argv); 222 | 223 | auto node = std::make_shared(); 224 | 225 | node->firstTime(); 226 | 227 | rclcpp::Rate loop_rate(1); 228 | while (rclcpp::ok()) { 229 | node->step(); 230 | rclcpp::spin_some(node); 231 | loop_rate.sleep(); 232 | } 233 | 234 | rclcpp::shutdown(); 235 | 236 | return 0; 237 | } -------------------------------------------------------------------------------- /marathon_ros2_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(marathon_ros2_bringup) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_BUILD_TYPE DEBUG) 6 | 7 | # find dependencies 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(rclpy REQUIRED) 11 | 12 | install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 13 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 14 | install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) 15 | install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) 16 | 17 | ament_export_dependencies(${dependencies}) 18 | 19 | ament_package() 20 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/behavior_trees/navigate_w_replanning_and_recovery.xml: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/kobuki/nav2_kobuki_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | import launch.substitutions 4 | import launch_ros.actions 5 | import os 6 | from pathlib import Path 7 | from launch.substitutions import LaunchConfiguration 8 | from ament_index_python.packages import get_package_share_directory 9 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 10 | IncludeLaunchDescription, RegisterEventHandler) 11 | from launch.conditions import IfCondition 12 | from launch.event_handlers import OnProcessExit 13 | from launch.events import Shutdown 14 | from launch.launch_description_sources import PythonLaunchDescriptionSource 15 | 16 | def generate_launch_description(): 17 | 18 | hardware_config = Path(get_package_share_directory('pilot_kobuki'), 'config', 'hardware.yaml') 19 | assert hardware_config.is_file() 20 | 21 | kobuki_dir = get_package_share_directory('pilot_kobuki') 22 | launch_dir = os.path.join(kobuki_dir, 'launch') 23 | 24 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 25 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 26 | 27 | #enable_align_depth = launch.substitutions.LaunchConfiguration('enable_aligned_depth', default="false") 28 | 29 | # Create the launch configuration variables 30 | params_file = LaunchConfiguration('params_file') 31 | total_distance_sum = LaunchConfiguration('total_distance_sum') 32 | next_wp = LaunchConfiguration('next_wp') 33 | 34 | autostart = LaunchConfiguration('autostart') 35 | 36 | declare_params_file_cmd = DeclareLaunchArgument( 37 | 'params_file', 38 | default_value=os.path.join(marathon_dir, 'params', 'nav2_marathon_kobuki_params.yaml'), 39 | description='Full path to the ROS2 parameters file to use for all launched nodes') 40 | 41 | declare_autostart_cmd = DeclareLaunchArgument( 42 | 'autostart', default_value='true', 43 | description='Automatically startup the nav2 stack') 44 | 45 | declare_total_distance_sum_cmd = DeclareLaunchArgument( 46 | 'total_distance_sum', 47 | default_value= '0.25', 48 | description='The distance accumulated in the experiment') 49 | 50 | declare_next_wp_cmd = DeclareLaunchArgument( 51 | 'next_wp', 52 | default_value= '0', 53 | description='Next waypoint for the navigation system') 54 | 55 | rplidar_cmd = IncludeLaunchDescription( 56 | PythonLaunchDescriptionSource(os.path.join( 57 | get_package_share_directory('rplidar_ros'), 58 | 'launch', 59 | 'rplidar.launch.py')) 60 | ) 61 | 62 | astra_node = launch_ros.actions.Node( 63 | package='astra_camera', 64 | node_executable='astra_camera_node', 65 | node_name='astra_camera_node', 66 | output='screen', 67 | parameters=[ 68 | {'width': 1280}, 69 | {'height': 1024}, 70 | {'framerate': 30.0}]) 71 | 72 | kobuki_node = launch_ros.actions.Node( 73 | package='turtlebot2_drivers', 74 | node_executable='kobuki_node', 75 | output='screen') 76 | 77 | depth_to_pointcloud2_node = launch_ros.actions.Node( 78 | package='depthimage_to_pointcloud2', 79 | node_executable='depthimage_to_pointcloud2_node', 80 | output='screen') 81 | 82 | tf_kobuki2laser_node = launch_ros.actions.Node( 83 | package='tf2_ros', 84 | node_executable='static_transform_publisher', 85 | output='screen', 86 | arguments=['0.0', '0.0', '0.40', 87 | '0', '1', '0', '0', 88 | 'base_link', 89 | 'laser_frame']) 90 | 91 | laserfilter_node = launch_ros.actions.Node( 92 | package='pilot_kobuki', 93 | node_executable='laser_filter_node') 94 | 95 | tf_kobuki2imu_node = launch_ros.actions.Node( 96 | package='tf2_ros', 97 | node_executable='static_transform_publisher', 98 | output='screen', 99 | arguments=['0.0', '0.0', '0.0', 100 | '0', '0', '0', '1', 101 | 'base_link', 102 | 'imu_link']) 103 | tf_kobuki2camera_node = launch_ros.actions.Node( 104 | package='tf2_ros', 105 | node_executable='static_transform_publisher', 106 | output='screen', 107 | arguments=['0.12', '0.0', '0.22', 108 | '0', '0', '0', '1', 109 | 'base_link', 110 | 'openni_color_optical_frame']) 111 | tf_kobuki2depth_node = launch_ros.actions.Node( 112 | package='tf2_ros', 113 | node_executable='static_transform_publisher', 114 | output='screen', 115 | arguments=['0.12', '0.0', '0.22', 116 | '-0.5', '0.5', '-0.5', '0.5', 117 | 'base_link', 118 | 'openni_depth_optical_frame']) 119 | nav2_cmd = IncludeLaunchDescription( 120 | PythonLaunchDescriptionSource(os.path.join( 121 | get_package_share_directory('marathon_ros2_bringup'), 'launch/tiago', 122 | 'nav2_tiago_launch.py')), 123 | launch_arguments={ 124 | 'autostart': 'true', 125 | 'params_file': params_file, 126 | 'cmd_vel_topic': '/cmd_vel' 127 | }.items()) 128 | 129 | log_node = launch_ros.actions.Node( 130 | package='marathon_log_nodes', 131 | node_executable='marathon_log_node', 132 | parameters=[ 133 | {'total_distance_sum': total_distance_sum}] 134 | ) 135 | 136 | wp_manager_node = launch_ros.actions.Node( 137 | package='marathon_ros2_wp_manager', 138 | node_executable='wp_manager_node', 139 | parameters=[ 140 | {'next_wp': next_wp}] 141 | ) 142 | 143 | return launch.LaunchDescription([ 144 | declare_params_file_cmd, 145 | declare_total_distance_sum_cmd, 146 | declare_next_wp_cmd, 147 | rplidar_cmd, 148 | laserfilter_node, 149 | astra_node, 150 | depth_to_pointcloud2_node, 151 | kobuki_node, 152 | tf_kobuki2laser_node, 153 | tf_kobuki2imu_node, 154 | tf_kobuki2camera_node, 155 | tf_kobuki2depth_node, 156 | nav2_cmd, 157 | log_node, 158 | wp_manager_node 159 | ]) 160 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/nav2_bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import (DeclareLaunchArgument, GroupAction, 21 | IncludeLaunchDescription, SetEnvironmentVariable) 22 | from launch.conditions import IfCondition 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration, PythonExpression 25 | from launch_ros.actions import PushRosNamespace 26 | 27 | 28 | def generate_launch_description(): 29 | # Get the launch directory 30 | bringup_dir = get_package_share_directory('nav2_bringup') 31 | launch_dir = os.path.join(bringup_dir, 'launch') 32 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 33 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 34 | 35 | # Create the launch configuration variables 36 | namespace = LaunchConfiguration('namespace') 37 | use_namespace = LaunchConfiguration('use_namespace') 38 | slam = LaunchConfiguration('slam') 39 | map_yaml_file = LaunchConfiguration('map') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | params_file = LaunchConfiguration('params_file') 42 | default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') 43 | autostart = LaunchConfiguration('autostart') 44 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 45 | 46 | stdout_linebuf_envvar = SetEnvironmentVariable( 47 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 48 | 49 | declare_namespace_cmd = DeclareLaunchArgument( 50 | 'namespace', 51 | default_value='', 52 | description='Top-level namespace') 53 | 54 | declare_use_namespace_cmd = DeclareLaunchArgument( 55 | 'use_namespace', 56 | default_value='false', 57 | description='Whether to apply a namespace to the navigation stack') 58 | 59 | declare_slam_cmd = DeclareLaunchArgument( 60 | 'slam', 61 | default_value='False', 62 | description='Whether run a SLAM') 63 | 64 | declare_map_yaml_cmd = DeclareLaunchArgument( 65 | 'map', 66 | description='Full path to map yaml file to load') 67 | 68 | declare_use_sim_time_cmd = DeclareLaunchArgument( 69 | 'use_sim_time', 70 | default_value='false', 71 | description='Use simulation (Gazebo) clock if true') 72 | 73 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 74 | 'cmd_vel_topic', 75 | default_value='cmd_vel', 76 | description='Command velocity topic') 77 | 78 | declare_params_file_cmd = DeclareLaunchArgument( 79 | 'params_file', 80 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 81 | description='Full path to the ROS2 parameters file to use for all launched nodes') 82 | 83 | declare_bt_xml_cmd = DeclareLaunchArgument( 84 | 'default_bt_xml_filename', 85 | default_value=os.path.join( 86 | get_package_share_directory('nav2_bt_navigator'), 87 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 88 | description='Full path to the behavior tree xml file to use') 89 | 90 | declare_autostart_cmd = DeclareLaunchArgument( 91 | 'autostart', default_value='true', 92 | description='Automatically startup the nav2 stack') 93 | 94 | # Specify the actions 95 | bringup_cmd_group = GroupAction([ 96 | PushRosNamespace( 97 | condition=IfCondition(use_namespace), 98 | namespace=namespace), 99 | 100 | IncludeLaunchDescription( 101 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), 102 | condition=IfCondition(slam), 103 | launch_arguments={'namespace': namespace, 104 | 'use_sim_time': use_sim_time, 105 | 'autostart': autostart, 106 | 'params_file': params_file}.items()), 107 | 108 | IncludeLaunchDescription( 109 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 110 | 'localization_launch.py')), 111 | condition=IfCondition(PythonExpression(['not ', slam])), 112 | launch_arguments={'namespace': namespace, 113 | 'map': map_yaml_file, 114 | 'use_sim_time': use_sim_time, 115 | 'autostart': autostart, 116 | 'params_file': params_file, 117 | 'use_lifecycle_mgr': 'false'}.items()), 118 | 119 | IncludeLaunchDescription( 120 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'navigation_launch.py')), 121 | launch_arguments={'namespace': namespace, 122 | 'use_sim_time': use_sim_time, 123 | 'autostart': autostart, 124 | 'params_file': params_file, 125 | 'default_bt_xml_filename': default_bt_xml_filename, 126 | 'use_lifecycle_mgr': 'false', 127 | 'map_subscribe_transient_local': 'true', 128 | 'cmd_vel_topic': cmd_vel_topic}.items()), 129 | ]) 130 | 131 | # Create the launch description and populate 132 | ld = LaunchDescription() 133 | 134 | # Set environment variables 135 | ld.add_action(stdout_linebuf_envvar) 136 | 137 | # Declare the launch options 138 | ld.add_action(declare_namespace_cmd) 139 | ld.add_action(declare_use_namespace_cmd) 140 | ld.add_action(declare_slam_cmd) 141 | ld.add_action(declare_map_yaml_cmd) 142 | ld.add_action(declare_use_sim_time_cmd) 143 | ld.add_action(declare_params_file_cmd) 144 | ld.add_action(declare_autostart_cmd) 145 | ld.add_action(declare_bt_xml_cmd) 146 | ld.add_action(declare_cmd_vel_topic_cmd) 147 | 148 | # Add the actions to launch all of the navigation nodes 149 | ld.add_action(bringup_cmd_group) 150 | 151 | return ld 152 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/nav2_contextual_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | import launch.substitutions 4 | import launch_ros.actions 5 | import os 6 | from pathlib import Path 7 | from launch.substitutions import LaunchConfiguration 8 | from ament_index_python.packages import get_package_share_directory 9 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 10 | IncludeLaunchDescription, RegisterEventHandler) 11 | from launch.conditions import IfCondition 12 | from launch.event_handlers import OnProcessExit 13 | from launch.events import Shutdown 14 | from launch.launch_description_sources import PythonLaunchDescriptionSource 15 | 16 | def generate_launch_description(): 17 | 18 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 19 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 20 | 21 | # Create the launch configuration variables 22 | next_wp = LaunchConfiguration('next_wp') 23 | 24 | autostart = LaunchConfiguration('autostart') 25 | 26 | declare_autostart_cmd = DeclareLaunchArgument( 27 | 'autostart', default_value='true', 28 | description='Automatically startup the nav2 stack') 29 | 30 | declare_next_wp_cmd = DeclareLaunchArgument( 31 | 'next_wp', 32 | default_value= '1', 33 | description='Next waypoint for the navigation system') 34 | 35 | #log_node = launch_ros.actions.Node( 36 | # package='marathon_log_nodes', 37 | # node_executable='marathon_log_node', 38 | #) 39 | 40 | wp_manager_node = launch_ros.actions.Node( 41 | package='marathon_ros2_wp_manager', 42 | node_executable='wp_manager_node', 43 | parameters=[ 44 | {'next_wp': next_wp}] 45 | ) 46 | 47 | topics_2_csv_node = launch_ros.actions.Node( 48 | package='marathon_ros2_csv', 49 | node_executable='topics_2_csv' 50 | ) 51 | 52 | return launch.LaunchDescription([ 53 | declare_next_wp_cmd, 54 | #log_node, 55 | wp_manager_node, 56 | topics_2_csv_node 57 | ]) 58 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/nav2_marathon_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | import launch.substitutions 4 | import launch_ros.actions 5 | import os 6 | from pathlib import Path 7 | from launch.substitutions import LaunchConfiguration 8 | from ament_index_python.packages import get_package_share_directory 9 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 10 | IncludeLaunchDescription, RegisterEventHandler) 11 | from launch.conditions import IfCondition 12 | from launch.event_handlers import OnProcessExit 13 | from launch.events import Shutdown 14 | from launch.launch_description_sources import PythonLaunchDescriptionSource 15 | 16 | def generate_launch_description(): 17 | 18 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 19 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 20 | 21 | # Create the launch configuration variables 22 | next_wp = LaunchConfiguration('next_wp') 23 | 24 | autostart = LaunchConfiguration('autostart') 25 | 26 | declare_autostart_cmd = DeclareLaunchArgument( 27 | 'autostart', default_value='true', 28 | description='Automatically startup the nav2 stack') 29 | 30 | declare_next_wp_cmd = DeclareLaunchArgument( 31 | 'next_wp', 32 | default_value= '0', 33 | description='Next waypoint for the navigation system') 34 | 35 | log_node = launch_ros.actions.Node( 36 | package='marathon_log_nodes', 37 | executable='marathon_log_node', 38 | ) 39 | 40 | wp_manager_node = launch_ros.actions.Node( 41 | package='marathon_ros2_wp_manager', 42 | executable='wp_manager_node', 43 | parameters=[ 44 | {'next_wp': next_wp}] 45 | ) 46 | 47 | topics_2_csv_node = launch_ros.actions.Node( 48 | package='marathon_ros2_csv', 49 | executable='topics_2_csv' 50 | ) 51 | 52 | return launch.LaunchDescription([ 53 | declare_next_wp_cmd, 54 | log_node, 55 | wp_manager_node, 56 | topics_2_csv_node 57 | ]) 58 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/nav2_navigation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 21 | from launch.conditions import IfCondition 22 | from launch.substitutions import LaunchConfiguration 23 | 24 | from launch_ros.actions import Node 25 | from nav2_common.launch import RewrittenYaml 26 | 27 | 28 | def generate_launch_description(): 29 | # Get the launch directory 30 | bringup_dir = get_package_share_directory('nav2_bringup') 31 | 32 | namespace = LaunchConfiguration('namespace') 33 | use_sim_time = LaunchConfiguration('use_sim_time') 34 | autostart = LaunchConfiguration('autostart') 35 | params_file = LaunchConfiguration('params_file') 36 | bt_xml_file = LaunchConfiguration('bt_xml_file') 37 | map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') 38 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 39 | 40 | lifecycle_nodes = ['controller_server', 41 | 'planner_server', 42 | 'recoveries_server', 43 | 'bt_navigator', 44 | 'waypoint_follower'] 45 | 46 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 47 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 48 | # https://github.com/ros/geometry2/issues/32 49 | # https://github.com/ros/robot_state_publisher/pull/30 50 | # TODO(orduno) Substitute with `PushNodeRemapping` 51 | # https://github.com/ros2/launch_ros/issues/56 52 | remappings = [('/tf', 'tf'), 53 | ('/tf_static', 'tf_static'), 54 | ('cmd_vel', cmd_vel_topic)] 55 | 56 | # Create our own temporary YAML files that include substitutions 57 | param_substitutions = { 58 | 'use_sim_time': use_sim_time, 59 | 'default_bt_xml_filename': bt_xml_file, 60 | 'autostart': autostart, 61 | 'map_subscribe_transient_local': map_subscribe_transient_local} 62 | 63 | configured_params = RewrittenYaml( 64 | source_file=params_file, 65 | root_key=namespace, 66 | param_rewrites=param_substitutions, 67 | convert_types=True) 68 | 69 | return LaunchDescription([ 70 | # Set env var to print messages to stdout immediately 71 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 72 | 73 | DeclareLaunchArgument( 74 | 'namespace', default_value='', 75 | description='Top-level namespace'), 76 | 77 | DeclareLaunchArgument( 78 | 'use_sim_time', default_value='false', 79 | description='Use simulation (Gazebo) clock if true'), 80 | 81 | DeclareLaunchArgument( 82 | 'autostart', default_value='true', 83 | description='Automatically startup the nav2 stack'), 84 | 85 | DeclareLaunchArgument( 86 | 'params_file', 87 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 88 | description='Full path to the ROS2 parameters file to use'), 89 | 90 | DeclareLaunchArgument( 91 | 'bt_xml_file', 92 | default_value=os.path.join( 93 | get_package_share_directory('nav2_bt_navigator'), 94 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 95 | description='Full path to the behavior tree xml file to use'), 96 | 97 | 98 | DeclareLaunchArgument( 99 | 'map_subscribe_transient_local', default_value='false', 100 | description='Whether to set the map subscriber QoS to transient local'), 101 | 102 | DeclareLaunchArgument( 103 | 'cmd_vel_topic', default_value='cmd_vel', 104 | description='Command velocity topic'), 105 | 106 | Node( 107 | package='nav2_controller', 108 | executable='controller_server', 109 | output='screen', 110 | parameters=[configured_params], 111 | remappings=remappings), 112 | 113 | Node( 114 | package='nav2_planner', 115 | executable='planner_server', 116 | name='planner_server', 117 | output='screen', 118 | parameters=[configured_params], 119 | remappings=remappings), 120 | 121 | Node( 122 | package='nav2_recoveries', 123 | executable='recoveries_server', 124 | name='recoveries_server', 125 | output='screen', 126 | parameters=[{'use_sim_time': use_sim_time}], 127 | remappings=remappings), 128 | 129 | Node( 130 | package='nav2_bt_navigator', 131 | executable='bt_navigator', 132 | name='bt_navigator', 133 | output='screen', 134 | parameters=[configured_params], 135 | remappings=remappings), 136 | 137 | Node( 138 | package='nav2_waypoint_follower', 139 | executable='waypoint_follower', 140 | name='waypoint_follower', 141 | output='screen', 142 | parameters=[configured_params], 143 | remappings=remappings), 144 | 145 | Node( 146 | package='nav2_lifecycle_manager', 147 | executable='lifecycle_manager', 148 | name='lifecycle_manager_navigation', 149 | output='screen', 150 | parameters=[{'use_sim_time': use_sim_time}, 151 | {'autostart': autostart}, 152 | {'node_names': lifecycle_nodes}]), 153 | 154 | ]) 155 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/navigation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | from nav2_common.launch import RewrittenYaml 24 | 25 | 26 | def generate_launch_description(): 27 | # Get the launch directory 28 | bringup_dir = get_package_share_directory('nav2_bringup') 29 | 30 | namespace = LaunchConfiguration('namespace') 31 | use_sim_time = LaunchConfiguration('use_sim_time') 32 | autostart = LaunchConfiguration('autostart') 33 | params_file = LaunchConfiguration('params_file') 34 | default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') 35 | map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') 36 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 37 | 38 | lifecycle_nodes = ['controller_server', 39 | 'planner_server', 40 | 'recoveries_server', 41 | 'bt_navigator', 42 | 'waypoint_follower'] 43 | 44 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 45 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 46 | # https://github.com/ros/geometry2/issues/32 47 | # https://github.com/ros/robot_state_publisher/pull/30 48 | # TODO(orduno) Substitute with `PushNodeRemapping` 49 | # https://github.com/ros2/launch_ros/issues/56 50 | remappings = [('/tf', 'tf'), 51 | ('/tf_static', 'tf_static'), 52 | ('cmd_vel', cmd_vel_topic)] 53 | 54 | # Create our own temporary YAML files that include substitutions 55 | param_substitutions = { 56 | 'use_sim_time': use_sim_time, 57 | 'default_bt_xml_filename': default_bt_xml_filename, 58 | 'autostart': autostart, 59 | 'map_subscribe_transient_local': map_subscribe_transient_local} 60 | 61 | configured_params = RewrittenYaml( 62 | source_file=params_file, 63 | root_key=namespace, 64 | param_rewrites=param_substitutions, 65 | convert_types=True) 66 | 67 | return LaunchDescription([ 68 | # Set env var to print messages to stdout immediately 69 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 70 | 71 | DeclareLaunchArgument( 72 | 'namespace', default_value='', 73 | description='Top-level namespace'), 74 | 75 | DeclareLaunchArgument( 76 | 'use_sim_time', default_value='false', 77 | description='Use simulation (Gazebo) clock if true'), 78 | 79 | DeclareLaunchArgument( 80 | 'autostart', default_value='true', 81 | description='Automatically startup the nav2 stack'), 82 | 83 | DeclareLaunchArgument( 84 | 'params_file', 85 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 86 | description='Full path to the ROS2 parameters file to use'), 87 | 88 | DeclareLaunchArgument( 89 | 'default_bt_xml_filename', 90 | default_value=os.path.join( 91 | get_package_share_directory('nav2_bt_navigator'), 92 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 93 | description='Full path to the behavior tree xml file to use'), 94 | 95 | DeclareLaunchArgument( 96 | 'map_subscribe_transient_local', default_value='false', 97 | description='Whether to set the map subscriber QoS to transient local'), 98 | 99 | Node( 100 | package='nav2_controller', 101 | executable='controller_server', 102 | output='screen', 103 | parameters=[configured_params], 104 | remappings=remappings), 105 | 106 | Node( 107 | package='nav2_planner', 108 | executable='planner_server', 109 | name='planner_server', 110 | output='screen', 111 | parameters=[configured_params], 112 | remappings=remappings), 113 | 114 | Node( 115 | package='nav2_recoveries', 116 | executable='recoveries_server', 117 | name='recoveries_server', 118 | output='screen', 119 | parameters=[configured_params], 120 | remappings=remappings), 121 | 122 | Node( 123 | package='nav2_bt_navigator', 124 | executable='bt_navigator', 125 | name='bt_navigator', 126 | output='screen', 127 | parameters=[configured_params], 128 | remappings=remappings), 129 | 130 | Node( 131 | package='nav2_waypoint_follower', 132 | executable='waypoint_follower', 133 | name='waypoint_follower', 134 | output='screen', 135 | parameters=[configured_params], 136 | remappings=remappings), 137 | 138 | Node( 139 | package='nav2_lifecycle_manager', 140 | executable='lifecycle_manager', 141 | name='lifecycle_manager_navigation', 142 | output='screen', 143 | parameters=[{'use_sim_time': use_sim_time}, 144 | {'autostart': autostart}, 145 | {'node_names': lifecycle_nodes}]), 146 | 147 | ]) 148 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/tiago/nav2_tiago_adapt_pp_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 23 | IncludeLaunchDescription, RegisterEventHandler) 24 | from launch.conditions import IfCondition 25 | from launch.event_handlers import OnProcessExit 26 | from launch.events import Shutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration 29 | 30 | from launch_ros.actions import Node 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 35 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 36 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 37 | 38 | 39 | # Create the launch configuration variables 40 | namespace = LaunchConfiguration('namespace') 41 | use_namespace = LaunchConfiguration('use_namespace') 42 | map_yaml_file = LaunchConfiguration('map') 43 | use_sim_time = LaunchConfiguration('use_sim_time') 44 | params_file = LaunchConfiguration('params_file') 45 | default_bt_xml_file = LaunchConfiguration('default_bt_xml_file') 46 | autostart = LaunchConfiguration('autostart') 47 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 48 | 49 | # Launch configuration variables specific to simulation 50 | rviz_config_file = LaunchConfiguration('rviz_config_file') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | 53 | # TODO(orduno) Remove once `PushNodeRemapping` is resolved 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [((namespace, '/tf'), '/tf'), 56 | ((namespace, '/tf_static'), '/tf_static'), 57 | ('/tf', 'tf'), 58 | ('/tf_static', 'tf_static')] 59 | 60 | # Declare the launch arguments 61 | declare_namespace_cmd = DeclareLaunchArgument( 62 | 'namespace', 63 | default_value='', 64 | description='Top-level namespace') 65 | declare_use_namespace_cmd = DeclareLaunchArgument( 66 | 'use_namespace', 67 | default_value='false', 68 | description='Whether to apply a namespace to the navigation stack') 69 | declare_map_yaml_cmd = DeclareLaunchArgument( 70 | 'map', 71 | default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'), 72 | description='Full path to map file to load') 73 | 74 | declare_use_sim_time_cmd = DeclareLaunchArgument( 75 | 'use_sim_time', 76 | default_value='false', 77 | description='Use simulation (Gazebo) clock if true') 78 | 79 | declare_params_file_cmd = DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(marathon_dir, 'params', 'nav2_adapt_pp_tiago_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use for all launched nodes') 83 | 84 | #declare_params_file_cmd = DeclareLaunchArgument( 85 | # 'params_file', 86 | # default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml'), 87 | # description='Full path to the ROS2 parameters file to use for all launched nodes') 88 | 89 | declare_bt_xml_cmd = DeclareLaunchArgument( 90 | 'default_bt_xml_file', 91 | default_value=os.path.join( 92 | get_package_share_directory('nav2_bt_navigator'), 93 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 94 | description='Full path to the behavior tree xml file to use') 95 | 96 | declare_autostart_cmd = DeclareLaunchArgument( 97 | 'autostart', default_value='true', 98 | description='Automatically startup the nav2 stack') 99 | 100 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 101 | 'rviz_config_file', 102 | default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 103 | description='Full path to the RVIZ config file to use') 104 | 105 | declare_use_rviz_cmd = DeclareLaunchArgument( 106 | 'use_rviz', 107 | default_value='True', 108 | description='Whether to start RVIZ') 109 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 110 | 'cmd_vel_topic', 111 | default_value='nav_vel', 112 | description='Command velocity topic') 113 | 114 | 115 | start_rviz_cmd = Node( 116 | condition=IfCondition(use_rviz), 117 | package='rviz2', 118 | executable='rviz2', 119 | arguments=['-d', rviz_config_file], 120 | output='log', 121 | remappings=[('/tf', 'tf'), 122 | ('/tf_static', 'tf_static'), 123 | ('goal_pose', 'goal_pose'), 124 | ('/clicked_point', 'clicked_point'), 125 | ('/initialpose', 'initialpose')]) 126 | 127 | exit_event_handler = RegisterEventHandler( 128 | event_handler=OnProcessExit( 129 | target_action=start_rviz_cmd, 130 | on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) 131 | 132 | bringup_cmd = IncludeLaunchDescription( 133 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_bringup_launch.py')), 134 | launch_arguments={'namespace': namespace, 135 | 'use_namespace': use_namespace, 136 | 'map': map_yaml_file, 137 | 'use_sim_time': use_sim_time, 138 | 'params_file': params_file, 139 | 'bt_xml_file': default_bt_xml_file, 140 | 'autostart': autostart, 141 | 'cmd_vel_topic': cmd_vel_topic}.items()) 142 | 143 | # contextual_cmd = IncludeLaunchDescription( 144 | # PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_contextual_launch.py')), 145 | # launch_arguments={'next_wp': '0'}.items()) 146 | 147 | # Create the launch description and populate 148 | ld = LaunchDescription() 149 | 150 | # Declare the launch options 151 | ld.add_action(declare_namespace_cmd) 152 | ld.add_action(declare_use_namespace_cmd) 153 | ld.add_action(declare_map_yaml_cmd) 154 | ld.add_action(declare_use_sim_time_cmd) 155 | ld.add_action(declare_params_file_cmd) 156 | ld.add_action(declare_bt_xml_cmd) 157 | ld.add_action(declare_autostart_cmd) 158 | 159 | ld.add_action(declare_rviz_config_file_cmd) 160 | ld.add_action(declare_use_rviz_cmd) 161 | ld.add_action(declare_cmd_vel_topic_cmd) 162 | 163 | # Add any conditioned actions 164 | ld.add_action(start_rviz_cmd) 165 | 166 | # Add other nodes and processes we need 167 | ld.add_action(exit_event_handler) 168 | 169 | # ld.add_action(contextual_cmd) 170 | 171 | # Add the actions to launch all of the navigation nodes 172 | ld.add_action(bringup_cmd) 173 | 174 | return ld 175 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/tiago/nav2_tiago_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 23 | IncludeLaunchDescription, RegisterEventHandler) 24 | from launch.conditions import IfCondition 25 | from launch.event_handlers import OnProcessExit 26 | from launch.events import Shutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration 29 | 30 | from launch_ros.actions import Node 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 35 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 36 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 37 | 38 | 39 | # Create the launch configuration variables 40 | namespace = LaunchConfiguration('namespace') 41 | use_namespace = LaunchConfiguration('use_namespace') 42 | map_yaml_file = LaunchConfiguration('map') 43 | use_sim_time = LaunchConfiguration('use_sim_time') 44 | params_file = LaunchConfiguration('params_file') 45 | default_bt_xml_file = LaunchConfiguration('default_bt_xml_file') 46 | autostart = LaunchConfiguration('autostart') 47 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 48 | 49 | # Launch configuration variables specific to simulation 50 | rviz_config_file = LaunchConfiguration('rviz_config_file') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | 53 | # TODO(orduno) Remove once `PushNodeRemapping` is resolved 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [((namespace, '/tf'), '/tf'), 56 | ((namespace, '/tf_static'), '/tf_static'), 57 | ('/tf', 'tf'), 58 | ('/tf_static', 'tf_static')] 59 | 60 | # Declare the launch arguments 61 | declare_namespace_cmd = DeclareLaunchArgument( 62 | 'namespace', 63 | default_value='', 64 | description='Top-level namespace') 65 | declare_use_namespace_cmd = DeclareLaunchArgument( 66 | 'use_namespace', 67 | default_value='false', 68 | description='Whether to apply a namespace to the navigation stack') 69 | declare_map_yaml_cmd = DeclareLaunchArgument( 70 | 'map', 71 | default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'), 72 | description='Full path to map file to load') 73 | 74 | declare_use_sim_time_cmd = DeclareLaunchArgument( 75 | 'use_sim_time', 76 | default_value='false', 77 | description='Use simulation (Gazebo) clock if true') 78 | 79 | declare_params_file_cmd = DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(marathon_dir, 'params', 'nav2_pp_tiago_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use for all launched nodes') 83 | 84 | #declare_params_file_cmd = DeclareLaunchArgument( 85 | # 'params_file', 86 | # default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml'), 87 | # description='Full path to the ROS2 parameters file to use for all launched nodes') 88 | 89 | declare_bt_xml_cmd = DeclareLaunchArgument( 90 | 'default_bt_xml_file', 91 | default_value=os.path.join( 92 | get_package_share_directory('nav2_bt_navigator'), 93 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 94 | description='Full path to the behavior tree xml file to use') 95 | 96 | declare_autostart_cmd = DeclareLaunchArgument( 97 | 'autostart', default_value='true', 98 | description='Automatically startup the nav2 stack') 99 | 100 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 101 | 'rviz_config_file', 102 | default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 103 | description='Full path to the RVIZ config file to use') 104 | 105 | declare_use_rviz_cmd = DeclareLaunchArgument( 106 | 'use_rviz', 107 | default_value='True', 108 | description='Whether to start RVIZ') 109 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 110 | 'cmd_vel_topic', 111 | default_value='nav_vel', 112 | description='Command velocity topic') 113 | 114 | 115 | start_rviz_cmd = Node( 116 | condition=IfCondition(use_rviz), 117 | package='rviz2', 118 | executable='rviz2', 119 | arguments=['-d', rviz_config_file], 120 | output='log', 121 | remappings=[('/tf', 'tf'), 122 | ('/tf_static', 'tf_static'), 123 | ('goal_pose', 'goal_pose'), 124 | ('/clicked_point', 'clicked_point'), 125 | ('/initialpose', 'initialpose')]) 126 | 127 | exit_event_handler = RegisterEventHandler( 128 | event_handler=OnProcessExit( 129 | target_action=start_rviz_cmd, 130 | on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) 131 | 132 | bringup_cmd = IncludeLaunchDescription( 133 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_bringup_launch.py')), 134 | launch_arguments={'namespace': namespace, 135 | 'use_namespace': use_namespace, 136 | 'map': map_yaml_file, 137 | 'use_sim_time': use_sim_time, 138 | 'params_file': params_file, 139 | 'bt_xml_file': default_bt_xml_file, 140 | 'autostart': autostart, 141 | 'cmd_vel_topic': cmd_vel_topic}.items()) 142 | 143 | # contextual_cmd = IncludeLaunchDescription( 144 | # PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_contextual_launch.py')), 145 | # launch_arguments={'next_wp': '0'}.items()) 146 | 147 | # Create the launch description and populate 148 | ld = LaunchDescription() 149 | 150 | # Declare the launch options 151 | ld.add_action(declare_namespace_cmd) 152 | ld.add_action(declare_use_namespace_cmd) 153 | ld.add_action(declare_map_yaml_cmd) 154 | ld.add_action(declare_use_sim_time_cmd) 155 | ld.add_action(declare_params_file_cmd) 156 | ld.add_action(declare_bt_xml_cmd) 157 | ld.add_action(declare_autostart_cmd) 158 | 159 | ld.add_action(declare_rviz_config_file_cmd) 160 | ld.add_action(declare_use_rviz_cmd) 161 | ld.add_action(declare_cmd_vel_topic_cmd) 162 | 163 | # Add any conditioned actions 164 | ld.add_action(start_rviz_cmd) 165 | 166 | # Add other nodes and processes we need 167 | ld.add_action(exit_event_handler) 168 | 169 | # ld.add_action(contextual_cmd) 170 | 171 | # Add the actions to launch all of the navigation nodes 172 | ld.add_action(bringup_cmd) 173 | 174 | return ld 175 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/tiago/nav2_tiago_pp_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 23 | IncludeLaunchDescription, RegisterEventHandler) 24 | from launch.conditions import IfCondition 25 | from launch.event_handlers import OnProcessExit 26 | from launch.events import Shutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration 29 | 30 | from launch_ros.actions import Node 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 35 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 36 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 37 | 38 | 39 | # Create the launch configuration variables 40 | namespace = LaunchConfiguration('namespace') 41 | use_namespace = LaunchConfiguration('use_namespace') 42 | map_yaml_file = LaunchConfiguration('map') 43 | use_sim_time = LaunchConfiguration('use_sim_time') 44 | params_file = LaunchConfiguration('params_file') 45 | default_bt_xml_file = LaunchConfiguration('default_bt_xml_file') 46 | autostart = LaunchConfiguration('autostart') 47 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 48 | 49 | # Launch configuration variables specific to simulation 50 | rviz_config_file = LaunchConfiguration('rviz_config_file') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | 53 | # TODO(orduno) Remove once `PushNodeRemapping` is resolved 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [((namespace, '/tf'), '/tf'), 56 | ((namespace, '/tf_static'), '/tf_static'), 57 | ('/tf', 'tf'), 58 | ('/tf_static', 'tf_static')] 59 | 60 | # Declare the launch arguments 61 | declare_namespace_cmd = DeclareLaunchArgument( 62 | 'namespace', 63 | default_value='', 64 | description='Top-level namespace') 65 | declare_use_namespace_cmd = DeclareLaunchArgument( 66 | 'use_namespace', 67 | default_value='false', 68 | description='Whether to apply a namespace to the navigation stack') 69 | declare_map_yaml_cmd = DeclareLaunchArgument( 70 | 'map', 71 | default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'), 72 | description='Full path to map file to load') 73 | 74 | declare_use_sim_time_cmd = DeclareLaunchArgument( 75 | 'use_sim_time', 76 | default_value='false', 77 | description='Use simulation (Gazebo) clock if true') 78 | 79 | declare_params_file_cmd = DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(marathon_dir, 'params', 'nav2_pp_tiago_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use for all launched nodes') 83 | 84 | #declare_params_file_cmd = DeclareLaunchArgument( 85 | # 'params_file', 86 | # default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml'), 87 | # description='Full path to the ROS2 parameters file to use for all launched nodes') 88 | 89 | declare_bt_xml_cmd = DeclareLaunchArgument( 90 | 'default_bt_xml_file', 91 | default_value=os.path.join( 92 | get_package_share_directory('nav2_bt_navigator'), 93 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 94 | description='Full path to the behavior tree xml file to use') 95 | 96 | declare_autostart_cmd = DeclareLaunchArgument( 97 | 'autostart', default_value='true', 98 | description='Automatically startup the nav2 stack') 99 | 100 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 101 | 'rviz_config_file', 102 | default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 103 | description='Full path to the RVIZ config file to use') 104 | 105 | declare_use_rviz_cmd = DeclareLaunchArgument( 106 | 'use_rviz', 107 | default_value='True', 108 | description='Whether to start RVIZ') 109 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 110 | 'cmd_vel_topic', 111 | default_value='nav_vel', 112 | description='Command velocity topic') 113 | 114 | 115 | start_rviz_cmd = Node( 116 | condition=IfCondition(use_rviz), 117 | package='rviz2', 118 | executable='rviz2', 119 | arguments=['-d', rviz_config_file], 120 | output='log', 121 | remappings=[('/tf', 'tf'), 122 | ('/tf_static', 'tf_static'), 123 | ('goal_pose', 'goal_pose'), 124 | ('/clicked_point', 'clicked_point'), 125 | ('/initialpose', 'initialpose')]) 126 | 127 | exit_event_handler = RegisterEventHandler( 128 | event_handler=OnProcessExit( 129 | target_action=start_rviz_cmd, 130 | on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) 131 | 132 | bringup_cmd = IncludeLaunchDescription( 133 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_bringup_launch.py')), 134 | launch_arguments={'namespace': namespace, 135 | 'use_namespace': use_namespace, 136 | 'map': map_yaml_file, 137 | 'use_sim_time': use_sim_time, 138 | 'params_file': params_file, 139 | 'bt_xml_file': default_bt_xml_file, 140 | 'autostart': autostart, 141 | 'cmd_vel_topic': cmd_vel_topic}.items()) 142 | 143 | # contextual_cmd = IncludeLaunchDescription( 144 | # PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_contextual_launch.py')), 145 | # launch_arguments={'next_wp': '0'}.items()) 146 | 147 | # Create the launch description and populate 148 | ld = LaunchDescription() 149 | 150 | # Declare the launch options 151 | ld.add_action(declare_namespace_cmd) 152 | ld.add_action(declare_use_namespace_cmd) 153 | ld.add_action(declare_map_yaml_cmd) 154 | ld.add_action(declare_use_sim_time_cmd) 155 | ld.add_action(declare_params_file_cmd) 156 | ld.add_action(declare_bt_xml_cmd) 157 | ld.add_action(declare_autostart_cmd) 158 | 159 | ld.add_action(declare_rviz_config_file_cmd) 160 | ld.add_action(declare_use_rviz_cmd) 161 | ld.add_action(declare_cmd_vel_topic_cmd) 162 | 163 | # Add any conditioned actions 164 | ld.add_action(start_rviz_cmd) 165 | 166 | # Add other nodes and processes we need 167 | ld.add_action(exit_event_handler) 168 | 169 | # ld.add_action(contextual_cmd) 170 | 171 | # Add the actions to launch all of the navigation nodes 172 | ld.add_action(bringup_cmd) 173 | 174 | return ld 175 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/tiago/nav2_tiago_regulated_pp_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 23 | IncludeLaunchDescription, RegisterEventHandler) 24 | from launch.conditions import IfCondition 25 | from launch.event_handlers import OnProcessExit 26 | from launch.events import Shutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration 29 | 30 | from launch_ros.actions import Node 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 35 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 36 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 37 | 38 | 39 | # Create the launch configuration variables 40 | namespace = LaunchConfiguration('namespace') 41 | use_namespace = LaunchConfiguration('use_namespace') 42 | map_yaml_file = LaunchConfiguration('map') 43 | use_sim_time = LaunchConfiguration('use_sim_time') 44 | params_file = LaunchConfiguration('params_file') 45 | default_bt_xml_file = LaunchConfiguration('default_bt_xml_file') 46 | autostart = LaunchConfiguration('autostart') 47 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 48 | 49 | # Launch configuration variables specific to simulation 50 | rviz_config_file = LaunchConfiguration('rviz_config_file') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | 53 | # TODO(orduno) Remove once `PushNodeRemapping` is resolved 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [((namespace, '/tf'), '/tf'), 56 | ((namespace, '/tf_static'), '/tf_static'), 57 | ('/tf', 'tf'), 58 | ('/tf_static', 'tf_static')] 59 | 60 | # Declare the launch arguments 61 | declare_namespace_cmd = DeclareLaunchArgument( 62 | 'namespace', 63 | default_value='', 64 | description='Top-level namespace') 65 | declare_use_namespace_cmd = DeclareLaunchArgument( 66 | 'use_namespace', 67 | default_value='false', 68 | description='Whether to apply a namespace to the navigation stack') 69 | declare_map_yaml_cmd = DeclareLaunchArgument( 70 | 'map', 71 | default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'), 72 | description='Full path to map file to load') 73 | 74 | declare_use_sim_time_cmd = DeclareLaunchArgument( 75 | 'use_sim_time', 76 | default_value='false', 77 | description='Use simulation (Gazebo) clock if true') 78 | 79 | declare_params_file_cmd = DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(marathon_dir, 'params', 'nav2_regulated_pp_tiago_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use for all launched nodes') 83 | 84 | #declare_params_file_cmd = DeclareLaunchArgument( 85 | # 'params_file', 86 | # default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml'), 87 | # description='Full path to the ROS2 parameters file to use for all launched nodes') 88 | 89 | declare_bt_xml_cmd = DeclareLaunchArgument( 90 | 'default_bt_xml_file', 91 | default_value=os.path.join( 92 | get_package_share_directory('nav2_bt_navigator'), 93 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 94 | description='Full path to the behavior tree xml file to use') 95 | 96 | declare_autostart_cmd = DeclareLaunchArgument( 97 | 'autostart', default_value='true', 98 | description='Automatically startup the nav2 stack') 99 | 100 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 101 | 'rviz_config_file', 102 | default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 103 | description='Full path to the RVIZ config file to use') 104 | 105 | declare_use_rviz_cmd = DeclareLaunchArgument( 106 | 'use_rviz', 107 | default_value='True', 108 | description='Whether to start RVIZ') 109 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 110 | 'cmd_vel_topic', 111 | default_value='nav_vel', 112 | description='Command velocity topic') 113 | 114 | 115 | start_rviz_cmd = Node( 116 | condition=IfCondition(use_rviz), 117 | package='rviz2', 118 | executable='rviz2', 119 | arguments=['-d', rviz_config_file], 120 | output='log', 121 | remappings=[('/tf', 'tf'), 122 | ('/tf_static', 'tf_static'), 123 | ('goal_pose', 'goal_pose'), 124 | ('/clicked_point', 'clicked_point'), 125 | ('/initialpose', 'initialpose')]) 126 | 127 | exit_event_handler = RegisterEventHandler( 128 | event_handler=OnProcessExit( 129 | target_action=start_rviz_cmd, 130 | on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) 131 | 132 | bringup_cmd = IncludeLaunchDescription( 133 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_bringup_launch.py')), 134 | launch_arguments={'namespace': namespace, 135 | 'use_namespace': use_namespace, 136 | 'map': map_yaml_file, 137 | 'use_sim_time': use_sim_time, 138 | 'params_file': params_file, 139 | 'bt_xml_file': default_bt_xml_file, 140 | 'autostart': autostart, 141 | 'cmd_vel_topic': cmd_vel_topic}.items()) 142 | 143 | # contextual_cmd = IncludeLaunchDescription( 144 | # PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_contextual_launch.py')), 145 | # launch_arguments={'next_wp': '0'}.items()) 146 | 147 | # Create the launch description and populate 148 | ld = LaunchDescription() 149 | 150 | # Declare the launch options 151 | ld.add_action(declare_namespace_cmd) 152 | ld.add_action(declare_use_namespace_cmd) 153 | ld.add_action(declare_map_yaml_cmd) 154 | ld.add_action(declare_use_sim_time_cmd) 155 | ld.add_action(declare_params_file_cmd) 156 | ld.add_action(declare_bt_xml_cmd) 157 | ld.add_action(declare_autostart_cmd) 158 | 159 | ld.add_action(declare_rviz_config_file_cmd) 160 | ld.add_action(declare_use_rviz_cmd) 161 | ld.add_action(declare_cmd_vel_topic_cmd) 162 | 163 | # Add any conditioned actions 164 | ld.add_action(start_rviz_cmd) 165 | 166 | # Add other nodes and processes we need 167 | ld.add_action(exit_event_handler) 168 | 169 | # ld.add_action(contextual_cmd) 170 | 171 | # Add the actions to launch all of the navigation nodes 172 | ld.add_action(bringup_cmd) 173 | 174 | return ld 175 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/launch/tiago/nav2_tiago_teb_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, 23 | IncludeLaunchDescription, RegisterEventHandler) 24 | from launch.conditions import IfCondition 25 | from launch.event_handlers import OnProcessExit 26 | from launch.events import Shutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration 29 | 30 | from launch_ros.actions import Node 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 35 | marathon_dir = get_package_share_directory('marathon_ros2_bringup') 36 | marathon_launch_dir = os.path.join(marathon_dir, 'launch') 37 | 38 | 39 | # Create the launch configuration variables 40 | namespace = LaunchConfiguration('namespace') 41 | use_namespace = LaunchConfiguration('use_namespace') 42 | map_yaml_file = LaunchConfiguration('map') 43 | use_sim_time = LaunchConfiguration('use_sim_time') 44 | params_file = LaunchConfiguration('params_file') 45 | bt_xml_file = LaunchConfiguration('bt_xml_file') 46 | autostart = LaunchConfiguration('autostart') 47 | cmd_vel_topic = LaunchConfiguration('cmd_vel_topic') 48 | 49 | # Launch configuration variables specific to simulation 50 | rviz_config_file = LaunchConfiguration('rviz_config_file') 51 | use_rviz = LaunchConfiguration('use_rviz') 52 | 53 | # TODO(orduno) Remove once `PushNodeRemapping` is resolved 54 | # https://github.com/ros2/launch_ros/issues/56 55 | remappings = [((namespace, '/tf'), '/tf'), 56 | ((namespace, '/tf_static'), '/tf_static'), 57 | ('/tf', 'tf'), 58 | ('/tf_static', 'tf_static')] 59 | 60 | # Declare the launch arguments 61 | declare_namespace_cmd = DeclareLaunchArgument( 62 | 'namespace', 63 | default_value='', 64 | description='Top-level namespace') 65 | declare_use_namespace_cmd = DeclareLaunchArgument( 66 | 'use_namespace', 67 | default_value='false', 68 | description='Whether to apply a namespace to the navigation stack') 69 | declare_map_yaml_cmd = DeclareLaunchArgument( 70 | 'map', 71 | default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'), 72 | description='Full path to map file to load') 73 | 74 | declare_use_sim_time_cmd = DeclareLaunchArgument( 75 | 'use_sim_time', 76 | default_value='false', 77 | description='Use simulation (Gazebo) clock if true') 78 | 79 | declare_params_file_cmd = DeclareLaunchArgument( 80 | 'params_file', 81 | default_value=os.path.join(marathon_dir, 'params', 'nav2_marathon_teb_tiago_params.yaml'), 82 | description='Full path to the ROS2 parameters file to use for all launched nodes') 83 | 84 | #declare_params_file_cmd = DeclareLaunchArgument( 85 | # 'params_file', 86 | # default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml'), 87 | # description='Full path to the ROS2 parameters file to use for all launched nodes') 88 | 89 | declare_bt_xml_cmd = DeclareLaunchArgument( 90 | 'bt_xml_file', 91 | default_value=os.path.join( 92 | get_package_share_directory('marathon_ros2_bringup'), 93 | 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), 94 | description='Full path to the behavior tree xml file to use') 95 | 96 | declare_autostart_cmd = DeclareLaunchArgument( 97 | 'autostart', default_value='true', 98 | description='Automatically startup the nav2 stack') 99 | 100 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 101 | 'rviz_config_file', 102 | default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz'), 103 | description='Full path to the RVIZ config file to use') 104 | 105 | declare_use_rviz_cmd = DeclareLaunchArgument( 106 | 'use_rviz', 107 | default_value='True', 108 | description='Whether to start RVIZ') 109 | declare_cmd_vel_topic_cmd = DeclareLaunchArgument( 110 | 'cmd_vel_topic', 111 | default_value='nav_vel', 112 | description='Command velocity topic') 113 | 114 | 115 | start_rviz_cmd = Node( 116 | condition=IfCondition(use_rviz), 117 | package='rviz2', 118 | node_executable='rviz2', 119 | arguments=['-d', rviz_config_file], 120 | output='log', 121 | remappings=[('/tf', 'tf'), 122 | ('/tf_static', 'tf_static'), 123 | ('goal_pose', 'goal_pose'), 124 | ('/clicked_point', 'clicked_point'), 125 | ('/initialpose', 'initialpose')]) 126 | 127 | exit_event_handler = RegisterEventHandler( 128 | event_handler=OnProcessExit( 129 | target_action=start_rviz_cmd, 130 | on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) 131 | 132 | bringup_cmd = IncludeLaunchDescription( 133 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_bringup_launch.py')), 134 | launch_arguments={'namespace': namespace, 135 | 'use_namespace': use_namespace, 136 | 'map': map_yaml_file, 137 | 'use_sim_time': use_sim_time, 138 | 'params_file': params_file, 139 | 'bt_xml_file': bt_xml_file, 140 | 'autostart': autostart, 141 | 'cmd_vel_topic': cmd_vel_topic}.items()) 142 | 143 | marathon_cmd = IncludeLaunchDescription( 144 | PythonLaunchDescriptionSource(os.path.join(marathon_launch_dir, 'nav2_marathon_launch.py')), 145 | launch_arguments={'total_distance_sum': '0.25', 146 | 'next_wp': '0'}.items()) 147 | 148 | # Create the launch description and populate 149 | ld = LaunchDescription() 150 | 151 | # Declare the launch options 152 | ld.add_action(declare_namespace_cmd) 153 | ld.add_action(declare_use_namespace_cmd) 154 | ld.add_action(declare_map_yaml_cmd) 155 | ld.add_action(declare_use_sim_time_cmd) 156 | ld.add_action(declare_params_file_cmd) 157 | ld.add_action(declare_bt_xml_cmd) 158 | ld.add_action(declare_autostart_cmd) 159 | 160 | ld.add_action(declare_rviz_config_file_cmd) 161 | ld.add_action(declare_use_rviz_cmd) 162 | ld.add_action(declare_cmd_vel_topic_cmd) 163 | 164 | # Add any conditioned actions 165 | ld.add_action(start_rviz_cmd) 166 | 167 | # Add other nodes and processes we need 168 | ld.add_action(exit_event_handler) 169 | 170 | ld.add_action(marathon_cmd) 171 | 172 | # Add the actions to launch all of the navigation nodes 173 | ld.add_action(bringup_cmd) 174 | 175 | return ld 176 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/maps/aulario/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_bringup/maps/aulario/map.pgm -------------------------------------------------------------------------------- /marathon_ros2_bringup/maps/aulario/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [0.0, 0.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/maps/aulario/map_obstacles.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_bringup/maps/aulario/map_obstacles.pgm -------------------------------------------------------------------------------- /marathon_ros2_bringup/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_bringup/maps/map.pgm -------------------------------------------------------------------------------- /marathon_ros2_bringup/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.1 3 | origin: [0.00000, 0.00000, 0.00000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | marathon_ros2_bringup 5 | 0.0.0 6 | TODO: Package description 7 | jgines 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | 14 | ament_lint_auto 15 | ament_lint_common 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_adapt_pp_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | set_initial_pose: True 42 | initial_pose: 43 | x: 10.35 44 | y: 51.77 45 | z: 0.0 46 | yaw: -0.625 47 | 48 | amcl_map_client: 49 | ros__parameters: 50 | use_sim_time: True 51 | 52 | amcl_rclcpp_node: 53 | ros__parameters: 54 | use_sim_time: True 55 | 56 | bt_navigator: 57 | ros__parameters: 58 | use_sim_time: True 59 | global_frame: map 60 | robot_base_frame: base_link 61 | odom_topic: /odom 62 | enable_groot_monitoring: True 63 | groot_zmq_publisher_port: 1666 64 | groot_zmq_server_port: 1667 65 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 66 | plugin_lib_names: 67 | - nav2_compute_path_to_pose_action_bt_node 68 | - nav2_follow_path_action_bt_node 69 | - nav2_back_up_action_bt_node 70 | - nav2_spin_action_bt_node 71 | - nav2_wait_action_bt_node 72 | - nav2_clear_costmap_service_bt_node 73 | - nav2_is_stuck_condition_bt_node 74 | - nav2_goal_reached_condition_bt_node 75 | - nav2_goal_updated_condition_bt_node 76 | - nav2_initial_pose_received_condition_bt_node 77 | - nav2_reinitialize_global_localization_service_bt_node 78 | - nav2_rate_controller_bt_node 79 | - nav2_distance_controller_bt_node 80 | - nav2_speed_controller_bt_node 81 | - nav2_truncate_path_action_bt_node 82 | - nav2_goal_updater_node_bt_node 83 | - nav2_recovery_node_bt_node 84 | - nav2_pipeline_sequence_bt_node 85 | - nav2_round_robin_node_bt_node 86 | - nav2_transform_available_condition_bt_node 87 | - nav2_time_expired_condition_bt_node 88 | - nav2_distance_traveled_condition_bt_node 89 | 90 | bt_navigator_rclcpp_node: 91 | ros__parameters: 92 | use_sim_time: True 93 | 94 | controller_server: 95 | ros__parameters: 96 | use_sim_time: True 97 | controller_frequency: 20.0 98 | min_x_velocity_threshold: 0.001 99 | min_y_velocity_threshold: 0.5 100 | min_theta_velocity_threshold: 0.001 101 | progress_checker_plugin: "progress_checker" 102 | goal_checker_plugin: "goal_checker" 103 | controller_plugins: ["FollowPath"] 104 | 105 | progress_checker: 106 | plugin: "nav2_controller::SimpleProgressChecker" 107 | required_movement_radius: 0.5 108 | movement_time_allowance: 10.0 109 | goal_checker: 110 | plugin: "nav2_controller::SimpleGoalChecker" 111 | xy_goal_tolerance: 0.25 112 | yaw_goal_tolerance: 0.25 113 | stateful: True 114 | FollowPath: 115 | plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" 116 | desired_linear_vel: 0.6 117 | max_linear_accel: 2.0 118 | max_linear_decel: 2.0 119 | lookahead_dist: 0.6 120 | min_lookahead_dist: 0.3 121 | max_lookahead_dist: 0.9 122 | lookahead_time: 1.5 123 | rotate_to_heading_angular_vel: 1.0 124 | transform_tolerance: 1.0 125 | use_velocity_scaled_lookahead_dist: true 126 | min_approach_linear_velocity: 0.05 127 | use_approach_linear_velocity_scaling: true 128 | max_allowed_time_to_collision: 1.0 129 | use_regulated_linear_velocity_scaling: false 130 | use_cost_regulated_linear_velocity_scaling: false 131 | regulated_linear_scaling_min_radius: 0.9 132 | regulated_linear_scaling_min_speed: 0.25 133 | use_rotate_to_heading: true 134 | rotate_to_heading_min_angle: 0.785 135 | max_angular_accel: 3.2 136 | goal_dist_tol: 0.25 137 | 138 | controller_server_rclcpp_node: 139 | ros__parameters: 140 | use_sim_time: True 141 | 142 | local_costmap: 143 | local_costmap: 144 | ros__parameters: 145 | update_frequency: 5.0 146 | publish_frequency: 2.0 147 | global_frame: odom 148 | robot_base_frame: base_link 149 | use_sim_time: True 150 | rolling_window: true 151 | width: 3 152 | height: 3 153 | resolution: 0.05 154 | robot_radius: 0.22 155 | plugins: ["voxel_layer", "inflation_layer"] 156 | inflation_layer: 157 | plugin: "nav2_costmap_2d::InflationLayer" 158 | cost_scaling_factor: 3.0 159 | inflation_radius: 0.55 160 | obstacle_layer: 161 | plugin: "nav2_costmap_2d::ObstacleLayer" 162 | enabled: True 163 | observation_sources: scan 164 | scan: 165 | topic: /scan 166 | max_obstacle_height: 2.0 167 | clearing: True 168 | marking: True 169 | data_type: "LaserScan" 170 | raytrace_max_range: 3.0 171 | raytrace_min_range: 0.0 172 | obstacle_max_range: 2.5 173 | obstacle_min_range: 0.0 174 | voxel_layer: 175 | plugin: "nav2_costmap_2d::VoxelLayer" 176 | enabled: True 177 | publish_voxel_map: True 178 | origin_z: 0.0 179 | z_resolution: 0.05 180 | z_voxels: 16 181 | max_obstacle_height: 2.0 182 | mark_threshold: 0 183 | observation_sources: scan 184 | scan: 185 | topic: /scan 186 | max_obstacle_height: 2.0 187 | clearing: True 188 | marking: True 189 | data_type: "LaserScan" 190 | raytrace_max_range: 3.0 191 | raytrace_min_range: 0.0 192 | obstacle_max_range: 2.5 193 | obstacle_min_range: 0.0 194 | static_layer: 195 | map_subscribe_transient_local: True 196 | always_send_full_costmap: True 197 | local_costmap_client: 198 | ros__parameters: 199 | use_sim_time: True 200 | local_costmap_rclcpp_node: 201 | ros__parameters: 202 | use_sim_time: True 203 | 204 | global_costmap: 205 | global_costmap: 206 | ros__parameters: 207 | update_frequency: 1.0 208 | publish_frequency: 1.0 209 | global_frame: map 210 | robot_base_frame: base_link 211 | use_sim_time: True 212 | robot_radius: 0.22 213 | resolution: 0.05 214 | track_unknown_space: true 215 | plugins: ["static_layer", "inflation_layer"] 216 | static_layer: 217 | plugin: "nav2_costmap_2d::StaticLayer" 218 | map_subscribe_transient_local: True 219 | inflation_layer: 220 | plugin: "nav2_costmap_2d::InflationLayer" 221 | cost_scaling_factor: 3.0 222 | inflation_radius: 0.55 223 | always_send_full_costmap: True 224 | global_costmap_client: 225 | ros__parameters: 226 | use_sim_time: True 227 | global_costmap_rclcpp_node: 228 | ros__parameters: 229 | use_sim_time: True 230 | 231 | map_server: 232 | ros__parameters: 233 | use_sim_time: True 234 | yaml_filename: "turtlebot3_world.yaml" 235 | 236 | map_saver: 237 | ros__parameters: 238 | use_sim_time: True 239 | save_map_timeout: 5.0 240 | free_thresh_default: 0.25 241 | occupied_thresh_default: 0.65 242 | map_subscribe_transient_local: True 243 | 244 | planner_server: 245 | ros__parameters: 246 | expected_planner_frequency: 20.0 247 | use_sim_time: True 248 | planner_plugins: ["GridBased"] 249 | GridBased: 250 | plugin: "nav2_navfn_planner/NavfnPlanner" 251 | tolerance: 0.5 252 | use_astar: false 253 | allow_unknown: true 254 | 255 | planner_server_rclcpp_node: 256 | ros__parameters: 257 | use_sim_time: True 258 | 259 | recoveries_server: 260 | ros__parameters: 261 | costmap_topic: local_costmap/costmap_raw 262 | footprint_topic: local_costmap/published_footprint 263 | cycle_frequency: 10.0 264 | recovery_plugins: ["spin", "backup", "wait"] 265 | spin: 266 | plugin: "nav2_recoveries/Spin" 267 | backup: 268 | plugin: "nav2_recoveries/BackUp" 269 | wait: 270 | plugin: "nav2_recoveries/Wait" 271 | global_frame: odom 272 | robot_base_frame: base_link 273 | transform_timeout: 0.1 274 | use_sim_time: true 275 | simulate_ahead_time: 2.0 276 | max_rotational_vel: 1.0 277 | min_rotational_vel: 0.4 278 | rotational_acc_lim: 3.2 279 | 280 | robot_state_publisher: 281 | ros__parameters: 282 | use_sim_time: True 283 | 284 | waypoint_follower: 285 | ros__parameters: 286 | loop_rate: 20 287 | stop_on_failure: false 288 | waypoint_task_executor_plugin: "wait_at_waypoint" 289 | wait_at_waypoint: 290 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 291 | enabled: True 292 | waypoint_pause_duration: 200 293 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_contextual_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | set_initial_pose: True 42 | initial_pose: 43 | # exp_1 44 | x: 2.56 45 | y: 50.6 46 | z: 0.0 47 | yaw: -0.675 48 | # exp_2 49 | #x: 12.4365 50 | #y: 50.3792 51 | #z: 0.0 52 | #yaw: -0.675 53 | # exp_3 54 | #x: 27.76 55 | #y: 55.39 56 | #z: 0.0 57 | #yaw: 0.8957 58 | 59 | 60 | amcl_map_client: 61 | ros__parameters: 62 | use_sim_time: True 63 | 64 | amcl_rclcpp_node: 65 | ros__parameters: 66 | use_sim_time: True 67 | 68 | bt_navigator: 69 | ros__parameters: 70 | use_sim_time: True 71 | global_frame: map 72 | robot_base_frame: base_link 73 | odom_topic: /odom 74 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 75 | plugin_lib_names: 76 | - nav2_compute_path_to_pose_action_bt_node 77 | - nav2_follow_path_action_bt_node 78 | - nav2_back_up_action_bt_node 79 | - nav2_spin_action_bt_node 80 | - nav2_wait_action_bt_node 81 | - nav2_log_action_bt_node 82 | - nav2_clear_costmap_service_bt_node 83 | - nav2_is_stuck_condition_bt_node 84 | - nav2_goal_reached_condition_bt_node 85 | - nav2_goal_updated_condition_bt_node 86 | - nav2_initial_pose_received_condition_bt_node 87 | - nav2_reinitialize_global_localization_service_bt_node 88 | - nav2_rate_controller_bt_node 89 | - nav2_distance_controller_bt_node 90 | - nav2_speed_controller_bt_node 91 | - nav2_truncate_path_action_bt_node 92 | - nav2_goal_updater_node_bt_node 93 | - nav2_recovery_node_bt_node 94 | - nav2_pipeline_sequence_bt_node 95 | - nav2_round_robin_node_bt_node 96 | - nav2_transform_available_condition_bt_node 97 | - nav2_time_expired_condition_bt_node 98 | - nav2_distance_traveled_condition_bt_node 99 | - nav2_context_updater_action_bt_node 100 | - nav2_is_specified_context_condition_bt_node 101 | 102 | 103 | bt_navigator_rclcpp_node: 104 | ros__parameters: 105 | use_sim_time: True 106 | 107 | controller_server: 108 | ros__parameters: 109 | use_sim_time: True 110 | controller_frequency: 20.0 111 | min_x_velocity_threshold: 0.001 112 | min_y_velocity_threshold: 0.5 113 | min_theta_velocity_threshold: 0.001 114 | progress_checker_plugin: "progress_checker" 115 | goal_checker_plugin: "goal_checker" 116 | controller_plugins: ["FollowPath", "DynamicFollowPath"] 117 | 118 | # Progress checker parameters 119 | progress_checker: 120 | plugin: "nav2_controller::SimpleProgressChecker" 121 | required_movement_radius: 0.25 122 | movement_time_allowance: 10.0 123 | # Goal checker parameters 124 | goal_checker: 125 | plugin: "nav2_controller::SimpleGoalChecker" 126 | xy_goal_tolerance: 0.2 127 | yaw_goal_tolerance: 0.1 128 | stateful: True 129 | # DWB parameters 130 | FollowPath: 131 | plugin: "dwb_core::DWBLocalPlanner" 132 | debug_trajectory_details: True 133 | min_vel_x: 0.0 134 | min_vel_y: 0.0 135 | max_vel_x: 0.23 #0.46 136 | max_vel_y: 0.0 137 | max_vel_theta: 0.5 #1.0 138 | min_speed_xy: 0.0 139 | max_speed_xy: 0.23 #0.46 140 | min_speed_theta: 0.0 141 | # Add high threshold velocity for turtlebot 3 issue. 142 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 143 | acc_lim_x: 2.5 144 | acc_lim_y: 0.0 145 | acc_lim_theta: 3.2 146 | decel_lim_x: -2.0 147 | decel_lim_y: 0.0 148 | decel_lim_theta: -3.2 149 | vx_samples: 20 150 | vy_samples: 5 151 | vtheta_samples: 40 152 | sim_time: 1.9 153 | linear_granularity: 0.05 154 | angular_granularity: 0.025 155 | transform_tolerance: 1.0 156 | xy_goal_tolerance: 0.2 157 | trans_stopped_velocity: 0.25 158 | short_circuit_trajectory_evaluation: True 159 | stateful: True 160 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 161 | BaseObstacle.scale: 0.015 162 | PathAlign.scale: 0.0 163 | #PathAlign.forward_point_distance: 0.1 164 | GoalAlign.scale: 0.0 165 | #GoalAlign.forward_point_distance: 0.1 166 | PathDist.scale: 32.0 167 | GoalDist.scale: 24.0 168 | RotateToGoal.scale: 32.0 169 | RotateToGoal.slowing_factor: 5.0 170 | RotateToGoal.lookahead_time: -1.0 171 | DynamicFollowPath: 172 | plugin: "teb_local_planner::TebLocalPlannerROS" 173 | map_frame: "map" 174 | footprint_model.type: circular 175 | footprint_model.radius: 0.22 176 | min_obstacle_dist: 0.1 177 | inflation_dist: 0.1 178 | costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH 179 | costmap_converter_spin_thread: True 180 | costmap_converter_rate: 15 181 | enable_homotopy_class_planning: True 182 | enable_multithreading: True 183 | # weight_obstacle: 100.0 184 | optimization_verbose: False 185 | teb_autoresize: True 186 | min_samples: 3 187 | max_samples: 20 188 | max_global_plan_lookahead_dist: 1.0 189 | visualize_hc_graph: True 190 | xy_goal_tolerance: 0.2 191 | yaw_goal_tolerance: 0.1 192 | max_vel_x: 0.23 #0.46 193 | max_vel_theta: 0.5 #1.0 194 | acc_lim_x: 2.5 195 | acc_lim_theta: 3.2 196 | 197 | controller_server_rclcpp_node: 198 | ros__parameters: 199 | use_sim_time: True 200 | 201 | local_costmap: 202 | local_costmap: 203 | ros__parameters: 204 | update_frequency: 5.0 205 | publish_frequency: 2.0 206 | global_frame: odom 207 | robot_base_frame: base_link 208 | use_sim_time: True 209 | rolling_window: true 210 | width: 3 211 | height: 3 212 | resolution: 0.05 213 | robot_radius: 0.22 214 | plugins: ["voxel_layer", "inflation_layer"] 215 | inflation_layer: 216 | plugin: "nav2_costmap_2d::InflationLayer" 217 | cost_scaling_factor: 2.0 218 | voxel_layer: 219 | plugin: "nav2_costmap_2d::VoxelLayer" 220 | enabled: True 221 | publish_voxel_map: True 222 | origin_z: 0.0 223 | z_resolution: 0.05 224 | z_voxels: 16 225 | max_obstacle_height: 2.0 226 | mark_threshold: 0 227 | observation_sources: scan 228 | scan: 229 | topic: /scan 230 | max_obstacle_height: 2.0 231 | clearing: True 232 | marking: True 233 | data_type: "LaserScan" 234 | stvl_layer: 235 | plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" 236 | enabled: true 237 | voxel_decay: 15. 238 | decay_model: 0 239 | voxel_size: 0.1 240 | track_unknown_space: true 241 | max_obstacle_height: 2.0 242 | unknown_threshold: 15 243 | mark_threshold: 0 244 | update_footprint_enabled: true 245 | combination_method: 1 246 | obstacle_range: 3.0 247 | origin_z: 0.0 248 | publish_voxel_map: true 249 | transform_tolerance: 1.0 250 | mapping_mode: false 251 | map_save_duration: 60.0 252 | observation_sources: pointcloud 253 | pointcloud: 254 | data_type: PointCloud2 255 | topic: /xtion/depth_registered/points 256 | marking: true 257 | clearing: true 258 | min_obstacle_height: 0.1 259 | max_obstacle_height: 2.0 260 | expected_update_rate: 0.0 261 | observation_persistence: 0.0 262 | inf_is_valid: false 263 | voxel_filter: true 264 | clear_after_reading: true 265 | max_z: 2.0 266 | min_z: 0.1 267 | vertical_fov_angle: 0.8745 268 | horizontal_fov_angle: 1.048 269 | decay_acceleration: 15.0 270 | model_type: 0 271 | static_layer: 272 | map_subscribe_transient_local: True 273 | always_send_full_costmap: True 274 | local_costmap_client: 275 | ros__parameters: 276 | use_sim_time: True 277 | local_costmap_rclcpp_node: 278 | ros__parameters: 279 | use_sim_time: True 280 | 281 | global_costmap: 282 | global_costmap: 283 | ros__parameters: 284 | update_frequency: 3.0 285 | publish_frequency: 1.0 286 | global_frame: map 287 | robot_base_frame: base_link 288 | use_sim_time: True 289 | robot_radius: 0.22 290 | resolution: 0.05 291 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 292 | obstacle_layer: 293 | plugin: "nav2_costmap_2d::ObstacleLayer" 294 | enabled: True 295 | observation_sources: scan 296 | scan: 297 | topic: /scan 298 | max_obstacle_height: 2.0 299 | clearing: True 300 | marking: True 301 | data_type: "LaserScan" 302 | static_layer: 303 | plugin: "nav2_costmap_2d::StaticLayer" 304 | map_subscribe_transient_local: True 305 | inflation_layer: 306 | plugin: "nav2_costmap_2d::InflationLayer" 307 | stvl_layer: 308 | plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" 309 | enabled: true 310 | voxel_decay: 15. 311 | decay_model: 0 312 | voxel_size: 0.1 313 | track_unknown_space: true 314 | max_obstacle_height: 2.0 315 | unknown_threshold: 15 316 | mark_threshold: 0 317 | update_footprint_enabled: true 318 | combination_method: 1 319 | obstacle_range: 3.0 320 | origin_z: 0.0 321 | publish_voxel_map: true 322 | transform_tolerance: 1.0 323 | mapping_mode: false 324 | map_save_duration: 60.0 325 | observation_sources: pointcloud 326 | pointcloud: 327 | data_type: PointCloud2 328 | topic: /xtion/depth_registered/points 329 | marking: true 330 | clearing: true 331 | min_obstacle_height: 0.2 332 | max_obstacle_height: 2.0 333 | expected_update_rate: 0.0 334 | observation_persistence: 0.0 335 | inf_is_valid: true 336 | voxel_filter: true 337 | clear_after_reading: true 338 | max_z: 7.0 339 | min_z: 0.2 340 | vertical_fov_angle: 0.8745 341 | horizontal_fov_angle: 1.048 342 | decay_acceleration: 15.0 343 | model_type: 0 344 | always_send_full_costmap: True 345 | global_costmap_client: 346 | ros__parameters: 347 | use_sim_time: True 348 | global_costmap_rclcpp_node: 349 | ros__parameters: 350 | use_sim_time: True 351 | 352 | map_server: 353 | ros__parameters: 354 | use_sim_time: True 355 | yaml_filename: "turtlebot3_world.yaml" 356 | 357 | map_saver: 358 | ros__parameters: 359 | use_sim_time: True 360 | save_map_timeout: 5000 361 | free_thresh_default: 0.25 362 | occupied_thresh_default: 0.65 363 | 364 | planner_server: 365 | ros__parameters: 366 | expected_planner_frequency: 20.0 367 | use_sim_time: True 368 | planner_plugins: ["GridBased"] 369 | GridBased: 370 | plugin: "nav2_navfn_planner/NavfnPlanner" 371 | tolerance: 1.2 372 | use_astar: false 373 | allow_unknown: true 374 | 375 | planner_server_rclcpp_node: 376 | ros__parameters: 377 | use_sim_time: True 378 | 379 | recoveries_server: 380 | ros__parameters: 381 | costmap_topic: local_costmap/costmap_raw 382 | footprint_topic: local_costmap/published_footprint 383 | cycle_frequency: 10.0 384 | recovery_plugins: ["spin", "backup", "wait", "log"] 385 | spin: 386 | plugin: "nav2_recoveries/Spin" 387 | backup: 388 | plugin: "nav2_recoveries/BackUp" 389 | wait: 390 | plugin: "nav2_recoveries/Wait" 391 | log: 392 | plugin: "nav2_recoveries/Logger" 393 | global_frame: odom 394 | robot_base_frame: base_link 395 | transform_timeout: 1.0 396 | use_sim_time: true 397 | simulate_ahead_time: 2.0 398 | max_rotational_vel: 1.0 399 | min_rotational_vel: 0.4 400 | rotational_acc_lim: 3.2 401 | 402 | robot_state_publisher: 403 | ros__parameters: 404 | use_sim_time: True 405 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_dynamic_follow_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | set_initial_pose: True 42 | initial_pose: 43 | # exp_1 44 | x: 2.56 45 | y: 50.6 46 | z: 0.0 47 | yaw: -0.675 48 | # exp_2 49 | #x: 12.4365 50 | #y: 50.3792 51 | #z: 0.0 52 | #yaw: -0.675 53 | # exp_3 54 | #x: 27.76 55 | #y: 55.39 56 | #z: 0.0 57 | #yaw: 0.8957 58 | 59 | 60 | amcl_map_client: 61 | ros__parameters: 62 | use_sim_time: True 63 | 64 | amcl_rclcpp_node: 65 | ros__parameters: 66 | use_sim_time: True 67 | 68 | bt_navigator: 69 | ros__parameters: 70 | use_sim_time: True 71 | global_frame: map 72 | robot_base_frame: base_link 73 | odom_topic: /odom 74 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 75 | plugin_lib_names: 76 | - nav2_compute_path_to_pose_action_bt_node 77 | - nav2_follow_path_action_bt_node 78 | - nav2_back_up_action_bt_node 79 | - nav2_spin_action_bt_node 80 | - nav2_wait_action_bt_node 81 | - nav2_log_action_bt_node 82 | - nav2_clear_costmap_service_bt_node 83 | - nav2_is_stuck_condition_bt_node 84 | - nav2_goal_reached_condition_bt_node 85 | - nav2_goal_updated_condition_bt_node 86 | - nav2_initial_pose_received_condition_bt_node 87 | - nav2_reinitialize_global_localization_service_bt_node 88 | - nav2_rate_controller_bt_node 89 | - nav2_distance_controller_bt_node 90 | - nav2_speed_controller_bt_node 91 | - nav2_truncate_path_action_bt_node 92 | - nav2_goal_updater_node_bt_node 93 | - nav2_recovery_node_bt_node 94 | - nav2_pipeline_sequence_bt_node 95 | - nav2_round_robin_node_bt_node 96 | - nav2_transform_available_condition_bt_node 97 | - nav2_time_expired_condition_bt_node 98 | - nav2_distance_traveled_condition_bt_node 99 | - nav2_context_updater_action_bt_node 100 | - nav2_is_specified_context_condition_bt_node 101 | 102 | 103 | bt_navigator_rclcpp_node: 104 | ros__parameters: 105 | use_sim_time: True 106 | 107 | controller_server: 108 | ros__parameters: 109 | use_sim_time: True 110 | controller_frequency: 20.0 111 | min_x_velocity_threshold: 0.001 112 | min_y_velocity_threshold: 0.5 113 | min_theta_velocity_threshold: 0.001 114 | progress_checker_plugin: "progress_checker" 115 | goal_checker_plugin: "goal_checker" 116 | controller_plugins: ["FollowPath", "DynamicFollowPath"] 117 | 118 | # Progress checker parameters 119 | progress_checker: 120 | plugin: "nav2_controller::SimpleProgressChecker" 121 | required_movement_radius: 0.25 122 | movement_time_allowance: 10.0 123 | # Goal checker parameters 124 | goal_checker: 125 | plugin: "nav2_controller::SimpleGoalChecker" 126 | xy_goal_tolerance: 0.2 127 | yaw_goal_tolerance: 0.1 128 | stateful: True 129 | # DWB parameters 130 | FollowPath: 131 | plugin: "dwb_core::DWBLocalPlanner" 132 | debug_trajectory_details: True 133 | min_vel_x: 0.0 134 | min_vel_y: 0.0 135 | max_vel_x: 0.23 #0.46 136 | max_vel_y: 0.0 137 | max_vel_theta: 0.5 #1.0 138 | min_speed_xy: 0.0 139 | max_speed_xy: 0.23 #0.46 140 | min_speed_theta: 0.0 141 | # Add high threshold velocity for turtlebot 3 issue. 142 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 143 | acc_lim_x: 2.5 144 | acc_lim_y: 0.0 145 | acc_lim_theta: 3.2 146 | decel_lim_x: -2.0 147 | decel_lim_y: 0.0 148 | decel_lim_theta: -3.2 149 | vx_samples: 20 150 | vy_samples: 5 151 | vtheta_samples: 40 152 | sim_time: 1.9 153 | linear_granularity: 0.05 154 | angular_granularity: 0.025 155 | transform_tolerance: 1.0 156 | xy_goal_tolerance: 0.2 157 | trans_stopped_velocity: 0.25 158 | short_circuit_trajectory_evaluation: True 159 | stateful: True 160 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 161 | BaseObstacle.scale: 0.015 162 | PathAlign.scale: 0.0 163 | #PathAlign.forward_point_distance: 0.1 164 | GoalAlign.scale: 0.0 165 | #GoalAlign.forward_point_distance: 0.1 166 | PathDist.scale: 32.0 167 | GoalDist.scale: 24.0 168 | RotateToGoal.scale: 32.0 169 | RotateToGoal.slowing_factor: 5.0 170 | RotateToGoal.lookahead_time: -1.0 171 | DynamicFollowPath: 172 | plugin: "teb_local_planner::TebLocalPlannerROS" 173 | map_frame: "map" 174 | footprint_model.type: circular 175 | footprint_model.radius: 0.22 176 | min_obstacle_dist: 0.1 177 | inflation_dist: 0.1 178 | costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH 179 | costmap_converter_spin_thread: True 180 | costmap_converter_rate: 15 181 | enable_homotopy_class_planning: True 182 | enable_multithreading: True 183 | # weight_obstacle: 100.0 184 | optimization_verbose: False 185 | teb_autoresize: True 186 | min_samples: 3 187 | max_samples: 20 188 | max_global_plan_lookahead_dist: 1.0 189 | visualize_hc_graph: True 190 | xy_goal_tolerance: 0.2 191 | yaw_goal_tolerance: 0.1 192 | max_vel_x: 0.23 #0.46 193 | max_vel_theta: 0.5 #1.0 194 | acc_lim_x: 2.5 195 | acc_lim_theta: 3.2 196 | 197 | controller_server_rclcpp_node: 198 | ros__parameters: 199 | use_sim_time: True 200 | 201 | local_costmap: 202 | local_costmap: 203 | ros__parameters: 204 | update_frequency: 5.0 205 | publish_frequency: 2.0 206 | global_frame: odom 207 | robot_base_frame: base_link 208 | use_sim_time: True 209 | rolling_window: true 210 | width: 4 211 | height: 4 212 | resolution: 0.05 213 | robot_radius: 0.22 214 | plugins: ["voxel_layer", "inflation_layer"] 215 | inflation_layer: 216 | plugin: "nav2_costmap_2d::InflationLayer" 217 | cost_scaling_factor: 2.0 218 | voxel_layer: 219 | plugin: "nav2_costmap_2d::VoxelLayer" 220 | enabled: True 221 | publish_voxel_map: True 222 | origin_z: 0.0 223 | z_resolution: 0.05 224 | z_voxels: 16 225 | max_obstacle_height: 2.0 226 | mark_threshold: 0 227 | observation_sources: scan 228 | scan: 229 | topic: /scan 230 | max_obstacle_height: 2.0 231 | raytrace_range: 6.0 232 | inf_is_valid: True 233 | clearing: True 234 | marking: True 235 | data_type: "LaserScan" 236 | stvl_layer: 237 | plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" 238 | enabled: true 239 | voxel_decay: 15. 240 | decay_model: 0 241 | voxel_size: 0.1 242 | track_unknown_space: true 243 | max_obstacle_height: 2.0 244 | unknown_threshold: 15 245 | mark_threshold: 0 246 | update_footprint_enabled: true 247 | combination_method: 1 248 | obstacle_range: 3.0 249 | origin_z: 0.0 250 | publish_voxel_map: true 251 | transform_tolerance: 1.0 252 | mapping_mode: false 253 | map_save_duration: 60.0 254 | observation_sources: pointcloud 255 | pointcloud: 256 | data_type: PointCloud2 257 | topic: /xtion/depth_registered/points 258 | marking: true 259 | clearing: true 260 | min_obstacle_height: 0.1 261 | max_obstacle_height: 2.0 262 | expected_update_rate: 0.0 263 | observation_persistence: 0.0 264 | inf_is_valid: false 265 | voxel_filter: true 266 | clear_after_reading: true 267 | max_z: 2.0 268 | min_z: 0.1 269 | vertical_fov_angle: 0.8745 270 | horizontal_fov_angle: 1.048 271 | decay_acceleration: 15.0 272 | model_type: 0 273 | static_layer: 274 | map_subscribe_transient_local: True 275 | always_send_full_costmap: True 276 | local_costmap_client: 277 | ros__parameters: 278 | use_sim_time: True 279 | local_costmap_rclcpp_node: 280 | ros__parameters: 281 | use_sim_time: True 282 | 283 | global_costmap: 284 | global_costmap: 285 | ros__parameters: 286 | update_frequency: 3.0 287 | publish_frequency: 1.0 288 | global_frame: map 289 | robot_base_frame: base_link 290 | use_sim_time: True 291 | robot_radius: 0.22 292 | resolution: 0.05 293 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 294 | obstacle_layer: 295 | plugin: "nav2_costmap_2d::ObstacleLayer" 296 | enabled: True 297 | observation_sources: scan 298 | scan: 299 | topic: /scan 300 | max_obstacle_height: 2.0 301 | clearing: True 302 | marking: True 303 | data_type: "LaserScan" 304 | static_layer: 305 | plugin: "nav2_costmap_2d::StaticLayer" 306 | map_subscribe_transient_local: True 307 | inflation_layer: 308 | plugin: "nav2_costmap_2d::InflationLayer" 309 | stvl_layer: 310 | plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" 311 | enabled: true 312 | voxel_decay: 15. 313 | decay_model: 0 314 | voxel_size: 0.1 315 | track_unknown_space: true 316 | max_obstacle_height: 2.0 317 | unknown_threshold: 15 318 | mark_threshold: 0 319 | update_footprint_enabled: true 320 | combination_method: 1 321 | obstacle_range: 3.0 322 | origin_z: 0.0 323 | publish_voxel_map: true 324 | transform_tolerance: 1.0 325 | mapping_mode: false 326 | map_save_duration: 60.0 327 | observation_sources: pointcloud 328 | pointcloud: 329 | data_type: PointCloud2 330 | topic: /xtion/depth_registered/points 331 | marking: true 332 | clearing: true 333 | min_obstacle_height: 0.2 334 | max_obstacle_height: 2.0 335 | expected_update_rate: 0.0 336 | observation_persistence: 0.0 337 | inf_is_valid: true 338 | voxel_filter: true 339 | clear_after_reading: true 340 | max_z: 7.0 341 | min_z: 0.2 342 | vertical_fov_angle: 0.8745 343 | horizontal_fov_angle: 1.048 344 | decay_acceleration: 15.0 345 | model_type: 0 346 | always_send_full_costmap: True 347 | global_costmap_client: 348 | ros__parameters: 349 | use_sim_time: True 350 | global_costmap_rclcpp_node: 351 | ros__parameters: 352 | use_sim_time: True 353 | 354 | map_server: 355 | ros__parameters: 356 | use_sim_time: True 357 | yaml_filename: "turtlebot3_world.yaml" 358 | 359 | map_saver: 360 | ros__parameters: 361 | use_sim_time: True 362 | save_map_timeout: 5000 363 | free_thresh_default: 0.25 364 | occupied_thresh_default: 0.65 365 | 366 | planner_server: 367 | ros__parameters: 368 | expected_planner_frequency: 20.0 369 | use_sim_time: True 370 | planner_plugins: ["GridBased"] 371 | GridBased: 372 | plugin: "nav2_navfn_planner/NavfnPlanner" 373 | tolerance: 1.2 374 | use_astar: false 375 | allow_unknown: true 376 | 377 | planner_server_rclcpp_node: 378 | ros__parameters: 379 | use_sim_time: True 380 | 381 | recoveries_server: 382 | ros__parameters: 383 | costmap_topic: local_costmap/costmap_raw 384 | footprint_topic: local_costmap/published_footprint 385 | cycle_frequency: 10.0 386 | recovery_plugins: ["spin", "backup", "wait", "log"] 387 | spin: 388 | plugin: "nav2_recoveries/Spin" 389 | backup: 390 | plugin: "nav2_recoveries/BackUp" 391 | wait: 392 | plugin: "nav2_recoveries/Wait" 393 | log: 394 | plugin: "nav2_recoveries/Logger" 395 | global_frame: odom 396 | robot_base_frame: base_link 397 | transform_timeout: 1.0 398 | use_sim_time: true 399 | simulate_ahead_time: 2.0 400 | max_rotational_vel: 1.0 401 | min_rotational_vel: 0.4 402 | rotational_acc_lim: 3.2 403 | 404 | robot_state_publisher: 405 | ros__parameters: 406 | use_sim_time: True 407 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_marathon_kobuki_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_link" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.05 35 | update_min_d: 0.1 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | #set_initial_pose: True 41 | #initial_pose: 42 | # x: 10.35 43 | # y: 51.776 44 | # z: 0.0 45 | # yaw: -0.625 46 | 47 | amcl_map_client: 48 | ros__parameters: 49 | use_sim_time: True 50 | 51 | amcl_rclcpp_node: 52 | ros__parameters: 53 | use_sim_time: True 54 | 55 | bt_navigator: 56 | ros__parameters: 57 | use_sim_time: True 58 | bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 59 | plugin_lib_names: 60 | - nav2_compute_path_to_pose_action_bt_node 61 | - nav2_follow_path_action_bt_node 62 | - nav2_back_up_action_bt_node 63 | - nav2_spin_action_bt_node 64 | - nav2_wait_action_bt_node 65 | - nav2_clear_costmap_service_bt_node 66 | - nav2_is_stuck_condition_bt_node 67 | - nav2_goal_reached_condition_bt_node 68 | - nav2_initial_pose_received_condition_bt_node 69 | - nav2_reinitialize_global_localization_service_bt_node 70 | - nav2_rate_controller_bt_node 71 | - nav2_recovery_node_bt_node 72 | - nav2_pipeline_sequence_bt_node 73 | - nav2_round_robin_node_bt_node 74 | - nav2_recovery_logger_action_bt_node 75 | 76 | bt_navigator_rclcpp_node: 77 | ros__parameters: 78 | use_sim_time: True 79 | 80 | controller_server: 81 | ros__parameters: 82 | use_sim_time: True 83 | controller_frequency: 20.0 84 | controller_plugin_types: ["dwb_core::DWBLocalPlanner"] 85 | controller_plugin_ids: ["DynamicFollowPath"] 86 | min_x_velocity_threshold: 0.001 87 | min_y_velocity_threshold: 0.5 88 | min_theta_velocity_threshold: 0.001 89 | 90 | # DWB parameters 91 | DynamicFollowPath.debug_trajectory_details: True 92 | DynamicFollowPath.min_vel_x: 0.0 93 | DynamicFollowPath.min_vel_y: 0.0 94 | DynamicFollowPath.max_vel_x: 0.36 95 | DynamicFollowPath.max_vel_y: 0.0 96 | DynamicFollowPath.max_vel_theta: 2.04 97 | DynamicFollowPath.min_speed_xy: 0.0 98 | DynamicFollowPath.max_speed_xy: 0.26 99 | DynamicFollowPath.min_speed_theta: 0.0 100 | # Add high threshold velocity for turtlebot 3 issue. 101 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 102 | DynamicFollowPath.acc_lim_x: 1.0 103 | DynamicFollowPath.acc_lim_y: 0.0 104 | DynamicFollowPath.acc_lim_theta: 3.2 105 | DynamicFollowPath.decel_lim_y: 0.0 106 | DynamicFollowPath.decel_lim_theta: -3.2 107 | DynamicFollowPath.vx_samples: 20 108 | DynamicFollowPath.vy_samples: 5 109 | DynamicFollowPath.vtheta_samples: 20 110 | DynamicFollowPath.sim_time: 1.7 111 | DynamicFollowPath.linear_granularity: 0.05 112 | DynamicFollowPath.angular_granularity: 0.025 113 | DynamicFollowPath.time_granularity: 0.05 114 | DynamicFollowPath.xy_goal_tolerance: 0.25 115 | DynamicFollowPath.transform_tolerance: 0.2 116 | DynamicFollowPath.critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 117 | DynamicFollowPath.BaseObstacle.scale: 0.02 118 | DynamicFollowPath.PathAlign.scale: 10.0 119 | DynamicFollowPath.GoalAlign.scale: 10.0 120 | DynamicFollowPath.PathDist.scale: 3.0 121 | DynamicFollowPath.GoalDist.scale: 4.0 122 | DynamicFollowPath.RotateToGoal.scale: 2.0 123 | 124 | controller_server_rclcpp_node: 125 | ros__parameters: 126 | use_sim_time: True 127 | 128 | local_costmap: 129 | local_costmap: 130 | ros__parameters: 131 | update_frequency: 5.0 132 | publish_frequency: 2.0 133 | robot_base_frame: base_link 134 | use_sim_time: True 135 | rolling_window: true 136 | width: 3 137 | height: 3 138 | resolution: 0.05 139 | plugin_names: ["obstacle_layer", "inflation_layer"] 140 | plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] 141 | robot_radius: 0.22 142 | inflation_layer: 143 | cost_scaling_factor: 3.0 144 | obstacle_layer: 145 | enabled: True 146 | observation_sources: scan 147 | scan: 148 | topic: /scan_filtered 149 | max_obstacle_height: 2.0 150 | clearing: True 151 | marking: True 152 | data_type: "LaserScan" 153 | static_layer: 154 | map_subscribe_transient_local: True 155 | always_send_full_costmap: True 156 | local_costmap_client: 157 | ros__parameters: 158 | use_sim_time: True 159 | local_costmap_rclcpp_node: 160 | ros__parameters: 161 | use_sim_time: True 162 | 163 | global_costmap: 164 | global_costmap: 165 | ros__parameters: 166 | update_frequency: 1.0 167 | publish_frequency: 1.0 168 | robot_base_frame: base_link 169 | global_frame: map 170 | use_sim_time: True 171 | plugin_names: [ 172 | "static_layer", 173 | "obstacle_layer", 174 | "stvl_layer", 175 | "inflation_layer"] 176 | plugin_types: [ 177 | "nav2_costmap_2d::StaticLayer", 178 | "nav2_costmap_2d::ObstacleLayer", 179 | "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer", 180 | "nav2_costmap_2d::InflationLayer"] 181 | robot_radius: 0.22 182 | resolution: 0.05 183 | stvl_layer: 184 | enabled: True 185 | voxel_decay: 15.0 186 | decay_model: 0 187 | voxel_size: 0.1 188 | track_unknown_space: True 189 | max_obstacle_height: 2.0 190 | unknown_threshold: 15 191 | mark_threshold: 0 192 | update_footprint_enabled: True 193 | combination_method: 1 194 | obstacle_range: 2.0 195 | origin_z: 0.0 196 | publish_voxel_map: False 197 | transform_tolerance: 1.0 198 | mapping_mode: False 199 | map_save_duration: 60.0 200 | observation_sources: pointcloud 201 | pointcloud: 202 | data_type: PointCloud2 203 | topic: /points2 204 | marking: True 205 | clearing: True 206 | min_obstacle_height: 0.0 207 | max_obstacle_height: 2.0 208 | expected_update_rate: 2.0 209 | observation_persistence: 0.0 210 | inf_is_valid: False 211 | voxel_filter: False 212 | clear_after_reading: True 213 | max_z: 7.0 214 | min_z: 0.0 215 | vertical_fov_angle: 0.8745 216 | horizontal_fov_angle: 1.048 217 | decay_acceleration: 15.0 218 | model_type: 0 219 | 220 | obstacle_layer: 221 | enabled: True 222 | observation_sources: scan 223 | scan: 224 | topic: /scan_filtered 225 | max_obstacle_height: 2.0 226 | clearing: True 227 | marking: True 228 | data_type: "LaserScan" 229 | static_layer: 230 | map_subscribe_transient_local: True 231 | inflation_layer: 232 | cost_scaling_factor: 5.0 233 | inflation_radius: 1.0 234 | always_send_full_costmap: True 235 | global_costmap_client: 236 | ros__parameters: 237 | use_sim_time: True 238 | global_costmap_rclcpp_node: 239 | ros__parameters: 240 | use_sim_time: True 241 | 242 | map_server: 243 | ros__parameters: 244 | use_sim_time: True 245 | yaml_filename: "turtlebot3_world.yaml" 246 | 247 | planner_server: 248 | ros__parameters: 249 | planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"] 250 | planner_plugin_ids: ["GridBased"] 251 | use_sim_time: True 252 | GridBased.tolerance: 2.0 253 | GridBased.use_astar: true 254 | GridBased.allow_unknown: true 255 | 256 | planner_server_rclcpp_node: 257 | ros__parameters: 258 | use_sim_time: True 259 | 260 | robot_state_publisher: 261 | ros__parameters: 262 | use_sim_time: True 263 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_marathon_teb_kobuki_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_link" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.05 35 | update_min_d: 0.1 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | #set_initial_pose: True 41 | #initial_pose: 42 | # x: 10.35 43 | # y: 51.776 44 | # z: 0.0 45 | # yaw: -0.625 46 | 47 | amcl_map_client: 48 | ros__parameters: 49 | use_sim_time: True 50 | 51 | amcl_rclcpp_node: 52 | ros__parameters: 53 | use_sim_time: True 54 | 55 | bt_navigator: 56 | ros__parameters: 57 | use_sim_time: True 58 | bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 59 | plugin_lib_names: 60 | - nav2_compute_path_to_pose_action_bt_node 61 | - nav2_follow_path_action_bt_node 62 | - nav2_back_up_action_bt_node 63 | - nav2_spin_action_bt_node 64 | - nav2_wait_action_bt_node 65 | - nav2_clear_costmap_service_bt_node 66 | - nav2_is_stuck_condition_bt_node 67 | - nav2_goal_reached_condition_bt_node 68 | - nav2_initial_pose_received_condition_bt_node 69 | - nav2_reinitialize_global_localization_service_bt_node 70 | - nav2_rate_controller_bt_node 71 | - nav2_recovery_node_bt_node 72 | - nav2_pipeline_sequence_bt_node 73 | - nav2_round_robin_node_bt_node 74 | - nav2_recovery_logger_action_bt_node 75 | 76 | bt_navigator_rclcpp_node: 77 | ros__parameters: 78 | use_sim_time: True 79 | 80 | controller_server: 81 | ros__parameters: 82 | use_sim_time: True 83 | controller_frequency: 20 84 | controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"] 85 | controller_plugin_ids: ["DynamicFollowPath"] 86 | DynamicFollowPath.footprint_model.type: circular 87 | DynamicFollowPath.footprint_model.radius: 0.28 88 | DynamicFollowPath.min_obstacle_dist: 0.1 89 | DynamicFollowPath.inflation_dist: 0.1 90 | DynamicFollowPath.costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH 91 | DynamicFollowPath.costmap_converter_spin_thread: True 92 | DynamicFollowPath.costmap_converter_rate: 15 93 | DynamicFollowPath.enable_homotopy_class_planning: True 94 | DynamicFollowPath.enable_multithreading: True 95 | #DynamicFollowPath.weight_obstacle: 100.0 96 | DynamicFollowPath.optimization_verbose: False 97 | DynamicFollowPath.teb_autoresize: True 98 | DynamicFollowPath.min_samples: 3 99 | DynamicFollowPath.max_samples: 20 100 | DynamicFollowPath.max_global_plan_lookahead_dist: 1.0 101 | DynamicFollowPath.visualize_hc_graph: True 102 | DynamicFollowPath.max_vel_x: 0.4 103 | DynamicFollowPath.max_vel_theta: 1.0 104 | DynamicFollowPath.acc_lim_x: 2.5 105 | DynamicFollowPath.acc_lim_theta: 3.2 106 | 107 | controller_server_rclcpp_node: 108 | ros__parameters: 109 | use_sim_time: True 110 | 111 | local_costmap: 112 | local_costmap: 113 | ros__parameters: 114 | update_frequency: 5.0 115 | publish_frequency: 2.0 116 | robot_base_frame: base_link 117 | use_sim_time: True 118 | rolling_window: true 119 | width: 3 120 | height: 3 121 | resolution: 0.05 122 | plugin_names: ["obstacle_layer", "inflation_layer"] 123 | plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] 124 | robot_radius: 0.22 125 | inflation_layer: 126 | cost_scaling_factor: 3.0 127 | obstacle_layer: 128 | enabled: True 129 | observation_sources: scan 130 | scan: 131 | topic: /scan_filtered 132 | max_obstacle_height: 2.0 133 | clearing: True 134 | marking: True 135 | data_type: "LaserScan" 136 | static_layer: 137 | map_subscribe_transient_local: True 138 | always_send_full_costmap: True 139 | local_costmap_client: 140 | ros__parameters: 141 | use_sim_time: True 142 | local_costmap_rclcpp_node: 143 | ros__parameters: 144 | use_sim_time: True 145 | 146 | global_costmap: 147 | global_costmap: 148 | ros__parameters: 149 | update_frequency: 1.0 150 | publish_frequency: 1.0 151 | robot_base_frame: base_link 152 | global_frame: map 153 | use_sim_time: True 154 | plugin_names: [ 155 | "static_layer", 156 | "obstacle_layer", 157 | "stvl_layer", 158 | "inflation_layer"] 159 | plugin_types: [ 160 | "nav2_costmap_2d::StaticLayer", 161 | "nav2_costmap_2d::ObstacleLayer", 162 | "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer", 163 | "nav2_costmap_2d::InflationLayer"] 164 | robot_radius: 0.22 165 | resolution: 0.05 166 | stvl_layer: 167 | enabled: True 168 | voxel_decay: 15.0 169 | decay_model: 0 170 | voxel_size: 0.1 171 | track_unknown_space: True 172 | max_obstacle_height: 2.0 173 | unknown_threshold: 15 174 | mark_threshold: 0 175 | update_footprint_enabled: True 176 | combination_method: 1 177 | obstacle_range: 2.0 178 | origin_z: 0.0 179 | publish_voxel_map: False 180 | transform_tolerance: 1.0 181 | mapping_mode: False 182 | map_save_duration: 60.0 183 | observation_sources: pointcloud 184 | pointcloud: 185 | data_type: PointCloud2 186 | topic: /points2 187 | marking: True 188 | clearing: True 189 | min_obstacle_height: 0.0 190 | max_obstacle_height: 2.0 191 | expected_update_rate: 2.0 192 | observation_persistence: 0.0 193 | inf_is_valid: False 194 | voxel_filter: False 195 | clear_after_reading: True 196 | max_z: 7.0 197 | min_z: 0.0 198 | vertical_fov_angle: 0.8745 199 | horizontal_fov_angle: 1.048 200 | decay_acceleration: 15.0 201 | model_type: 0 202 | 203 | obstacle_layer: 204 | enabled: True 205 | observation_sources: scan 206 | scan: 207 | topic: /scan_filtered 208 | max_obstacle_height: 2.0 209 | clearing: True 210 | marking: True 211 | data_type: "LaserScan" 212 | static_layer: 213 | map_subscribe_transient_local: True 214 | inflation_layer: 215 | cost_scaling_factor: 5.0 216 | inflation_radius: 1.0 217 | always_send_full_costmap: True 218 | global_costmap_client: 219 | ros__parameters: 220 | use_sim_time: True 221 | global_costmap_rclcpp_node: 222 | ros__parameters: 223 | use_sim_time: True 224 | 225 | map_server: 226 | ros__parameters: 227 | use_sim_time: True 228 | yaml_filename: "turtlebot3_world.yaml" 229 | 230 | planner_server: 231 | ros__parameters: 232 | planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"] 233 | planner_plugin_ids: ["GridBased"] 234 | use_sim_time: True 235 | GridBased.tolerance: 2.0 236 | GridBased.use_astar: false 237 | GridBased.allow_unknown: true 238 | 239 | planner_server_rclcpp_node: 240 | ros__parameters: 241 | use_sim_time: True 242 | 243 | robot_state_publisher: 244 | ros__parameters: 245 | use_sim_time: True 246 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_marathon_teb_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | set_initial_pose: True 41 | initial_pose: 42 | x: 10.35 43 | y: 51.776 44 | z: 0.0 45 | yaw: -0.625 46 | 47 | amcl_map_client: 48 | ros__parameters: 49 | use_sim_time: True 50 | 51 | amcl_rclcpp_node: 52 | ros__parameters: 53 | use_sim_time: True 54 | 55 | bt_navigator: 56 | ros__parameters: 57 | use_sim_time: True 58 | bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 59 | plugin_lib_names: 60 | - nav2_compute_path_to_pose_action_bt_node 61 | - nav2_follow_path_action_bt_node 62 | - nav2_back_up_action_bt_node 63 | - nav2_spin_action_bt_node 64 | - nav2_wait_action_bt_node 65 | - nav2_clear_costmap_service_bt_node 66 | - nav2_is_stuck_condition_bt_node 67 | - nav2_goal_reached_condition_bt_node 68 | - nav2_initial_pose_received_condition_bt_node 69 | - nav2_reinitialize_global_localization_service_bt_node 70 | - nav2_rate_controller_bt_node 71 | - nav2_recovery_node_bt_node 72 | - nav2_pipeline_sequence_bt_node 73 | - nav2_round_robin_node_bt_node 74 | - nav2_recovery_logger_action_bt_node 75 | 76 | bt_navigator_rclcpp_node: 77 | ros__parameters: 78 | use_sim_time: True 79 | 80 | controller_server: 81 | ros__parameters: 82 | use_sim_time: True 83 | controller_frequency: 20 84 | controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"] 85 | controller_plugin_ids: ["FollowPath"] 86 | FollowPath.footprint_model.type: circular 87 | FollowPath.footprint_model.radius: 0.28 88 | FollowPath.min_obstacle_dist: 0.1 89 | FollowPath.inflation_dist: 0.1 90 | FollowPath.costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH 91 | FollowPath.costmap_converter_spin_thread: True 92 | FollowPath.costmap_converter_rate: 15 93 | FollowPath.enable_homotopy_class_planning: True 94 | FollowPath.enable_multithreading: True 95 | # FollowPath.weight_obstacle: 100.0 96 | FollowPath.optimization_verbose: False 97 | FollowPath.teb_autoresize: True 98 | FollowPath.min_samples: 3 99 | FollowPath.max_samples: 20 100 | FollowPath.max_global_plan_lookahead_dist: 1.0 101 | FollowPath.visualize_hc_graph: True 102 | FollowPath.max_vel_x: 0.46 103 | FollowPath.max_vel_theta: 1.0 104 | FollowPath.acc_lim_x: 2.5 105 | FollowPath.acc_lim_theta: 3.2 106 | 107 | costmap_converter: 108 | ros__parameters: 109 | use_sim_time: True 110 | 111 | controller_server_rclcpp_node: 112 | ros__parameters: 113 | use_sim_time: True 114 | 115 | local_costmap: 116 | local_costmap: 117 | ros__parameters: 118 | update_frequency: 5.0 119 | publish_frequency: 2.0 120 | robot_base_frame: base_link 121 | use_sim_time: True 122 | #global_frame: odom 123 | rolling_window: true 124 | width: 3 125 | height: 3 126 | resolution: 0.05 127 | plugin_names: ["obstacle_layer", "inflation_layer"] 128 | plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] 129 | robot_radius: 0.22 130 | inflation_layer: 131 | cost_scaling_factor: 3.0 132 | obstacle_layer: 133 | enabled: True 134 | observation_sources: scan 135 | scan: 136 | topic: /scan 137 | max_obstacle_height: 2.0 138 | clearing: True 139 | marking: True 140 | data_type: "LaserScan" 141 | static_layer: 142 | map_subscribe_transient_local: True 143 | always_send_full_costmap: False 144 | local_costmap_client: 145 | ros__parameters: 146 | use_sim_time: True 147 | local_costmap_rclcpp_node: 148 | ros__parameters: 149 | use_sim_time: True 150 | 151 | global_costmap: 152 | global_costmap: 153 | ros__parameters: 154 | update_frequency: 1.0 155 | publish_frequency: 1.0 156 | robot_base_frame: base_link 157 | global_frame: map 158 | use_sim_time: True 159 | plugin_names: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] 160 | plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer", "nav2_costmap_2d::InflationLayer"] 161 | robot_radius: 0.22 162 | resolution: 0.05 163 | obstacle_layer: 164 | enabled: True 165 | observation_sources: scan 166 | scan: 167 | topic: /scan 168 | max_obstacle_height: 2.0 169 | clearing: True 170 | marking: True 171 | data_type: "LaserScan" 172 | stvl_layer: 173 | enabled: true 174 | voxel_decay: 15. 175 | decay_model: 0 176 | voxel_size: 0.05 177 | track_unknown_space: true 178 | max_obstacle_height: 2.0 179 | unknown_threshold: 15 180 | mark_threshold: 0 181 | update_footprint_enabled: true 182 | combination_method: 1 183 | obstacle_range: 3.0 184 | origin_z: 0.0 185 | publish_voxel_map: true 186 | transform_tolerance: 0.2 187 | mapping_mode: false 188 | map_save_duration: 60.0 189 | observation_sources: pointcloud 190 | pointcloud: 191 | data_type: PointCloud2 192 | topic: /xtion/depth_registered/points 193 | marking: true 194 | clearing: true 195 | min_obstacle_height: 0.1 196 | max_obstacle_height: 2.0 197 | expected_update_rate: 0.0 198 | observation_persistence: 0.0 199 | inf_is_valid: true 200 | voxel_filter: true 201 | clear_after_reading: true 202 | max_z: 7.0 203 | min_z: 0.1 204 | vertical_fov_angle: 0.8745 205 | horizontal_fov_angle: 1.048 206 | decay_acceleration: 15.0 207 | model_type: 0 208 | static_layer: 209 | map_subscribe_transient_local: True 210 | always_send_full_costmap: True 211 | global_costmap_client: 212 | ros__parameters: 213 | use_sim_time: True 214 | global_costmap_rclcpp_node: 215 | ros__parameters: 216 | use_sim_time: True 217 | 218 | map_server: 219 | ros__parameters: 220 | use_sim_time: True 221 | yaml_filename: "lab_map.yaml" 222 | 223 | planner_server: 224 | ros__parameters: 225 | planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"] 226 | planner_plugin_ids: ["GridBased"] 227 | use_sim_time: True 228 | GridBased.tolerance: 2.0 229 | GridBased.use_astar: false 230 | GridBased.allow_unknown: false 231 | 232 | planner_server_rclcpp_node: 233 | ros__parameters: 234 | use_sim_time: True 235 | 236 | robot_state_publisher: 237 | ros__parameters: 238 | use_sim_time: True 239 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_marathon_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | set_initial_pose: True 41 | initial_pose: 42 | x: 10.35 43 | y: 51.776 44 | z: 0.0 45 | yaw: -0.625 46 | 47 | amcl_map_client: 48 | ros__parameters: 49 | use_sim_time: True 50 | 51 | amcl_rclcpp_node: 52 | ros__parameters: 53 | use_sim_time: True 54 | 55 | bt_navigator: 56 | ros__parameters: 57 | use_sim_time: True 58 | global_frame: map 59 | robot_base_frame: base_link 60 | odom_topic: /odom 61 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 62 | plugin_lib_names: 63 | - nav2_compute_path_to_pose_action_bt_node 64 | - nav2_follow_path_action_bt_node 65 | - nav2_back_up_action_bt_node 66 | - nav2_spin_action_bt_node 67 | - nav2_wait_action_bt_node 68 | - nav2_clear_costmap_service_bt_node 69 | - nav2_is_stuck_condition_bt_node 70 | - nav2_goal_reached_condition_bt_node 71 | - nav2_goal_updated_condition_bt_node 72 | - nav2_initial_pose_received_condition_bt_node 73 | - nav2_reinitialize_global_localization_service_bt_node 74 | - nav2_rate_controller_bt_node 75 | - nav2_distance_controller_bt_node 76 | - nav2_speed_controller_bt_node 77 | - nav2_truncate_path_action_bt_node 78 | - nav2_goal_updater_node_bt_node 79 | - nav2_recovery_node_bt_node 80 | - nav2_pipeline_sequence_bt_node 81 | - nav2_round_robin_node_bt_node 82 | - nav2_transform_available_condition_bt_node 83 | - nav2_time_expired_condition_bt_node 84 | - nav2_distance_traveled_condition_bt_node 85 | 86 | bt_navigator_rclcpp_node: 87 | ros__parameters: 88 | use_sim_time: True 89 | 90 | controller_server: 91 | ros__parameters: 92 | use_sim_time: True 93 | controller_frequency: 20.0 94 | min_x_velocity_threshold: 0.001 95 | min_y_velocity_threshold: 0.5 96 | min_theta_velocity_threshold: 0.001 97 | progress_checker_plugin: "progress_checker" 98 | goal_checker_plugin: "goal_checker" 99 | controller_plugins: ["FollowPath", "DynamicFollowPath"] 100 | # Progress checker parameters 101 | progress_checker: 102 | plugin: "nav2_controller::SimpleProgressChecker" 103 | required_movement_radius: 0.5 104 | movement_time_allowance: 10.0 105 | # Goal checker parameters 106 | goal_checker: 107 | plugin: "nav2_controller::SimpleGoalChecker" 108 | xy_goal_tolerance: 0.25 109 | yaw_goal_tolerance: 0.25 110 | stateful: True 111 | # DWB parameters 112 | FollowPath: 113 | plugin: "dwb_core::DWBLocalPlanner" 114 | debug_trajectory_details: True 115 | min_vel_x: 0.0 116 | min_vel_y: 0.0 117 | max_vel_x: 0.4 118 | max_vel_y: 0.0 119 | max_vel_theta: 1.0 120 | min_speed_xy: 0.0 121 | max_speed_xy: 0.4 122 | min_speed_theta: 0.0 123 | # Add high threshold velocity for turtlebot 3 issue. 124 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 125 | acc_lim_x: 2.0 126 | acc_lim_y: 0.0 127 | acc_lim_theta: 3.2 128 | decel_lim_x: -2.0 129 | decel_lim_y: 0.0 130 | decel_lim_theta: -3.2 131 | vx_samples: 20 132 | vy_samples: 5 133 | vtheta_samples: 40 134 | sim_time: 1.9 135 | linear_granularity: 0.05 136 | angular_granularity: 0.025 137 | transform_tolerance: 1.0 138 | xy_goal_tolerance: 0.25 139 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 140 | BaseObstacle.scale: 0.015 141 | PathAlign.scale: 0.0 142 | GoalAlign.scale: 0.0 143 | PathDist.scale: 32.0 144 | GoalDist.scale: 24.0 145 | RotateToGoal.scale: 32.0 146 | short_circuit_trajectory_evaluation: True 147 | trans_stopped_velocity: 0.25 148 | RotateToGoal.slowing_factor: 5.0 149 | RotateToGoal.lookahead_time: -1.0 150 | stateful: True 151 | DynamicFollowPath: 152 | plugin: "teb_local_planner::TebLocalPlannerROS" 153 | footprint_model.type: circular 154 | footprint_model.radius: 0.22 155 | min_obstacle_dist: 0.1 156 | inflation_dist: 0.1 157 | costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH 158 | costmap_converter_spin_thread: True 159 | costmap_converter_rate: 15 160 | enable_homotopy_class_planning: True 161 | enable_multithreading: True 162 | # weight_obstacle: 100.0 163 | optimization_verbose: False 164 | teb_autoresize: True 165 | min_samples: 3 166 | max_samples: 20 167 | max_global_plan_lookahead_dist: 1.0 168 | visualize_hc_graph: True 169 | max_vel_x: 0.26 170 | max_vel_theta: 1.0 171 | acc_lim_x: 2.5 172 | acc_lim_theta: 3.2 173 | 174 | controller_server_rclcpp_node: 175 | ros__parameters: 176 | use_sim_time: True 177 | 178 | local_costmap: 179 | local_costmap: 180 | ros__parameters: 181 | update_frequency: 5.0 182 | publish_frequency: 2.0 183 | robot_base_frame: base_link 184 | use_sim_time: True 185 | #global_frame: odom 186 | rolling_window: true 187 | width: 3 188 | height: 3 189 | resolution: 0.05 190 | robot_radius: 0.22 191 | plugins: ["voxel_layer"] 192 | inflation_layer: 193 | enabled: True 194 | plugin: "nav2_costmap_2d::InflationLayer" 195 | cost_scaling_factor: 3.0 196 | voxel_layer: 197 | plugin: "nav2_costmap_2d::VoxelLayer" 198 | enabled: True 199 | publish_voxel_map: True 200 | origin_z: 0.0 201 | z_resolution: 0.05 202 | z_voxels: 16 203 | max_obstacle_height: 2.0 204 | mark_threshold: 0 205 | observation_sources: scan 206 | scan: 207 | topic: /scan 208 | max_obstacle_height: 2.0 209 | clearing: True 210 | marking: True 211 | data_type: "LaserScan" 212 | static_layer: 213 | map_subscribe_transient_local: True 214 | always_send_full_costmap: False 215 | local_costmap_client: 216 | ros__parameters: 217 | use_sim_time: True 218 | local_costmap_rclcpp_node: 219 | ros__parameters: 220 | use_sim_time: True 221 | 222 | global_costmap: 223 | global_costmap: 224 | ros__parameters: 225 | update_frequency: 1.0 226 | publish_frequency: 1.0 227 | robot_base_frame: base_link 228 | global_frame: map 229 | use_sim_time: True 230 | plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] 231 | robot_radius: 0.22 232 | resolution: 0.05 233 | obstacle_layer: 234 | plugin: "nav2_costmap_2d::ObstacleLayer" 235 | enabled: True 236 | observation_sources: scan 237 | scan: 238 | topic: /scan 239 | max_obstacle_height: 2.0 240 | clearing: True 241 | marking: True 242 | data_type: "LaserScan" 243 | stvl_layer: 244 | plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" 245 | enabled: true 246 | voxel_decay: 15. 247 | decay_model: 0 248 | voxel_size: 0.05 249 | track_unknown_space: true 250 | max_obstacle_height: 2.0 251 | unknown_threshold: 15 252 | mark_threshold: 0 253 | update_footprint_enabled: true 254 | combination_method: 1 255 | obstacle_range: 3.0 256 | origin_z: 0.0 257 | publish_voxel_map: true 258 | transform_tolerance: 1.0 259 | mapping_mode: false 260 | map_save_duration: 60.0 261 | observation_sources: pointcloud 262 | pointcloud: 263 | data_type: PointCloud2 264 | topic: /xtion/depth_registered/points 265 | marking: true 266 | clearing: true 267 | min_obstacle_height: 0.2 268 | max_obstacle_height: 2.0 269 | expected_update_rate: 0.0 270 | observation_persistence: 0.0 271 | inf_is_valid: true 272 | voxel_filter: true 273 | clear_after_reading: true 274 | max_z: 7.0 275 | min_z: 0.2 276 | vertical_fov_angle: 0.8745 277 | horizontal_fov_angle: 1.048 278 | decay_acceleration: 15.0 279 | model_type: 0 280 | inflation_layer: 281 | plugin: "nav2_costmap_2d::InflationLayer" 282 | static_layer: 283 | plugin: "nav2_costmap_2d::StaticLayer" 284 | map_subscribe_transient_local: True 285 | always_send_full_costmap: True 286 | global_costmap_client: 287 | ros__parameters: 288 | use_sim_time: True 289 | global_costmap_rclcpp_node: 290 | ros__parameters: 291 | use_sim_time: True 292 | 293 | map_server: 294 | ros__parameters: 295 | use_sim_time: True 296 | yaml_filename: "lab_map.yaml" 297 | 298 | planner_server: 299 | ros__parameters: 300 | expected_planner_frequency: 20.0 301 | use_sim_time: True 302 | planner_plugins: ["GridBased"] 303 | GridBased: 304 | plugin: "nav2_navfn_planner/NavfnPlanner" 305 | tolerance: 0.5 306 | use_astar: false 307 | allow_unknown: true 308 | 309 | planner_server_rclcpp_node: 310 | ros__parameters: 311 | use_sim_time: True 312 | 313 | recoveries_server: 314 | ros__parameters: 315 | costmap_topic: local_costmap/costmap_raw 316 | footprint_topic: local_costmap/published_footprint 317 | cycle_frequency: 10.0 318 | recovery_plugins: ["spin", "backup", "wait"] 319 | spin: 320 | plugin: "nav2_recoveries/Spin" 321 | backup: 322 | plugin: "nav2_recoveries/BackUp" 323 | wait: 324 | plugin: "nav2_recoveries/Wait" 325 | global_frame: odom 326 | robot_base_frame: base_link 327 | transform_timeout: 0.1 328 | use_sim_time: true 329 | simulate_ahead_time: 2.0 330 | max_rotational_vel: 1.0 331 | min_rotational_vel: 0.4 332 | rotational_acc_lim: 3.2 333 | 334 | robot_state_publisher: 335 | ros__parameters: 336 | use_sim_time: True 337 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_pp_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | set_initial_pose: True 42 | initial_pose: 43 | x: 10.35 44 | y: 51.77 45 | z: 0.0 46 | yaw: -0.625 47 | 48 | amcl_map_client: 49 | ros__parameters: 50 | use_sim_time: True 51 | 52 | amcl_rclcpp_node: 53 | ros__parameters: 54 | use_sim_time: True 55 | 56 | bt_navigator: 57 | ros__parameters: 58 | use_sim_time: True 59 | global_frame: map 60 | robot_base_frame: base_link 61 | odom_topic: /odom 62 | enable_groot_monitoring: True 63 | groot_zmq_publisher_port: 1666 64 | groot_zmq_server_port: 1667 65 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 66 | plugin_lib_names: 67 | - nav2_compute_path_to_pose_action_bt_node 68 | - nav2_follow_path_action_bt_node 69 | - nav2_back_up_action_bt_node 70 | - nav2_spin_action_bt_node 71 | - nav2_wait_action_bt_node 72 | - nav2_clear_costmap_service_bt_node 73 | - nav2_is_stuck_condition_bt_node 74 | - nav2_goal_reached_condition_bt_node 75 | - nav2_goal_updated_condition_bt_node 76 | - nav2_initial_pose_received_condition_bt_node 77 | - nav2_reinitialize_global_localization_service_bt_node 78 | - nav2_rate_controller_bt_node 79 | - nav2_distance_controller_bt_node 80 | - nav2_speed_controller_bt_node 81 | - nav2_truncate_path_action_bt_node 82 | - nav2_goal_updater_node_bt_node 83 | - nav2_recovery_node_bt_node 84 | - nav2_pipeline_sequence_bt_node 85 | - nav2_round_robin_node_bt_node 86 | - nav2_transform_available_condition_bt_node 87 | - nav2_time_expired_condition_bt_node 88 | - nav2_distance_traveled_condition_bt_node 89 | 90 | bt_navigator_rclcpp_node: 91 | ros__parameters: 92 | use_sim_time: True 93 | 94 | controller_server: 95 | ros__parameters: 96 | use_sim_time: True 97 | controller_frequency: 20.0 98 | min_x_velocity_threshold: 0.001 99 | min_y_velocity_threshold: 0.5 100 | min_theta_velocity_threshold: 0.001 101 | progress_checker_plugin: "progress_checker" 102 | goal_checker_plugin: "goal_checker" 103 | controller_plugins: ["FollowPath"] 104 | 105 | progress_checker: 106 | plugin: "nav2_controller::SimpleProgressChecker" 107 | required_movement_radius: 0.5 108 | movement_time_allowance: 10.0 109 | goal_checker: 110 | plugin: "nav2_controller::SimpleGoalChecker" 111 | xy_goal_tolerance: 0.25 112 | yaw_goal_tolerance: 0.25 113 | stateful: True 114 | FollowPath: 115 | plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" 116 | desired_linear_vel: 0.6 117 | max_linear_accel: 2.0 118 | max_linear_decel: 2.0 119 | lookahead_dist: 0.6 120 | min_lookahead_dist: 0.3 121 | max_lookahead_dist: 0.9 122 | lookahead_time: 1.5 123 | rotate_to_heading_angular_vel: 1.0 124 | transform_tolerance: 1.0 125 | use_velocity_scaled_lookahead_dist: false 126 | min_approach_linear_velocity: 0.05 127 | use_approach_linear_velocity_scaling: true 128 | max_allowed_time_to_collision: 1.0 129 | use_regulated_linear_velocity_scaling: false 130 | use_cost_regulated_linear_velocity_scaling: false 131 | regulated_linear_scaling_min_radius: 0.9 132 | regulated_linear_scaling_min_speed: 0.25 133 | use_rotate_to_heading: true 134 | rotate_to_heading_min_angle: 0.785 135 | max_angular_accel: 3.2 136 | goal_dist_tol: 0.25 137 | 138 | controller_server_rclcpp_node: 139 | ros__parameters: 140 | use_sim_time: True 141 | 142 | local_costmap: 143 | local_costmap: 144 | ros__parameters: 145 | update_frequency: 5.0 146 | publish_frequency: 2.0 147 | global_frame: odom 148 | robot_base_frame: base_link 149 | use_sim_time: True 150 | rolling_window: true 151 | width: 3 152 | height: 3 153 | resolution: 0.05 154 | robot_radius: 0.22 155 | plugins: ["voxel_layer", "inflation_layer"] 156 | inflation_layer: 157 | plugin: "nav2_costmap_2d::InflationLayer" 158 | cost_scaling_factor: 3.0 159 | inflation_radius: 0.55 160 | obstacle_layer: 161 | plugin: "nav2_costmap_2d::ObstacleLayer" 162 | enabled: True 163 | observation_sources: scan 164 | scan: 165 | topic: /scan 166 | max_obstacle_height: 2.0 167 | clearing: True 168 | marking: True 169 | data_type: "LaserScan" 170 | raytrace_max_range: 3.0 171 | raytrace_min_range: 0.0 172 | obstacle_max_range: 2.5 173 | obstacle_min_range: 0.0 174 | voxel_layer: 175 | plugin: "nav2_costmap_2d::VoxelLayer" 176 | enabled: True 177 | publish_voxel_map: True 178 | origin_z: 0.0 179 | z_resolution: 0.05 180 | z_voxels: 16 181 | max_obstacle_height: 2.0 182 | mark_threshold: 0 183 | observation_sources: scan 184 | scan: 185 | topic: /scan 186 | max_obstacle_height: 2.0 187 | clearing: True 188 | marking: True 189 | data_type: "LaserScan" 190 | raytrace_max_range: 3.0 191 | raytrace_min_range: 0.0 192 | obstacle_max_range: 2.5 193 | obstacle_min_range: 0.0 194 | static_layer: 195 | map_subscribe_transient_local: True 196 | always_send_full_costmap: True 197 | local_costmap_client: 198 | ros__parameters: 199 | use_sim_time: True 200 | local_costmap_rclcpp_node: 201 | ros__parameters: 202 | use_sim_time: True 203 | 204 | global_costmap: 205 | global_costmap: 206 | ros__parameters: 207 | update_frequency: 1.0 208 | publish_frequency: 1.0 209 | global_frame: map 210 | robot_base_frame: base_link 211 | use_sim_time: True 212 | robot_radius: 0.22 213 | resolution: 0.05 214 | track_unknown_space: true 215 | plugins: ["static_layer", "inflation_layer"] 216 | static_layer: 217 | plugin: "nav2_costmap_2d::StaticLayer" 218 | map_subscribe_transient_local: True 219 | inflation_layer: 220 | plugin: "nav2_costmap_2d::InflationLayer" 221 | cost_scaling_factor: 3.0 222 | inflation_radius: 0.55 223 | always_send_full_costmap: True 224 | global_costmap_client: 225 | ros__parameters: 226 | use_sim_time: True 227 | global_costmap_rclcpp_node: 228 | ros__parameters: 229 | use_sim_time: True 230 | 231 | map_server: 232 | ros__parameters: 233 | use_sim_time: True 234 | yaml_filename: "turtlebot3_world.yaml" 235 | 236 | map_saver: 237 | ros__parameters: 238 | use_sim_time: True 239 | save_map_timeout: 5.0 240 | free_thresh_default: 0.25 241 | occupied_thresh_default: 0.65 242 | map_subscribe_transient_local: True 243 | 244 | planner_server: 245 | ros__parameters: 246 | expected_planner_frequency: 20.0 247 | use_sim_time: True 248 | planner_plugins: ["GridBased"] 249 | GridBased: 250 | plugin: "nav2_navfn_planner/NavfnPlanner" 251 | tolerance: 0.5 252 | use_astar: false 253 | allow_unknown: true 254 | 255 | planner_server_rclcpp_node: 256 | ros__parameters: 257 | use_sim_time: True 258 | 259 | recoveries_server: 260 | ros__parameters: 261 | costmap_topic: local_costmap/costmap_raw 262 | footprint_topic: local_costmap/published_footprint 263 | cycle_frequency: 10.0 264 | recovery_plugins: ["spin", "backup", "wait"] 265 | spin: 266 | plugin: "nav2_recoveries/Spin" 267 | backup: 268 | plugin: "nav2_recoveries/BackUp" 269 | wait: 270 | plugin: "nav2_recoveries/Wait" 271 | global_frame: odom 272 | robot_base_frame: base_link 273 | transform_timeout: 0.1 274 | use_sim_time: true 275 | simulate_ahead_time: 2.0 276 | max_rotational_vel: 1.0 277 | min_rotational_vel: 0.4 278 | rotational_acc_lim: 3.2 279 | 280 | robot_state_publisher: 281 | ros__parameters: 282 | use_sim_time: True 283 | 284 | waypoint_follower: 285 | ros__parameters: 286 | loop_rate: 20 287 | stop_on_failure: false 288 | waypoint_task_executor_plugin: "wait_at_waypoint" 289 | wait_at_waypoint: 290 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 291 | enabled: True 292 | waypoint_pause_duration: 200 293 | -------------------------------------------------------------------------------- /marathon_ros2_bringup/params/nav2_regulated_pp_tiago_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | set_initial_pose: True 42 | initial_pose: 43 | x: 10.35 44 | y: 51.77 45 | z: 0.0 46 | yaw: -0.625 47 | 48 | amcl_map_client: 49 | ros__parameters: 50 | use_sim_time: True 51 | 52 | amcl_rclcpp_node: 53 | ros__parameters: 54 | use_sim_time: True 55 | 56 | bt_navigator: 57 | ros__parameters: 58 | use_sim_time: True 59 | global_frame: map 60 | robot_base_frame: base_link 61 | odom_topic: /odom 62 | enable_groot_monitoring: True 63 | groot_zmq_publisher_port: 1666 64 | groot_zmq_server_port: 1667 65 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 66 | plugin_lib_names: 67 | - nav2_compute_path_to_pose_action_bt_node 68 | - nav2_follow_path_action_bt_node 69 | - nav2_back_up_action_bt_node 70 | - nav2_spin_action_bt_node 71 | - nav2_wait_action_bt_node 72 | - nav2_clear_costmap_service_bt_node 73 | - nav2_is_stuck_condition_bt_node 74 | - nav2_goal_reached_condition_bt_node 75 | - nav2_goal_updated_condition_bt_node 76 | - nav2_initial_pose_received_condition_bt_node 77 | - nav2_reinitialize_global_localization_service_bt_node 78 | - nav2_rate_controller_bt_node 79 | - nav2_distance_controller_bt_node 80 | - nav2_speed_controller_bt_node 81 | - nav2_truncate_path_action_bt_node 82 | - nav2_goal_updater_node_bt_node 83 | - nav2_recovery_node_bt_node 84 | - nav2_pipeline_sequence_bt_node 85 | - nav2_round_robin_node_bt_node 86 | - nav2_transform_available_condition_bt_node 87 | - nav2_time_expired_condition_bt_node 88 | - nav2_distance_traveled_condition_bt_node 89 | 90 | bt_navigator_rclcpp_node: 91 | ros__parameters: 92 | use_sim_time: True 93 | 94 | controller_server: 95 | ros__parameters: 96 | use_sim_time: True 97 | controller_frequency: 20.0 98 | min_x_velocity_threshold: 0.001 99 | min_y_velocity_threshold: 0.5 100 | min_theta_velocity_threshold: 0.001 101 | progress_checker_plugin: "progress_checker" 102 | goal_checker_plugin: "goal_checker" 103 | controller_plugins: ["FollowPath"] 104 | 105 | progress_checker: 106 | plugin: "nav2_controller::SimpleProgressChecker" 107 | required_movement_radius: 0.5 108 | movement_time_allowance: 10.0 109 | goal_checker: 110 | plugin: "nav2_controller::SimpleGoalChecker" 111 | xy_goal_tolerance: 0.25 112 | yaw_goal_tolerance: 0.25 113 | stateful: True 114 | FollowPath: 115 | plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" 116 | desired_linear_vel: 0.6 117 | max_linear_accel: 2.0 118 | max_linear_decel: 2.0 119 | lookahead_dist: 0.6 120 | min_lookahead_dist: 0.3 121 | max_lookahead_dist: 0.9 122 | lookahead_time: 1.5 123 | rotate_to_heading_angular_vel: 1.0 124 | transform_tolerance: 1.0 125 | use_velocity_scaled_lookahead_dist: true 126 | min_approach_linear_velocity: 0.05 127 | use_approach_linear_velocity_scaling: true 128 | max_allowed_time_to_collision: 1.0 129 | use_regulated_linear_velocity_scaling: true 130 | use_cost_regulated_linear_velocity_scaling: true 131 | regulated_linear_scaling_min_radius: 0.9 132 | regulated_linear_scaling_min_speed: 0.25 133 | use_rotate_to_heading: true 134 | rotate_to_heading_min_angle: 0.785 135 | max_angular_accel: 3.2 136 | goal_dist_tol: 0.25 137 | 138 | controller_server_rclcpp_node: 139 | ros__parameters: 140 | use_sim_time: True 141 | 142 | local_costmap: 143 | local_costmap: 144 | ros__parameters: 145 | update_frequency: 5.0 146 | publish_frequency: 2.0 147 | global_frame: odom 148 | robot_base_frame: base_link 149 | use_sim_time: True 150 | rolling_window: true 151 | width: 3 152 | height: 3 153 | resolution: 0.05 154 | robot_radius: 0.22 155 | plugins: ["voxel_layer", "inflation_layer"] 156 | inflation_layer: 157 | plugin: "nav2_costmap_2d::InflationLayer" 158 | cost_scaling_factor: 3.0 159 | inflation_radius: 0.55 160 | obstacle_layer: 161 | plugin: "nav2_costmap_2d::ObstacleLayer" 162 | enabled: True 163 | observation_sources: scan 164 | scan: 165 | topic: /scan 166 | max_obstacle_height: 2.0 167 | clearing: True 168 | marking: True 169 | data_type: "LaserScan" 170 | raytrace_max_range: 3.0 171 | raytrace_min_range: 0.0 172 | obstacle_max_range: 2.5 173 | obstacle_min_range: 0.0 174 | voxel_layer: 175 | plugin: "nav2_costmap_2d::VoxelLayer" 176 | enabled: True 177 | publish_voxel_map: True 178 | origin_z: 0.0 179 | z_resolution: 0.05 180 | z_voxels: 16 181 | max_obstacle_height: 2.0 182 | mark_threshold: 0 183 | observation_sources: scan 184 | scan: 185 | topic: /scan 186 | max_obstacle_height: 2.0 187 | clearing: True 188 | marking: True 189 | data_type: "LaserScan" 190 | raytrace_max_range: 3.0 191 | raytrace_min_range: 0.0 192 | obstacle_max_range: 2.5 193 | obstacle_min_range: 0.0 194 | static_layer: 195 | map_subscribe_transient_local: True 196 | always_send_full_costmap: True 197 | local_costmap_client: 198 | ros__parameters: 199 | use_sim_time: True 200 | local_costmap_rclcpp_node: 201 | ros__parameters: 202 | use_sim_time: True 203 | 204 | global_costmap: 205 | global_costmap: 206 | ros__parameters: 207 | update_frequency: 1.0 208 | publish_frequency: 1.0 209 | global_frame: map 210 | robot_base_frame: base_link 211 | use_sim_time: True 212 | robot_radius: 0.22 213 | resolution: 0.05 214 | track_unknown_space: true 215 | plugins: ["static_layer", "inflation_layer"] 216 | static_layer: 217 | plugin: "nav2_costmap_2d::StaticLayer" 218 | map_subscribe_transient_local: True 219 | inflation_layer: 220 | plugin: "nav2_costmap_2d::InflationLayer" 221 | cost_scaling_factor: 3.0 222 | inflation_radius: 0.55 223 | always_send_full_costmap: True 224 | global_costmap_client: 225 | ros__parameters: 226 | use_sim_time: True 227 | global_costmap_rclcpp_node: 228 | ros__parameters: 229 | use_sim_time: True 230 | 231 | map_server: 232 | ros__parameters: 233 | use_sim_time: True 234 | yaml_filename: "turtlebot3_world.yaml" 235 | 236 | map_saver: 237 | ros__parameters: 238 | use_sim_time: True 239 | save_map_timeout: 5.0 240 | free_thresh_default: 0.25 241 | occupied_thresh_default: 0.65 242 | map_subscribe_transient_local: True 243 | 244 | planner_server: 245 | ros__parameters: 246 | expected_planner_frequency: 20.0 247 | use_sim_time: True 248 | planner_plugins: ["GridBased"] 249 | GridBased: 250 | plugin: "nav2_navfn_planner/NavfnPlanner" 251 | tolerance: 0.5 252 | use_astar: false 253 | allow_unknown: true 254 | 255 | planner_server_rclcpp_node: 256 | ros__parameters: 257 | use_sim_time: True 258 | 259 | recoveries_server: 260 | ros__parameters: 261 | costmap_topic: local_costmap/costmap_raw 262 | footprint_topic: local_costmap/published_footprint 263 | cycle_frequency: 10.0 264 | recovery_plugins: ["spin", "backup", "wait"] 265 | spin: 266 | plugin: "nav2_recoveries/Spin" 267 | backup: 268 | plugin: "nav2_recoveries/BackUp" 269 | wait: 270 | plugin: "nav2_recoveries/Wait" 271 | global_frame: odom 272 | robot_base_frame: base_link 273 | transform_timeout: 0.1 274 | use_sim_time: true 275 | simulate_ahead_time: 2.0 276 | max_rotational_vel: 1.0 277 | min_rotational_vel: 0.4 278 | rotational_acc_lim: 3.2 279 | 280 | robot_state_publisher: 281 | ros__parameters: 282 | use_sim_time: True 283 | 284 | waypoint_follower: 285 | ros__parameters: 286 | loop_rate: 20 287 | stop_on_failure: false 288 | waypoint_task_executor_plugin: "wait_at_waypoint" 289 | wait_at_waypoint: 290 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 291 | enabled: True 292 | waypoint_pause_duration: 200 293 | -------------------------------------------------------------------------------- /marathon_ros2_csv/marathon_ros2_csv/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_csv/marathon_ros2_csv/__init__.py -------------------------------------------------------------------------------- /marathon_ros2_csv/marathon_ros2_csv/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_csv/marathon_ros2_csv/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /marathon_ros2_csv/marathon_ros2_csv/__pycache__/topics_2_csv.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_csv/marathon_ros2_csv/__pycache__/topics_2_csv.cpython-36.pyc -------------------------------------------------------------------------------- /marathon_ros2_csv/marathon_ros2_csv/plot_csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | import pandas as pd 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | import os 8 | exp_folder = "exp_3/teb_star/" 9 | url = "/home/ubuntu/exp_data/" + exp_folder 10 | n = len(os.listdir(url)) + 1 11 | numberofcsvs = range(1,n) 12 | extension = ".csv" 13 | df=pd.DataFrame(columns=["time","distance","recoveries_executed","vel_x","vel_theta", "scan_min"]) 14 | for i in numberofcsvs: 15 | url_num=url+str(i)+extension 16 | df_ = pd.read_csv(url_num, keep_default_na= False) 17 | df = pd.concat([df, df_]) 18 | 19 | df.head(100) 20 | 21 | i = 0 22 | for n in df['time'].values: 23 | df['time'].values[i] = i 24 | i = i + 1 25 | 26 | 27 | #plt.plot(df['distance'].values) #distancia por instante 28 | #plt.xlabel('Time (s)') 29 | #plt.ylabel('Distance (miles)') 30 | #plt.title('Distance traveled') 31 | #plt.savefig("figures/distance.pdf") 32 | #plt.clf() 33 | 34 | #plt.plot(df['distance'].values.cumsum()) # distancia acumulada 35 | #plt.xlabel('Time (s)') 36 | #plt.ylabel('Distance (miles)') 37 | #plt.title('Distance traveled') 38 | #plt.savefig("figures/distance_cumsum.pdf") 39 | #plt.clf() 40 | 41 | 42 | 43 | #plt.plot(df['distance'].values.cumsum(), linewidth=2) # distancia acumulada 44 | #plt.xlabel('Time (s)') 45 | #plt.ylabel('Distance (miles)') 46 | #plt.title('Distance traveled') 47 | #plt.savefig("figures/distance_cumsum.pdf") 48 | # 49 | #total_dist = df['distance'].values.cumsum() 50 | # 51 | #test = np.empty(len(total_dist), dtype=object) 52 | # 53 | #i = 0 54 | #for n in df['recovery_behavior_executed'].values: 55 | # if n == '1': 56 | # test[i] = total_dist[i] 57 | # i = i + 1 58 | # 59 | #plt.plot(test, color='black', marker='|', markersize=7) 60 | #plt.xlabel('Time (s)') 61 | #plt.ylabel('Distance (miles)') 62 | #plt.title('Recovery behavior executed on time') 63 | #plt.savefig("figures/behaviors_vs_distance.pdf") 64 | #plt.clf() 65 | 66 | #i = 0 67 | #for n in df['psi_personal'].values: 68 | # df['psi_personal'].values[i] = n * 100.0 69 | # i = i + 1 70 | 71 | print ("-------------- Extracted data from " + exp_folder + " ------------") 72 | print("Total time (s): " + str((df['time'].values[-1] - df['time'].values[0]))) 73 | print("Total distance (m): " + str(df['distance'].values.cumsum()[-1])) 74 | print("Linear vel: " + str(df['vel_x'].mean()) + " (" + str(df['vel_x'].std()) + ")") 75 | print("Linear vel max: " + str(df['vel_x'].max())) 76 | print("Min distance to an obstacle: " + str(df['scan_min'].min())) -------------------------------------------------------------------------------- /marathon_ros2_csv/marathon_ros2_csv/topics_2_csv.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import os.path 17 | 18 | import rclpy 19 | from rclpy.node import Node 20 | from rclpy.executors import MultiThreadedExecutor 21 | from rclpy.callback_groups import ReentrantCallbackGroup 22 | from rclpy.action import ActionServer, CancelResponse, GoalResponse 23 | from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy 24 | from std_msgs.msg import Float64, Empty, Int16 25 | from geometry_msgs.msg import Twist, PoseWithCovarianceStamped 26 | from sensor_msgs.msg import LaserScan 27 | import math 28 | 29 | import csv 30 | from datetime import datetime 31 | 32 | class Topics2csv(Node): 33 | def __init__(self, last_contexts=None): 34 | super().__init__('topics_2_csv') 35 | #self.distance_sub_ = self.create_subscription( 36 | # Float64, 37 | # "/marathon_ros2/distance", 38 | # self.distance_cb, 1) 39 | 40 | amcl_qos_profile = QoSProfile( 41 | depth=1, 42 | # reliability=QoSDurabilityPolicy.RELIABLE, 43 | durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) 44 | 45 | scan_qos_profile = QoSProfile( 46 | depth=1, 47 | reliability=QoSReliabilityPolicy.BEST_EFFORT, 48 | durability=QoSDurabilityPolicy.VOLATILE) 49 | 50 | self.amcl_sub_ = self.create_subscription( 51 | PoseWithCovarianceStamped, 52 | "/amcl_pose", 53 | self.amcl_cb, 1) 54 | 55 | self.scan_sub_ = self.create_subscription( 56 | LaserScan, 57 | "/scan", 58 | self.scan_cb, 59 | scan_qos_profile) 60 | 61 | self.recoveries_sub_ = self.create_subscription( 62 | Int16, 63 | "/marathon_ros2/number_of_recoveries", 64 | self.recoveries_cb, 1) 65 | 66 | self.it_sub_ = self.create_subscription( 67 | Empty, 68 | "/marathon_ros2/iteration_finished", 69 | self.it_cb, 1) 70 | 71 | self.vel_sub_ = self.create_subscription( 72 | Twist, 73 | "/nav_vel", 74 | self.vel_cb, 1) 75 | 76 | self.distance_ = 0.0 77 | self.old_x_ = 0.0 78 | self.old_y_ = 0.0 79 | self.scan_min_ = 10.0 80 | self.n_recoveries_ = 0 81 | self.vel_ = Twist() 82 | 83 | #self.get_logger().info("DF_CLIENT: Ready!") 84 | self.fieldnames_ = ['time', 'distance', 'recoveries_executed', 'vel_x', 'vel_theta', 'scan_min'] 85 | 86 | username = os.environ['USER'] 87 | self.path = "/home/" + username + "/exp_data/data/" 88 | self.update_csv_file_name() 89 | 90 | with open(self.path + self.csv_filename , mode='w+') as csv_file: 91 | writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames_) 92 | writer.writeheader() 93 | timer_period = 1.0 # seconds 94 | timer = self.create_timer(timer_period, self.step) 95 | 96 | def destroy(self): 97 | super().destroy_node() 98 | 99 | def amcl_cb(self, msg): 100 | current_x = msg.pose.pose.position.x 101 | current_y = msg.pose.pose.position.y 102 | meters = 0.0 103 | if self.old_x_ != 0.0 and self.old_y_ != 0.0: 104 | meters = self.calculate_distance(current_x, current_y, self.old_x_, self.old_y_) 105 | print ("-----------------------") 106 | print (meters) 107 | self.old_x_ = current_x 108 | self.old_y_ = current_y 109 | #miles = metersToMiles(meters_); 110 | else: 111 | self.old_x_ = current_x 112 | self.old_y_ = current_y 113 | 114 | self.distance_ = meters 115 | 116 | def scan_cb(self, msg): 117 | for measure in msg.ranges: 118 | if measure < self.scan_min_ and not math.isinf(measure): 119 | self.scan_min_ = measure 120 | 121 | def recoveries_cb(self, msg): 122 | self.n_recoveries_ = msg.data 123 | 124 | def it_cb(self, msg): 125 | self.update_csv_file_name() 126 | 127 | def vel_cb(self, msg): 128 | self.vel_ = msg 129 | 130 | def update_csv_file_name(self): 131 | filename_list = [] 132 | for file in os.listdir(self.path): 133 | filename_list.append(int(file[:-4])) 134 | filename_list_sorted = sorted(filename_list) 135 | 136 | if len(filename_list_sorted) == 0: 137 | self.csv_filename = str(1) + ".csv" 138 | else: 139 | self.last_file_number = filename_list_sorted[-1] 140 | self.csv_filename = str(self.last_file_number + 1) + ".csv" 141 | 142 | def calculate_distance(self, current_x, current_y, old_x, old_y): 143 | return math.sqrt((pow(current_x - old_x, 2) + pow(current_y - old_y, 2))) 144 | 145 | def step(self): 146 | with open(self.path + self.csv_filename, mode='a+') as csv_file: 147 | writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames_) 148 | writer.writerow({ 149 | 'time': self.get_clock().now().to_msg().sec, 150 | 'distance': self.distance_, 151 | 'recoveries_executed': self.n_recoveries_, 152 | 'vel_x': self.vel_.linear.x, 153 | 'vel_theta':self.vel_.angular.z, 154 | 'scan_min': self.scan_min_}) 155 | self.distance_ = 0.0 156 | self.n_recoveries_ = 0 157 | 158 | 159 | def main(args=None): 160 | rclpy.init(args=args) 161 | node = Topics2csv() 162 | rclpy.spin(node) 163 | node.destroy() 164 | rclpy.shutdown() 165 | 166 | if __name__ == '__main__': 167 | main() -------------------------------------------------------------------------------- /marathon_ros2_csv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | marathon_ros2_csv 5 | 0.0.1 6 | TODO: Package description 7 | jgines 8 | Apache License, Version 2.0 9 | 10 | rclpy 11 | std_msgs 12 | 13 | ament_lint_auto 14 | ament_lint_common 15 | 16 | 17 | ament_python 18 | 19 | 20 | -------------------------------------------------------------------------------- /marathon_ros2_csv/resource/marathon_ros2_csv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelligentRoboticsLabs/marathon_ros2/b331bcfed4b94d2acf09cc13581a40751e9e0f87/marathon_ros2_csv/resource/marathon_ros2_csv -------------------------------------------------------------------------------- /marathon_ros2_csv/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/marathon_ros2_csv 3 | [install] 4 | install-scripts=$base/lib/marathon_ros2_csv -------------------------------------------------------------------------------- /marathon_ros2_csv/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import find_packages 4 | from setuptools import setup 5 | 6 | package_name = 'marathon_ros2_csv' 7 | 8 | setup( 9 | name=package_name, 10 | version='0.0.1', 11 | packages=find_packages(exclude=['test']), 12 | data_files=[ 13 | ('share/ament_index/resource_index/packages', 14 | ['resource/' + package_name]), 15 | ('share/' + package_name, ['package.xml']), 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | author='Jonatan Gines', 20 | author_email='jonatan.gines@urjc.es', 21 | maintainer='Jonatan Gines', 22 | maintainer_email='jonatan.gines@urjc.es', 23 | keywords=['ROS2', 'csv'], 24 | classifiers=[ 25 | 'Intended Audience :: Developers', 26 | 'License :: OSI Approved :: Apache Software License', 27 | 'Programming Language :: Python', 28 | 'Topic :: Software Development', 29 | ], 30 | description=( 31 | 'The marathon_ros2_csv package' 32 | ), 33 | license='Apache License, Version 2.0', 34 | tests_require=['pytest'], 35 | entry_points={ 36 | 'console_scripts': [ 37 | 'topics_2_csv = marathon_ros2_csv.topics_2_csv:main' 38 | ], 39 | }, 40 | ) 41 | -------------------------------------------------------------------------------- /marathon_ros2_wp_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(marathon_ros2_wp_manager) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | # find dependencies 7 | find_package(ament_cmake REQUIRED) 8 | find_package(rclcpp REQUIRED) 9 | find_package(nav2_msgs REQUIRED) 10 | find_package(rclcpp_action REQUIRED) 11 | find_package(tf2 REQUIRED) 12 | 13 | set(dependencies 14 | rclcpp 15 | rclcpp_action 16 | nav2_msgs 17 | tf2 18 | ) 19 | 20 | add_executable(wp_manager_node src/wp_manager.cpp) 21 | ament_target_dependencies(wp_manager_node ${dependencies}) 22 | 23 | #install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 24 | #install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 25 | 26 | install(TARGETS 27 | wp_manager_node 28 | DESTINATION lib/${PROJECT_NAME} 29 | ) 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /marathon_ros2_wp_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | marathon_ros2_wp_manager 5 | 0.0.0 6 | TODO: Package description 7 | jgines 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | nav2_msgs 14 | rclcpp_action 15 | tf2 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /marathon_ros2_wp_manager/src/wp_manager.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Intelligent Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * * Neither the name of Intelligent Robotics nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Jonatan Ginés jonatan.gines@urjc.es */ 34 | 35 | /* Mantainer: Jonatan Ginés jonatan.gines@urjc.es */ 36 | 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include "rclcpp/rclcpp.hpp" 45 | #include "rclcpp_action/rclcpp_action.hpp" 46 | 47 | #include "nav2_msgs/action/navigate_to_pose.hpp" 48 | 49 | #include "geometry_msgs/msg/pose_stamped.hpp" 50 | #include "geometry_msgs/msg/pose.hpp" 51 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 52 | #include "std_msgs/msg/empty.hpp" 53 | #include "std_msgs/msg/int16.hpp" 54 | 55 | 56 | #include 57 | #include "tf2/convert.h" 58 | 59 | using namespace std::placeholders; 60 | using NavigationGoalHandle = 61 | rclcpp_action::ClientGoalHandle; 62 | 63 | class WaypointManager: public rclcpp::Node 64 | { 65 | public: 66 | explicit WaypointManager(): Node("waypoint_manager"), goal_sended_(false), starting_(false) { 67 | //waypoints_[0] = newWp(2.56, 50.6, 0.675); 68 | //waypoints_[1] = newWp(12.4365, 50.3792, 0.675); 69 | 70 | //waypoints_[0] = newWp(19.81, 46.06, 0.675); 71 | //waypoints_[1] = newWp(27.76, 55.39, 0.675); 72 | //waypoints_[2] = newWp(12.4365, 50.3792, 0.675 + M_PI/2); 73 | 74 | 75 | //waypoints_[0] = newWp(37.3, 55.4, 0.675 - M_PI/2); 76 | //waypoints_[1] = newWp(32, 63.7, 0.675 + M_PI/2); 77 | //waypoints_[2] = newWp(26.4, 58.2, 0.675 + M_PI); 78 | //waypoints_[3] = newWp(27.76, 55.39, 0.675); 79 | 80 | //waypoints_[0] = newWp(2.56, 50.6, 0.675); 81 | waypoints_[0] = newWp(12.4365, 50.3792, 0.675); 82 | waypoints_[1] = newWp(19.81, 46.06, 0.675); 83 | waypoints_[2] = newWp(27.76, 55.39, 0.675); 84 | waypoints_[3] = newWp(107.07, 7.81266, 2.56); 85 | waypoints_[4] = newWp(32, 63.7, 0.675 + M_PI/2); 86 | waypoints_[5] = newWp(26.4, 58.2, 0.675 + M_PI); 87 | 88 | // waypoints_[0] = newWp(-0-65, -1.8, 0.675); 89 | // waypoints_[1] = newWp(0.85, -1.87, 0.675); 90 | // waypoints_[2] = newWp(1.82, -0.56, 0.675); 91 | // waypoints_[3] = newWp(1.86, 0.51, 0.675); 92 | // waypoints_[4] = newWp(0.62, 1.75, 0.675 - M_PI/2); 93 | // waypoints_[5] = newWp(-0.56, 1.80, 0.675 + M_PI/2); 94 | // waypoints_[6] = newWp(-2.0, 0.48, 0.675 + M_PI); 95 | 96 | 97 | declare_parameter("next_wp", 0); 98 | get_parameter("next_wp", next_wp_); 99 | 100 | start_sub_ = create_subscription( 101 | "/start_navigate", 102 | 1, 103 | std::bind(&WaypointManager::start_cb, this, _1)); 104 | n_recoveries_pub_ = 105 | create_publisher("/marathon_ros2/number_of_recoveries", rclcpp::QoS(1)); 106 | it_finished_pub_ = 107 | create_publisher("/marathon_ros2/iteration_finished", rclcpp::QoS(1)); 108 | 109 | navigation_action_client_ = rclcpp_action::create_client( 110 | get_node_base_interface(), 111 | get_node_graph_interface(), 112 | get_node_logging_interface(), 113 | get_node_waitables_interface(), 114 | "/navigate_to_pose"); 115 | n_recoveries_ = 0; 116 | } 117 | 118 | geometry_msgs::msg::PoseStamped newWp(float x, float y, float yaw) { 119 | tf2::Quaternion q; 120 | geometry_msgs::msg::PoseStamped p; 121 | 122 | p.header.frame_id = "map"; 123 | p.pose.position.x = x; 124 | p.pose.position.y = y; 125 | q.setRPY(0, 0, yaw); 126 | q.normalize(); 127 | p.pose.orientation.x = q.x(); 128 | p.pose.orientation.y = q.y(); 129 | p.pose.orientation.z = q.z(); 130 | p.pose.orientation.w = q.w(); 131 | 132 | return p; 133 | } 134 | 135 | void feedback_callback( 136 | NavigationGoalHandle::SharedPtr, 137 | const std::shared_ptr feedback) { 138 | distance_remaining_ = feedback->distance_remaining; 139 | n_recoveries_ = feedback->number_of_recoveries; 140 | //RCLCPP_WARN(get_logger(), "distance_remaining_ %f", distance_remaining_); 141 | if (distance_remaining_ < 0.3 && now() - goal_sended_stamp_ > rclcpp::Duration::from_seconds(3.0) && goal_sended_) { 142 | RCLCPP_WARN(get_logger(), "OK Next wp"); 143 | next_wp_++; 144 | goal_sended_ = false; 145 | std_msgs::msg::Int16 msg; 146 | msg.data = n_recoveries_; 147 | n_recoveries_pub_->publish(msg); 148 | if (next_wp_ == waypoints_.size()) { 149 | next_wp_ = 0; 150 | } else if (next_wp_ == 1) { 151 | std_msgs::msg::Empty it_msg; 152 | it_finished_pub_->publish(it_msg); 153 | } 154 | } 155 | } 156 | 157 | void result_callback( const NavigationGoalHandle::WrappedResult & result) { 158 | RCLCPP_WARN(get_logger(), "result_callback"); 159 | goal_sended_ = false; 160 | } 161 | 162 | void start_cb(const std_msgs::msg::Empty::SharedPtr msg) { 163 | starting_ = true; 164 | } 165 | 166 | double getDistance(const geometry_msgs::msg::Pose & pos1, const geometry_msgs::msg::Pose & pos2) { 167 | return sqrt((pos1.position.x - pos2.position.x) * (pos1.position.x - pos2.position.x) + 168 | (pos1.position.y - pos2.position.y) * (pos1.position.y - pos2.position.y)); 169 | } 170 | 171 | 172 | void navigate_to_pose(geometry_msgs::msg::PoseStamped goal_pose) { 173 | //rclcpp::spin_some(node_->get_node_base_interface()); 174 | bool is_action_server_ready = false; 175 | 176 | do { 177 | RCLCPP_WARN(get_logger(), "Waiting for action server"); 178 | is_action_server_ready = navigation_action_client_->wait_for_action_server(std::chrono::seconds(1)); 179 | } while (!is_action_server_ready); 180 | 181 | RCLCPP_WARN(get_logger(), "Starting navigation"); 182 | 183 | goal_sended_stamp_ = now(); 184 | goal_sended_ = true; 185 | 186 | navigation_goal_.pose = goal_pose; 187 | dist_to_move = getDistance(goal_pose.pose, current_pos_); 188 | 189 | auto send_goal_options = 190 | rclcpp_action::Client::SendGoalOptions(); 191 | send_goal_options.result_callback = std::bind(&WaypointManager::result_callback, this, _1); 192 | send_goal_options.feedback_callback = std::bind(&WaypointManager::feedback_callback, this, _1, _2); 193 | 194 | goal_handle_future_ = 195 | navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); 196 | 197 | 198 | if (rclcpp::spin_until_future_complete(shared_from_this(), goal_handle_future_) != 199 | rclcpp::FutureReturnCode::SUCCESS) 200 | { 201 | RCLCPP_ERROR(get_logger(), "send goal call failed :("); 202 | goal_sended_ = false; 203 | return; 204 | } 205 | 206 | navigation_goal_handle_ = goal_handle_future_.get(); 207 | if (!navigation_goal_handle_) { 208 | goal_sended_ = false; 209 | RCLCPP_ERROR(get_logger(), "Goal was rejected by server"); 210 | } 211 | } 212 | 213 | void step() 214 | { 215 | if (!goal_sended_ && starting_) 216 | { 217 | //RCLCPP_WARN(this->get_logger(), "navigate_to_pose"); 218 | navigate_to_pose(waypoints_[next_wp_]); 219 | } 220 | 221 | } 222 | 223 | private: 224 | 225 | std::map waypoints_; 226 | nav2_msgs::action::NavigateToPose::Goal navigation_goal_; 227 | NavigationGoalHandle::SharedPtr navigation_goal_handle_; 228 | double dist_to_move; 229 | geometry_msgs::msg::Pose current_pos_; 230 | geometry_msgs::msg::PoseStamped goal_pos_; 231 | rclcpp_action::Client::SharedPtr navigation_action_client_; 232 | std::shared_future goal_handle_future_; 233 | rclcpp::Subscription::SharedPtr start_sub_; 234 | rclcpp::Publisher::SharedPtr n_recoveries_pub_; 235 | rclcpp::Publisher::SharedPtr it_finished_pub_; 236 | int next_wp_, n_recoveries_; 237 | float distance_remaining_; 238 | bool goal_sended_, starting_; 239 | rclcpp::Time goal_sended_stamp_; 240 | 241 | }; 242 | 243 | int main(int argc, char * argv[]) 244 | { 245 | rclcpp::init(argc, argv); 246 | 247 | auto node = std::make_shared(); 248 | 249 | rclcpp::Rate loop_rate(5); 250 | while (rclcpp::ok()) { 251 | node->step(); 252 | rclcpp::spin_some(node); 253 | loop_rate.sleep(); 254 | } 255 | 256 | rclcpp::shutdown(); 257 | 258 | return 0; 259 | } --------------------------------------------------------------------------------