unable to use Smac planner based Nav2 params

[Rak-r]

> <!--
>
> 
> planner_server-2] [INFO] [1690472729.622689661] [planner_server]: Destroying plugin GridBased of type SmacPlannerHybrid
> [planner_server-2] [ERROR] [1690472729.623135100] []: Caught exception in callback for transition 10
> [planner_server-2] [ERROR] [1690472729.623495478] []: Original error: std::bad_alloc
> [planner_server-2] [WARN] [1690472729.623823753] []: Error occurred while doing error handling.
> [planner_server-2] [FATAL] [1690472729.624107830] [planner_server]: Lifecycle node planner_server does not have error state implemented
> [lifecycle_manager-5] [ERROR] [1690472729.624688917] [lifecycle_manager_navigation]: Failed to change state for node: planner_server
> [lifecycle_manager-5] [ERROR] [1690472729.624713106] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.
> -->

Required Info:

• Operating System:
  -Ubuntu 22.04
• ROS2 Version:
  • humble
• Version or commit hash:
  • binary install" -->
• DDS implementation:
  • cycloneDDS

Expected behavior

I want to run NAV2 with Slam_toolbox for creating map while navigating. Slam toolbox is detecting the LiDAR data from Gazebo simulation.
Should run NAV2

Actual behavior

When running NAV2, it is showing some errors. I am using Smac Planner Hybrid. When I launch default nav2_bringup it launches but with Nav2fn planner. When providing other plugin as mentioned above it doesn't work.

[SteveMacenski]

Please get a backtrace to debug: https://navigation.ros.org/tutorials/docs/get_backtrace.html

I don’t see that on my side, so need some additional info from you 🙂. Please also post your global costmap & smac planner parameters.

[Rak-r]

Here the full nav2 params I am using.

amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.1
    base_frame_id: "base_link"
    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: 50.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 1000
    min_particles: 10000
    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
    map_topic: map
    set_initial_pose: false
    always_reset_initial_pose: false
    first_map_only: false
    initial_pose:
      x: 0.0
      y: 0.0
      z: 0.0
      yaw: 0.0
amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True
    
bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /model/podcar/odometry
    bt_loop_duration: 10
    default_nav_to_pose_bt_xml: will_be_handled_by_rewritten_yaml
    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_are_error_codes_active_condition_bt_node
      - nav2_would_a_controller_recovery_help_condition_bt_node
      - nav2_would_a_planner_recovery_help_condition_bt_node
      - nav2_would_a_smoother_recovery_help_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_rclcpp_node:
  ros__parameters:
    use_sim_time: True
#Local planner in ROS1 are now renamed as Controllers. Controllers are used to follow the global path and they publish to the velocity smoother which is used for collision avoidance. Setting up the configurations of local planner (ROS1)/ Controller (ROS2).


controller_server:
  ros__parameters:
    # controller server parameters (see Controller Server for more info)
    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
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB controller 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
      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
      GoalAlign.scale: 24.0
      PathAlign.forward_point_distance: 0.1
      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
      
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True
#local costmap configurations
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 0.5
      publish_frequency: 0.5
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 10
      height: 18
      resolution: 0.25
      footprint_padding: 0.01
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.2
        cost_scaling_factor: 30.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.5
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True
#global costmap configurations
global_costmap:
  global_costmap:
    ros__parameters:
      footprint_padding: 0.05
      update_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.5 # radius set and used, so no footprint points
      resolution: 0.001
      transform_tolerance: 0.5
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        combination_method: 1
        scan:
          topic: /scan
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          inf_is_valid: false
       
        static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
         enabled: true
         subscribe_to_updates: true
         transform_tolerance: 0.1
        inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         enabled: true
         inflation_radius: 0.5
         cost_scaling_factor: 1.0
         inflate_unknown: false
         inflate_around_unknown: true
         always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True
#Planner server in ROS1 were Trajectory Rollout Planner and Dynamic Window Approach(DWA). Time elastic band plannar (TEB) implements a plugin to base_local_plannar in the 2D nav stack.
# A global plannar generates a trajectroy which is optimized during runtime while reducing the trajectroy execution time.

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      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)
      tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      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 after within tolerances to continue to try to find exact solution
      max_planning_time: 5.0              # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP"    # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72         # Number of angle bins for search
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      minimum_turning_radius: 0.40        # minimum turning radius in m of path / vehicle
      reverse_penalty: 2.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.2           # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.015
      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path

      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2
        
planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True
    
    
#the map_server package has been changed to nav2_map_server which implements a server to handle the map load request and host a map topic. It has a map saver server running in background which saves the maps according to the requests. There are numerous parameters for the nav2_map_server. look at: https://navigation.ros.org/configuration/packages/configuring-map-server.html

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "map.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 100.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

##behavior server configurations
behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_costmap_topic: global_costmap/costmap_raw
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    
    wait:
      plugin: "nav2_behaviors/Wait"
    #local_frame: odom
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    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

[Rak-r]

This is my launch file.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('pod2_navigation')
    # nav2_params_path = os.path.join(
    #     get_package_share_directory('pod2_navigation'),
    #     'config/',
    #     'nav2_params.yaml')


    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml')
    map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')

    lifecycle_nodes = ['controller_server',
                       'planner_server',
                       'behavior_server',
                       'bt_navigator']

    # Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    # remappings = [('/tf', 'tf'),
                #   ('/tf_static', 'tf_static')]

    # Create our own temporary YAML files that include substitutions
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'autostart': autostart,
        # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
        # 'map_subscribe_transient_local': map_subscribe_transient_local
        }

    configured_params = RewrittenYaml(
            source_file=params_file,
            root_key=namespace,
            param_rewrites=param_substitutions,
            convert_types=True)

    return LaunchDescription([
        # Set env var to print messages to stdout immediately
        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),

        DeclareLaunchArgument(
            'namespace', default_value='',
            description='Top-level namespace'),

        DeclareLaunchArgument(
            'use_sim_time', default_value='true',
            description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument(
            'autostart', default_value='true',
            description='Automatically startup the nav2 stack'),

        DeclareLaunchArgument(
            'params_file',
            default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'),
            description='Full path to the ROS2 parameters file to use'),

        DeclareLaunchArgument(
            'default_nav_to_pose_bt_xml',
            default_value=os.path.join(bringup_dir, 'behavior_trees', 'default.xml'),
            description='Full path to the behavior tree xml file to use'),

        DeclareLaunchArgument(
            'map_subscribe_transient_local', default_value='true',
            description='Whether to set the map subscriber QoS to transient local'),

        Node(
            package='nav2_controller',
            executable='controller_server',
            output='screen',
            parameters=[configured_params],
            # remappings=remappings
            ),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[configured_params],
            # remappings=remappings
            ),

        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='behavior_server',
            output='screen',
            parameters=[configured_params],
            # remappings=remappings
            ),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[configured_params],
            # remappings=remappings
            ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_navigation',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': lifecycle_nodes}]),

       
    ])

[SteveMacenski]

Please also grab that traceback

[SteveMacenski]

Off the top of my head, this resolution resolution: 0.001 is kind of insane. Why do you need 1mm level costmap representations? That seems nonsensical. That lookup table generation for that low of a resolution would be 800000000 bytes, or 0.8GB of your RAM alone - let alone the costmap, each of the layers, and such. You may quite literally have run out of memory for use.

[Rak-r]

Apologies for out of topic question.
Could tell how to copy this text from gdb, I have got the backtrace but its not copying.

[SteveMacenski]

https://www.makeuseof.com/copy-paste-text-in-linux-terminal/

[Rak-r]

This I know. However, there nothing in the terminal. Its the xtrem which is open and I cannot copy the content

[SteveMacenski]

Send a screen shot I guess

[Rak-r]

[SteveMacenski]

Did you compile with debug flags? It should give you a line somewhere which I don't see in those screen shots. Basically #14 in that traceback that is ?? is what is the useful information.

[Rak-r]

I tried to add (-g) in the cmkaelists.txt as mentioned in the guide

cmake_minimum_required(VERSION 3.8)
project(pod2_navigation)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic -g)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

install(
  DIRECTORY behavior_trees launch rviz2 config maps
  DESTINATION share/${PROJECT_NAME}
)


ament_package()

[SteveMacenski]

You have to do that for the smac planner package, not your highest level package.

[Rak-r]

I have done this for the navigation package only which contains nav2 params file

[SteveMacenski]

Exactly. You have to add -g to nav2_smac_planner.

[Rak-r]

In the params file?

[Rak-r]

Could you give a simple demo of it

[SteveMacenski]

Look at your cmake above, add the -g in the Smac Planner version. This is straight forward stuff, check out the tutorial & your own code blob above. Compile it with debug flags.

[Rak-r]

Thanks a lot. I will do and will post the output

[Rak-r]

One more thing, if we look at the screenshots, it is mentioning fastdds but I downloaded the cycloneDDS using

sudo apt-get install ros-humble-cycloneDDS

And then changed it using export
I checked using
ros2 doctor --report | grep middleware

It returned CycloneDDS

Is it strange

[SteveMacenski]

That's not really relevant to this issue (probably).

[Rak-r]

Ok. Just want to give the possibe information which I have. Using Gazebo garden's Ackermann plugin for twist messages and ground truth odometry from Gazebo using ros_gz_bridge.

[SteveMacenski]

Did you get a traceback? And/or did you try reducing the resolution to something more reasonable like 2.5-10cm?

[Rak-r]

Exactly. You have to add -g to nav2_smac_planner.

I tried changing the resolution, planner server error is not showing now, I am getting the topics related to planner and it seems other error this time. But this is coming, I am attaching the backtrace as well. Also, i don't have any specific package of nav2_smac_planner in my workspace. The package contains launch files, config for nav2 and slam-toolbox.

[SteveMacenski]

planner server error is not showing now

sounds like the fix. Without you providing the traceback, that’s the best we can do. Closing.

[Rak-r]

What this other message means? I have provided the bt-navgator params

I tried the get the backtrace but as described earlier I have added -g to my navigation package.

[Rak-r]

I tried to look at this error previously but still couldn't figure out the root. I have provided the backtrace for this as well in the above screenshots.

[lourencorcc]

Hey man, getting the same error here! Did you manage to fix it? Thanks

[Rak-r]

I believe just go through the parameters again, set with defaults and also copy the default one from humble branch not from the ros2 website. See if that works.@lourencorcc

[lourencorcc]

Where on github?

[Rak-r]

On this nav2 github page ->select the branch name as per your ROS2 version and go to nav2_smac_planner. Find the params there and try it out.@lourencorcc