Attempting to use the ros navigation stack for autonomy. I'm able to get Laserscan information in rviz, but unable to avoid obstacles using the rplidar, SLAM_gmapping, and move_base. When giving the robot a 2d nav_goal in rviz the robot shifts right and left then fails to go in a straight line. Any feedback would be appreciated. Most of the necessary files are below!
gmapping.launch
move_base.launch
move_base.yaml
shutdown_costmaps: false
track_unkown_sources: true
controller_frequency: 5.0
controller_patience: 15.0
planner_frequency: 2.0
planner_patience: 2.0
oscillation_timeout: 0.0
oscillation_distance: 0.5
recovery_behavior_enabled: true
clearing_rotation_allowed: true
base_local_planner.yaml
TrajectoryPlannerROS:
# Robot Configuration Parameters
# Set the aceleration limits of the robot
acc_lim_x: 2.5
acc_im_y: 0.0
# The rotational acceleration limit of the robot (default setting)
acc_lim_theta: 3.2
# Set the velocity limits of the robot
max_vel_x: 2.0
min_vel_x: -2.0
max_vel_theta: 4.0
min_vel_theta: -4.0
max_rotational_vel: 1.0
min_in_place_rotational_vel: 2.0
min_in_place_vel_theta: 2.0
y_vels: [0, 0, 0, 0]
# The velocity the robot will command when trying to escape from a stuck situation
escape_vel: -1.0
holonomic_robot: false
# Goal Tolerance Default Parameters
yaw_goal_tolerance: 0.05
xy_goal_tolerance: 0.10
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 1.5
sim_granularity: 0.025
angular_sim_granularity: 0.02
vx_samples: 15.0
vtheta_samples: 20.0
controller_frequency: 10.0
# Trajectory scoring parameters
meter_scoring: true
occdist_scale: 0.1
pdist_scale: 0.75
heading_lookahead: 0.325
heading_scoring: false
heading_scoring_timestep: 0.8
dwa: false
simple_attractor: false
publish_cost_grid_pc: true
oscillation_reset_dist: 0.05
escape_reset_dist: 0.1
escape_reset_theta: 0.1
costmap_common_params.yaml
map_type: costmap_2d
origin_z: 0.0
z_resolution: 1
z_voxels: 2
obstacle_range: 2.5
raytrace_range: 3.0
publish_voxel_map: false
transform_tolerance: 0.5
meter_scoring: true
footprint: [[-0.21, -0.165], [-0.21, 0.165], [0.21, 0.165], [0.21, -0.165]]
footprint_padding: 0.1
observation_sources: laser_scan_sensor
laser_scan_sensor: [sensor_frame: /laser , data_type: LaserScan, topic: /scan, marking: true, clearing: true]
plugins:
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
obstacles_layer:
observation_sources: scan
enabled: true
scan: {sensor_frame: /laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: -2.0, max_obstacle_height: 2.0, obstacle_range: 2.5, raytrace_range: 3.0}
inflater_layer:
enabled: true
cost_scaling_factor: 5.0
inflation_radius: 0.10
local_costmap_params.yaml
local_costmap:
global_frame: "map"
robot_base_frame: "base_link"
tansform_tolerance: 0.2
update_frequency: 1.0
publish_frequency: 10.0
width: 15.0
height: 15.0
resolution: 0.05
origin_x: 0.0
origin_y: 0.0
static_map: false
rolling_window: true
always_send_full_costmap: false
plugins:
#- {name: static_layer, type: "costmap_2d::StaticLayer"}#
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
##static_layer:
##unknown_cost_value: -1.0
##lethal_cost_threshold: 100
##map_topic: "map"
##first_map_only: false
##subscribe_to_updates: false
##track_unknown_space: true
##use_maximum: false
##trinary_costmap: true
obstacles_layer:
track_unknown_space: false
footprint_clearing_enabled: true
inflater_layer:
inflation_radius: 0.10
cost_scaling_factor: 5.0
global_costmap_params.yaml
global_costmap:
global_frame: "map"
robot_base_frame: "base_link"
tansform_tolerance: 0.2
update_frequency: 1.0
publish_frequency: 10.0
width: 15.0
height: 15.0
resolution: 0.05
origin_x: 0.0
origin_y: 0.0
static_map: true
rolling_window: false
always_send_full_costmap: false
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
static_layer:
unknown_cost_value: -1.0
lethal_cost_threshold: 100
map_topic: "map"
first_map_only: false
subscribe_to_updates: false
track_unknown_space: true
use_maximum: false
trinary_costmap: true
obstacles_layer:
track_unknown_space: false
footprint_clearing_enabled: true
inflater_layer:
inflation_radius: 0.10
cost_scaling_factor: 5.0
↧