Local planner behaves unordinary
Hello,
I am using base_local_planner (TrajectoryPlannerROS with DWA enabled) and it works in general, it sends the commands to cmd_vel and my robot interprets them quite good using this simple inverse kinematics formula (I am cancelling negative velocities as my robot cannot handle them well):
v_right = (cmd_angular_z*self.ROBOT_BASE_LENGTH)/2.0 + cmd_linear_x
v_left = cmd_linear_x*2.0 - v_right
if v_right < 0:
v_right = 0
v_left *= 2
elif v_left < 0:
v_left = 0
v_right *= 2
But when I set a goal, the local planner publishes short and strange plans. Even though it does like that, sometimes my robot is able to get to the goal and stop but sometimes it collides to the objects.
Here is the video: LINK
The red line is the local planner and green line is the global planner.
Here is my TrajectroyPlannerROS params:
recovery_behavior_enabled: true clearing_rotation_allowed: true
TrajectoryPlannerROS:
acc_lim_x: 1
acc_lim_y: 0 # as we have a differential robot
acc_lim_theta: 1.6
max_vel_x: 0.6 # configure, was 0.2
min_vel_x: 0.2 # was 0.4
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 1.6 # 0.35
min_vel_theta: 3.0 #-0.35
min_in_place_vel_theta: 3.0 #0.25
min_rot_vel: 3.0
escape_vel: 0.0
holonomic_robot: false
meter_scoring: true
xy_goal_tolerance: 0.3 # 0.15, 0.3->about 20 cm
yaw_goal_tolerance: 0.20 # 0.25, about 11 degrees
latch_xy_goal_tolerance: true #false
sim_time: 3 # <2 values not enough time for path planning and can't go through narrow
sim_granularity: 0.05 # How frquent should the points on trajectory be examined
angular_sim_granularity: 0.025
vx_samples: 20
vy_samples: 0 # 0 for differential robots
vtheta_samples: 5 # 20
controller_frequency: 15
pdist_scale: 0.8
gdist_scale: 0.4
occdist_scale: 0.01
oscillation_reset_dist: 0.2
heading_scoring: true
heading_lookahead: 1 # how far to look ahead in meters
heading_scoring_timestep: 0.8
dwa: true
publish_cost_grid_pc: false
global_frame_id: odom
simple_attractor: false
reset_distance: 4
prune_plan: true
The behaviour is a result of preventing negative velocities. For a differential drive to turn clockwise in place, you will need positive left wheel velocity and negative right wheel velocity. The local planner is drawing an arc towards the goal but because it isn't being executed, it has to replan.
@curi_ROS But how can I enable negative velocities and also state that small values (both negative and positive) should be greater than 1.2? Because if I put min_vel_theta -1.2, it produces angular velocities like 0.08 and my robot does not move (too low velocity)