From 551a10da77808058f51680bfbf0ed27c2bbd78ea Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 16 Oct 2023 15:39:41 +0200 Subject: [PATCH 01/12] Update robot_joy_teleop.launch.py --- launch/robot_joy_teleop.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/robot_joy_teleop.launch.py b/launch/robot_joy_teleop.launch.py index 747444b..8806c54 100644 --- a/launch/robot_joy_teleop.launch.py +++ b/launch/robot_joy_teleop.launch.py @@ -80,7 +80,8 @@ def generate_launch_description(): teleop_spawner = Node( package="rmp220_teleop", executable="rmp220_teleop", - remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] + #remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] + remappings=[('/cmd_vel','/diffbot_base_controller/cmd_vel_unstamped')] ) cam_node = Node( @@ -117,4 +118,4 @@ def generate_launch_description(): #cam_node, #lidar_node, #twist_mux - ]) \ No newline at end of file + ]) From 2e1888e22e4adae484d8adf03944c327289a06df Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 08:24:37 +0100 Subject: [PATCH 02/12] Update README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index a03106d..ab43326 100644 --- a/README.md +++ b/README.md @@ -1 +1,2 @@ -# bot_mini_bringup +# cps_loki_bringup +This repo houses ROS2 bringup package for CPS Loki robot (small green one). From 7c579e01ddbc37ae9d0a47b112c10822c1669567 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 08:26:38 +0100 Subject: [PATCH 03/12] Update package.xml --- package.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/package.xml b/package.xml index 11a1ae4..b48721d 100644 --- a/package.xml +++ b/package.xml @@ -1,9 +1,9 @@ - bot_mini_bringup - 0.0.0 - TODO: Package description + cps_loki_bringup + 0.0.2 + ROS2 bringup package for CPS Loki Robot. bjorn TODO: License declaration @@ -13,7 +13,7 @@ python3-pytest odrive_demo_description - rmp220_teleop + teleop_twist_joy nav2_bringup navigation2 ros2_control From 7bc15326281b1fbe8b2c82c45ca3dbbfff1624a6 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 08:28:27 +0100 Subject: [PATCH 04/12] update --- {bot_mini_bringup => cps_loki_bringup}/__init__.py | 0 resource/{bot_mini_bringup => cps_loki_bringup} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename {bot_mini_bringup => cps_loki_bringup}/__init__.py (100%) rename resource/{bot_mini_bringup => cps_loki_bringup} (100%) diff --git a/bot_mini_bringup/__init__.py b/cps_loki_bringup/__init__.py similarity index 100% rename from bot_mini_bringup/__init__.py rename to cps_loki_bringup/__init__.py diff --git a/resource/bot_mini_bringup b/resource/cps_loki_bringup similarity index 100% rename from resource/bot_mini_bringup rename to resource/cps_loki_bringup From cf665b848ff5c3f2d91c5583f38b6ed907a89923 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 08:31:54 +0100 Subject: [PATCH 05/12] Update setup.cfg --- setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index c10ffd0..bdd18b5 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,4 +1,4 @@ [develop] -script_dir=$base/lib/bot_mini_bringup +script_dir=$base/lib/cps_loki_bringup [install] -install_scripts=$base/lib/bot_mini_bringup +install_scripts=$base/lib/cps_loki_bringup From 5d9c2de9eedb6afbab5108f06ab0c1aa2668811d Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 10:04:21 +0100 Subject: [PATCH 06/12] update --- launch/full_bot_mini.launch.py | 4 ++-- launch/min_bot_mini.launch.py | 4 ++-- launch/nav2.launch.py | 2 +- launch/robot_cam.launch.py | 4 ++-- launch/robot_controller.launch.py | 4 ++-- launch/robot_joy_teleop.launch.py | 6 +++--- launch/robot_lidar.launch.py | 4 ++-- launch/robot_localization.launch.py | 6 +++--- launch/robot_mapper.launch.py | 6 +++--- launch/robot_navigation.launch.py | 2 +- launch/robot_twist_mux.launch.py | 6 +++--- launch/rsp.launch.py | 4 ++-- launch/sim.launch.py | 10 +++++----- launch/sim_bot_mini.launch.py | 4 ++-- setup.py | 2 +- 15 files changed, 34 insertions(+), 34 deletions(-) diff --git a/launch/full_bot_mini.launch.py b/launch/full_bot_mini.launch.py index 942cb24..5f2abcb 100644 --- a/launch/full_bot_mini.launch.py +++ b/launch/full_bot_mini.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/min_bot_mini.launch.py b/launch/min_bot_mini.launch.py index e7c4d5d..4c892d8 100644 --- a/launch/min_bot_mini.launch.py +++ b/launch/min_bot_mini.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/nav2.launch.py b/launch/nav2.launch.py index f016df9..3c5ca58 100644 --- a/launch/nav2.launch.py +++ b/launch/nav2.launch.py @@ -28,7 +28,7 @@ from nav2_common.launch import RewrittenYaml def generate_launch_description(): # Get the launch directory - bringup_dir = get_package_share_directory('bot_mini_bringup') + bringup_dir = get_package_share_directory('cps_loki_bringup') namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') diff --git a/launch/robot_cam.launch.py b/launch/robot_cam.launch.py index f3b7a60..30ae325 100644 --- a/launch/robot_cam.launch.py +++ b/launch/robot_cam.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/robot_controller.launch.py b/launch/robot_controller.launch.py index 3dbdbb3..e1a337f 100644 --- a/launch/robot_controller.launch.py +++ b/launch/robot_controller.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/robot_joy_teleop.launch.py b/launch/robot_joy_teleop.launch.py index 8806c54..5d2e475 100644 --- a/launch/robot_joy_teleop.launch.py +++ b/launch/robot_joy_teleop.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] @@ -100,7 +100,7 @@ def generate_launch_description(): parameters=[lidar_dir], ) - twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux_params = os.path.join(get_package_share_directory('cps_loki_bringup'),'config','twist_mux.yaml') twist_mux = Node( package="twist_mux", executable="twist_mux", diff --git a/launch/robot_lidar.launch.py b/launch/robot_lidar.launch.py index d64648c..cf4af81 100644 --- a/launch/robot_lidar.launch.py +++ b/launch/robot_lidar.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/robot_localization.launch.py b/launch/robot_localization.launch.py index 36c61eb..e969f6a 100644 --- a/launch/robot_localization.launch.py +++ b/launch/robot_localization.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -41,7 +41,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] @@ -91,7 +91,7 @@ def generate_launch_description(): use_sim_time = False slam_params_file = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "mapper_params_online_async.yaml" ] diff --git a/launch/robot_mapper.launch.py b/launch/robot_mapper.launch.py index 36c61eb..e969f6a 100644 --- a/launch/robot_mapper.launch.py +++ b/launch/robot_mapper.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -41,7 +41,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] @@ -91,7 +91,7 @@ def generate_launch_description(): use_sim_time = False slam_params_file = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "mapper_params_online_async.yaml" ] diff --git a/launch/robot_navigation.launch.py b/launch/robot_navigation.launch.py index f016df9..3c5ca58 100644 --- a/launch/robot_navigation.launch.py +++ b/launch/robot_navigation.launch.py @@ -28,7 +28,7 @@ from nav2_common.launch import RewrittenYaml def generate_launch_description(): # Get the launch directory - bringup_dir = get_package_share_directory('bot_mini_bringup') + bringup_dir = get_package_share_directory('cps_loki_bringup') namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') diff --git a/launch/robot_twist_mux.launch.py b/launch/robot_twist_mux.launch.py index 33c1b01..013e231 100644 --- a/launch/robot_twist_mux.launch.py +++ b/launch/robot_twist_mux.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] @@ -99,7 +99,7 @@ def generate_launch_description(): parameters=[lidar_dir], ) - twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux_params = os.path.join(get_package_share_directory('cps_loki_bringup'),'config','twist_mux.yaml') twist_mux = Node( package="twist_mux", executable="twist_mux", diff --git a/launch/rsp.launch.py b/launch/rsp.launch.py index 4dbe3a5..0463cd8 100644 --- a/launch/rsp.launch.py +++ b/launch/rsp.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -40,7 +40,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] diff --git a/launch/sim.launch.py b/launch/sim.launch.py index a8dcfa6..5799833 100644 --- a/launch/sim.launch.py +++ b/launch/sim.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("bot_mini_description"), + FindPackageShare("cps_loki_description"), "urdf", "odrive_diffbot.urdf.xacro" ] @@ -43,7 +43,7 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "diffbot_controllers.yaml", ] @@ -88,7 +88,7 @@ def generate_launch_description(): joystick_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('bot_mini_bringup'),'joy_teleop.launch.py' + get_package_share_directory('cps_loki_bringup'),'joy_teleop.launch.py' )]), launch_arguments={'use_sim_time': 'true'}.items() ) @@ -108,7 +108,7 @@ def generate_launch_description(): parameters=[lidar_dir], ) - twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux_params = os.path.join(get_package_share_directory('cps_loki_bringup'),'config','twist_mux.yaml') twist_mux = Node( package="twist_mux", executable="twist_mux", @@ -116,7 +116,7 @@ def generate_launch_description(): remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')] ) - gazebo_params_file = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','gazebo_params.yaml') + gazebo_params_file = os.path.join(get_package_share_directory('cps_loki_bringup'),'config','gazebo_params.yaml') # Include the Gazebo launch file, provided by the gazebo_ros package gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( diff --git a/launch/sim_bot_mini.launch.py b/launch/sim_bot_mini.launch.py index aa4c26c..dcdbd3d 100644 --- a/launch/sim_bot_mini.launch.py +++ b/launch/sim_bot_mini.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): rviz_config_file = PathJoinSubstitution( [ - FindPackageShare("bot_mini_bringup"), + FindPackageShare("cps_loki_bringup"), "config", "bot_mini.rviz" ] @@ -87,7 +87,7 @@ def generate_launch_description(): arguments=["-d", rviz_config_file], ) - package_name='bot_mini_bringup' + package_name='cps_loki_bringup' gazebo_params_file = os.path.join(get_package_share_directory(package_name),'config','gazebo_params.yaml') # Include the Gazebo launch file, provided by the gazebo_ros package diff --git a/setup.py b/setup.py index 0ce3dbe..f3c8ecc 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ from setuptools import setup import os from glob import glob -package_name = 'bot_mini_bringup' +package_name = 'cps_loki_bringup' setup( name=package_name, From d2170ea534ed96b8e60681fba52a3c3c3abb3312 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 10:13:22 +0100 Subject: [PATCH 07/12] update --- config/mapper_params_online_async.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index c50ffda..732a434 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -19,7 +19,7 @@ slam_toolbox: # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given # will use pose - map_file_name: /home/bjorn/Documents/ros-projects/cps_bot_mini_ws/my_map_serial + #map_file_name: /home/bjorn/Documents/ros-projects/cps_bot_mini_ws/my_map_serial # map_start_pose: [0.0, 0.0, 0.0] map_start_at_dock: true From 012a762069f3bc2b598fe88556ec3b7f8e82ae89 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 10:28:28 +0100 Subject: [PATCH 08/12] update --- .vscode/c_cpp_properties.json | 22 ---------------------- .vscode/settings.json | 14 -------------- 2 files changed, 36 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 710c782..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,22 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/install/odrive_hardware_interface/include/**", - "/opt/ros/humble/include/**", - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/src/odrive_ros2_control/odrive_hardware_interface/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 3de7662..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/install/rmp220_teleop/lib/python3.10/site-packages", - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/install/cam_opencv/lib/python3.10/site-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "python.analysis.extraPaths": [ - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/install/rmp220_teleop/lib/python3.10/site-packages", - "/home/bjorn/Documents/ros-projects/cps_bot_mini_ws/install/cam_opencv/lib/python3.10/site-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ] -} \ No newline at end of file From 8f25e51a60b6491270b8c7d2f29f24120400cb76 Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Mon, 20 Nov 2023 10:28:48 +0100 Subject: [PATCH 09/12] update --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..600d2d3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode \ No newline at end of file From 9b688e2c4dfac7fb43019b281e9c063199f6cf7f Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Tue, 21 Nov 2023 08:01:57 +0100 Subject: [PATCH 10/12] update --- config/nav2_params copy.yaml | 348 +++++++++++++++++++++++++++++++++++ config/nav2_params.yaml | 323 +++++++++++++++++++------------- 2 files changed, 548 insertions(+), 123 deletions(-) create mode 100644 config/nav2_params copy.yaml diff --git a/config/nav2_params copy.yaml b/config/nav2_params copy.yaml new file mode 100644 index 0000000..14fe6e4 --- /dev/null +++ b/config/nav2_params copy.yaml @@ -0,0 +1,348 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 14fe6e4..a6ac7cf 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -1,3 +1,79 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + #scan_topic: /scan + scan_topic: /scan_filtered + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + amcl: ros__parameters: use_sim_time: True @@ -14,7 +90,7 @@ amcl: global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 + laser_max_range: 7.0 #was 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 @@ -37,14 +113,14 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 - scan_topic: scan + scan_topic: scan_filtered bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /odometry/filtered bt_loop_duration: 10 default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -52,52 +128,53 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: @@ -110,73 +187,49 @@ bt_navigator_navigate_to_pose_rclcpp_node: controller_server: ros__parameters: use_sim_time: True - controller_frequency: 20.0 + controller_frequency: 100.0 # was 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] - # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True - general_goal_checker: - stateful: True + goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True + xy_goal_tolerance: 0.25 # was 0.25 + yaw_goal_tolerance: 0.25 # was 0.25 stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.007 # was 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.25 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: false # was true, cannot be set together with allow_reversing + allow_reversing: true # was false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true # was false local_costmap: local_costmap: @@ -190,11 +243,12 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.22 + #robot_radius: 0.22 + footprint: "[ [0.18, 0.200], [0.18, -0.200], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 + cost_scaling_factor: 10.0 # was 3.0 inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" @@ -205,9 +259,9 @@ local_costmap: z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan + observation_sources: rplidar oakd + rplidar: + topic: /scan_filtered max_obstacle_height: 2.0 clearing: True marking: True @@ -216,6 +270,17 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 + oak-d: # no frame set, uses frame from message + topic: /stereo/points + max_obstacle_height: 1.5 + min_obstacle_height: 0.02 + obstacle_max_range: 3.0 + obstacle_min_range: 0.0 + raytrace_max_range: 3.2 + raytrace_min_range: 0.0 + clearing: True + marking: True + data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -229,7 +294,8 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: True - robot_radius: 0.22 + #robot_radius: 0.22 + footprint: "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] @@ -238,7 +304,7 @@ global_costmap: enabled: True observation_sources: scan scan: - topic: /scan + topic: /scan_filtered max_obstacle_height: 2.0 clearing: True marking: True @@ -252,7 +318,7 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 + cost_scaling_factor: 10.0 # was 3.0 inflation_radius: 0.55 always_send_full_costmap: True @@ -270,17 +336,28 @@ map_saver: free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True - + planner_server: ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + use_sim_time: True + GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: true # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 0.1 smoother_server: ros__parameters: @@ -337,12 +414,12 @@ velocity_smoother: use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False - feedback: "OPEN_LOOP" + feedback: "CLOSED_LOOP" # was OPEN_LOOP max_velocity: [0.26, 0.0, 1.0] min_velocity: [-0.26, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 + odom_topic: "/odometry/filtered" + odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 \ No newline at end of file From d30da7874a117b91610e75e0ba1896febc4d389e Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Tue, 21 Nov 2023 08:06:14 +0100 Subject: [PATCH 11/12] update --- config/nav2_params.yaml | 39 ++++++++++++++------------------------- 1 file changed, 14 insertions(+), 25 deletions(-) diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index a6ac7cf..deb59a4 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -13,8 +13,8 @@ slam_toolbox: odom_frame: odom map_frame: map base_frame: base_footprint - #scan_topic: /scan - scan_topic: /scan_filtered + scan_topic: /scan + #scan_topic: /scan_filtered use_map_saver: true mode: mapping #localization @@ -35,7 +35,7 @@ slam_toolbox: transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true + enable_interactive_mode: false # General Parameters use_scan_matching: true @@ -113,14 +113,14 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 - scan_topic: scan_filtered + scan_topic: scan bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odometry/filtered + odom_topic: odom bt_loop_duration: 10 default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -259,9 +259,9 @@ local_costmap: z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: rplidar oakd - rplidar: - topic: /scan_filtered + observation_sources: lslidar + lslidar: + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -270,17 +270,6 @@ local_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - oak-d: # no frame set, uses frame from message - topic: /stereo/points - max_obstacle_height: 1.5 - min_obstacle_height: 0.02 - obstacle_max_range: 3.0 - obstacle_min_range: 0.0 - raytrace_max_range: 3.2 - raytrace_min_range: 0.0 - clearing: True - marking: True - data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -302,9 +291,9 @@ global_costmap: obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - observation_sources: scan - scan: - topic: /scan_filtered + observation_sources: lslidar + lslidar: + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -336,7 +325,7 @@ map_saver: free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True - + planner_server: ros__parameters: planner_plugins: ["GridBased"] @@ -414,12 +403,12 @@ velocity_smoother: use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False - feedback: "CLOSED_LOOP" # was OPEN_LOOP + feedback: "OPEN_LOOP" # was OPEN_LOOP max_velocity: [0.26, 0.0, 1.0] min_velocity: [-0.26, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] - odom_topic: "/odometry/filtered" + odom_topic: "odom" odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 \ No newline at end of file From 0f91f188d84761a5593af19c6dc813528ca7afbf Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Wed, 22 Nov 2023 11:24:19 +0100 Subject: [PATCH 12/12] update --- config/nav2_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index deb59a4..7e9886d 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -244,7 +244,7 @@ local_costmap: height: 3 resolution: 0.05 #robot_radius: 0.22 - footprint: "[ [0.18, 0.200], [0.18, -0.200], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" + footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer"