Closed Rak-r closed 1 year ago
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.
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
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}]),
])
Please also grab that traceback
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.
Apologies for out of topic question. Could tell how to copy this text from gdb, I have got the backtrace but its not copying.
This I know. However, there nothing in the terminal. Its the xtrem which is open and I cannot copy the content
Send a screen shot I guess
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.
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()
You have to do that for the smac planner package, not your highest level package.
I have done this for the navigation package only which contains nav2 params file
Exactly. You have to add -g
to nav2_smac_planner
.
In the params file?
Could you give a simple demo of it
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.
Thanks a lot. I will do and will post the output
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
That's not really relevant to this issue (probably).
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.
Did you get a traceback? And/or did you try reducing the resolution to something more reasonable like 2.5-10cm?
Exactly. You have to add
-g
tonav2_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.
planner server error is not showing now
sounds like the fix. Without you providing the traceback, that’s the best we can do. Closing.
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.
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.
Hey man, getting the same error here! Did you manage to fix it? Thanks
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
Where on github?
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
Required Info:
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.