Robot bumping into obstacles and not following planned path
Details: HW: - Robot: Upsquared Robomaker prokit (https://up-shop.org/up-squared-roboma...) - Laserscan input: RPLidar A1
SW: - Operating System: Ubuntu 20.04 - ROS2 Version: Foxy, installed using apt - Version or commit hash: 0.4.7-1focal.20210423.031543 - DDS implementation: Fast-RTPS
Steps to reproduce issue
This is my param file:
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_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_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_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_distance_traveled_condition_bt_node
bt_navigator_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
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "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
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.2
stateful: True
# TEB Parameters
FollowPath:
plugin: "teb_local_planner::TebLocalPlannerROS"
#debug_trajectory_details: True
# Obstacle Parameters
min_obstacle_dist: 0.2
inflation_dist: 0.25 #has to be greater than min_obstacle_dist
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" # costmap cells are converted from point obstacles to polygons
costmap_converter_rate: 5
# Parallel planning params
enable_homotopy_class_planning: False
enable_multithreading: True
visualize_hc_graph: False
#h_signature_prescaler: 0.5
#obstacle_heading_threshold: 0.45
# Trajectory params
teb_autoresize: True
max_samples: 20
min_samples: 3
max_global_plan_lookahead_dist: 2.0
feasibility_check_no_poses: 2
# Robot params
max_vel_x: 0.35
max_vel_x_backwards: 0.2
max_vel_theta: 1.5
acc_lim_x: 0.2
acc_lim_theta: 0.6
#ackermann drive related params
min_turning_radius: 0.4
wheelbase: 0.16
cmd_angle_instead_rotvel: False
#describe robot outline
footprint_model.type: "polygon"
footprint_model.vertices: "[[0.12, 0.09], [0.12, -0.09], [-0.12, -0.09], [-0.12, 0.09]]"
# Optimization params
no_inner_iterations: 3
no_outer_iterations: 2
weight_optimaltime: 10.0
weight_obstacle: 20.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 30.0
publish_frequency: 30.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint_padding: 0.05
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
footprint: "[[0.12, 0.09], [0.12, -0.09], [-0.12, -0.09], [-0.12, 0.09]]"
#robot_radius: 0.15
footprint_padding: 0.05
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"
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.5
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_server:
ros__parameters:
use_sim_time: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5000
free_thresh_default: 0.25
occupied_thresh_default: 0.65
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "smac_planner/SmacPlanner"
tolerance: 0.5
allow_unknown: true
smooth_path: false
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
minimum_turning_radius: 0.40
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 0.0
min_rotational_vel: 0.0
rotational_acc_lim: 0.0
robot_state_publisher:
ros__parameters:
use_sim_time: True
The output of the navigation2 package is cmd_vel. The Upsquared Robomaker ...
You need to provide information. What kind of robot? What kind of sensors? Does the output map register the object in question? Is it bumping into things occasionally or all the time? The information you have provided isn't sufficient to solve this problem.
Updated my question with the required details. After posting the initial question I realized several mistakes of mine: 1. navFn planner will not work for an ackermann drive robot. I switched to smac_planner and used the redds-shepp since the robot is capable of going in reverse. 2. I switched to use Teb local planner instead of DWB.
The map registers the objects detected by the lidar. I believe the bumping into obstacle as a symptom of the larger problem, which is the robot not following the path planned by the smac planner. There are 2 areas of suspicion: 1. the teb planner settings that can cause incorrect twist values for cmd_vel. Not sure how to debug this yet. One idea is to just echo the topic and look at the values printed. 2. the conversion of cmd_vel to ackermann. I have this computation from the manufacturer. So it is difficult ...(more)
I have verified that the cmd_vel is outputting msgs that instruct the robot to run counter to the global path that was planned. So the issue lies with the param settings. I have run through my settings several times and cannot identify what the issue is. Can you please help?