├── .gitignore ├── README.md ├── nav2_costmap_filters_demo ├── CMakeLists.txt ├── launch │ └── costmap_filter_info.launch.py ├── maps │ ├── keepout_mask.pgm │ ├── keepout_mask.yaml │ ├── speed_mask.pgm │ └── speed_mask.yaml ├── package.xml └── params │ ├── keepout_params.yaml │ └── speed_params.yaml ├── nav2_gps_waypoint_follower_demo ├── config │ ├── demo_waypoints.yaml │ ├── dual_ekf_navsat_params.yaml │ ├── gps_wpf_demo.mvc │ └── nav2_no_map_params.yaml ├── launch │ ├── dual_ekf_navsat.launch.py │ ├── gazebo_gps_world.launch.py │ ├── gps_waypoint_follower.launch.py │ └── mapviz.launch.py ├── models │ └── turtlebot_waffle_gps │ │ ├── model.config │ │ └── model.sdf ├── nav2_gps_waypoint_follower_demo │ ├── __init__.py │ ├── gps_waypoint_logger.py │ ├── interactive_waypoint_follower.py │ ├── logged_waypoint_follower.py │ └── utils │ │ ├── __init__.py │ │ └── gps_utils.py ├── package.xml ├── resource │ └── nav2_gps_waypoint_follower_demo ├── setup.cfg ├── setup.py ├── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── urdf │ └── turtlebot3_waffle_gps.urdf └── worlds │ └── sonoma_raceway.world ├── nav2_gradient_costmap_plugin ├── CMakeLists.txt ├── gradient_layer.xml ├── include │ └── nav2_gradient_costmap_plugin │ │ └── gradient_layer.hpp ├── package.xml └── src │ └── gradient_layer.cpp ├── nav2_pure_pursuit_controller ├── CMakeLists.txt ├── README.md ├── doc │ └── lookahead_algorithm.png ├── include │ └── nav2_pure_pursuit_controller │ │ └── pure_pursuit_controller.hpp ├── nav2_pure_pursuit_controller.xml ├── package.xml └── src │ └── pure_pursuit_controller.cpp ├── nav2_sms_behavior ├── CMakeLists.txt ├── action │ └── SendSms.action ├── behavior_plugin.xml ├── include │ └── nav2_sms_behavior │ │ ├── send_sms.hpp │ │ ├── twilio.hpp │ │ └── type_conversion.hpp ├── package.xml └── src │ ├── send_sms.cpp │ └── twilio.cpp ├── nav2_straightline_planner ├── CMakeLists.txt ├── global_planner_plugin.xml ├── include │ └── nav2_straightline_planner │ │ └── straight_line_planner.hpp ├── package.xml └── src │ └── straight_line_planner.cpp └── sam_bot_description ├── CMakeLists.txt ├── README.md ├── config ├── bridge_config.yaml ├── ekf.yaml └── nav2_params.yaml ├── launch └── display.launch.py ├── package.xml ├── rviz └── config.rviz ├── src └── description │ ├── sam_bot_description.sdf │ └── sam_bot_description.urdf └── world └── my_world.sdf /.gitignore: -------------------------------------------------------------------------------- 1 | # Python artifacts 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # navigation2_tutorials 2 | Tutorial code referenced in https://docs.nav2.org/ 3 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_costmap_filters_demo) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | 8 | install(DIRECTORY launch 9 | DESTINATION share/${PROJECT_NAME} 10 | ) 11 | 12 | install(DIRECTORY maps 13 | DESTINATION share/${PROJECT_NAME} 14 | ) 15 | 16 | install(DIRECTORY params 17 | DESTINATION share/${PROJECT_NAME} 18 | ) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/launch/costmap_filter_info.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020 Samsung Research Russia 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import DeclareLaunchArgument, GroupAction 23 | from launch.conditions import IfCondition 24 | from launch.substitutions import LaunchConfiguration, PythonExpression 25 | from launch.substitutions import NotEqualsSubstitution 26 | from launch_ros.actions import Node, LoadComposableNodes 27 | from launch_ros.actions import PushRosNamespace 28 | from launch_ros.descriptions import ComposableNode 29 | from nav2_common.launch import RewrittenYaml 30 | 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | costmap_filters_demo_dir = get_package_share_directory('nav2_costmap_filters_demo') 35 | 36 | lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server'] 37 | 38 | # Parameters 39 | namespace = LaunchConfiguration('namespace') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | autostart = LaunchConfiguration('autostart') 42 | params_file = LaunchConfiguration('params_file') 43 | mask_yaml_file = LaunchConfiguration('mask') 44 | use_composition = LaunchConfiguration('use_composition') 45 | container_name = LaunchConfiguration('container_name') 46 | container_name_full = (namespace, '/', container_name) 47 | 48 | # Declare the launch arguments 49 | declare_namespace_cmd = DeclareLaunchArgument( 50 | 'namespace', 51 | default_value='', 52 | description='Top-level namespace') 53 | 54 | declare_use_sim_time_cmd = DeclareLaunchArgument( 55 | 'use_sim_time', 56 | default_value='true', 57 | description='Use simulation (Gazebo) clock if true') 58 | 59 | declare_autostart_cmd = DeclareLaunchArgument( 60 | 'autostart', default_value='true', 61 | description='Automatically startup the nav2 stack') 62 | 63 | declare_params_file_cmd = DeclareLaunchArgument( 64 | 'params_file', 65 | description='Full path to the ROS2 parameters file to use') 66 | 67 | declare_mask_yaml_file_cmd = DeclareLaunchArgument( 68 | 'mask', 69 | description='Full path to filter mask yaml file to load') 70 | 71 | declare_use_composition_cmd = DeclareLaunchArgument( 72 | 'use_composition', default_value='True', 73 | description='Use composed bringup if True') 74 | 75 | declare_container_name_cmd = DeclareLaunchArgument( 76 | 'container_name', default_value='nav2_container', 77 | description='The name of container that nodes will load in if use composition') 78 | 79 | # Make re-written yaml 80 | param_substitutions = { 81 | 'use_sim_time': use_sim_time, 82 | 'yaml_filename': mask_yaml_file} 83 | 84 | configured_params = RewrittenYaml( 85 | source_file=params_file, 86 | root_key=namespace, 87 | param_rewrites=param_substitutions, 88 | convert_types=True) 89 | 90 | load_nodes = GroupAction( 91 | condition=IfCondition(PythonExpression(['not ', use_composition])), 92 | actions=[ 93 | Node( 94 | package='nav2_map_server', 95 | executable='map_server', 96 | name='filter_mask_server', 97 | namespace=namespace, 98 | output='screen', 99 | emulate_tty=True, # https://github.com/ros2/launch/issues/188 100 | parameters=[configured_params]), 101 | Node( 102 | package='nav2_map_server', 103 | executable='costmap_filter_info_server', 104 | name='costmap_filter_info_server', 105 | namespace=namespace, 106 | output='screen', 107 | emulate_tty=True, # https://github.com/ros2/launch/issues/188 108 | parameters=[configured_params]), 109 | Node( 110 | package='nav2_lifecycle_manager', 111 | executable='lifecycle_manager', 112 | name='lifecycle_manager_costmap_filters', 113 | namespace=namespace, 114 | output='screen', 115 | emulate_tty=True, # https://github.com/ros2/launch/issues/188 116 | parameters=[{'use_sim_time': use_sim_time}, 117 | {'autostart': autostart}, 118 | {'node_names': lifecycle_nodes}]) 119 | ] 120 | ) 121 | 122 | load_composable_nodes = GroupAction( 123 | condition=IfCondition(use_composition), 124 | actions=[ 125 | PushRosNamespace( 126 | condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), 127 | namespace=namespace), 128 | LoadComposableNodes( 129 | target_container=container_name_full, 130 | composable_node_descriptions=[ 131 | ComposableNode( 132 | package='nav2_map_server', 133 | plugin='nav2_map_server::MapServer', 134 | name='filter_mask_server', 135 | parameters=[configured_params]), 136 | ComposableNode( 137 | package='nav2_map_server', 138 | plugin='nav2_map_server::CostmapFilterInfoServer', 139 | name='costmap_filter_info_server', 140 | parameters=[configured_params]), 141 | ComposableNode( 142 | package='nav2_lifecycle_manager', 143 | plugin='nav2_lifecycle_manager::LifecycleManager', 144 | name='lifecycle_manager_costmap_filters', 145 | parameters=[{'use_sim_time': use_sim_time}, 146 | {'autostart': autostart}, 147 | {'node_names': lifecycle_nodes}]), 148 | ] 149 | ) 150 | ] 151 | ) 152 | 153 | ld = LaunchDescription() 154 | 155 | ld.add_action(declare_namespace_cmd) 156 | ld.add_action(declare_use_sim_time_cmd) 157 | ld.add_action(declare_autostart_cmd) 158 | ld.add_action(declare_params_file_cmd) 159 | ld.add_action(declare_mask_yaml_file_cmd) 160 | 161 | ld.add_action(declare_use_composition_cmd) 162 | ld.add_action(declare_container_name_cmd) 163 | 164 | ld.add_action(load_nodes) 165 | ld.add_action(load_composable_nodes) 166 | 167 | return ld 168 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/maps/keepout_mask.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_costmap_filters_demo/maps/keepout_mask.pgm -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/maps/keepout_mask.yaml: -------------------------------------------------------------------------------- 1 | image: keepout_mask.pgm 2 | mode: scale 3 | resolution: 0.100000 4 | origin: [-0.450000, -0.450000, 0.000000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/maps/speed_mask.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_costmap_filters_demo/maps/speed_mask.pgm -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/maps/speed_mask.yaml: -------------------------------------------------------------------------------- 1 | image: speed_mask.pgm 2 | mode: scale 3 | resolution: 0.050000 4 | origin: [-10.000000, -10.000000, 0.000000] 5 | negate: 0 6 | occupied_thresh: 1.0 7 | free_thresh: 0.0 8 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_costmap_filters_demo 5 | 1.0.0 6 | Costmap Filters demo 7 | Alexey Merzlyakov 8 | Apache 2.0 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/params/keepout_params.yaml: -------------------------------------------------------------------------------- 1 | costmap_filter_info_server: 2 | ros__parameters: 3 | use_sim_time: true 4 | type: 0 5 | filter_info_topic: "/costmap_filter_info" 6 | mask_topic: "/keepout_filter_mask" 7 | base: 0.0 8 | multiplier: 1.0 9 | filter_mask_server: 10 | ros__parameters: 11 | use_sim_time: true 12 | frame_id: "map" 13 | topic_name: "/keepout_filter_mask" 14 | yaml_filename: "keepout_mask.yaml" 15 | -------------------------------------------------------------------------------- /nav2_costmap_filters_demo/params/speed_params.yaml: -------------------------------------------------------------------------------- 1 | costmap_filter_info_server: 2 | ros__parameters: 3 | use_sim_time: true 4 | type: 1 5 | filter_info_topic: "/costmap_filter_info" 6 | mask_topic: "/speed_filter_mask" 7 | base: 100.0 8 | multiplier: -1.0 9 | filter_mask_server: 10 | ros__parameters: 11 | use_sim_time: true 12 | frame_id: "map" 13 | topic_name: "/speed_filter_mask" 14 | yaml_filename: "speed_mask.yaml" 15 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/config/demo_waypoints.yaml: -------------------------------------------------------------------------------- 1 | waypoints: 2 | - latitude: 38.161491054181276 3 | longitude: -122.45464431092836 4 | yaw: 0.0 5 | - latitude: 38.161587576524845 6 | longitude: -122.4547994038464 7 | yaw: 1.57 8 | - latitude: 38.161708040316405 9 | longitude: -122.45499603070951 10 | yaw: 3.14 11 | - latitude: 38.16180165780551 12 | longitude: -122.45515645020123 13 | yaw: 4.71 14 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml: -------------------------------------------------------------------------------- 1 | # For parameter descriptions, please refer to the template parameter files for each node. 2 | 3 | ekf_filter_node_odom: 4 | ros__parameters: 5 | frequency: 30.0 6 | two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments 7 | print_diagnostics: true 8 | debug: false 9 | publish_tf: true 10 | 11 | map_frame: map 12 | odom_frame: odom 13 | base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin 14 | world_frame: odom 15 | 16 | odom0: odom 17 | odom0_config: [false, false, false, 18 | false, false, false, 19 | true, true, true, 20 | false, false, true, 21 | false, false, false] 22 | odom0_queue_size: 10 23 | odom0_differential: false 24 | odom0_relative: false 25 | 26 | imu0: imu 27 | imu0_config: [false, false, false, 28 | false, false, true, 29 | false, false, false, 30 | false, false, false, 31 | false, false, false] 32 | imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate 33 | imu0_relative: false 34 | imu0_queue_size: 10 35 | imu0_remove_gravitational_acceleration: true 36 | 37 | use_control: false 38 | 39 | process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 45 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 53 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] 54 | 55 | ekf_filter_node_map: 56 | ros__parameters: 57 | frequency: 30.0 58 | two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments 59 | print_diagnostics: true 60 | debug: false 61 | publish_tf: true 62 | 63 | map_frame: map 64 | odom_frame: odom 65 | base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin 66 | world_frame: map 67 | 68 | odom0: odom 69 | odom0_config: [false, false, false, 70 | false, false, false, 71 | true, true, true, 72 | false, false, true, 73 | false, false, false] 74 | odom0_queue_size: 10 75 | odom0_differential: false 76 | odom0_relative: false 77 | 78 | odom1: odometry/gps 79 | odom1_config: [true, true, false, 80 | false, false, false, 81 | false, false, false, 82 | false, false, false, 83 | false, false, false] 84 | odom1_queue_size: 10 85 | odom1_differential: false 86 | odom1_relative: false 87 | 88 | imu0: imu 89 | imu0_config: [false, false, false, 90 | false, false, true, 91 | false, false, false, 92 | false, false, false, 93 | false, false, false] 94 | imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate 95 | imu0_relative: false 96 | imu0_queue_size: 10 97 | imu0_remove_gravitational_acceleration: true 98 | 99 | use_control: false 100 | 101 | process_noise_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 102 | 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 103 | 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 104 | 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 105 | 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 106 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 107 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 108 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 109 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 110 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 111 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 112 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 113 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 114 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 115 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] 116 | 117 | navsat_transform: 118 | ros__parameters: 119 | frequency: 30.0 120 | delay: 3.0 121 | magnetic_declination_radians: 0.0 122 | yaw_offset: 0.0 123 | zero_altitude: true 124 | broadcast_utm_transform: true 125 | publish_filtered_gps: true 126 | use_odometry_yaw: true 127 | wait_for_datum: false 128 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc: -------------------------------------------------------------------------------- 1 | capture_directory: "~" 2 | fixed_frame: map 3 | target_frame: map 4 | fix_orientation: false 5 | rotate_90: false 6 | enable_antialiasing: true 7 | show_displays: true 8 | show_status_bar: true 9 | show_capture_tools: true 10 | window_width: 1920 11 | window_height: 1016 12 | view_scale: 0.564473748 13 | offset_x: -55.0469894 14 | offset_y: -4.65194941 15 | use_latest_transforms: true 16 | background: "#a0a0a4" 17 | image_transport: raw 18 | displays: 19 | - type: mapviz_plugins/tile_map 20 | name: new display 21 | config: 22 | visible: true 23 | collapsed: false 24 | custom_sources: 25 | [] 26 | bing_api_key: "" 27 | source: Bing Maps (terrain) 28 | - type: mapviz_plugins/point_click_publisher 29 | name: new display 30 | config: 31 | visible: true 32 | collapsed: false 33 | topic: clicked_point 34 | output_frame: wgs84 35 | - type: mapviz_plugins/tf_frame 36 | name: new display 37 | config: 38 | visible: true 39 | collapsed: false 40 | frame: base_link 41 | color: "#00ff00" 42 | draw_style: arrows 43 | position_tolerance: 0 44 | buffer_size: 1 45 | static_arrow_sizes: true 46 | arrow_size: 53 47 | - type: mapviz_plugins/navsat 48 | name: new display 49 | config: 50 | visible: true 51 | collapsed: false 52 | topic: /gps/fix 53 | color: "#55aaff" 54 | draw_style: points 55 | position_tolerance: 0 56 | buffer_size: 1 -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml: -------------------------------------------------------------------------------- 1 | # GPS WPF CHANGES: 2 | # - amcl params where removed. They are not needed because global localization is provided 3 | # by an ekf node on robot_localization fusing gps data with local odometry sources 4 | # - static layer is removed from both costmaps, in this tutorial we assume there is no map 5 | # of the environment 6 | # - global costmap is set to be rolling to allow the robot to traverse big environment by 7 | # following successive relatively close waypoints that fit in a smaller rolling costmap 8 | 9 | bt_navigator: 10 | ros__parameters: 11 | global_frame: map 12 | robot_base_frame: base_link 13 | odom_topic: /odom 14 | bt_loop_duration: 10 15 | default_server_timeout: 20 16 | navigators: ["navigate_to_pose", "navigate_through_poses"] 17 | navigate_to_pose: 18 | plugin: "nav2_bt_navigator/NavigateToPoseNavigator" 19 | navigate_through_poses: 20 | plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" 21 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 22 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 23 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 24 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 25 | plugin_lib_names: 26 | - nav2_compute_path_to_pose_action_bt_node 27 | - nav2_compute_path_through_poses_action_bt_node 28 | - nav2_smooth_path_action_bt_node 29 | - nav2_follow_path_action_bt_node 30 | - nav2_spin_action_bt_node 31 | - nav2_wait_action_bt_node 32 | - nav2_assisted_teleop_action_bt_node 33 | - nav2_back_up_action_bt_node 34 | - nav2_drive_on_heading_bt_node 35 | - nav2_clear_costmap_service_bt_node 36 | - nav2_is_stuck_condition_bt_node 37 | - nav2_goal_reached_condition_bt_node 38 | - nav2_goal_updated_condition_bt_node 39 | - nav2_globally_updated_goal_condition_bt_node 40 | - nav2_is_path_valid_condition_bt_node 41 | - nav2_are_error_codes_active_condition_bt_node 42 | - nav2_would_a_controller_recovery_help_condition_bt_node 43 | - nav2_would_a_planner_recovery_help_condition_bt_node 44 | - nav2_would_a_smoother_recovery_help_condition_bt_node 45 | - nav2_initial_pose_received_condition_bt_node 46 | - nav2_reinitialize_global_localization_service_bt_node 47 | - nav2_rate_controller_bt_node 48 | - nav2_distance_controller_bt_node 49 | - nav2_speed_controller_bt_node 50 | - nav2_truncate_path_action_bt_node 51 | - nav2_truncate_path_local_action_bt_node 52 | - nav2_goal_updater_node_bt_node 53 | - nav2_recovery_node_bt_node 54 | - nav2_pipeline_sequence_bt_node 55 | - nav2_round_robin_node_bt_node 56 | - nav2_transform_available_condition_bt_node 57 | - nav2_time_expired_condition_bt_node 58 | - nav2_path_expiring_timer_condition 59 | - nav2_distance_traveled_condition_bt_node 60 | - nav2_single_trigger_bt_node 61 | - nav2_goal_updated_controller_bt_node 62 | - nav2_is_battery_low_condition_bt_node 63 | - nav2_navigate_through_poses_action_bt_node 64 | - nav2_navigate_to_pose_action_bt_node 65 | - nav2_remove_passed_goals_action_bt_node 66 | - nav2_planner_selector_bt_node 67 | - nav2_controller_selector_bt_node 68 | - nav2_goal_checker_selector_bt_node 69 | - nav2_controller_cancel_bt_node 70 | - nav2_path_longer_on_approach_bt_node 71 | - nav2_wait_cancel_bt_node 72 | - nav2_spin_cancel_bt_node 73 | - nav2_back_up_cancel_bt_node 74 | - nav2_assisted_teleop_cancel_bt_node 75 | - nav2_drive_on_heading_cancel_bt_node 76 | - nav2_is_battery_charging_condition_bt_node 77 | error_code_name_prefixes: 78 | - assisted_teleop 79 | - backup 80 | - compute_path 81 | - dock_robot 82 | - drive_on_heading 83 | - follow_path 84 | - nav_thru_poses 85 | - nav_to_pose 86 | - route 87 | - spin 88 | - undock_robot 89 | - wait 90 | 91 | controller_server: 92 | ros__parameters: 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 | failure_tolerance: 0.3 98 | progress_checker_plugins: ["progress_checker"] 99 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 100 | controller_plugins: ["FollowPath"] 101 | 102 | # Progress checker parameters 103 | progress_checker: 104 | plugin: "nav2_controller::SimpleProgressChecker" 105 | required_movement_radius: 0.5 106 | movement_time_allowance: 10.0 107 | # Goal checker parameters 108 | #precise_goal_checker: 109 | # plugin: "nav2_controller::SimpleGoalChecker" 110 | # xy_goal_tolerance: 0.25 111 | # yaw_goal_tolerance: 0.25 112 | # stateful: True 113 | general_goal_checker: 114 | stateful: True 115 | plugin: "nav2_controller::SimpleGoalChecker" 116 | xy_goal_tolerance: 0.25 117 | yaw_goal_tolerance: 0.25 118 | # DWB parameters 119 | FollowPath: 120 | plugin: "dwb_core::DWBLocalPlanner" 121 | debug_trajectory_details: True 122 | min_vel_x: 0.0 123 | min_vel_y: 0.0 124 | max_vel_x: 0.26 125 | max_vel_y: 0.0 126 | max_vel_theta: 1.0 127 | min_speed_xy: 0.0 128 | max_speed_xy: 0.26 129 | min_speed_theta: 0.0 130 | # Add high threshold velocity for turtlebot 3 issue. 131 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 132 | acc_lim_x: 2.5 133 | acc_lim_y: 0.0 134 | acc_lim_theta: 3.2 135 | decel_lim_x: -2.5 136 | decel_lim_y: 0.0 137 | decel_lim_theta: -3.2 138 | vx_samples: 20 139 | vy_samples: 5 140 | vtheta_samples: 20 141 | sim_time: 1.7 142 | linear_granularity: 0.05 143 | angular_granularity: 0.025 144 | transform_tolerance: 0.2 145 | xy_goal_tolerance: 0.25 146 | trans_stopped_velocity: 0.25 147 | short_circuit_trajectory_evaluation: True 148 | stateful: True 149 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 150 | BaseObstacle.scale: 0.02 151 | PathAlign.scale: 32.0 152 | PathAlign.forward_point_distance: 0.1 153 | GoalAlign.scale: 24.0 154 | GoalAlign.forward_point_distance: 0.1 155 | PathDist.scale: 32.0 156 | GoalDist.scale: 24.0 157 | RotateToGoal.scale: 32.0 158 | RotateToGoal.slowing_factor: 5.0 159 | RotateToGoal.lookahead_time: -1.0 160 | 161 | # GPS WPF CHANGE: Remove static layer 162 | local_costmap: 163 | local_costmap: 164 | ros__parameters: 165 | update_frequency: 5.0 166 | publish_frequency: 2.0 167 | global_frame: odom 168 | robot_base_frame: base_link 169 | rolling_window: true 170 | width: 3 171 | height: 3 172 | resolution: 0.05 173 | robot_radius: 0.22 174 | plugins: ["voxel_layer", "inflation_layer"] 175 | inflation_layer: 176 | plugin: "nav2_costmap_2d::InflationLayer" 177 | cost_scaling_factor: 3.0 178 | inflation_radius: 0.55 179 | voxel_layer: 180 | plugin: "nav2_costmap_2d::VoxelLayer" 181 | enabled: True 182 | publish_voxel_map: True 183 | origin_z: 0.0 184 | z_resolution: 0.05 185 | z_voxels: 16 186 | max_obstacle_height: 2.0 187 | mark_threshold: 0 188 | observation_sources: scan 189 | scan: 190 | topic: /scan 191 | max_obstacle_height: 2.0 192 | clearing: True 193 | marking: True 194 | data_type: "LaserScan" 195 | raytrace_max_range: 3.0 196 | raytrace_min_range: 0.0 197 | obstacle_max_range: 2.5 198 | obstacle_min_range: 0.0 199 | always_send_full_costmap: True 200 | 201 | # GPS WPF CHANGE: Remove static layer 202 | # GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below 203 | global_costmap: 204 | global_costmap: 205 | ros__parameters: 206 | update_frequency: 1.0 207 | publish_frequency: 1.0 208 | global_frame: map 209 | robot_base_frame: base_link 210 | robot_radius: 0.22 211 | resolution: 0.1 212 | # When using GPS navigation you will potentially traverse huge environments which are not practical to 213 | # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to 214 | # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 215 | rolling_window: True 216 | width: 50 217 | height: 50 218 | track_unknown_space: true 219 | # no static map 220 | plugins: ["obstacle_layer", "inflation_layer"] 221 | obstacle_layer: 222 | plugin: "nav2_costmap_2d::ObstacleLayer" 223 | enabled: True 224 | observation_sources: scan 225 | scan: 226 | topic: /scan 227 | max_obstacle_height: 2.0 228 | clearing: True 229 | marking: True 230 | data_type: "LaserScan" 231 | raytrace_max_range: 3.0 232 | raytrace_min_range: 0.0 233 | obstacle_max_range: 2.5 234 | obstacle_min_range: 0.0 235 | # outdoors there will probably be more inf points 236 | inf_is_valid: true 237 | inflation_layer: 238 | plugin: "nav2_costmap_2d::InflationLayer" 239 | cost_scaling_factor: 3.0 240 | inflation_radius: 0.55 241 | always_send_full_costmap: True 242 | 243 | # The yaml_filename does not need to be specified since it going to be set by defaults in launch. 244 | # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py 245 | # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. 246 | # map_server: 247 | # ros__parameters: 248 | # yaml_filename: "" 249 | 250 | map_saver: 251 | ros__parameters: 252 | save_map_timeout: 5.0 253 | free_thresh_default: 0.25 254 | occupied_thresh_default: 0.65 255 | map_subscribe_transient_local: True 256 | 257 | planner_server: 258 | ros__parameters: 259 | expected_planner_frequency: 20.0 260 | planner_plugins: ["GridBased"] 261 | GridBased: 262 | plugin: "nav2_navfn_planner/NavfnPlanner" 263 | tolerance: 0.5 264 | use_astar: false 265 | allow_unknown: true 266 | 267 | smoother_server: 268 | ros__parameters: 269 | smoother_plugins: ["simple_smoother"] 270 | simple_smoother: 271 | plugin: "nav2_smoother::SimpleSmoother" 272 | tolerance: 1.0e-10 273 | max_its: 1000 274 | do_refinement: True 275 | 276 | behavior_server: 277 | ros__parameters: 278 | local_costmap_topic: local_costmap/costmap_raw 279 | global_costmap_topic: global_costmap/costmap_raw 280 | local_footprint_topic: local_costmap/published_footprint 281 | global_footprint_topic: global_costmap/published_footprint 282 | cycle_frequency: 10.0 283 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 284 | spin: 285 | plugin: "nav2_behaviors/Spin" 286 | backup: 287 | plugin: "nav2_behaviors/BackUp" 288 | drive_on_heading: 289 | plugin: "nav2_behaviors/DriveOnHeading" 290 | wait: 291 | plugin: "nav2_behaviors/Wait" 292 | assisted_teleop: 293 | plugin: "nav2_behaviors/AssistedTeleop" 294 | local_frame: odom 295 | global_frame: map 296 | robot_base_frame: base_link 297 | transform_tolerance: 0.1 298 | simulate_ahead_time: 2.0 299 | max_rotational_vel: 1.0 300 | min_rotational_vel: 0.4 301 | rotational_acc_lim: 3.2 302 | 303 | waypoint_follower: 304 | ros__parameters: 305 | loop_rate: 20 306 | stop_on_failure: false 307 | waypoint_task_executor_plugin: "wait_at_waypoint" 308 | wait_at_waypoint: 309 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 310 | enabled: True 311 | waypoint_pause_duration: 200 312 | 313 | velocity_smoother: 314 | ros__parameters: 315 | smoothing_frequency: 20.0 316 | scale_velocities: False 317 | feedback: "OPEN_LOOP" 318 | max_velocity: [0.26, 0.0, 1.0] 319 | min_velocity: [-0.26, 0.0, -1.0] 320 | max_accel: [2.5, 0.0, 3.2] 321 | max_decel: [-2.5, 0.0, -3.2] 322 | odom_topic: "odom" 323 | odom_duration: 0.1 324 | deadband_velocity: [0.0, 0.0, 0.0] 325 | velocity_timeout: 1.0 326 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/launch/dual_ekf_navsat.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # Licensed under the Apache License, Version 2.0 (the "License"); 3 | # you may not use this file except in compliance with the License. 4 | # You may obtain a copy of the License at 5 | # 6 | # http://www.apache.org/licenses/LICENSE-2.0 7 | # 8 | # Unless required by applicable law or agreed to in writing, software 9 | # distributed under the License is distributed on an "AS IS" BASIS, 10 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | # See the License for the specific language governing permissions and 12 | # limitations under the License. 13 | 14 | from launch import LaunchDescription 15 | from ament_index_python.packages import get_package_share_directory 16 | from launch.actions import IncludeLaunchDescription 17 | from launch.launch_description_sources import PythonLaunchDescriptionSource 18 | import launch_ros.actions 19 | import os 20 | import launch.actions 21 | 22 | 23 | def generate_launch_description(): 24 | gps_wpf_dir = get_package_share_directory( 25 | "nav2_gps_waypoint_follower_demo") 26 | rl_params_file = os.path.join( 27 | gps_wpf_dir, "config", "dual_ekf_navsat_params.yaml") 28 | 29 | return LaunchDescription( 30 | [ 31 | launch.actions.DeclareLaunchArgument( 32 | "output_final_position", default_value="false" 33 | ), 34 | launch.actions.DeclareLaunchArgument( 35 | "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" 36 | ), 37 | launch_ros.actions.Node( 38 | package="robot_localization", 39 | executable="ekf_node", 40 | name="ekf_filter_node_odom", 41 | output="screen", 42 | parameters=[rl_params_file, {"use_sim_time": True}], 43 | remappings=[("odometry/filtered", "odometry/local")], 44 | ), 45 | launch_ros.actions.Node( 46 | package="robot_localization", 47 | executable="ekf_node", 48 | name="ekf_filter_node_map", 49 | output="screen", 50 | parameters=[rl_params_file, {"use_sim_time": True}], 51 | remappings=[("odometry/filtered", "odometry/global")], 52 | ), 53 | launch_ros.actions.Node( 54 | package="robot_localization", 55 | executable="navsat_transform_node", 56 | name="navsat_transform", 57 | output="screen", 58 | parameters=[rl_params_file, {"use_sim_time": True}], 59 | remappings=[ 60 | ("imu/data", "imu/data"), 61 | ("gps/fix", "gps/fix"), 62 | ("gps/filtered", "gps/filtered"), 63 | ("odometry/gps", "odometry/gps"), 64 | ("odometry/filtered", "odometry/global"), 65 | ], 66 | ), 67 | ] 68 | ) 69 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.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 ExecuteProcess, SetEnvironmentVariable 21 | from launch_ros.actions import Node 22 | 23 | 24 | def generate_launch_description(): 25 | # Get the launch directory 26 | gps_wpf_dir = get_package_share_directory( 27 | "nav2_gps_waypoint_follower_demo") 28 | launch_dir = os.path.join(gps_wpf_dir, 'launch') 29 | world = os.path.join(gps_wpf_dir, "worlds", "sonoma_raceway.world") 30 | 31 | urdf = os.path.join(gps_wpf_dir, 'urdf', 'turtlebot3_waffle_gps.urdf') 32 | with open(urdf, 'r') as infp: 33 | robot_description = infp.read() 34 | 35 | models_dir = os.path.join(gps_wpf_dir, "models") 36 | models_dir += os.pathsep + \ 37 | f"/opt/ros/{os.getenv('ROS_DISTRO')}/share/turtlebot3_gazebo/models" 38 | set_gazebo_model_path_cmd = None 39 | 40 | if 'GAZEBO_MODEL_PATH' in os.environ: 41 | gazebo_model_path = os.environ['GAZEBO_MODEL_PATH'] + \ 42 | os.pathsep + models_dir 43 | set_gazebo_model_path_cmd = SetEnvironmentVariable( 44 | "GAZEBO_MODEL_PATH", gazebo_model_path) 45 | else: 46 | set_gazebo_model_path_cmd = SetEnvironmentVariable( 47 | "GAZEBO_MODEL_PATH", models_dir) 48 | 49 | set_tb3_model_cmd = SetEnvironmentVariable("TURTLEBOT3_MODEL", "waffle") 50 | 51 | # Specify the actions 52 | start_gazebo_server_cmd = ExecuteProcess( 53 | cmd=['gzserver', '-s', 'libgazebo_ros_init.so', 54 | '-s', 'libgazebo_ros_factory.so', world], 55 | cwd=[launch_dir], output='both') 56 | 57 | start_gazebo_client_cmd = ExecuteProcess( 58 | cmd=['gzclient'], 59 | cwd=[launch_dir], output='both') 60 | 61 | start_robot_state_publisher_cmd = Node( 62 | package='robot_state_publisher', 63 | executable='robot_state_publisher', 64 | name='robot_state_publisher', 65 | output='both', 66 | parameters=[{'robot_description': robot_description}]) 67 | 68 | # Create the launch description and populate 69 | ld = LaunchDescription() 70 | 71 | # Set gazebo up to find models properly 72 | ld.add_action(set_gazebo_model_path_cmd) 73 | ld.add_action(set_tb3_model_cmd) 74 | 75 | # simulator launch 76 | ld.add_action(start_gazebo_server_cmd) 77 | ld.add_action(start_gazebo_client_cmd) 78 | 79 | # robot state publisher launch 80 | ld.add_action(start_robot_state_publisher_cmd) 81 | 82 | return ld 83 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/launch/gps_waypoint_follower.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.substitutions import LaunchConfiguration 21 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.conditions import IfCondition 24 | from nav2_common.launch import RewrittenYaml 25 | 26 | 27 | def generate_launch_description(): 28 | # Get the launch directory 29 | bringup_dir = get_package_share_directory('nav2_bringup') 30 | gps_wpf_dir = get_package_share_directory( 31 | "nav2_gps_waypoint_follower_demo") 32 | launch_dir = os.path.join(gps_wpf_dir, 'launch') 33 | params_dir = os.path.join(gps_wpf_dir, "config") 34 | nav2_params = os.path.join(params_dir, "nav2_no_map_params.yaml") 35 | configured_params = RewrittenYaml( 36 | source_file=nav2_params, root_key="", param_rewrites="", convert_types=True 37 | ) 38 | 39 | use_rviz = LaunchConfiguration('use_rviz') 40 | use_mapviz = LaunchConfiguration('use_mapviz') 41 | 42 | declare_use_rviz_cmd = DeclareLaunchArgument( 43 | 'use_rviz', 44 | default_value='False', 45 | description='Whether to start RVIZ') 46 | 47 | declare_use_mapviz_cmd = DeclareLaunchArgument( 48 | 'use_mapviz', 49 | default_value='False', 50 | description='Whether to start mapviz') 51 | 52 | gazebo_cmd = IncludeLaunchDescription( 53 | PythonLaunchDescriptionSource( 54 | os.path.join(launch_dir, 'gazebo_gps_world.launch.py')) 55 | ) 56 | 57 | robot_localization_cmd = IncludeLaunchDescription( 58 | PythonLaunchDescriptionSource( 59 | os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')) 60 | ) 61 | 62 | navigation2_cmd = IncludeLaunchDescription( 63 | PythonLaunchDescriptionSource( 64 | os.path.join(bringup_dir, "launch", "navigation_launch.py") 65 | ), 66 | launch_arguments={ 67 | "use_sim_time": "True", 68 | "params_file": configured_params, 69 | "autostart": "True", 70 | }.items(), 71 | ) 72 | 73 | rviz_cmd = IncludeLaunchDescription( 74 | PythonLaunchDescriptionSource( 75 | os.path.join(bringup_dir, "launch", 'rviz_launch.py')), 76 | condition=IfCondition(use_rviz) 77 | ) 78 | 79 | mapviz_cmd = IncludeLaunchDescription( 80 | PythonLaunchDescriptionSource( 81 | os.path.join(launch_dir, 'mapviz.launch.py')), 82 | condition=IfCondition(use_mapviz) 83 | ) 84 | 85 | # Create the launch description and populate 86 | ld = LaunchDescription() 87 | 88 | # simulator launch 89 | ld.add_action(gazebo_cmd) 90 | 91 | # robot localization launch 92 | ld.add_action(robot_localization_cmd) 93 | 94 | # navigation2 launch 95 | ld.add_action(navigation2_cmd) 96 | 97 | # viz launch 98 | ld.add_action(declare_use_rviz_cmd) 99 | ld.add_action(rviz_cmd) 100 | ld.add_action(declare_use_mapviz_cmd) 101 | ld.add_action(mapviz_cmd) 102 | 103 | return ld 104 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/launch/mapviz.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | import launch.actions 3 | import launch.substitutions 4 | import launch_ros.actions 5 | import os 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | gps_wpf_dir = get_package_share_directory("nav2_gps_waypoint_follower_demo") 9 | mapviz_config_file = os.path.join(gps_wpf_dir, "config", "gps_wpf_demo.mvc") 10 | 11 | 12 | def generate_launch_description(): 13 | return launch.LaunchDescription([ 14 | launch_ros.actions.Node( 15 | package="mapviz", 16 | executable="mapviz", 17 | name="mapviz", 18 | parameters=[{"config": mapviz_config_file}] 19 | ), 20 | launch_ros.actions.Node( 21 | package="swri_transform_util", 22 | executable="initialize_origin.py", 23 | name="initialize_origin", 24 | remappings=[ 25 | ("fix", "gps/fix"), 26 | ], 27 | ), 28 | launch_ros.actions.Node( 29 | package="tf2_ros", 30 | executable="static_transform_publisher", 31 | name="swri_transform", 32 | arguments=["0", "0", "0", "0", "0", "0", "map", "origin"] 33 | ) 34 | ]) 35 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turtlebot_waffle_gps 5 | 2.0 6 | model.sdf 7 | 8 | 9 | Taehun Lim(Darby) 10 | thlim@robotis.com 11 | 12 | Melih Erdogan(mlherd) 13 | h.meliherdogan@gmail.com 14 | 15 | 16 | 17 | TurtleBot3 Waffle with a gps sensor 18 | 19 | 20 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 0.0 0.0 0.0 0.0 0.0 0.0 10 | 11 | 12 | 13 | 14 | 15 | 16 | -0.064 0 0.048 0 0 0 17 | 18 | 0.001 19 | 0.000 20 | 0.000 21 | 0.001 22 | 0.000 23 | 0.001 24 | 25 | 1.0 26 | 27 | 28 | 29 | -0.064 0 0.048 0 0 0 30 | 31 | 32 | 0.265 0.265 0.089 33 | 34 | 35 | 36 | 37 | 38 | -0.064 0 0 0 0 0 39 | 40 | 41 | model://turtlebot3_common/meshes/waffle_base.dae 42 | 0.001 0.001 0.001 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | true 51 | 200 52 | 53 | 54 | 55 | 56 | 0.0 57 | 2e-4 58 | 59 | 60 | 61 | 62 | 0.0 63 | 2e-4 64 | 65 | 66 | 67 | 68 | 0.0 69 | 2e-4 70 | 71 | 72 | 73 | 74 | 75 | 76 | 0.0 77 | 1.7e-2 78 | 79 | 80 | 81 | 82 | 0.0 83 | 1.7e-2 84 | 85 | 86 | 87 | 88 | 0.0 89 | 1.7e-2 90 | 91 | 92 | 93 | 94 | 95 | false 96 | 97 | 98 | ~/out:=imu 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | -0.052 0 0.111 0 0 0 108 | 109 | 0.001 110 | 0.001 111 | 0.000 112 | 0.000 113 | 0.001 114 | 0.000 115 | 0.001 116 | 117 | 0.125 118 | 119 | 120 | true 121 | 1 122 | 0 0 0 0 0 0 123 | 124 | 125 | 126 | 127 | 0.0 128 | 0.01 129 | 130 | 131 | 132 | 133 | 0.0 134 | 0.01 135 | 136 | 137 | 138 | 139 | 140 | 141 | ~/out:=/gps/fix 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | -0.064 0 0.121 0 0 0 150 | 151 | 0.001 152 | 0.000 153 | 0.000 154 | 0.001 155 | 0.000 156 | 0.001 157 | 158 | 0.125 159 | 160 | 161 | 162 | -0.052 0 0.111 0 0 0 163 | 164 | 165 | 0.0508 166 | 0.055 167 | 168 | 169 | 170 | 171 | 172 | -0.064 0 0.121 0 0 0 173 | 174 | 175 | model://turtlebot3_common/meshes/lds.dae 176 | 0.001 0.001 0.001 177 | 178 | 179 | 180 | 181 | 182 | true 183 | false 184 | -0.064 0 0.15 0 0 0 185 | 5 186 | 187 | 188 | 189 | 360 190 | 1.000000 191 | 0.000000 192 | 6.280000 193 | 194 | 195 | 196 | 0.00000 197 | 20.0 198 | 0.015000 199 | 200 | 201 | gaussian 202 | 0.0 203 | 0.01 204 | 205 | 206 | 207 | 208 | 209 | ~/out:=scan 210 | 211 | sensor_msgs/LaserScan 212 | base_scan 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 0.0 0.144 0.023 -1.57 0 0 221 | 222 | 0.001 223 | 0.000 224 | 0.000 225 | 0.001 226 | 0.000 227 | 0.001 228 | 229 | 0.1 230 | 231 | 232 | 233 | 0.0 0.144 0.023 -1.57 0 0 234 | 235 | 236 | 0.033 237 | 0.018 238 | 239 | 240 | 241 | 242 | 243 | 244 | 100000.0 245 | 100000.0 246 | 0 0 0 247 | 0.0 248 | 0.0 249 | 250 | 251 | 252 | 253 | 0 254 | 0.2 255 | 1e+5 256 | 1 257 | 0.01 258 | 0.001 259 | 260 | 261 | 262 | 263 | 264 | 265 | 0.0 0.144 0.023 0 0 0 266 | 267 | 268 | model://turtlebot3_common/meshes/tire.dae 269 | 0.001 0.001 0.001 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 0.0 -0.144 0.023 -1.57 0 0 279 | 280 | 0.001 281 | 0.000 282 | 0.000 283 | 0.001 284 | 0.000 285 | 0.001 286 | 287 | 0.1 288 | 289 | 290 | 291 | 0.0 -0.144 0.023 -1.57 0 0 292 | 293 | 294 | 0.033 295 | 0.018 296 | 297 | 298 | 299 | 300 | 301 | 302 | 100000.0 303 | 100000.0 304 | 0 0 0 305 | 0.0 306 | 0.0 307 | 308 | 309 | 310 | 311 | 0 312 | 0.2 313 | 1e+5 314 | 1 315 | 0.01 316 | 0.001 317 | 318 | 319 | 320 | 321 | 322 | 323 | 0.0 -0.144 0.023 0 0 0 324 | 325 | 326 | model://turtlebot3_common/meshes/tire.dae 327 | 0.001 0.001 0.001 328 | 329 | 330 | 331 | 332 | 333 | 334 | -0.177 -0.064 -0.004 0 0 0 335 | 336 | 0.001 337 | 338 | 0.00001 339 | 0.000 340 | 0.000 341 | 0.00001 342 | 0.000 343 | 0.00001 344 | 345 | 346 | 347 | 348 | 349 | 0.005000 350 | 351 | 352 | 353 | 354 | 355 | 0 356 | 0.2 357 | 1e+5 358 | 1 359 | 0.01 360 | 0.001 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | -0.177 0.064 -0.004 0 0 0 369 | 370 | 0.001 371 | 372 | 0.00001 373 | 0.000 374 | 0.000 375 | 0.00001 376 | 0.000 377 | 0.00001 378 | 379 | 380 | 381 | 382 | 383 | 0.005000 384 | 385 | 386 | 387 | 388 | 389 | 0 390 | 0.2 391 | 1e+5 392 | 1 393 | 0.01 394 | 0.001 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 0.069 -0.047 0.107 0 0 0 404 | 405 | 0.001 406 | 0.000 407 | 0.000 408 | 0.001 409 | 0.000 410 | 0.001 411 | 412 | 0.035 413 | 414 | 415 | 0 0.047 -0.005 0 0 0 416 | 417 | 418 | 0.008 0.130 0.022 419 | 420 | 421 | 422 | 423 | 0.069 -0.047 0.107 0 0 0 424 | 425 | 426 | 1 427 | 5 428 | 0.064 -0.047 0.107 0 0 0 429 | 430 | 431 | 432 | 433 | 434 | intel_realsense_r200_depth 435 | camera_depth_frame 436 | 0.07 437 | 0.001 438 | 5.0 439 | 440 | 441 | 442 | 443 | 444 | base_footprint 445 | base_link 446 | 0.0 0.0 0.010 0 0 0 447 | 448 | 449 | 450 | base_link 451 | imu_link 452 | -0.032 0 0.068 0 0 0 453 | 454 | 0 0 1 455 | 456 | 457 | 458 | 459 | base_link 460 | gps_link 461 | -0.05 0 0.05 0 0 0 462 | 463 | 0 0 1 464 | 465 | 466 | 467 | 468 | base_link 469 | wheel_left_link 470 | 0.0 0.144 0.023 -1.57 0 0 471 | 472 | 0 0 1 473 | 474 | 475 | 476 | 477 | base_link 478 | wheel_right_link 479 | 0.0 -0.144 0.023 -1.57 0 0 480 | 481 | 0 0 1 482 | 483 | 484 | 485 | 486 | base_link 487 | caster_back_right_link 488 | 489 | 490 | 491 | base_link 492 | caster_back_left_link 493 | 494 | 495 | 496 | base_link 497 | base_scan 498 | -0.064 0 0.121 0 0 0 499 | 500 | 0 0 1 501 | 502 | 503 | 504 | 505 | base_link 506 | camera_link 507 | 0.064 -0.065 0.094 0 0 0 508 | 509 | 0 0 1 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 519 | /tf:=tf 520 | 521 | 522 | 30 523 | 524 | 525 | wheel_left_joint 526 | wheel_right_joint 527 | 528 | 529 | 0.287 530 | 0.066 531 | 532 | 533 | 20 534 | 1.0 535 | 536 | cmd_vel 537 | 538 | 539 | 540 | true 541 | false 542 | false 543 | 544 | odom 545 | odom 546 | base_footprint 547 | 548 | 549 | 550 | 551 | 552 | 553 | ~/out:=joint_states 554 | 555 | 30 556 | wheel_left_joint 557 | wheel_right_joint 558 | 559 | 560 | 561 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/__init__.py -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/gps_waypoint_logger.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from sensor_msgs.msg import NavSatFix, Imu 4 | import yaml 5 | import os 6 | import sys 7 | import tkinter as tk 8 | from tkinter import messagebox 9 | from nav2_gps_waypoint_follower_demo.utils.gps_utils import euler_from_quaternion 10 | 11 | 12 | class GpsGuiLogger(tk.Tk, Node): 13 | """ 14 | ROS2 node to log GPS waypoints to a file 15 | """ 16 | 17 | def __init__(self, logging_file_path): 18 | tk.Tk.__init__(self) 19 | Node.__init__(self, 'gps_waypoint_logger') 20 | self.title("GPS Waypoint Logger") 21 | 22 | self.logging_file_path = logging_file_path 23 | 24 | self.gps_pose_label = tk.Label(self, text="Current Coordinates:") 25 | self.gps_pose_label.pack() 26 | self.gps_pose_textbox = tk.Label(self, text="", width=45) 27 | self.gps_pose_textbox.pack() 28 | 29 | self.log_gps_wp_button = tk.Button(self, text="Log GPS Waypoint", 30 | command=self.log_waypoint) 31 | self.log_gps_wp_button.pack() 32 | 33 | self.gps_subscription = self.create_subscription( 34 | NavSatFix, 35 | '/gps/fix', 36 | self.gps_callback, 37 | 1 38 | ) 39 | self.last_gps_position = NavSatFix() 40 | 41 | self.imu_subscription = self.create_subscription( 42 | Imu, 43 | '/imu', 44 | self.imu_callback, 45 | 1 46 | ) 47 | self.last_heading = 0.0 48 | 49 | def gps_callback(self, msg: NavSatFix): 50 | """ 51 | Callback to store the last GPS pose 52 | """ 53 | self.last_gps_position = msg 54 | self.updateTextBox() 55 | 56 | def imu_callback(self, msg: Imu): 57 | """ 58 | Callback to store the last heading 59 | """ 60 | _, _, self.last_heading = euler_from_quaternion(msg.orientation) 61 | self.updateTextBox() 62 | 63 | def updateTextBox(self): 64 | """ 65 | Function to update the GUI with the last coordinates 66 | """ 67 | self.gps_pose_textbox.config( 68 | text=f"Lat: {self.last_gps_position.latitude:.6f}, Lon: {self.last_gps_position.longitude:.6f}, yaw: {self.last_heading:.2f} rad") 69 | 70 | def log_waypoint(self): 71 | """ 72 | Function to save a new waypoint to a file 73 | """ 74 | # read existing waypoints 75 | try: 76 | with open(self.logging_file_path, 'r') as yaml_file: 77 | existing_data = yaml.safe_load(yaml_file) 78 | # in case the file does not exist, create with the new wps 79 | except FileNotFoundError: 80 | existing_data = {"waypoints": []} 81 | # if other exception, raise the warining 82 | except Exception as ex: 83 | messagebox.showerror( 84 | "Error", f"Error logging position: {str(ex)}") 85 | return 86 | 87 | # build new waypoint object 88 | data = { 89 | "latitude": self.last_gps_position.latitude, 90 | "longitude": self.last_gps_position.longitude, 91 | "yaw": self.last_heading 92 | } 93 | existing_data["waypoints"].append(data) 94 | 95 | # write updated waypoints 96 | try: 97 | with open(self.logging_file_path, 'w') as yaml_file: 98 | yaml.dump(existing_data, yaml_file, default_flow_style=False) 99 | except Exception as ex: 100 | messagebox.showerror( 101 | "Error", f"Error logging position: {str(ex)}") 102 | return 103 | 104 | messagebox.showinfo("Info", "Waypoint logged succesfully") 105 | 106 | 107 | def main(args=None): 108 | rclpy.init(args=args) 109 | 110 | # allow to pass the logging path as an argument 111 | default_yaml_file_path = os.path.expanduser("~/gps_waypoints.yaml") 112 | if len(sys.argv) > 1: 113 | yaml_file_path = sys.argv[1] 114 | else: 115 | yaml_file_path = default_yaml_file_path 116 | 117 | gps_gui_logger = GpsGuiLogger(yaml_file_path) 118 | 119 | while rclpy.ok(): 120 | # we spin both the ROS system and the interface 121 | rclpy.spin_once(gps_gui_logger, timeout_sec=0.1) # Run ros2 callbacks 122 | gps_gui_logger.update() # Update the tkinter interface 123 | 124 | rclpy.shutdown() 125 | 126 | 127 | if __name__ == '__main__': 128 | main() 129 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/interactive_waypoint_follower.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from nav2_simple_commander.robot_navigator import BasicNavigator 4 | from geometry_msgs.msg import PointStamped 5 | from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose 6 | 7 | 8 | class InteractiveGpsWpCommander(Node): 9 | """ 10 | ROS2 node to send gps waypoints to nav2 received from mapviz's point click publisher 11 | """ 12 | 13 | def __init__(self): 14 | super().__init__(node_name="gps_wp_commander") 15 | self.navigator = BasicNavigator("basic_navigator") 16 | 17 | self.mapviz_wp_sub = self.create_subscription( 18 | PointStamped, "/clicked_point", self.mapviz_wp_cb, 1) 19 | 20 | def mapviz_wp_cb(self, msg: PointStamped): 21 | """ 22 | clicked point callback, sends received point to nav2 gps waypoint follower if its a geographic point 23 | """ 24 | if msg.header.frame_id != "wgs84": 25 | self.get_logger().warning( 26 | "Received point from mapviz that ist not in wgs84 frame. This is not a gps point and wont be followed") 27 | return 28 | 29 | self.navigator.waitUntilNav2Active(localizer='robot_localization') 30 | wp = [latLonYaw2Geopose(msg.point.y, msg.point.x)] 31 | self.navigator.followGpsWaypoints(wp) 32 | if (self.navigator.isTaskComplete()): 33 | self.get_logger().info("wps completed successfully") 34 | 35 | 36 | def main(): 37 | rclpy.init() 38 | gps_wpf = InteractiveGpsWpCommander() 39 | rclpy.spin(gps_wpf) 40 | 41 | 42 | if __name__ == "__main__": 43 | main() 44 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/logged_waypoint_follower.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from nav2_simple_commander.robot_navigator import BasicNavigator 3 | import yaml 4 | from ament_index_python.packages import get_package_share_directory 5 | import os 6 | import sys 7 | import time 8 | 9 | from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose 10 | 11 | 12 | class YamlWaypointParser: 13 | """ 14 | Parse a set of gps waypoints from a yaml file 15 | """ 16 | 17 | def __init__(self, wps_file_path: str) -> None: 18 | with open(wps_file_path, 'r') as wps_file: 19 | self.wps_dict = yaml.safe_load(wps_file) 20 | 21 | def get_wps(self): 22 | """ 23 | Get an array of geographic_msgs/msg/GeoPose objects from the yaml file 24 | """ 25 | gepose_wps = [] 26 | for wp in self.wps_dict["waypoints"]: 27 | latitude, longitude, yaw = wp["latitude"], wp["longitude"], wp["yaw"] 28 | gepose_wps.append(latLonYaw2Geopose(latitude, longitude, yaw)) 29 | return gepose_wps 30 | 31 | 32 | class GpsWpCommander(): 33 | """ 34 | Class to use nav2 gps waypoint follower to follow a set of waypoints logged in a yaml file 35 | """ 36 | 37 | def __init__(self, wps_file_path): 38 | self.navigator = BasicNavigator("basic_navigator") 39 | self.wp_parser = YamlWaypointParser(wps_file_path) 40 | 41 | def start_wpf(self): 42 | """ 43 | Function to start the waypoint following 44 | """ 45 | self.navigator.waitUntilNav2Active(localizer='robot_localization') 46 | wps = self.wp_parser.get_wps() 47 | self.navigator.followGpsWaypoints(wps) 48 | while (not self.navigator.isTaskComplete()): 49 | time.sleep(0.1) 50 | print("wps completed successfully") 51 | 52 | 53 | def main(): 54 | rclpy.init() 55 | 56 | # allow to pass the waypoints file as an argument 57 | default_yaml_file_path = os.path.join(get_package_share_directory( 58 | "nav2_gps_waypoint_follower_demo"), "config", "demo_waypoints.yaml") 59 | if len(sys.argv) > 1: 60 | yaml_file_path = sys.argv[1] 61 | else: 62 | yaml_file_path = default_yaml_file_path 63 | 64 | gps_wpf = GpsWpCommander(yaml_file_path) 65 | gps_wpf.start_wpf() 66 | 67 | 68 | if __name__ == "__main__": 69 | main() 70 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/utils/__init__.py -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/utils/gps_utils.py: -------------------------------------------------------------------------------- 1 | import math 2 | from geographic_msgs.msg import GeoPose 3 | from geometry_msgs.msg import Quaternion 4 | 5 | 6 | def quaternion_from_euler(roll, pitch, yaw): 7 | """ 8 | Converts euler roll, pitch, yaw to quaternion 9 | """ 10 | cy = math.cos(yaw * 0.5) 11 | sy = math.sin(yaw * 0.5) 12 | cp = math.cos(pitch * 0.5) 13 | sp = math.sin(pitch * 0.5) 14 | cr = math.cos(roll * 0.5) 15 | sr = math.sin(roll * 0.5) 16 | 17 | q = Quaternion() 18 | q.w = cy * cp * cr + sy * sp * sr 19 | q.x = cy * cp * sr - sy * sp * cr 20 | q.y = sy * cp * sr + cy * sp * cr 21 | q.z = sy * cp * cr - cy * sp * sr 22 | return q 23 | 24 | 25 | def euler_from_quaternion(q: Quaternion): 26 | """ 27 | Convert a quaternion into euler angles 28 | taken from: https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/ 29 | """ 30 | t0 = +2.0 * (q.w * q.x + q.y * q.z) 31 | t1 = +1.0 - 2.0 * (q.x * q.x + q.y * q.y) 32 | roll_x = math.atan2(t0, t1) 33 | 34 | t2 = +2.0 * (q.w * q.y - q.z * q.x) 35 | t2 = +1.0 if t2 > +1.0 else t2 36 | t2 = -1.0 if t2 < -1.0 else t2 37 | pitch_y = math.asin(t2) 38 | 39 | t3 = +2.0 * (q.w * q.z + q.x * q.y) 40 | t4 = +1.0 - 2.0 * (q.y * q.y + q.z * q.z) 41 | yaw_z = math.atan2(t3, t4) 42 | 43 | return roll_x, pitch_y, yaw_z 44 | 45 | 46 | def latLonYaw2Geopose(latitude: float, longitude: float, yaw: float = 0.0) -> GeoPose: 47 | """ 48 | Creates a geographic_msgs/msg/GeoPose object from latitude, longitude and yaw 49 | """ 50 | geopose = GeoPose() 51 | geopose.position.latitude = latitude 52 | geopose.position.longitude = longitude 53 | geopose.orientation = quaternion_from_euler(0.0, 0.0, yaw) 54 | return geopose 55 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_gps_waypoint_follower_demo 5 | 0.0.0 6 | Demo package for following GPS waypoints with nav2 7 | pepisg 8 | Steve Macenski 9 | MIT 10 | 11 | navigation2 12 | robot_localization 13 | mapviz 14 | mapviz_plugins 15 | tile_map 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/resource/nav2_gps_waypoint_follower_demo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_gps_waypoint_follower_demo/resource/nav2_gps_waypoint_follower_demo -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/nav2_gps_waypoint_follower_demo 3 | [install] 4 | install_scripts=$base/lib/nav2_gps_waypoint_follower_demo 5 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | import os 3 | from glob import glob 4 | 5 | package_name = 'nav2_gps_waypoint_follower_demo' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.1', 10 | packages=find_packages(exclude=['test']), 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), 16 | (os.path.join('share', package_name, 'config'), glob('config/*')), 17 | (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), 18 | (os.path.join('share', package_name, 'worlds'), glob('worlds/*')), 19 | (os.path.join('share', package_name, 'models/turtlebot_waffle_gps'), 20 | glob('models/turtlebot_waffle_gps/*')), 21 | ], 22 | install_requires=['setuptools'], 23 | zip_safe=True, 24 | maintainer='root', 25 | maintainer_email='pedro.gonzalez@eia.edu.co', 26 | description='Demo package for following GPS waypoints with nav2', 27 | license='MIT', 28 | tests_require=['pytest'], 29 | entry_points={ 30 | 'console_scripts': [ 31 | 'logged_waypoint_follower = nav2_gps_waypoint_follower_demo.logged_waypoint_follower:main', 32 | 'interactive_waypoint_follower = nav2_gps_waypoint_follower_demo.interactive_waypoint_follower:main', 33 | 'gps_waypoint_logger = nav2_gps_waypoint_follower_demo.gps_waypoint_logger:main' 34 | ], 35 | }, 36 | ) 37 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 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 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 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 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 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 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/urdf/turtlebot3_waffle_gps.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | -------------------------------------------------------------------------------- /nav2_gps_waypoint_follower_demo/worlds/sonoma_raceway.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.95 0.95 0.95 1 7 | 0.3 0.3 0.3 1 8 | true 9 | 10 | 11 | 3 12 | 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | model://sonoma_raceway 20 | -287.5 143.5 -7 0 0 0 21 | 22 | 23 | 27 | EARTH_WGS84 28 | 38.161479 29 | -122.454630 30 | 488.0 31 | 180 32 | 33 | 34 | 35 | 100.0 36 | 0.01 37 | 1 38 | 39 | 40 | quick 41 | 150 42 | 0 43 | 1.400000 44 | 1 45 | 46 | 47 | 0.00001 48 | 0.2 49 | 2000.000000 50 | 0.01000 51 | 52 | 53 | 54 | 55 | 56 | model://turtlebot_waffle_gps 57 | 2 -2.5 0.3 0 0 0 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /nav2_gradient_costmap_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_gradient_costmap_plugin) 3 | 4 | set(lib_name ${PROJECT_NAME}_core) 5 | 6 | # === Environment === 7 | 8 | # Default to C99 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 99) 11 | endif() 12 | 13 | # Default to C++14 14 | if(NOT CMAKE_CXX_STANDARD) 15 | set(CMAKE_CXX_STANDARD 14) 16 | endif() 17 | 18 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 19 | add_compile_options(-Wall -Wextra -Wpedantic) 20 | endif() 21 | 22 | # === Dependencies === 23 | 24 | find_package(ament_cmake REQUIRED) 25 | find_package(rclcpp REQUIRED) 26 | find_package(nav2_costmap_2d REQUIRED) 27 | find_package(pluginlib REQUIRED) 28 | 29 | set(dep_pkgs 30 | rclcpp 31 | nav2_costmap_2d 32 | pluginlib) 33 | 34 | # === Build === 35 | 36 | add_library(${lib_name} SHARED 37 | src/gradient_layer.cpp) 38 | include_directories(include) 39 | 40 | # === Installation === 41 | 42 | install(TARGETS ${lib_name} 43 | DESTINATION lib) 44 | 45 | # === Ament work === 46 | 47 | # pluginlib_export_plugin_description_file() installs gradient_layer.xml 48 | # file into "share" directory and sets ament indexes for it. 49 | # This allows the plugin to be discovered as a plugin of required type. 50 | pluginlib_export_plugin_description_file(nav2_costmap_2d gradient_layer.xml) 51 | ament_target_dependencies(${lib_name} ${dep_pkgs}) 52 | ament_package() 53 | -------------------------------------------------------------------------------- /nav2_gradient_costmap_plugin/gradient_layer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is an example plugin which puts repeating costs gradients to costmap 4 | 5 | 6 | -------------------------------------------------------------------------------- /nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * Copyright (c) 2020, Samsung R&D Institute Russia 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Eitan Marder-Eppstein 37 | * David V. Lu!! 38 | * Alexey Merzlyakov 39 | * 40 | * Reference tutorial: 41 | * https://navigation.ros.org/tutorials/docs/writing_new_costmap2d_plugin.html 42 | *********************************************************************/ 43 | #ifndef GRADIENT_LAYER_HPP_ 44 | #define GRADIENT_LAYER_HPP_ 45 | 46 | #include "rclcpp/rclcpp.hpp" 47 | #include "nav2_costmap_2d/layer.hpp" 48 | #include "nav2_costmap_2d/layered_costmap.hpp" 49 | 50 | namespace nav2_gradient_costmap_plugin 51 | { 52 | 53 | class GradientLayer : public nav2_costmap_2d::Layer 54 | { 55 | public: 56 | GradientLayer(); 57 | 58 | virtual void onInitialize(); 59 | virtual void updateBounds( 60 | double robot_x, double robot_y, double robot_yaw, double * min_x, 61 | double * min_y, 62 | double * max_x, 63 | double * max_y); 64 | virtual void updateCosts( 65 | nav2_costmap_2d::Costmap2D & master_grid, 66 | int min_i, int min_j, int max_i, int max_j); 67 | 68 | virtual void reset() 69 | { 70 | return; 71 | } 72 | 73 | virtual void onFootprintChanged(); 74 | 75 | virtual bool isClearable() {return false;} 76 | 77 | private: 78 | double last_min_x_, last_min_y_, last_max_x_, last_max_y_; 79 | 80 | // Indicates that the entire gradient should be recalculated next time. 81 | bool need_recalculation_; 82 | 83 | // Size of gradient in cells 84 | int GRADIENT_SIZE = 20; 85 | // Step of increasing cost per one cell in gradient 86 | int GRADIENT_FACTOR = 10; 87 | }; 88 | 89 | } // namespace nav2_gradient_costmap_plugin 90 | 91 | #endif // GRADIENT_LAYER_HPP_ 92 | -------------------------------------------------------------------------------- /nav2_gradient_costmap_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_gradient_costmap_plugin 5 | 1.0.0 6 | Run-time plugin for Costmap2D gradient later 7 | Alexey Merzlyakov 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | nav2_costmap_2d 16 | pluginlib 17 | rclcpp 18 | 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /nav2_gradient_costmap_plugin/src/gradient_layer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 6 | * Copyright (c) 2020, Samsung R&D Institute Russia 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Eitan Marder-Eppstein 37 | * David V. Lu!! 38 | * Alexey Merzlyakov 39 | * 40 | * Reference tutorial: 41 | * https://navigation.ros.org/tutorials/docs/writing_new_costmap2d_plugin.html 42 | *********************************************************************/ 43 | #include "nav2_gradient_costmap_plugin/gradient_layer.hpp" 44 | 45 | #include "nav2_costmap_2d/costmap_math.hpp" 46 | #include "nav2_costmap_2d/footprint.hpp" 47 | #include "rclcpp/parameter_events_filter.hpp" 48 | 49 | using nav2_costmap_2d::LETHAL_OBSTACLE; 50 | using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; 51 | using nav2_costmap_2d::NO_INFORMATION; 52 | 53 | namespace nav2_gradient_costmap_plugin 54 | { 55 | 56 | GradientLayer::GradientLayer() 57 | : last_min_x_(-std::numeric_limits::max()), 58 | last_min_y_(-std::numeric_limits::max()), 59 | last_max_x_(std::numeric_limits::max()), 60 | last_max_y_(std::numeric_limits::max()) 61 | { 62 | } 63 | 64 | // This method is called at the end of plugin initialization. 65 | // It contains ROS parameter(s) declaration and initialization 66 | // of need_recalculation_ variable. 67 | void 68 | GradientLayer::onInitialize() 69 | { 70 | auto node = node_.lock(); 71 | declareParameter("enabled", rclcpp::ParameterValue(true)); 72 | node->get_parameter(name_ + "." + "enabled", enabled_); 73 | 74 | need_recalculation_ = false; 75 | current_ = true; 76 | } 77 | 78 | // The method is called to ask the plugin: which area of costmap it needs to update. 79 | // Inside this method window bounds are re-calculated if need_recalculation_ is true 80 | // and updated independently on its value. 81 | void 82 | GradientLayer::updateBounds( 83 | double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, 84 | double * min_y, double * max_x, double * max_y) 85 | { 86 | if (need_recalculation_) { 87 | last_min_x_ = *min_x; 88 | last_min_y_ = *min_y; 89 | last_max_x_ = *max_x; 90 | last_max_y_ = *max_y; 91 | // For some reason when I make these -::max() it does not 92 | // work with Costmap2D::worldToMapEnforceBounds(), so I'm using 93 | // -::max() instead. 94 | *min_x = -std::numeric_limits::max(); 95 | *min_y = -std::numeric_limits::max(); 96 | *max_x = std::numeric_limits::max(); 97 | *max_y = std::numeric_limits::max(); 98 | need_recalculation_ = false; 99 | } else { 100 | double tmp_min_x = last_min_x_; 101 | double tmp_min_y = last_min_y_; 102 | double tmp_max_x = last_max_x_; 103 | double tmp_max_y = last_max_y_; 104 | last_min_x_ = *min_x; 105 | last_min_y_ = *min_y; 106 | last_max_x_ = *max_x; 107 | last_max_y_ = *max_y; 108 | *min_x = std::min(tmp_min_x, *min_x); 109 | *min_y = std::min(tmp_min_y, *min_y); 110 | *max_x = std::max(tmp_max_x, *max_x); 111 | *max_y = std::max(tmp_max_y, *max_y); 112 | } 113 | } 114 | 115 | // The method is called when footprint was changed. 116 | // Here it just resets need_recalculation_ variable. 117 | void 118 | GradientLayer::onFootprintChanged() 119 | { 120 | need_recalculation_ = true; 121 | 122 | RCLCPP_DEBUG(rclcpp::get_logger( 123 | "nav2_costmap_2d"), "GradientLayer::onFootprintChanged(): num footprint points: %lu", 124 | layered_costmap_->getFootprint().size()); 125 | } 126 | 127 | // The method is called when costmap recalculation is required. 128 | // It updates the costmap within its window bounds. 129 | // Inside this method the costmap gradient is generated and is writing directly 130 | // to the resulting costmap master_grid without any merging with previous layers. 131 | void 132 | GradientLayer::updateCosts( 133 | nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, 134 | int max_i, 135 | int max_j) 136 | { 137 | if (!enabled_) { 138 | return; 139 | } 140 | 141 | // master_array - is a direct pointer to the resulting master_grid. 142 | // master_grid - is a resulting costmap combined from all layers. 143 | // By using this pointer all layers will be overwritten! 144 | // To work with costmap layer and merge it with other costmap layers, 145 | // please use costmap_ pointer instead (this is pointer to current 146 | // costmap layer grid) and then call one of updates methods: 147 | // - updateWithAddition() 148 | // - updateWithMax() 149 | // - updateWithOverwrite() 150 | // - updateWithTrueOverwrite() 151 | // In this case using master_array pointer is equal to modifying local costmap_ 152 | // pointer and then calling updateWithTrueOverwrite(): 153 | unsigned char * master_array = master_grid.getCharMap(); 154 | unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); 155 | 156 | // {min_i, min_j} - {max_i, max_j} - are update-window coordinates. 157 | // These variables are used to update the costmap only within this window 158 | // avoiding the updates of whole area. 159 | // 160 | // Fixing window coordinates with map size if necessary. 161 | min_i = std::max(0, min_i); 162 | min_j = std::max(0, min_j); 163 | max_i = std::min(static_cast(size_x), max_i); 164 | max_j = std::min(static_cast(size_y), max_j); 165 | 166 | // Simply computing one-by-one cost per each cell 167 | int gradient_index; 168 | for (int j = min_j; j < max_j; j++) { 169 | // Reset gradient_index each time when reaching the end of re-calculated window 170 | // by OY axis. 171 | gradient_index = 0; 172 | for (int i = min_i; i < max_i; i++) { 173 | int index = master_grid.getIndex(i, j); 174 | // setting the gradient cost 175 | unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255; 176 | if (gradient_index <= GRADIENT_SIZE) { 177 | gradient_index++; 178 | } else { 179 | gradient_index = 0; 180 | } 181 | master_array[index] = cost; 182 | } 183 | } 184 | } 185 | 186 | } // namespace nav2_gradient_costmap_plugin 187 | 188 | // This is the macro allowing a nav2_gradient_costmap_plugin::GradientLayer class 189 | // to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer. 190 | // Usually places in the end of cpp-file where the loadable class written. 191 | #include "pluginlib/class_list_macros.hpp" 192 | PLUGINLIB_EXPORT_CLASS(nav2_gradient_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer) 193 | -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_pure_pursuit_controller) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(nav2_core REQUIRED) 7 | find_package(nav2_costmap_2d REQUIRED) 8 | find_package(nav2_util REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(nav_msgs REQUIRED) 12 | find_package(pluginlib REQUIRED) 13 | find_package(tf2 REQUIRED) 14 | 15 | nav2_package() 16 | 17 | include_directories( 18 | include 19 | ) 20 | 21 | set(dependencies 22 | rclcpp 23 | geometry_msgs 24 | nav2_costmap_2d 25 | pluginlib 26 | nav_msgs 27 | nav2_util 28 | nav2_core 29 | tf2 30 | ) 31 | 32 | add_library(nav2_pure_pursuit_controller SHARED 33 | src/pure_pursuit_controller.cpp) 34 | 35 | # prevent pluginlib from using boost 36 | target_compile_definitions(nav2_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 37 | 38 | ament_target_dependencies(nav2_pure_pursuit_controller 39 | ${dependencies} 40 | ) 41 | 42 | install(TARGETS nav2_pure_pursuit_controller 43 | ARCHIVE DESTINATION lib 44 | LIBRARY DESTINATION lib 45 | RUNTIME DESTINATION bin 46 | ) 47 | 48 | install(DIRECTORY include/ 49 | DESTINATION include/ 50 | ) 51 | 52 | if(BUILD_TESTING) 53 | find_package(ament_lint_auto REQUIRED) 54 | # the following line skips the linter which checks for copyrights 55 | set(ament_cmake_copyright_FOUND TRUE) 56 | ament_lint_auto_find_test_dependencies() 57 | endif() 58 | 59 | ament_export_include_directories(include) 60 | ament_export_libraries(nav2_pure_pursuit_controller) 61 | ament_export_dependencies(${dependencies}) 62 | 63 | pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml) 64 | 65 | ament_package() 66 | 67 | -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Pure pursuit controller 2 | Tutorial code referenced in https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html 3 | 4 | This controller implements a the pure pursuit algorithm to track a path. 5 | 6 | ## How the algorithm works 7 | The global path is continuously pruned to the closest point to the robot (see the figure below). 8 | Then the path is transformed to the robot frame and a lookahead point is determined. 9 | This lookahead point will be given to the pure pursuite algorithm to calculate a command velocity. 10 | 11 | ![Lookahead algorithm](./doc/lookahead_algorithm.png) 12 | -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/doc/lookahead_algorithm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_tutorials/8ddf0d6690d9195751990bd7e6449bd0fe350349/nav2_pure_pursuit_controller/doc/lookahead_algorithm.png -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-License-Identifier: BSD-3-Clause 3 | * 4 | * Author(s): Shrijit Singh 5 | * 6 | */ 7 | 8 | #ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ 9 | #define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "nav2_core/controller.hpp" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "pluginlib/class_loader.hpp" 18 | #include "pluginlib/class_list_macros.hpp" 19 | 20 | namespace nav2_pure_pursuit_controller 21 | { 22 | 23 | class PurePursuitController : public nav2_core::Controller 24 | { 25 | public: 26 | PurePursuitController() = default; 27 | ~PurePursuitController() override = default; 28 | 29 | void configure( 30 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 31 | std::string name, const std::shared_ptr tf, 32 | const std::shared_ptr costmap_ros) override; 33 | 34 | 35 | void cleanup() override; 36 | void activate() override; 37 | void deactivate() override; 38 | void setSpeedLimit(const double & speed_limit, const bool & percentage) override; 39 | 40 | geometry_msgs::msg::TwistStamped computeVelocityCommands( 41 | const geometry_msgs::msg::PoseStamped & pose, 42 | const geometry_msgs::msg::Twist & velocity, 43 | nav2_core::GoalChecker * goal_checker) override; 44 | 45 | void setPlan(const nav_msgs::msg::Path & path) override; 46 | 47 | protected: 48 | nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); 49 | 50 | bool transformPose( 51 | const std::shared_ptr tf, 52 | const std::string frame, 53 | const geometry_msgs::msg::PoseStamped & in_pose, 54 | geometry_msgs::msg::PoseStamped & out_pose, 55 | const rclcpp::Duration & transform_tolerance 56 | ) const; 57 | 58 | rclcpp_lifecycle::LifecycleNode::WeakPtr node_; 59 | std::shared_ptr tf_; 60 | std::string plugin_name_; 61 | std::shared_ptr costmap_ros_; 62 | rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")}; 63 | rclcpp::Clock::SharedPtr clock_; 64 | 65 | double desired_linear_vel_; 66 | double lookahead_dist_; 67 | double max_angular_vel_; 68 | rclcpp::Duration transform_tolerance_ {0, 0}; 69 | 70 | nav_msgs::msg::Path global_plan_; 71 | std::shared_ptr> global_pub_; 72 | }; 73 | 74 | } // namespace nav2_pure_pursuit_controller 75 | 76 | #endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | nav2_pure_pursuit_controller 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_pure_pursuit_controller 5 | 1.0.0 6 | Pure pursuit controller 7 | Shrijit Singh 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | nav2_common 13 | nav2_core 14 | nav2_util 15 | nav2_costmap_2d 16 | rclcpp 17 | geometry_msgs 18 | nav2_msgs 19 | pluginlib 20 | tf2 21 | 22 | nav2_bringup 23 | 24 | ament_cmake_gtest 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-License-Identifier: BSD-3-Clause 3 | * 4 | * Author(s): Shrijit Singh 5 | * Contributor: Pham Cong Trang 6 | * Contributor: Mitchell Sayer 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "nav2_core/controller_exceptions.hpp" 14 | #include "nav2_core/planner_exceptions.hpp" 15 | #include "nav2_util/node_utils.hpp" 16 | #include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp" 17 | #include "nav2_util/geometry_utils.hpp" 18 | 19 | using std::hypot; 20 | using std::min; 21 | using std::max; 22 | using std::abs; 23 | using nav2_util::declare_parameter_if_not_declared; 24 | using nav2_util::geometry_utils::euclidean_distance; 25 | 26 | namespace nav2_pure_pursuit_controller 27 | { 28 | 29 | /** 30 | * Find element in iterator with the minimum calculated value 31 | */ 32 | template 33 | Iter min_by(Iter begin, Iter end, Getter getCompareVal) 34 | { 35 | if (begin == end) { 36 | return end; 37 | } 38 | auto lowest = getCompareVal(*begin); 39 | Iter lowest_it = begin; 40 | for (Iter it = ++begin; it != end; ++it) { 41 | auto comp = getCompareVal(*it); 42 | if (comp < lowest) { 43 | lowest = comp; 44 | lowest_it = it; 45 | } 46 | } 47 | return lowest_it; 48 | } 49 | 50 | void PurePursuitController::configure( 51 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 52 | std::string name, const std::shared_ptr tf, 53 | const std::shared_ptr costmap_ros) 54 | { 55 | node_ = parent; 56 | 57 | auto node = node_.lock(); 58 | 59 | costmap_ros_ = costmap_ros; 60 | tf_ = tf; 61 | plugin_name_ = name; 62 | logger_ = node->get_logger(); 63 | clock_ = node->get_clock(); 64 | 65 | declare_parameter_if_not_declared( 66 | node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue( 67 | 0.2)); 68 | declare_parameter_if_not_declared( 69 | node, plugin_name_ + ".lookahead_dist", 70 | rclcpp::ParameterValue(0.4)); 71 | declare_parameter_if_not_declared( 72 | node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue( 73 | 1.0)); 74 | declare_parameter_if_not_declared( 75 | node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue( 76 | 0.1)); 77 | 78 | node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); 79 | node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); 80 | node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_); 81 | double transform_tolerance; 82 | node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); 83 | transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); 84 | 85 | global_pub_ = node->create_publisher("received_global_plan", 1); 86 | } 87 | 88 | void PurePursuitController::cleanup() 89 | { 90 | RCLCPP_INFO( 91 | logger_, 92 | "Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController", 93 | plugin_name_.c_str()); 94 | global_pub_.reset(); 95 | } 96 | 97 | void PurePursuitController::activate() 98 | { 99 | RCLCPP_INFO( 100 | logger_, 101 | "Activating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", 102 | plugin_name_.c_str(),plugin_name_.c_str()); 103 | global_pub_->on_activate(); 104 | } 105 | 106 | void PurePursuitController::deactivate() 107 | { 108 | RCLCPP_INFO( 109 | logger_, 110 | "Dectivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", 111 | plugin_name_.c_str(),plugin_name_.c_str()); 112 | global_pub_->on_deactivate(); 113 | } 114 | 115 | void PurePursuitController::setSpeedLimit(const double& speed_limit, const bool& percentage) 116 | { 117 | (void) speed_limit; 118 | (void) percentage; 119 | } 120 | 121 | geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( 122 | const geometry_msgs::msg::PoseStamped & pose, 123 | const geometry_msgs::msg::Twist & velocity, 124 | nav2_core::GoalChecker * goal_checker) 125 | { 126 | (void)velocity; 127 | (void)goal_checker; 128 | 129 | auto transformed_plan = transformGlobalPlan(pose); 130 | 131 | // Find the first pose which is at a distance greater than the specified lookahed distance 132 | auto goal_pose_it = std::find_if( 133 | transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { 134 | return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist_; 135 | }); 136 | 137 | // If the last pose is still within lookahed distance, take the last pose 138 | if (goal_pose_it == transformed_plan.poses.end()) { 139 | goal_pose_it = std::prev(transformed_plan.poses.end()); 140 | } 141 | auto goal_pose = goal_pose_it->pose; 142 | 143 | double linear_vel, angular_vel; 144 | 145 | // If the goal pose is in front of the robot then compute the velocity using the pure pursuit 146 | // algorithm, else rotate with the max angular velocity until the goal pose is in front of the 147 | // robot 148 | if (goal_pose.position.x > 0) { 149 | auto curvature = 2.0 * goal_pose.position.y / 150 | (goal_pose.position.x * goal_pose.position.x + goal_pose.position.y * goal_pose.position.y); 151 | linear_vel = desired_linear_vel_; 152 | angular_vel = desired_linear_vel_ * curvature; 153 | } else { 154 | linear_vel = 0.0; 155 | angular_vel = max_angular_vel_; 156 | } 157 | 158 | // Create and publish a TwistStamped message with the desired velocity 159 | geometry_msgs::msg::TwistStamped cmd_vel; 160 | cmd_vel.header.frame_id = pose.header.frame_id; 161 | cmd_vel.header.stamp = clock_->now(); 162 | cmd_vel.twist.linear.x = linear_vel; 163 | cmd_vel.twist.angular.z = max( 164 | -1.0 * abs(max_angular_vel_), min( 165 | angular_vel, abs( 166 | max_angular_vel_))); 167 | 168 | return cmd_vel; 169 | } 170 | 171 | void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) 172 | { 173 | global_pub_->publish(path); 174 | global_plan_ = path; 175 | } 176 | 177 | nav_msgs::msg::Path 178 | PurePursuitController::transformGlobalPlan( 179 | const geometry_msgs::msg::PoseStamped & pose) 180 | { 181 | // Original mplementation taken fron nav2_dwb_controller 182 | 183 | if (global_plan_.poses.empty()) { 184 | throw nav2_core::PlannerException("Received plan with zero length"); 185 | } 186 | 187 | // let's get the pose of the robot in the frame of the plan 188 | geometry_msgs::msg::PoseStamped robot_pose; 189 | if (!transformPose( 190 | tf_, global_plan_.header.frame_id, pose, 191 | robot_pose, transform_tolerance_)) 192 | { 193 | throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); 194 | } 195 | 196 | // We'll discard points on the plan that are outside the local costmap 197 | nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); 198 | double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * 199 | costmap->getResolution() / 2.0; 200 | 201 | // First find the closest pose on the path to the robot 202 | auto transformation_begin = 203 | min_by( 204 | global_plan_.poses.begin(), global_plan_.poses.end(), 205 | [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { 206 | return euclidean_distance(robot_pose, ps); 207 | }); 208 | 209 | // From the closest point, look for the first point that's further then dist_threshold from the 210 | // robot. These points are definitely outside of the costmap so we won't transform them. 211 | auto transformation_end = std::find_if( 212 | transformation_begin, end(global_plan_.poses), 213 | [&](const auto & global_plan_pose) { 214 | return euclidean_distance(robot_pose, global_plan_pose) > dist_threshold; 215 | }); 216 | 217 | // Helper function for the transform below. Transforms a PoseStamped from global frame to local 218 | auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { 219 | // We took a copy of the pose, let's lookup the transform at the current time 220 | geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; 221 | stamped_pose.header.frame_id = global_plan_.header.frame_id; 222 | stamped_pose.header.stamp = pose.header.stamp; 223 | stamped_pose.pose = global_plan_pose.pose; 224 | transformPose( 225 | tf_, costmap_ros_->getBaseFrameID(), 226 | stamped_pose, transformed_pose, transform_tolerance_); 227 | return transformed_pose; 228 | }; 229 | 230 | // Transform the near part of the global plan into the robot's frame of reference. 231 | nav_msgs::msg::Path transformed_plan; 232 | std::transform( 233 | transformation_begin, transformation_end, 234 | std::back_inserter(transformed_plan.poses), 235 | transformGlobalPoseToLocal); 236 | transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); 237 | transformed_plan.header.stamp = pose.header.stamp; 238 | 239 | // Remove the portion of the global plan that we've already passed so we don't 240 | // process it on the next iteration (this is called path pruning) 241 | global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); 242 | global_pub_->publish(transformed_plan); 243 | 244 | if (transformed_plan.poses.empty()) { 245 | throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); 246 | } 247 | 248 | return transformed_plan; 249 | } 250 | 251 | bool PurePursuitController::transformPose( 252 | const std::shared_ptr tf, 253 | const std::string frame, 254 | const geometry_msgs::msg::PoseStamped & in_pose, 255 | geometry_msgs::msg::PoseStamped & out_pose, 256 | const rclcpp::Duration & transform_tolerance 257 | ) const 258 | { 259 | // Implementation taken as is fron nav_2d_utils in nav2_dwb_controller 260 | 261 | if (in_pose.header.frame_id == frame) { 262 | out_pose = in_pose; 263 | return true; 264 | } 265 | 266 | try { 267 | tf->transform(in_pose, out_pose, frame); 268 | return true; 269 | } catch (tf2::ExtrapolationException & ex) { 270 | auto transform = tf->lookupTransform( 271 | frame, 272 | in_pose.header.frame_id, 273 | tf2::TimePointZero 274 | ); 275 | if ( 276 | (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > 277 | transform_tolerance) 278 | { 279 | RCLCPP_ERROR( 280 | rclcpp::get_logger("tf_help"), 281 | "Transform data too old when converting from %s to %s", 282 | in_pose.header.frame_id.c_str(), 283 | frame.c_str() 284 | ); 285 | RCLCPP_ERROR( 286 | rclcpp::get_logger("tf_help"), 287 | "Data time: %ds %uns, Transform time: %ds %uns", 288 | in_pose.header.stamp.sec, 289 | in_pose.header.stamp.nanosec, 290 | transform.header.stamp.sec, 291 | transform.header.stamp.nanosec 292 | ); 293 | return false; 294 | } else { 295 | tf2::doTransform(in_pose, out_pose, transform); 296 | return true; 297 | } 298 | } catch (tf2::TransformException & ex) { 299 | RCLCPP_ERROR( 300 | rclcpp::get_logger("tf_help"), 301 | "Exception in transformPose: %s", 302 | ex.what() 303 | ); 304 | return false; 305 | } 306 | return false; 307 | } 308 | 309 | } // namespace nav2_pure_pursuit_controller 310 | 311 | // Register this controller as a nav2_core plugin 312 | PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller) -------------------------------------------------------------------------------- /nav2_sms_behavior/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_sms_behavior) 3 | 4 | # Default to C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | # find dependencies 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(rclcpp_action REQUIRED) 11 | find_package(rclcpp_lifecycle REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(visualization_msgs REQUIRED) 14 | find_package(nav2_util REQUIRED) 15 | find_package(nav2_msgs REQUIRED) 16 | find_package(nav_msgs REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(builtin_interfaces REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(nav2_costmap_2d REQUIRED) 21 | find_package(nav2_core REQUIRED) 22 | find_package(nav2_behaviors REQUIRED) 23 | find_package(pluginlib REQUIRED) 24 | find_package(rosidl_default_generators REQUIRED) 25 | find_package(action_msgs REQUIRED) 26 | find_package(CURL REQUIRED) 27 | 28 | include_directories( 29 | include 30 | ) 31 | 32 | set(library_name ${PROJECT_NAME}_plugin) 33 | 34 | set(dependencies 35 | rclcpp 36 | rclcpp_action 37 | rclcpp_lifecycle 38 | std_msgs 39 | visualization_msgs 40 | nav2_util 41 | nav2_msgs 42 | nav_msgs 43 | geometry_msgs 44 | builtin_interfaces 45 | tf2_ros 46 | nav2_costmap_2d 47 | nav2_core 48 | pluginlib 49 | nav2_behaviors 50 | CURL 51 | ) 52 | 53 | rosidl_generate_interfaces(${PROJECT_NAME} 54 | "action/SendSms.action" 55 | DEPENDENCIES builtin_interfaces std_msgs 56 | ) 57 | 58 | add_library(${library_name} SHARED 59 | src/send_sms.cpp 60 | src/twilio.cpp 61 | ) 62 | 63 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 64 | 65 | ament_target_dependencies(${library_name} 66 | ${dependencies} 67 | ) 68 | 69 | rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") 70 | target_link_libraries(${library_name} "${cpp_typesupport_target}") 71 | 72 | 73 | pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml) 74 | 75 | install(TARGETS ${library_name} 76 | ARCHIVE DESTINATION lib 77 | LIBRARY DESTINATION lib 78 | RUNTIME DESTINATION lib/${PROJECT_NAME} 79 | ) 80 | 81 | install(DIRECTORY include/ 82 | DESTINATION include/ 83 | ) 84 | 85 | install(FILES behavior_plugin.xml 86 | DESTINATION share/${PROJECT_NAME} 87 | ) 88 | 89 | if(BUILD_TESTING) 90 | find_package(ament_lint_auto REQUIRED) 91 | # the following line skips the linter which checks for copyrights 92 | # uncomment the line when a copyright and license is not present in all source files 93 | #set(ament_cmake_copyright_FOUND TRUE) 94 | # the following line skips cpplint (only works in a git repo) 95 | # uncomment the line when this package is not in a git repo 96 | #set(ament_cmake_cpplint_FOUND TRUE) 97 | ament_lint_auto_find_test_dependencies() 98 | endif() 99 | 100 | ament_export_dependencies(rosidl_default_runtime) 101 | ament_export_include_directories(include) 102 | ament_export_libraries(${library_name}) 103 | ament_export_dependencies(${dependencies}) 104 | ament_package() 105 | -------------------------------------------------------------------------------- /nav2_sms_behavior/action/SendSms.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | string message 3 | --- 4 | #result definition 5 | 6 | # Error codes 7 | # Note: The expected priority order of the errors should match the message order 8 | uint16 NONE=0 9 | uint16 UNKNOWN=51110 10 | uint16 SMS_SEND_FAILED=51111 11 | 12 | builtin_interfaces/Duration total_elapsed_time 13 | uint16 error_code 14 | string error_msg 15 | --- 16 | #feedback definition 17 | -------------------------------------------------------------------------------- /nav2_sms_behavior/behavior_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is an example plugin which produces an SMS text message behavior. 4 | 5 | 6 | -------------------------------------------------------------------------------- /nav2_sms_behavior/include/nav2_sms_behavior/send_sms.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Samsung Research America 2 | // This code is licensed under MIT license (see LICENSE.txt for details) 3 | 4 | #ifndef NAV2_SMS_RECOVEY__SMS_RECOVERY_HPP_ 5 | #define NAV2_SMS_RECOVEY__SMS_RECOVERY_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "nav2_behaviors/timed_behavior.hpp" 12 | #include "nav2_sms_behavior/action/send_sms.hpp" 13 | #include "nav2_sms_behavior/twilio.hpp" 14 | 15 | namespace nav2_sms_behavior 16 | { 17 | 18 | using namespace nav2_behaviors; // NOLINT 19 | using Action = nav2_sms_behavior::action::SendSms; 20 | using ActionResult = Action::Result; 21 | 22 | class SendSms : public TimedBehavior 23 | { 24 | public: 25 | SendSms(); 26 | ~SendSms(); 27 | 28 | ResultStatus onRun(const std::shared_ptr command) override; 29 | 30 | ResultStatus onCycleUpdate() override; 31 | 32 | void onConfigure() override; 33 | 34 | /** 35 | * @brief Method to determine the required costmap info 36 | * @return costmap resources needed 37 | */ 38 | nav2_core::CostmapInfoType getResourceInfo() override {return nav2_core::CostmapInfoType::NONE;} 39 | 40 | protected: 41 | std::string _account_sid; 42 | std::string _auth_token; 43 | std::string _from_number; 44 | std::string _to_number; 45 | std::shared_ptr _twilio; 46 | }; 47 | 48 | } // namespace nav2_sms_recovery 49 | 50 | #endif // NAV2_SMS_RECOVEY__SMS_RECOVERY_HPP_ 51 | -------------------------------------------------------------------------------- /nav2_sms_behavior/include/nav2_sms_behavior/twilio.hpp: -------------------------------------------------------------------------------- 1 | // Taken from https://www.twilio.com/docs/sms/tutorials/how-to-send-sms-messages-cpp 2 | // Under MIT license 3 | 4 | #pragma once 5 | 6 | #include 7 | #include "type_conversion.hpp" 8 | 9 | namespace twilio { 10 | 11 | class Twilio { 12 | public: 13 | Twilio(std::string const& account_sid_in, 14 | std::string const& auth_token_in) 15 | : account_sid(account_sid_in) 16 | , auth_token(auth_token_in) 17 | {} 18 | // Nothing in destructor 19 | ~Twilio() = default; 20 | 21 | bool send_message( 22 | std::string const& to_number, 23 | std::string const& from_number, 24 | std::string const& message_body, 25 | std::string& response, 26 | std::string const& picture_url = "", 27 | bool verbose = false 28 | ); 29 | 30 | private: 31 | // Account SID and Auth Token come from the Twilio console. 32 | // See: https://twilio.com/console for more. 33 | 34 | // Used for the username of the auth header 35 | std::string const account_sid; 36 | // Used for the password of the auth header 37 | std::string const auth_token; 38 | 39 | // Portably ignore curl response 40 | static size_t _null_write(char *, size_t, size_t, void *); 41 | // Write curl response to a stringstream 42 | static size_t _stream_write(char *, size_t, size_t, void *); 43 | }; 44 | 45 | } // end namespace twilio 46 | -------------------------------------------------------------------------------- /nav2_sms_behavior/include/nav2_sms_behavior/type_conversion.hpp: -------------------------------------------------------------------------------- 1 | // Taken from https://www.twilio.com/docs/sms/tutorials/how-to-send-sms-messages-cpp 2 | // Under MIT license 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | // Given a UTF-8 encoded string return a new UCS-2 string. 11 | inline std::u16string 12 | utf8_to_ucs2(std::string const& input) 13 | { 14 | std::wstring_convert, char16_t> convert; 15 | try { 16 | return convert.from_bytes(input); 17 | } catch(const std::range_error& e) { 18 | throw std::range_error( 19 | "Failed UCS-2 conversion of message body. Check all " 20 | "characters are valid GSM-7, GSM 8-bit text, or UCS-2 " 21 | "characters." 22 | ); 23 | } 24 | } 25 | 26 | // Given a UCS-2 string return a new UTF-8 encoded string. 27 | inline std::string 28 | ucs2_to_utf8(std::u16string const& input) 29 | { 30 | std::wstring_convert, char16_t> convert; 31 | return convert.to_bytes(input); 32 | } 33 | -------------------------------------------------------------------------------- /nav2_sms_behavior/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_sms_behavior 5 | 1.0.0 6 | Simple SMS send behavior. 7 | Steve Macenski 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rosidl_interface_packages 13 | rosidl_default_generators 14 | rosidl_default_runtime 15 | 16 | rclcpp 17 | rclcpp_action 18 | rclcpp_lifecycle 19 | std_msgs 20 | visualization_msgs 21 | nav2_util 22 | nav2_msgs 23 | nav_msgs 24 | geometry_msgs 25 | builtin_interfaces 26 | tf2_ros 27 | nav2_costmap_2d 28 | nav2_behaviors 29 | nav2_core 30 | pluginlib 31 | action_msgs 32 | curl 33 | 34 | ament_lint_auto 35 | ament_lint_common 36 | 37 | 38 | ament_cmake 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /nav2_sms_behavior/src/send_sms.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Samsung Research America 2 | // This code is licensed under MIT license (see LICENSE.txt for details) 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "nav2_sms_behavior/send_sms.hpp" 9 | 10 | namespace nav2_sms_behavior 11 | { 12 | 13 | SendSms::SendSms() 14 | : TimedBehavior() 15 | { 16 | } 17 | 18 | SendSms::~SendSms() 19 | { 20 | } 21 | 22 | void SendSms::onConfigure() 23 | { 24 | auto node = node_.lock(); 25 | node->declare_parameter("account_sid",""); 26 | _account_sid = node->get_parameter("account_sid").as_string(); 27 | node->declare_parameter("auth_token",""); 28 | _auth_token = node->get_parameter("auth_token").as_string(); 29 | node->declare_parameter("from_number",""); 30 | _from_number = node->get_parameter("from_number").as_string(); 31 | node->declare_parameter("to_number",""); 32 | _to_number = node->get_parameter("to_number").as_string(); 33 | _twilio = std::make_shared(_account_sid, _auth_token); 34 | } 35 | 36 | ResultStatus SendSms::onRun(const std::shared_ptr command) 37 | { 38 | auto node = node_.lock(); 39 | std::string response; 40 | bool message_success = _twilio->send_message( 41 | _to_number, 42 | _from_number, 43 | command->message, 44 | response, 45 | "", 46 | false); 47 | if (!message_success) { 48 | RCLCPP_INFO(node->get_logger(), "SMS send failed: %s.", response.c_str()); 49 | return ResultStatus{Status::FAILED, ActionResult::SMS_SEND_FAILED, response}; 50 | } 51 | 52 | RCLCPP_INFO(node->get_logger(), "SMS sent successfully!"); 53 | return ResultStatus{Status::SUCCEEDED}; 54 | } 55 | 56 | ResultStatus SendSms::onCycleUpdate() 57 | { 58 | return ResultStatus{Status::SUCCEEDED}; 59 | } 60 | 61 | } // namespace nav2_sms_behavior 62 | 63 | #include "pluginlib/class_list_macros.hpp" 64 | PLUGINLIB_EXPORT_CLASS(nav2_sms_behavior::SendSms, nav2_core::Behavior) 65 | -------------------------------------------------------------------------------- /nav2_sms_behavior/src/twilio.cpp: -------------------------------------------------------------------------------- 1 | // Taken from https://www.twilio.com/docs/sms/tutorials/how-to-send-sms-messages-cpp 2 | // Under MIT license 3 | 4 | #include 5 | #include 6 | 7 | #include "nav2_sms_behavior/twilio.hpp" 8 | 9 | 10 | namespace twilio { 11 | 12 | // Portably ignore curl response 13 | size_t Twilio::_null_write( 14 | char *ptr, 15 | size_t size, 16 | size_t nmemb, 17 | void *userdata) 18 | { 19 | return size*nmemb; 20 | } 21 | 22 | // Write curl response to a stringstream 23 | size_t Twilio::_stream_write( 24 | char *ptr, 25 | size_t size, 26 | size_t nmemb, 27 | void *userdata) 28 | { 29 | size_t response_size = size * nmemb; 30 | std::stringstream *ss = (std::stringstream*)userdata; 31 | ss->write(ptr, response_size); 32 | return response_size; 33 | } 34 | 35 | // Method send_message: 36 | // Returns 'true' if the result of the eventual HTTP post to Twilio is status 37 | // code 200 or 201. Either other status codes or errors in curl will cause 38 | // a false result. 39 | // Inputs: 40 | // - to_number: Where to send the MMS or SMS 41 | // - from_number: Number in your Twilio account to use as a sender. 42 | // - message_body: (Max: 1600 unicode characters) The body of the MMS 43 | // or SMS message which will be sent to the to_number. 44 | // 45 | // Outputs: 46 | // - response: Either the curl error message or the Twilio response 47 | // if verbose. 48 | // Optional: 49 | // - picture_url: If picture URL is included, a MMS will be sent 50 | // - verbose: Whether to print all the responses 51 | bool Twilio::send_message( 52 | std::string const& to_number, 53 | std::string const& from_number, 54 | std::string const& message_body, 55 | std::string& response, 56 | std::string const& picture_url, 57 | bool verbose) 58 | { 59 | std::stringstream response_stream; 60 | std::u16string converted_message_body; 61 | 62 | // Assume UTF-8 input, convert to UCS-2 to check size 63 | // See: https://www.twilio.com/docs/api/rest/sending-messages for 64 | // information on Twilio body size limits. 65 | try { 66 | converted_message_body = utf8_to_ucs2(message_body); 67 | } catch(const std::range_error& e) { 68 | response = e.what(); 69 | return false; 70 | } 71 | 72 | if (converted_message_body.size() > 1600) { 73 | response_stream << "Message body must have 1600 or fewer" 74 | << " characters. Cannot send message with " 75 | << converted_message_body.size() << " characters."; 76 | response = response_stream.str(); 77 | return false; 78 | } 79 | 80 | CURL *curl; 81 | curl_global_init(CURL_GLOBAL_ALL); 82 | curl = curl_easy_init(); 83 | 84 | // Percent encode special characters 85 | char *message_body_escaped = curl_easy_escape( 86 | curl, 87 | message_body.c_str(), 88 | 0 89 | ); 90 | 91 | 92 | std::stringstream url; 93 | std::string url_string; 94 | url << "https://api.twilio.com/2010-04-01/Accounts/" << account_sid 95 | << "/Messages"; 96 | url_string = url.str(); 97 | 98 | 99 | std::stringstream parameters; 100 | std::string parameter_string; 101 | parameters << "To=" << to_number << "&From=" << from_number 102 | << "&Body=" << message_body_escaped; 103 | if (!picture_url.empty()) { 104 | parameters << "&MediaUrl=" << picture_url; 105 | } 106 | parameter_string = parameters.str(); 107 | 108 | 109 | curl_easy_setopt(curl, CURLOPT_POST, 1); 110 | curl_easy_setopt(curl, CURLOPT_URL, url_string.c_str()); 111 | curl_easy_setopt(curl, CURLOPT_POSTFIELDS, parameter_string.c_str()); 112 | curl_easy_setopt(curl, CURLOPT_USERNAME, account_sid.c_str()); 113 | curl_easy_setopt(curl, CURLOPT_PASSWORD, auth_token.c_str()); 114 | if (!verbose) { 115 | curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, _null_write); 116 | } else { 117 | curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, _stream_write); 118 | curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response_stream); 119 | } 120 | 121 | 122 | CURLcode res = curl_easy_perform(curl); 123 | curl_free(message_body_escaped); 124 | curl_easy_cleanup(curl); 125 | long http_code = 0; 126 | curl_easy_getinfo (curl, CURLINFO_RESPONSE_CODE, &http_code); 127 | 128 | // Check for curl errors and Twilio failure status codes. 129 | if (res != CURLE_OK) { 130 | response = curl_easy_strerror(res); 131 | return false; 132 | } else if (http_code != 200 && http_code != 201) { 133 | response = response_stream.str(); 134 | return false; 135 | } else { 136 | response = response_stream.str(); 137 | return true; 138 | } 139 | } 140 | 141 | } // end namespace twilio -------------------------------------------------------------------------------- /nav2_straightline_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_straightline_planner) 3 | 4 | # Default to C99 5 | set(CMAKE_C_STANDARD 99) 6 | 7 | 8 | # Default to C++14 9 | set(CMAKE_CXX_STANDARD 14) 10 | 11 | # find dependencies 12 | find_package(ament_cmake REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(rclcpp_action REQUIRED) 15 | find_package(rclcpp_lifecycle REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | find_package(visualization_msgs REQUIRED) 18 | find_package(nav2_util REQUIRED) 19 | find_package(nav2_msgs REQUIRED) 20 | find_package(nav_msgs REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(builtin_interfaces REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(nav2_costmap_2d REQUIRED) 25 | find_package(nav2_core REQUIRED) 26 | find_package(pluginlib REQUIRED) 27 | 28 | include_directories( 29 | include 30 | ) 31 | 32 | set(library_name ${PROJECT_NAME}_plugin) 33 | 34 | set(dependencies 35 | rclcpp 36 | rclcpp_action 37 | rclcpp_lifecycle 38 | std_msgs 39 | visualization_msgs 40 | nav2_util 41 | nav2_msgs 42 | nav_msgs 43 | geometry_msgs 44 | builtin_interfaces 45 | tf2_ros 46 | nav2_costmap_2d 47 | nav2_core 48 | pluginlib 49 | ) 50 | 51 | add_library(${library_name} SHARED 52 | src/straight_line_planner.cpp 53 | ) 54 | 55 | ament_target_dependencies(${library_name} 56 | ${dependencies} 57 | ) 58 | 59 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 60 | 61 | 62 | pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) 63 | 64 | install(TARGETS ${library_name} 65 | ARCHIVE DESTINATION lib 66 | LIBRARY DESTINATION lib 67 | RUNTIME DESTINATION lib/${PROJECT_NAME} 68 | ) 69 | 70 | install(DIRECTORY include/ 71 | DESTINATION include/ 72 | ) 73 | 74 | install(FILES global_planner_plugin.xml 75 | DESTINATION share/${PROJECT_NAME} 76 | ) 77 | 78 | if(BUILD_TESTING) 79 | find_package(ament_lint_auto REQUIRED) 80 | # the following line skips the linter which checks for copyrights 81 | # uncomment the line when a copyright and license is not present in all source files 82 | #set(ament_cmake_copyright_FOUND TRUE) 83 | # the following line skips cpplint (only works in a git repo) 84 | # uncomment the line when this package is not in a git repo 85 | #set(ament_cmake_cpplint_FOUND TRUE) 86 | ament_lint_auto_find_test_dependencies() 87 | endif() 88 | 89 | 90 | ament_export_include_directories(include) 91 | ament_export_libraries(${library_name}) 92 | ament_export_dependencies(${dependencies}) 93 | ament_package() 94 | -------------------------------------------------------------------------------- /nav2_straightline_planner/global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is an example plugin which produces straight path. 4 | 5 | -------------------------------------------------------------------------------- /nav2_straightline_planner/include/nav2_straightline_planner/straight_line_planner.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2020 Shivang Patel 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Shivang Patel 36 | * 37 | * Reference tutorial: 38 | * https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html 39 | *********************************************************************/ 40 | 41 | #ifndef NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_ 42 | #define NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_ 43 | 44 | #include 45 | #include 46 | 47 | #include "rclcpp/rclcpp.hpp" 48 | #include "geometry_msgs/msg/point.hpp" 49 | #include "geometry_msgs/msg/pose_stamped.hpp" 50 | 51 | #include "nav2_core/global_planner.hpp" 52 | #include "nav_msgs/msg/path.hpp" 53 | #include "nav2_util/robot_utils.hpp" 54 | #include "nav2_util/lifecycle_node.hpp" 55 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 56 | 57 | namespace nav2_straightline_planner 58 | { 59 | 60 | class StraightLine : public nav2_core::GlobalPlanner 61 | { 62 | public: 63 | StraightLine() = default; 64 | ~StraightLine() = default; 65 | 66 | // plugin configure 67 | void configure( 68 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 69 | std::string name, std::shared_ptr tf, 70 | std::shared_ptr costmap_ros) override; 71 | 72 | // plugin cleanup 73 | void cleanup() override; 74 | 75 | // plugin activate 76 | void activate() override; 77 | 78 | // plugin deactivate 79 | void deactivate() override; 80 | 81 | // This method creates path for given start and goal pose. 82 | nav_msgs::msg::Path createPlan( 83 | const geometry_msgs::msg::PoseStamped & start, 84 | const geometry_msgs::msg::PoseStamped & goal, 85 | std::function cancel_checker) override; 86 | 87 | private: 88 | // TF buffer 89 | std::shared_ptr tf_; 90 | 91 | // node ptr 92 | nav2_util::LifecycleNode::SharedPtr node_; 93 | 94 | // Global Costmap 95 | nav2_costmap_2d::Costmap2D * costmap_; 96 | 97 | // The global frame of the costmap 98 | std::string global_frame_, name_; 99 | 100 | double interpolation_resolution_; 101 | }; 102 | 103 | } // namespace nav2_straightline_planner 104 | 105 | #endif // NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_ 106 | -------------------------------------------------------------------------------- /nav2_straightline_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_straightline_planner 5 | 1.0.0 6 | Simple straight line planner. 7 | Shivang Patel 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | std_msgs 16 | visualization_msgs 17 | nav2_util 18 | nav2_msgs 19 | nav_msgs 20 | geometry_msgs 21 | builtin_interfaces 22 | tf2_ros 23 | nav2_costmap_2d 24 | nav2_core 25 | pluginlib 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /nav2_straightline_planner/src/straight_line_planner.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2020 Shivang Patel 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Shivang Patel 36 | * 37 | * Reference tutorial: 38 | * https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html 39 | *********************************************************************/ 40 | 41 | #include 42 | #include 43 | #include 44 | #include "nav2_util/node_utils.hpp" 45 | 46 | #include "nav2_straightline_planner/straight_line_planner.hpp" 47 | 48 | namespace nav2_straightline_planner 49 | { 50 | 51 | void StraightLine::configure( 52 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 53 | std::string name, std::shared_ptr tf, 54 | std::shared_ptr costmap_ros) 55 | { 56 | node_ = parent.lock(); 57 | name_ = name; 58 | tf_ = tf; 59 | costmap_ = costmap_ros->getCostmap(); 60 | global_frame_ = costmap_ros->getGlobalFrameID(); 61 | 62 | // Parameter initialization 63 | nav2_util::declare_parameter_if_not_declared( 64 | node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue( 65 | 0.1)); 66 | node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_); 67 | } 68 | 69 | void StraightLine::cleanup() 70 | { 71 | RCLCPP_INFO( 72 | node_->get_logger(), "CleaningUp plugin %s of type NavfnPlanner", 73 | name_.c_str()); 74 | } 75 | 76 | void StraightLine::activate() 77 | { 78 | RCLCPP_INFO( 79 | node_->get_logger(), "Activating plugin %s of type NavfnPlanner", 80 | name_.c_str()); 81 | } 82 | 83 | void StraightLine::deactivate() 84 | { 85 | RCLCPP_INFO( 86 | node_->get_logger(), "Deactivating plugin %s of type NavfnPlanner", 87 | name_.c_str()); 88 | } 89 | 90 | nav_msgs::msg::Path StraightLine::createPlan( 91 | const geometry_msgs::msg::PoseStamped & start, 92 | const geometry_msgs::msg::PoseStamped & goal, 93 | std::function /*cancel_checker*/) 94 | { 95 | nav_msgs::msg::Path global_path; 96 | 97 | // Checking if the goal and start state is in the global frame 98 | if (start.header.frame_id != global_frame_) { 99 | RCLCPP_ERROR( 100 | node_->get_logger(), "Planner will only except start position from %s frame", 101 | global_frame_.c_str()); 102 | return global_path; 103 | } 104 | 105 | if (goal.header.frame_id != global_frame_) { 106 | RCLCPP_INFO( 107 | node_->get_logger(), "Planner will only except goal position from %s frame", 108 | global_frame_.c_str()); 109 | return global_path; 110 | } 111 | 112 | global_path.poses.clear(); 113 | global_path.header.stamp = node_->now(); 114 | global_path.header.frame_id = global_frame_; 115 | // calculating the number of loops for current value of interpolation_resolution_ 116 | int total_number_of_loop = std::hypot( 117 | goal.pose.position.x - start.pose.position.x, 118 | goal.pose.position.y - start.pose.position.y) / 119 | interpolation_resolution_; 120 | double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop; 121 | double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop; 122 | 123 | for (int i = 0; i < total_number_of_loop; ++i) { 124 | geometry_msgs::msg::PoseStamped pose; 125 | pose.pose.position.x = start.pose.position.x + x_increment * i; 126 | pose.pose.position.y = start.pose.position.y + y_increment * i; 127 | pose.pose.position.z = 0.0; 128 | pose.pose.orientation.x = 0.0; 129 | pose.pose.orientation.y = 0.0; 130 | pose.pose.orientation.z = 0.0; 131 | pose.pose.orientation.w = 1.0; 132 | pose.header.stamp = node_->now(); 133 | pose.header.frame_id = global_frame_; 134 | global_path.poses.push_back(pose); 135 | } 136 | 137 | geometry_msgs::msg::PoseStamped goal_pose = goal; 138 | goal_pose.header.stamp = node_->now(); 139 | goal_pose.header.frame_id = global_frame_; 140 | global_path.poses.push_back(goal_pose); 141 | 142 | return global_path; 143 | } 144 | 145 | } // namespace nav2_straightline_planner 146 | 147 | #include "pluginlib/class_list_macros.hpp" 148 | PLUGINLIB_EXPORT_CLASS(nav2_straightline_planner::StraightLine, nav2_core::GlobalPlanner) 149 | -------------------------------------------------------------------------------- /sam_bot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sam_bot_description) 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 14) 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(xacro REQUIRED) 21 | # uncomment the following section in order to fill in 22 | # further dependencies manually. 23 | # find_package( REQUIRED) 24 | 25 | install( 26 | DIRECTORY src launch rviz config world 27 | DESTINATION share/${PROJECT_NAME} 28 | ) 29 | 30 | if(BUILD_TESTING) 31 | find_package(ament_lint_auto REQUIRED) 32 | # the following line skips the linter which checks for copyrights 33 | # uncomment the line when a copyright and license is not present in all source files 34 | #set(ament_cmake_copyright_FOUND TRUE) 35 | # the following line skips cpplint (only works in a git repo) 36 | # uncomment the line when this package is not in a git repo 37 | #set(ament_cmake_cpplint_FOUND TRUE) 38 | ament_lint_auto_find_test_dependencies() 39 | endif() 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /sam_bot_description/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 URDF Setup Tutorial - Differential Drive Robot 2 | Tutorial code referenced in https://navigation.ros.org/setup_guides/urdf/setup_urdf.html 3 | 4 | This package implements a URDF description for a simple differential drive robot. It includes the complete urdf files, launch files, build files and rviz configuration to replicate the tutorial in the link above -------------------------------------------------------------------------------- /sam_bot_description/config/bridge_config.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | - ros_topic_name: "/clock" 3 | gz_topic_name: "/clock" 4 | ros_type_name: "rosgraph_msgs/msg/Clock" 5 | gz_type_name: "gz.msgs.Clock" 6 | direction: GZ_TO_ROS 7 | 8 | - ros_topic_name: "/demo/imu" 9 | gz_topic_name: "/demo/imu" 10 | ros_type_name: "sensor_msgs/msg/Imu" 11 | gz_type_name: "gz.msgs.IMU" 12 | direction: GZ_TO_ROS 13 | 14 | # Topic published by DiffDrive plugin 15 | - ros_topic_name: "/demo/odom" 16 | gz_topic_name: "/demo/odom" 17 | ros_type_name: "nav_msgs/msg/Odometry" 18 | gz_type_name: "gz.msgs.Odometry" 19 | direction: GZ_TO_ROS 20 | 21 | # Topic published by JointStatePublisher plugin 22 | - ros_topic_name: "/joint_states" 23 | gz_topic_name: "/joint_states" 24 | ros_type_name: "sensor_msgs/msg/JointState" 25 | gz_type_name: "gz.msgs.Model" 26 | direction: GZ_TO_ROS 27 | 28 | # Topic subscribed to by DiffDrive plugin 29 | - ros_topic_name: "/demo/cmd_vel" 30 | gz_topic_name: "/demo/cmd_vel" 31 | ros_type_name: "geometry_msgs/msg/TwistStamped" 32 | gz_type_name: "gz.msgs.Twist" 33 | direction: ROS_TO_GZ 34 | 35 | - ros_topic_name: "/scan" 36 | gz_topic_name: "/scan" 37 | ros_type_name: "sensor_msgs/msg/LaserScan" 38 | gz_type_name: "gz.msgs.LaserScan" 39 | direction: GZ_TO_ROS 40 | 41 | - ros_topic_name: "/scan/points" 42 | gz_topic_name: "/scan/points" 43 | ros_type_name: "sensor_msgs/msg/PointCloud2" 44 | gz_type_name: "gz.msgs.PointCloudPacked" 45 | direction: GZ_TO_ROS 46 | 47 | - ros_topic_name: "/depth_camera/camera_info" 48 | gz_topic_name: "/depth_camera/camera_info" 49 | ros_type_name: "sensor_msgs/msg/CameraInfo" 50 | gz_type_name: "gz.msgs.CameraInfo" 51 | direction: GZ_TO_ROS 52 | 53 | - ros_topic_name: "/depth_camera/points" 54 | gz_topic_name: "/depth_camera/points" 55 | ros_type_name: "sensor_msgs/msg/PointCloud2" 56 | gz_type_name: "gz.msgs.PointCloudPacked" 57 | direction: GZ_TO_ROS 58 | -------------------------------------------------------------------------------- /sam_bot_description/config/ekf.yaml: -------------------------------------------------------------------------------- 1 | ### ekf config file ### 2 | ekf_node: 3 | ros__parameters: 4 | # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin 5 | # computation until it receives at least one message from one of the inputs. It will then run continuously at the 6 | # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. 7 | frequency: 30.0 8 | 9 | # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is 10 | # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar 11 | # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected 12 | # by, for example, an IMU. Defaults to false if unspecified. 13 | two_d_mode: false 14 | 15 | # Whether to publish the acceleration state. Defaults to false if unspecified. 16 | publish_acceleration: true 17 | 18 | # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. 19 | publish_tf: true 20 | 21 | # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. 22 | # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame. 23 | # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" 24 | # to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. 25 | # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark 26 | # observations) then: 27 | # 3a. Set your "world_frame" to your map_frame value 28 | # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node 29 | # from robot_localization! However, that instance should *not* fuse the global data. 30 | map_frame: map # Defaults to "map" if unspecified 31 | odom_frame: odom # Defaults to "odom" if unspecified 32 | base_link_frame: base_link # Defaults to "base_link" if unspecified 33 | world_frame: odom # Defaults to the value of odom_frame if unspecified 34 | 35 | odom0: demo/odom 36 | odom0_config: [false, false, false, 37 | false, false, false, 38 | true, true, false, 39 | false, false, true, 40 | false, false, false] 41 | 42 | imu0: demo/imu 43 | imu0_config: [false, false, false, 44 | false, false, false, 45 | false, false, false, 46 | false, false, true, 47 | false, false, false] 48 | -------------------------------------------------------------------------------- /sam_bot_description/config/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | alpha1: 0.2 4 | alpha2: 0.2 5 | alpha3: 0.2 6 | alpha4: 0.2 7 | alpha5: 0.2 8 | base_frame_id: "base_footprint" 9 | beam_skip_distance: 0.5 10 | beam_skip_error_threshold: 0.9 11 | beam_skip_threshold: 0.3 12 | do_beamskip: false 13 | global_frame_id: "map" 14 | lambda_short: 0.1 15 | laser_likelihood_max_dist: 2.0 16 | laser_max_range: 100.0 17 | laser_min_range: -1.0 18 | laser_model_type: "likelihood_field" 19 | max_beams: 60 20 | max_particles: 2000 21 | min_particles: 500 22 | odom_frame_id: "odom" 23 | pf_err: 0.05 24 | pf_z: 0.99 25 | recovery_alpha_fast: 0.0 26 | recovery_alpha_slow: 0.0 27 | resample_interval: 1 28 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 29 | save_pose_rate: 0.5 30 | sigma_hit: 0.2 31 | tf_broadcast: true 32 | transform_tolerance: 1.0 33 | update_min_a: 0.2 34 | update_min_d: 0.25 35 | z_hit: 0.5 36 | z_max: 0.05 37 | z_rand: 0.5 38 | z_short: 0.05 39 | scan_topic: scan 40 | 41 | bt_navigator: 42 | ros__parameters: 43 | global_frame: map 44 | robot_base_frame: base_link 45 | odom_topic: /demo/odom 46 | bt_loop_duration: 10 47 | default_server_timeout: 20 48 | wait_for_service_timeout: 1000 49 | action_server_result_timeout: 900.0 50 | navigators: ["navigate_to_pose", "navigate_through_poses"] 51 | navigate_to_pose: 52 | plugin: "nav2_bt_navigator::NavigateToPoseNavigator" 53 | navigate_through_poses: 54 | plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" 55 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 56 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 57 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 58 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 59 | 60 | # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). 61 | # Built-in plugins are added automatically 62 | # plugin_lib_names: [] 63 | 64 | error_code_name_prefixes: 65 | - assisted_teleop 66 | - backup 67 | - compute_path 68 | - dock_robot 69 | - drive_on_heading 70 | - follow_path 71 | - nav_thru_poses 72 | - nav_to_pose 73 | - route 74 | - spin 75 | - undock_robot 76 | - wait 77 | 78 | controller_server: 79 | ros__parameters: 80 | controller_frequency: 20.0 81 | costmap_update_timeout: 0.30 82 | min_x_velocity_threshold: 0.001 83 | min_y_velocity_threshold: 0.5 84 | min_theta_velocity_threshold: 0.001 85 | failure_tolerance: 0.3 86 | progress_checker_plugins: ["progress_checker"] 87 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 88 | controller_plugins: ["FollowPath"] 89 | odom_topic: /demo/odom 90 | use_realtime_priority: false 91 | enable_stamped_cmd_vel: true 92 | 93 | # Progress checker parameters 94 | progress_checker: 95 | plugin: "nav2_controller::SimpleProgressChecker" 96 | required_movement_radius: 0.5 97 | movement_time_allowance: 10.0 98 | # Goal checker parameters 99 | #precise_goal_checker: 100 | # plugin: "nav2_controller::SimpleGoalChecker" 101 | # xy_goal_tolerance: 0.25 102 | # yaw_goal_tolerance: 0.25 103 | # stateful: True 104 | general_goal_checker: 105 | stateful: True 106 | plugin: "nav2_controller::SimpleGoalChecker" 107 | xy_goal_tolerance: 0.25 108 | yaw_goal_tolerance: 0.25 109 | FollowPath: 110 | plugin: "nav2_mppi_controller::MPPIController" 111 | time_steps: 56 112 | model_dt: 0.05 113 | batch_size: 2000 114 | ax_max: 3.0 115 | ax_min: -3.0 116 | ay_max: 3.0 117 | az_max: 3.5 118 | vx_std: 0.2 119 | vy_std: 0.2 120 | wz_std: 0.4 121 | vx_max: 0.5 122 | vx_min: -0.35 123 | vy_max: 0.5 124 | wz_max: 1.9 125 | iteration_count: 1 126 | prune_distance: 1.7 127 | transform_tolerance: 0.1 128 | temperature: 0.3 129 | gamma: 0.015 130 | motion_model: "DiffDrive" 131 | visualize: true 132 | regenerate_noises: true 133 | TrajectoryVisualizer: 134 | trajectory_step: 5 135 | time_step: 3 136 | AckermannConstraints: 137 | min_turning_r: 0.2 138 | critics: [ 139 | "ConstraintCritic", "CostCritic", "GoalCritic", 140 | "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", 141 | "PathAngleCritic", "PreferForwardCritic"] 142 | ConstraintCritic: 143 | enabled: true 144 | cost_power: 1 145 | cost_weight: 4.0 146 | GoalCritic: 147 | enabled: true 148 | cost_power: 1 149 | cost_weight: 5.0 150 | threshold_to_consider: 1.4 151 | GoalAngleCritic: 152 | enabled: true 153 | cost_power: 1 154 | cost_weight: 3.0 155 | threshold_to_consider: 0.5 156 | PreferForwardCritic: 157 | enabled: true 158 | cost_power: 1 159 | cost_weight: 5.0 160 | threshold_to_consider: 0.5 161 | CostCritic: 162 | enabled: true 163 | cost_power: 1 164 | cost_weight: 3.81 165 | critical_cost: 300.0 166 | consider_footprint: true 167 | collision_cost: 1000000.0 168 | near_goal_distance: 1.0 169 | trajectory_point_step: 2 170 | PathAlignCritic: 171 | enabled: true 172 | cost_power: 1 173 | cost_weight: 14.0 174 | max_path_occupancy_ratio: 0.05 175 | trajectory_point_step: 4 176 | threshold_to_consider: 0.5 177 | offset_from_furthest: 20 178 | use_path_orientations: false 179 | PathFollowCritic: 180 | enabled: true 181 | cost_power: 1 182 | cost_weight: 5.0 183 | offset_from_furthest: 5 184 | threshold_to_consider: 1.4 185 | PathAngleCritic: 186 | enabled: true 187 | cost_power: 1 188 | cost_weight: 2.0 189 | offset_from_furthest: 4 190 | threshold_to_consider: 0.5 191 | max_angle_to_furthest: 1.0 192 | mode: 0 193 | # TwirlingCritic: 194 | # enabled: true 195 | # twirling_cost_power: 1 196 | # twirling_cost_weight: 10.0 197 | 198 | local_costmap: 199 | local_costmap: 200 | ros__parameters: 201 | update_frequency: 5.0 202 | publish_frequency: 2.0 203 | global_frame: odom 204 | robot_base_frame: base_link 205 | rolling_window: true 206 | width: 3 207 | height: 3 208 | resolution: 0.05 209 | footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]" 210 | plugins: ["voxel_layer", "inflation_layer"] 211 | inflation_layer: 212 | plugin: "nav2_costmap_2d::InflationLayer" 213 | cost_scaling_factor: 3.0 214 | inflation_radius: 0.55 215 | voxel_layer: 216 | plugin: "nav2_costmap_2d::VoxelLayer" 217 | enabled: True 218 | publish_voxel_map: True 219 | origin_z: 0.0 220 | z_resolution: 0.05 221 | z_voxels: 16 222 | max_obstacle_height: 2.0 223 | mark_threshold: 0 224 | observation_sources: scan 225 | scan: 226 | # A relative topic will be appended to the parent of the local_costmap namespace. 227 | # For example: 228 | # * User chosen namespace is `tb4`. 229 | # * User chosen topic is `scan`. 230 | # * Topic will be remapped to `/tb4/scan` without `local_costmap`. 231 | # * Use global topic `/scan` if you do not wish the node namespace to apply 232 | topic: scan 233 | max_obstacle_height: 2.0 234 | clearing: True 235 | marking: True 236 | data_type: "LaserScan" 237 | raytrace_max_range: 3.0 238 | raytrace_min_range: 0.0 239 | obstacle_max_range: 2.5 240 | obstacle_min_range: 0.0 241 | static_layer: 242 | plugin: "nav2_costmap_2d::StaticLayer" 243 | map_subscribe_transient_local: True 244 | always_send_full_costmap: True 245 | 246 | global_costmap: 247 | global_costmap: 248 | ros__parameters: 249 | update_frequency: 1.0 250 | publish_frequency: 1.0 251 | global_frame: map 252 | robot_base_frame: base_link 253 | robot_radius: 0.3 254 | resolution: 0.05 255 | track_unknown_space: true 256 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 257 | obstacle_layer: 258 | plugin: "nav2_costmap_2d::ObstacleLayer" 259 | enabled: True 260 | observation_sources: scan 261 | scan: 262 | # A relative topic will be appended to the parent of the global_costmap namespace. 263 | # For example: 264 | # * User chosen namespace is `tb4`. 265 | # * User chosen topic is `scan`. 266 | # * Topic will be remapped to `/tb4/scan` without `global_costmap`. 267 | # * Use global topic `/scan` if you do not wish the node namespace to apply 268 | topic: scan 269 | max_obstacle_height: 2.0 270 | clearing: True 271 | marking: True 272 | data_type: "LaserScan" 273 | raytrace_max_range: 3.0 274 | raytrace_min_range: 0.0 275 | obstacle_max_range: 2.5 276 | obstacle_min_range: 0.0 277 | static_layer: 278 | plugin: "nav2_costmap_2d::StaticLayer" 279 | map_subscribe_transient_local: True 280 | inflation_layer: 281 | plugin: "nav2_costmap_2d::InflationLayer" 282 | cost_scaling_factor: 3.0 283 | inflation_radius: 0.55 284 | always_send_full_costmap: True 285 | 286 | # The yaml_filename does not need to be specified since it going to be set by defaults in launch. 287 | # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py 288 | # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. 289 | # map_server: 290 | # ros__parameters: 291 | # yaml_filename: "" 292 | 293 | map_saver: 294 | ros__parameters: 295 | save_map_timeout: 5.0 296 | free_thresh_default: 0.25 297 | occupied_thresh_default: 0.65 298 | map_subscribe_transient_local: True 299 | 300 | planner_server: 301 | ros__parameters: 302 | expected_planner_frequency: 20.0 303 | planner_plugins: ["GridBased"] 304 | costmap_update_timeout: 1.0 305 | GridBased: 306 | plugin: "nav2_navfn_planner::NavfnPlanner" 307 | tolerance: 0.5 308 | use_astar: false 309 | allow_unknown: true 310 | 311 | smoother_server: 312 | ros__parameters: 313 | smoother_plugins: ["simple_smoother"] 314 | simple_smoother: 315 | plugin: "nav2_smoother::SimpleSmoother" 316 | tolerance: 1.0e-10 317 | max_its: 1000 318 | do_refinement: True 319 | 320 | behavior_server: 321 | ros__parameters: 322 | local_costmap_topic: local_costmap/costmap_raw 323 | global_costmap_topic: global_costmap/costmap_raw 324 | local_footprint_topic: local_costmap/published_footprint 325 | global_footprint_topic: global_costmap/published_footprint 326 | cycle_frequency: 10.0 327 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 328 | spin: 329 | plugin: "nav2_behaviors::Spin" 330 | backup: 331 | plugin: "nav2_behaviors::BackUp" 332 | drive_on_heading: 333 | plugin: "nav2_behaviors::DriveOnHeading" 334 | wait: 335 | plugin: "nav2_behaviors::Wait" 336 | assisted_teleop: 337 | plugin: "nav2_behaviors::AssistedTeleop" 338 | local_frame: odom 339 | global_frame: map 340 | robot_base_frame: base_link 341 | transform_tolerance: 0.1 342 | simulate_ahead_time: 2.0 343 | max_rotational_vel: 1.0 344 | min_rotational_vel: 0.4 345 | rotational_acc_lim: 3.2 346 | enable_stamped_cmd_vel: true 347 | 348 | waypoint_follower: 349 | ros__parameters: 350 | loop_rate: 20 351 | stop_on_failure: false 352 | action_server_result_timeout: 900.0 353 | waypoint_task_executor_plugin: "wait_at_waypoint" 354 | wait_at_waypoint: 355 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 356 | enabled: True 357 | waypoint_pause_duration: 200 358 | 359 | velocity_smoother: 360 | ros__parameters: 361 | smoothing_frequency: 20.0 362 | scale_velocities: False 363 | feedback: "OPEN_LOOP" 364 | max_velocity: [0.5, 0.0, 2.0] 365 | min_velocity: [-0.5, 0.0, -2.0] 366 | max_accel: [2.5, 0.0, 3.2] 367 | max_decel: [-2.5, 0.0, -3.2] 368 | odom_topic: "/demo/odom" 369 | odom_duration: 0.1 370 | deadband_velocity: [0.0, 0.0, 0.0] 371 | velocity_timeout: 1.0 372 | enable_stamped_cmd_vel: true 373 | 374 | collision_monitor: 375 | ros__parameters: 376 | base_frame_id: "base_footprint" 377 | odom_frame_id: "odom" 378 | cmd_vel_in_topic: "cmd_vel_smoothed" 379 | cmd_vel_out_topic: "/demo/cmd_vel" 380 | enable_stamped_cmd_vel: true 381 | state_topic: "collision_monitor_state" 382 | transform_tolerance: 0.2 383 | source_timeout: 1.0 384 | base_shift_correction: True 385 | stop_pub_timeout: 2.0 386 | # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, 387 | # and robot footprint for "approach" action type. 388 | polygons: ["FootprintApproach"] 389 | FootprintApproach: 390 | type: "polygon" 391 | action_type: "approach" 392 | footprint_topic: "local_costmap/published_footprint" 393 | time_before_collision: 1.2 394 | simulation_time_step: 0.1 395 | min_points: 6 396 | visualize: False 397 | enabled: True 398 | observation_sources: ["scan"] 399 | scan: 400 | type: "scan" 401 | topic: "scan" 402 | min_height: 0.15 403 | max_height: 2.0 404 | enabled: True 405 | 406 | docking_server: 407 | ros__parameters: 408 | controller_frequency: 50.0 409 | initial_perception_timeout: 5.0 410 | wait_charge_timeout: 5.0 411 | dock_approach_timeout: 30.0 412 | undock_linear_tolerance: 0.05 413 | undock_angular_tolerance: 0.1 414 | max_retries: 3 415 | base_frame: "base_link" 416 | fixed_frame: "odom" 417 | dock_backwards: false 418 | dock_prestaging_tolerance: 0.5 419 | 420 | # Types of docks 421 | dock_plugins: ['simple_charging_dock'] 422 | simple_charging_dock: 423 | plugin: 'opennav_docking::SimpleChargingDock' 424 | docking_threshold: 0.05 425 | staging_x_offset: -0.7 426 | use_external_detection_pose: true 427 | use_battery_status: false # true 428 | use_stall_detection: false # true 429 | 430 | external_detection_timeout: 1.0 431 | external_detection_translation_x: -0.18 432 | external_detection_translation_y: 0.0 433 | external_detection_rotation_roll: -1.57 434 | external_detection_rotation_pitch: -1.57 435 | external_detection_rotation_yaw: 0.0 436 | filter_coef: 0.1 437 | 438 | # Dock instances 439 | # The following example illustrates configuring dock instances. 440 | # docks: ['home_dock'] # Input your docks here 441 | # home_dock: 442 | # type: 'simple_charging_dock' 443 | # frame: map 444 | # pose: [0.0, 0.0, 0.0] 445 | 446 | controller: 447 | k_phi: 3.0 448 | k_delta: 2.0 449 | v_linear_min: 0.15 450 | v_linear_max: 0.15 451 | use_collision_detection: true 452 | costmap_topic: "/local_costmap/costmap_raw" 453 | footprint_topic: "/local_costmap/published_footprint" 454 | transform_tolerance: 0.1 455 | projection_time: 5.0 456 | simulation_step: 0.1 457 | dock_collision_threshold: 0.3 458 | 459 | loopback_simulator: 460 | ros__parameters: 461 | base_frame_id: "base_footprint" 462 | odom_frame_id: "odom" 463 | map_frame_id: "map" 464 | scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' 465 | update_duration: 0.02 466 | enable_stamped_cmd_vel: true 467 | -------------------------------------------------------------------------------- /sam_bot_description/launch/display.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | from launch.substitutions import Command, LaunchConfiguration 8 | from launch_ros.actions import Node 9 | from ros_gz_bridge.actions import RosGzBridge 10 | from ros_gz_sim.actions import GzServer 11 | 12 | 13 | def generate_launch_description(): 14 | pkg_share = get_package_share_directory('sam_bot_description') 15 | ros_gz_sim_share = get_package_share_directory('ros_gz_sim') 16 | gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py") 17 | default_model_path = os.path.join(pkg_share, 'src', 'description', 'sam_bot_description.sdf') 18 | default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'config.rviz') 19 | world_path = os.path.join(pkg_share, 'world', 'my_world.sdf') 20 | bridge_config_path = os.path.join(pkg_share, 'config', 'bridge_config.yaml') 21 | 22 | robot_state_publisher_node = Node( 23 | package='robot_state_publisher', 24 | executable='robot_state_publisher', 25 | parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}, {'use_sim_time': LaunchConfiguration('use_sim_time')}] 26 | ) 27 | rviz_node = Node( 28 | package='rviz2', 29 | executable='rviz2', 30 | name='rviz2', 31 | output='screen', 32 | arguments=['-d', LaunchConfiguration('rvizconfig')], 33 | ) 34 | gz_server = GzServer( 35 | world_sdf_file=world_path, 36 | container_name='ros_gz_container', 37 | create_own_container='True', 38 | use_composition='True', 39 | ) 40 | ros_gz_bridge = RosGzBridge( 41 | bridge_name='ros_gz_bridge', 42 | config_file=bridge_config_path, 43 | container_name='ros_gz_container', 44 | create_own_container='False', 45 | use_composition='True', 46 | ) 47 | camera_bridge_image = Node( 48 | package='ros_gz_image', 49 | executable='image_bridge', 50 | name='bridge_gz_ros_camera_image', 51 | output='screen', 52 | parameters=[{'use_sim_time': True}], 53 | arguments=['/depth_camera/image'], 54 | ) 55 | camera_bridge_depth = Node( 56 | package='ros_gz_image', 57 | executable='image_bridge', 58 | name='bridge_gz_ros_camera_depth', 59 | output='screen', 60 | parameters=[{'use_sim_time': True}], 61 | arguments=['/depth_camera/depth_image'], 62 | ) 63 | spawn_entity = IncludeLaunchDescription( 64 | PythonLaunchDescriptionSource(gz_spawn_model_launch_source), 65 | launch_arguments={ 66 | 'world': 'my_world', 67 | 'topic': '/robot_description', 68 | 'entity_name': 'sam_bot', 69 | 'z': '0.65', 70 | }.items(), 71 | ) 72 | robot_localization_node = Node( 73 | package='robot_localization', 74 | executable='ekf_node', 75 | name='ekf_node', 76 | output='screen', 77 | parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}], 78 | ) 79 | 80 | return LaunchDescription([ 81 | DeclareLaunchArgument(name='model', default_value=default_model_path, description='Absolute path to robot model file'), 82 | DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, description='Absolute path to rviz config file'), 83 | DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'), 84 | ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen'), 85 | robot_state_publisher_node, 86 | rviz_node, 87 | gz_server, 88 | ros_gz_bridge, 89 | camera_bridge_image, 90 | camera_bridge_depth, 91 | spawn_entity, 92 | robot_localization_node, 93 | ]) 94 | -------------------------------------------------------------------------------- /sam_bot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sam_bot_description 5 | 1.0.0 6 | URDF Setup Demo 7 | Josen Daniel De Leon 8 | Apache 2.0 9 | 10 | ament_cmake 11 | 12 | robot_localization 13 | robot_state_publisher 14 | ros_gz_bridge 15 | ros_gz_sim 16 | rviz2 17 | sdformat_urdf 18 | xacro 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /sam_bot_description/rviz/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1/Links1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 557 13 | Visualization Manager: 14 | Class: "" 15 | Displays: 16 | - Alpha: 0.5 17 | Cell Size: 1 18 | Class: rviz_default_plugins/Grid 19 | Color: 160; 160; 164 20 | Enabled: true 21 | Name: Grid 22 | - Alpha: 0.6 23 | Class: rviz_default_plugins/RobotModel 24 | Description Topic: 25 | Depth: 5 26 | Durability Policy: Volatile 27 | History Policy: Keep Last 28 | Reliability Policy: Reliable 29 | Value: /robot_description 30 | Enabled: true 31 | Name: RobotModel 32 | Visual Enabled: true 33 | - Class: rviz_default_plugins/TF 34 | Enabled: true 35 | Name: TF 36 | Marker Scale: 0.3 37 | Show Arrows: true 38 | Show Axes: true 39 | Show Names: true 40 | - Class: rviz_default_plugins/Map 41 | Enabled: false 42 | Name: Map 43 | Topic: 44 | Depth: 5 45 | Durability Policy: Transient Local 46 | History Policy: Keep Last 47 | Reliability Policy: Reliable 48 | Value: /map 49 | Update Topic: 50 | Depth: 5 51 | Durability Policy: Transient Local 52 | History Policy: Keep Last 53 | Reliability Policy: Reliable 54 | Value: /map_updates 55 | Use Timestamp: false 56 | Value: true 57 | Enabled: true 58 | Global Options: 59 | Background Color: 48; 48; 48 60 | Fixed Frame: base_link 61 | Frame Rate: 30 62 | Name: root 63 | Tools: 64 | - Class: rviz_default_plugins/Interact 65 | Hide Inactive Objects: true 66 | - Class: rviz_default_plugins/MoveCamera 67 | - Class: rviz_default_plugins/Select 68 | - Class: rviz_default_plugins/FocusCamera 69 | - Class: rviz_default_plugins/Measure 70 | Line color: 128; 128; 0 71 | Transformation: 72 | Current: 73 | Class: rviz_default_plugins/TF 74 | Value: true 75 | Views: 76 | Current: 77 | Class: rviz_default_plugins/Orbit 78 | Name: Current View 79 | Target Frame: 80 | Value: Orbit (rviz) 81 | Saved: ~ 82 | -------------------------------------------------------------------------------- /sam_bot_description/src/description/sam_bot_description.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0 0 0 ${pi/2} 0 ${pi/2} 22 | ${m} 23 | 24 | ${(m/12) * (h*h + d*d)} 25 | 0.0 26 | 0.0 27 | ${(m/12) * (w*w + d*d)} 28 | 0.0 29 | ${(m/12) * (w*w + h*h)} 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0 0 0 ${pi/2} 0 0 37 | ${m} 38 | 39 | ${(m/12) * (3*r*r + h*h)} 40 | 0.0 41 | 0.0 42 | ${(m/12) * (3*r*r + h*h)} 43 | 0.0 44 | ${(m/2) * (r*r)} 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | ${m} 53 | 54 | ${(2/5) * m * (r*r)} 55 | 0.0 56 | 0.0 57 | ${(2/5) * m * (r*r)} 58 | 0.0 59 | ${(2/5) * m * (r*r)} 60 | 61 | 62 | 63 | 64 | 65 | 66 | true 67 | 68 | 69 | 70 | ${base_length} ${base_width} ${base_height} 71 | 72 | 73 | 74 | 0 1 1 1 75 | 0 1 1 1 76 | 77 | 78 | 79 | 80 | 81 | 82 | ${base_length} ${base_width} ${base_height} 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | base_link 96 | base_footprint 97 | 0.0 0.0 ${-(wheel_radius+wheel_zoff)} 0 0 0 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 0 0 0 ${pi/2} 0 0 108 | 109 | 110 | ${wheel_radius} 111 | ${wheel_width} 112 | 113 | 114 | 115 | 0.3 0.3 0.3 1.0 116 | 0.7 0.7 0.7 1.0 117 | 118 | 119 | 120 | 121 | 0 0 0 ${pi/2} 0 0 122 | 123 | 124 | ${wheel_radius} 125 | ${wheel_width} 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | base_link 135 | ${prefix}_link 136 | ${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff} 0 0 0 137 | 138 | 0 1 0 139 | 140 | -inf 141 | inf 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | ${(wheel_radius+wheel_zoff-(base_height/2))} 157 | 158 | 159 | 160 | 0 1 1 1 161 | 0 1 1 1 162 | 163 | 164 | 165 | 166 | 167 | 168 | ${(wheel_radius+wheel_zoff-(base_height/2))} 169 | 170 | 171 | 172 | 0.001 173 | 0.001 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | base_link 182 | front_caster 183 | ${caster_xoff} 0.0 ${-(base_height/2)} 0 0 0 184 | 185 | 186 | 187 | base_link 188 | imu_link 189 | 0.0 0.0 0.01 0 0 0 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 0.1 0.1 0.1 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 0.1 0.1 0.1 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | true 214 | 100 215 | true 216 | demo/imu 217 | imu_link 218 | 219 | 220 | 221 | 222 | 0.0 223 | 2e-4 224 | 0.0000075 225 | 0.0000008 226 | 227 | 228 | 229 | 230 | 0.0 231 | 2e-4 232 | 0.0000075 233 | 0.0000008 234 | 235 | 236 | 237 | 238 | 0.0 239 | 2e-4 240 | 0.0000075 241 | 0.0000008 242 | 243 | 244 | 245 | 246 | 247 | 248 | 0.0 249 | 1.7e-2 250 | 0.1 251 | 0.001 252 | 253 | 254 | 255 | 256 | 0.0 257 | 1.7e-2 258 | 0.1 259 | 0.001 260 | 261 | 262 | 263 | 264 | 0.0 265 | 1.7e-2 266 | 0.1 267 | 0.001 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | drivewhl_l_joint 278 | drivewhl_r_joint 279 | 280 | 281 | 0.4 282 | ${wheel_radius} 283 | 284 | 285 | 0.1 286 | 287 | 288 | /demo/cmd_vel 289 | 290 | 291 | /demo/odom 292 | /tf 293 | 294 | odom 295 | base_link 296 | 297 | 298 | 301 | joint_states 302 | 303 | 304 | 305 | base_link 306 | lidar_link 307 | 0.0 0.0 0.12 0 0 0 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 0.0508 316 | 0.055 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 0.0508 325 | 0.055 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | true 334 | true 335 | 5 336 | scan 337 | lidar_link 338 | 339 | 340 | 341 | 360 342 | 1.000000 343 | 0.000000 344 | 6.280000 345 | 346 | 347 | 348 | 0.120000 349 | 3.5 350 | 0.015000 351 | 352 | 353 | gaussian 354 | 0.0 355 | 0.01 356 | 357 | 358 | 359 | 360 | 361 | 362 | base_link 363 | camera_link 364 | 0.215 0 0.05 0 0 0 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 0.015 0.130 0.0222 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 0.015 0.130 0.0222 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | true 389 | true 390 | 5.0 391 | depth_camera 392 | camera_link 393 | 394 | 1.047198 395 | 396 | 640 397 | 480 398 | 399 | 400 | 0.05 401 | 3 402 | 403 | 404 | 0.2 405 | 0.5 406 | 3.0 407 | 0 408 | 0 409 | 0 410 | 0 411 | 0 412 | 0 413 | 0 414 | 0 415 | 0 416 | 0 417 | 418 | 419 | 420 | 421 | -------------------------------------------------------------------------------- /sam_bot_description/src/description/sam_bot_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 4 | 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 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | -------------------------------------------------------------------------------- /sam_bot_description/world/my_world.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.001 5 | 1.0 6 | 7 | 10 | 11 | 14 | 15 | 18 | 19 | 21 | 22 | 25 | ogre2 26 | 27 | 28 | 1 29 | 0 0 10 0 -0 0 30 | 0.8 0.8 0.8 1 31 | 0.2 0.2 0.2 1 32 | 33 | 1000 34 | 0.9 35 | 0.01 36 | 0.001 37 | 38 | -0.5 0.1 -0.9 39 | 40 | 0 41 | 0 42 | 0 43 | 44 | 45 | 46 | 1 47 | 48 | 49 | 50 | 51 | 0 0 1 52 | 100 100 53 | 54 | 55 | 56 | 57 | 58 | 100 59 | 50 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 10 71 | 72 | 73 | 0 74 | 75 | 76 | 0 0 1 77 | 100 100 78 | 79 | 80 | 81 | 85 | 86 | 87 | 0 88 | 0 89 | 0 90 | 91 | 92 | 0 0 -9.8 93 | 6e-06 2.3e-05 -4.2e-05 94 | 95 | 96 | 0.001 97 | 1 98 | 1000 99 | 100 | 101 | 0.4 0.4 0.4 1 102 | 0.7 0.7 0.7 1 103 | 1 104 | 105 | 106 | 107 | EARTH_WGS84 108 | 0 109 | 0 110 | 0 111 | 0 112 | 113 | 114 | 1.51271 -0.181418 0.5 0 -0 0 115 | 116 | 117 | 1 118 | 119 | 0.166667 120 | 0 121 | 0 122 | 0.166667 123 | 0 124 | 0.166667 125 | 126 | 0 0 0 0 -0 0 127 | 128 | 129 | 130 | 131 | 1 1 1 132 | 133 | 134 | 10 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 1 1 1 152 | 153 | 154 | 155 | 159 | 160 | 161 | 0 162 | 0 163 | 0 164 | 165 | 166 | 167 | -1.89496 2.36764 0.5 0 -0 0 168 | 169 | 170 | 1 171 | 172 | 0.1 173 | 0 174 | 0 175 | 0.1 176 | 0 177 | 0.1 178 | 179 | 0 0 0 0 -0 0 180 | 181 | 182 | 183 | 184 | 0.5 185 | 186 | 187 | 10 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 0.5 205 | 206 | 207 | 208 | 212 | 213 | 214 | 0 215 | 0 216 | 0 217 | 218 | 219 | 220 | 0 0 221 | 0 0 222 | 1626668720 808592627 223 | 0 224 | 225 | 0 0 0 0 -0 0 226 | 1 1 1 227 | 228 | 0 0 0 0 -0 0 229 | 0 0 0 0 -0 0 230 | 0 0 0 0 -0 0 231 | 0 0 0 0 -0 0 232 | 233 | 234 | 235 | 1.51272 -0.181418 0.499995 0 1e-05 0 236 | 1 1 1 237 | 238 | 1.51272 -0.181418 0.499995 0 1e-05 0 239 | 0 0 0 0 -0 0 240 | 0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05 241 | 0.010615 -0.006191 -9.78231 0 -0 0 242 | 243 | 244 | 245 | -0.725833 1.36206 0.5 0 -0 0 246 | 1 1 1 247 | 248 | -0.944955 1.09802 0.5 0 -0 0 249 | 0 0 0 0 -0 0 250 | 0 0 0 0 -0 0 251 | 0 0 0 0 -0 0 252 | 253 | 254 | 255 | 0 0 10 0 -0 0 256 | 257 | 258 | 259 | 260 | 3.17226 -5.10401 6.58845 0 0.739643 2.19219 261 | orbit 262 | perspective 263 | 264 | 265 | 266 | 267 | --------------------------------------------------------------------------------