diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro index 0fad2ca..9a9f04c 100644 --- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro +++ b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro @@ -7,13 +7,67 @@ controller_manager inside gz-sim and reads the controller yaml. Per-wheel mecanum friction is emitted by the wheel macro when gazebo_ignition (sim) is true, so it is NOT repeated here. + + controllers_file : primary controller params (the AGV base controllers). + controllers_file_extra : OPTIONAL second params file, loaded into the SAME + controller_manager. Used when the AGV is composed + into a larger robot (e.g. a mobile manipulator) so + the additional controllers (e.g. an arm_controller) + share the single gz_ros2_control controller_manager. + Empty by default -> standalone AGV unaffected. --> - + ${controllers_file} + + ${controllers_file_extra} + + + + + + 0 0 0 0 0 0 + scan + ${prefix}laser_link + 10 + 1 + true + + + + 360 + 1 + -3.141592653589793 + 3.141592653589793 + + + + + 0.15 + 12.0 + 0.01 + + + + \ No newline at end of file diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro index 696f688..e4e7282 100644 --- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro +++ b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro @@ -18,6 +18,13 @@ + + + + @@ -146,7 +153,8 @@ - + \ No newline at end of file diff --git a/src/elephant_robotics/agv_pro_gazebo/config/scan_filter.yaml b/src/elephant_robotics/agv_pro_gazebo/config/scan_filter.yaml new file mode 100644 index 0000000..e345816 --- /dev/null +++ b/src/elephant_robotics/agv_pro_gazebo/config/scan_filter.yaml @@ -0,0 +1,30 @@ +# laser_filters chain for the AGV Pro 2D lidar. +# +# The 360 deg lidar is mounted at the front of the robot (laser_link is ~0.179 m +# ahead of base_link), so its rearward beams strike the robot's own chassis and +# report returns from ~0.19 m out to ~0.38 m. Left unfiltered, those self-returns +# were baked into the SLAM map and both Nav2 costmaps, marking the robot's own +# cell lethal (cost 253) so the planner refused to plan from an in-collision +# start ("Failed to create plan with tolerance"). +# +# The box filter drops any scan point that falls inside the chassis outline +# (expressed in base_footprint), which removes the self-returns while keeping all +# real obstacles in front of and beside the robot. The physical robot runs the +# same filter, so the /scan consumed by SLAM/Nav2 is identical to simulation. +scan_filter_chain: + ros__parameters: + filter1: + name: chassis_box_filter + type: laser_filters/LaserScanBoxFilter + params: + box_frame: base_footprint + # Chassis extent in base_footprint (metres). The laser sits at x=+0.179, + # so the body occupies roughly x in [-0.35, 0.18] and y in [-0.30, 0.30]. + min_x: -0.35 + max_x: 0.18 + min_y: -0.30 + max_y: 0.30 + min_z: -1.0 + max_z: 1.0 + # false => remove points that fall INSIDE the box (the chassis). + invert: false diff --git a/src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py b/src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py index 41b1f45..a7c83c4 100644 --- a/src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py +++ b/src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py @@ -29,12 +29,13 @@ def generate_launch_description(): # Shared robot description (agv_pro_description) built for simulation. xacro_file = os.path.join(pkg_description, 'urdf', 'agv_pro.urdf.xacro') controllers_file = os.path.join(pkg_gazebo, 'config', 'agv_control.yaml') - world_file = os.path.join(pkg_gazebo, 'worlds', 'empty.world') + default_world = os.path.join(pkg_gazebo, 'worlds', 'empty.world') rviz_config = os.path.join(pkg_gazebo, 'rviz', 'agvpro_display.rviz') use_sim_time = LaunchConfiguration('use_sim_time') use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') + world = LaunchConfiguration('world') robot_description = { 'robot_description': ParameterValue( @@ -64,9 +65,8 @@ def generate_launch_description(): # Start gz-sim (Gazebo Harmonic) with the world. # '-s' (server only) is added when headless:=true. gz_args = PythonExpression([ - "'-r -v4 -s ' + ", repr(world_file), - " if '", headless, "' == 'true' ", - "else '-r -v4 ' + ", repr(world_file), + "'-r -v4 -s ' + '", world, "' if '", headless, + "' == 'true' else '-r -v4 ' + '", world, "'", ]) gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -104,6 +104,35 @@ def generate_launch_description(): output='screen', ) + # Bridge the 2D lidar: gz LaserScan -> ROS /scan_raw (sensor_msgs/LaserScan). + # The sensor is defined on laser_link in agv_pro.gazebo.xacro. The raw scan + # goes to /scan_raw and is cleaned by the box filter below before Nav2/SLAM + # consume /scan. use_sim_time so the scan stamps use the Gazebo clock. + scan_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan'], + parameters=[{'use_sim_time': use_sim_time}], + remappings=[('/scan', '/scan_raw')], + output='screen', + ) + + # Remove the robot's own chassis returns from the front-mounted lidar. + # The box filter (config/scan_filter.yaml) drops any point inside the chassis + # outline in base_footprint, publishing the cleaned scan on /scan. Without it + # the self-returns mark the robot's own cell lethal and Nav2 cannot plan. + scan_filter = Node( + package='laser_filters', + executable='scan_to_scan_filter_chain', + name='scan_filter_chain', + parameters=[ + os.path.join(pkg_gazebo, 'config', 'scan_filter.yaml'), + {'use_sim_time': use_sim_time}, + ], + remappings=[('scan', '/scan_raw'), ('scan_filtered', '/scan')], + output='screen', + ) + # Controllers (controller_manager runs inside the gz_ros2_control plugin) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -117,15 +146,20 @@ def generate_launch_description(): # Remap the controller's reference topic (~/reference) to the standard # /cmd_vel so teleop and nav2 (which publish geometry_msgs/TwistStamped) - # drive the robot directly. NOTE: the remap key must be the private name - # '~/reference' — a bare 'reference:=/cmd_vel' is silently ignored. + # drive the robot directly. Also remap the controller's private odometry + # outputs to the global names the rest of the stack expects: + # ~/tf_odometry -> /tf (so odom->base_footprint reaches the TF tree) + # ~/odometry -> /odom (nav2 odom_topic, robot_localization, etc.) + # NOTE: the remap keys must be the private names ('~/...'); a bare + # 'reference:=/cmd_vel' is silently ignored. mecanum_drive_controller_spawner = Node( package='controller_manager', executable='spawner', arguments=[ 'mecanum_drive_controller', '--controller-manager', '/controller_manager', - '--controller-ros-args', '-r ~/reference:=/cmd_vel', + '--controller-ros-args', + '-r ~/reference:=/cmd_vel -r ~/tf_odometry:=/tf -r ~/odometry:=/odom', ], output='screen', ) @@ -156,10 +190,17 @@ def generate_launch_description(): default_value='false', description='Run gz-sim without the GUI (server only)', ), + DeclareLaunchArgument( + 'world', + default_value=default_world, + description='Full path to the Gazebo world (SDF) to load', + ), gz_resource_path, gz_sim, robot_state_publisher, clock_bridge, + scan_bridge, + scan_filter, delayed_spawn, # Load controllers only after the robot has been spawned RegisterEventHandler( diff --git a/src/elephant_robotics/agv_pro_gazebo/package.xml b/src/elephant_robotics/agv_pro_gazebo/package.xml index 57e4ec0..572f4f9 100644 --- a/src/elephant_robotics/agv_pro_gazebo/package.xml +++ b/src/elephant_robotics/agv_pro_gazebo/package.xml @@ -18,6 +18,7 @@ ros_gz_sim ros_gz_bridge + laser_filters gz_ros2_control robot_state_publisher diff --git a/src/elephant_robotics/agv_pro_gazebo/worlds/room.world b/src/elephant_robotics/agv_pro_gazebo/worlds/room.world new file mode 100644 index 0000000..fc99534 --- /dev/null +++ b/src/elephant_robotics/agv_pro_gazebo/worlds/room.world @@ -0,0 +1,133 @@ + + + + + + + + + + + + + ogre2 + + + + 0.001 + 1.0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + 0 0 1100 100 + + + + + 0 0 1100 100 + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + true + + + + 4 0 0.5 0 0 0 + 0.1 8 1 + + + 4 0 0.5 0 0 0 + 0.1 8 1 + 0.6 0.6 0.65 10.6 0.6 0.65 1 + + + + -4 0 0.5 0 0 0 + 0.1 8 1 + + + -4 0 0.5 0 0 0 + 0.1 8 1 + 0.6 0.6 0.65 10.6 0.6 0.65 1 + + + + 0 4 0.5 0 0 0 + 8 0.1 1 + + + 0 4 0.5 0 0 0 + 8 0.1 1 + 0.6 0.6 0.65 10.6 0.6 0.65 1 + + + + 0 -4 0.5 0 0 0 + 8 0.1 1 + + + 0 -4 0.5 0 0 0 + 8 0.1 1 + 0.6 0.6 0.65 10.6 0.6 0.65 1 + + + + + + + true + + 1.5 1.0 0.5 0 0 0 + 0.251.0 + 0.251.0 + 0.7 0.5 0.3 10.7 0.5 0.3 1 + + + + true + + -1.8 -1.5 0.5 0 0 0 + 0.5 0.5 1.0 + 0.5 0.5 1.0 + 0.3 0.5 0.7 10.3 0.5 0.7 1 + + + + + diff --git a/src/elephant_robotics/agv_pro_navigation2/CMakeLists.txt b/src/elephant_robotics/agv_pro_navigation2/CMakeLists.txt index b885e3b..ff524ed 100644 --- a/src/elephant_robotics/agv_pro_navigation2/CMakeLists.txt +++ b/src/elephant_robotics/agv_pro_navigation2/CMakeLists.txt @@ -24,7 +24,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY launch map param rviz scripts + DIRECTORY config launch map param rviz scripts DESTINATION share/${PROJECT_NAME} ) diff --git a/src/elephant_robotics/agv_pro_navigation2/config/slam_toolbox_sim.yaml b/src/elephant_robotics/agv_pro_navigation2/config/slam_toolbox_sim.yaml new file mode 100644 index 0000000..bcbb178 --- /dev/null +++ b/src/elephant_robotics/agv_pro_navigation2/config/slam_toolbox_sim.yaml @@ -0,0 +1,46 @@ +# slam_toolbox online-async mapping config for the AGV Pro in simulation. +# Builds a map from /scan and publishes the map -> odom transform that Nav2 +# needs. Frames match the robot: odom (from mecanum_drive_controller) and +# base_footprint (robot root). use_sim_time is supplied by the launch file. +slam_toolbox: + ros__parameters: + # Frames / topics + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping + + # Solver + 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 + + # Mapping behaviour + map_update_interval: 1.0 + resolution: 0.05 + max_laser_range: 12.0 + minimum_time_interval: 0.2 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 + enable_interactive_mode: true + + # Scan matching + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.3 + minimum_travel_heading: 0.3 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 12.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 diff --git a/src/elephant_robotics/agv_pro_navigation2/launch/simulation.launch.py b/src/elephant_robotics/agv_pro_navigation2/launch/simulation.launch.py new file mode 100644 index 0000000..f68b256 --- /dev/null +++ b/src/elephant_robotics/agv_pro_navigation2/launch/simulation.launch.py @@ -0,0 +1,115 @@ +""" +simulation.launch.py — Nav2 + SLAM in Gazebo for the AGV Pro. + +Brings up the full autonomous-navigation stack in simulation: + + 1. Gazebo (agv_pro_gazebo.launch.py) in the walled `room.world`, with the + 2D lidar publishing /scan and the mecanum_drive_controller publishing + odom -> base_footprint (TF) and /odom, consuming /cmd_vel. + 2. slam_toolbox (online async) — builds a map from /scan and publishes + map -> odom. + 3. Nav2 (navigation_launch.py) — planner / controller (MPPI, Omni motion + model for the holonomic base) / behaviours / bt_navigator, using + param/nav2_sim.yaml. + 4. RViz with the navigation view. + +Everything runs on the Gazebo clock (use_sim_time:=true). + +Usage: + ros2 launch agv_pro_navigation2 simulation.launch.py + ros2 launch agv_pro_navigation2 simulation.launch.py headless:=true + ros2 launch agv_pro_navigation2 simulation.launch.py use_rviz:=false +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_gazebo = get_package_share_directory('agv_pro_gazebo') + pkg_nav2 = get_package_share_directory('agv_pro_navigation2') + pkg_slam = get_package_share_directory('slam_toolbox') + pkg_nav2_bringup = get_package_share_directory('nav2_bringup') + + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + world = LaunchConfiguration('world') + + default_world = os.path.join(pkg_gazebo, 'worlds', 'room.world') + slam_params = os.path.join(pkg_nav2, 'config', 'slam_toolbox_sim.yaml') + nav2_params = os.path.join(pkg_nav2, 'param', 'nav2_sim.yaml') + rviz_config = os.path.join(pkg_nav2, 'rviz', 'agvpro_navigation2.rviz') + + # 1. Gazebo + robot + lidar + controllers + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo, 'launch', 'agv_pro_gazebo.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'true', + 'use_rviz': 'false', + 'headless': headless, + 'world': world, + }.items(), + ) + + # 2. SLAM (map -> odom). Delayed so /scan and odom TF are up first. + slam = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_slam, 'launch', 'online_async_launch.py') + ), + launch_arguments={ + 'use_sim_time': 'true', + 'slam_params_file': slam_params, + }.items(), + ) + + # 3. Nav2 navigation stack (no localization; SLAM provides map -> odom). + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_nav2_bringup, 'launch', 'navigation_launch.py') + ), + launch_arguments={ + 'use_sim_time': 'true', + 'params_file': nav2_params, + }.items(), + ) + + # 4. RViz + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config], + parameters=[{'use_sim_time': True}], + condition=IfCondition(use_rviz), + output='screen', + ) + + # Give Gazebo time to spawn the robot and activate the controllers (which + # publish odom -> base_footprint) before SLAM and Nav2 start looking for TF. + delayed_bringup = TimerAction(period=8.0, actions=[slam, nav2, rviz]) + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_rviz', default_value='true', + description='Launch RViz with the navigation view'), + DeclareLaunchArgument( + 'headless', default_value='false', + description='Run gz-sim without the GUI (server only)'), + DeclareLaunchArgument( + 'world', default_value=default_world, + description='Full path to the Gazebo world (SDF) to load'), + gazebo, + delayed_bringup, + ]) diff --git a/src/elephant_robotics/agv_pro_navigation2/package.xml b/src/elephant_robotics/agv_pro_navigation2/package.xml index 19f0be0..5c21d86 100644 --- a/src/elephant_robotics/agv_pro_navigation2/package.xml +++ b/src/elephant_robotics/agv_pro_navigation2/package.xml @@ -12,6 +12,10 @@ ament_lint_auto ament_lint_common nav2_bringup + slam_toolbox + agv_pro_gazebo + agv_pro_description + rviz2 ament_cmake diff --git a/src/elephant_robotics/agv_pro_navigation2/param/agvpro.yaml b/src/elephant_robotics/agv_pro_navigation2/param/agvpro.yaml index 84814a3..03a7fa7 100644 --- a/src/elephant_robotics/agv_pro_navigation2/param/agvpro.yaml +++ b/src/elephant_robotics/agv_pro_navigation2/param/agvpro.yaml @@ -109,6 +109,9 @@ bt_navigator_rclcpp_node: controller_server: ros__parameters: use_sim_time: False + # Publish /cmd_vel as geometry_msgs/TwistStamped to match the + # mecanum_drive_controller's ~/reference input (both sim and real robot). + enable_stamped_cmd_vel: true controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 diff --git a/src/elephant_robotics/agv_pro_navigation2/param/nav2_sim.yaml b/src/elephant_robotics/agv_pro_navigation2/param/nav2_sim.yaml new file mode 100644 index 0000000..4887924 --- /dev/null +++ b/src/elephant_robotics/agv_pro_navigation2/param/nav2_sim.yaml @@ -0,0 +1,476 @@ +amcl: + ros__parameters: + 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: + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # '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 is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + # Mecanum controller consumes geometry_msgs/TwistStamped on ~/reference. + enable_stamped_cmd_vel: true + controller_frequency: 20.0 + costmap_update_timeout: 0.30 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # 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 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Omni" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.30 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + 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_footprint + robot_radius: 0.30 + 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.7 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + 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 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + enable_stamped_cmd_vel: true + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_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" + local_frame: odom + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + +velocity_smoother: + ros__parameters: + enable_stamped_cmd_vel: true + smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.5, 2.0] + min_velocity: [-0.5, -0.5, -2.0] + max_accel: [2.5, 2.5, 3.2] + max_decel: [-2.5, -2.5, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + # Publish the final /cmd_vel as TwistStamped to match the mecanum controller. + enable_stamped_cmd_vel: true + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true