Navigation2 replanning problem with 4-wheeled front-steer vehicle
Hello! I am building a 4-wheeled vehicle in Ros2 (humble). Im using a joint_trajectory_controller to steer the front wheels and a diff_drive_controller to drive the vehicle forward/backward, which works great.
Now I am trying to implement Navigation2 but having some problems with the planning. Im using the Regulated Pure Pursuit controller with the SmacPlannerHybrid. When the vehicle approaches the goal it seems to replan and begins to do a loop.
(Im unable to upload my pictures due to not having enough points in the forum).
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_robot::SimpleRobotModel"
initial_pose:
x: 0.0
y: 0.0
yaw: 0.0
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:
controller_frequency: 2.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.50
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: 2.0
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.05
approach_velocity_scaling_dist: 1.0
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: true
regulated_linear_scaling_min_radius: 2.35
regulated_linear_scaling_min_speed: 0.25
use_fixed_curvature_lookahead: false
curvature_lookahead_dist: 1.0
use_rotate_to_heading: false
rotate_to_heading_min_angle: 0.785
max_angular_accel: 3.2
max_robot_pose_search_dist: -1.0
use_interpolation: true
cost_scaling_dist: 0.3
cost_scaling_gain: 1.0
inflation_cost_scaling_factor: 3.0
allow_reversing: false
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: 5
height: 5
resolution: 0.1
#robot_radius: 0.775
footprint: "[[-0.775, -0.855], [-0.775, 0.855], [0.775, 0.855], [0.775, -0.855]]"
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 ...
Your controller configuration is highly unusual, and commands will need to be sent with great care or the wheels will experience excessive friction. Can the front wheels rotate to 90°? Are they locked to the same angle, or can they rotate independently?
Hi Mike. We couldnt find any existing controller in humble for a car-like vehicle so that is why we used this unusual setup...The wheels cannot rotate 90 degrees and we have set a maximum angle of approx 0.4 radians. We then have a script that will calculate the Ackermann angle for each wheel dependent on controller input.
Hi crippaccino. did you find any solution, I'm facing a similar issue with my ackermann steer robot too using the same controller and planner.