Nav2 not rotating to goal

edit

Hi!

I am struggling with making my robot reach the goal. I get to it easily in pose, but not in orientation. After I get to the goal in pose, the robot struggles to rotate towards the correct orientation, in fact, it rotates really slow or it doesn't even rotate.

I have tried playing with the parameters of the DWB controller critics, specially with the RotateToGoal one, which has an slowing_factor to reduce the angular speed. I set this parameter to 1 instead of to 5 as it is by default, but I haven't obtained anything, even after setting a huge max_vel_theta of 4.5 rad/s.

What's more, sometimes the robot reaches the goal with the correct orientation but the Controller doesn't seem to acknowledge it and keeps looking for a path. I checked the Goal Checker and it seems all the tolerances are perfectly defined and are not especially too low.

I also made the controller publish the cost_grid_pc, but I only get a pointcloud without any information, so I don't know whether the controller plugins are doing anything...

Any idea on what I can do or on where the problem might be?

Pd: here is my Controller config:

controller_server:
  ros__parameters:
    use_sim_time: True
    odom_topic: /odom/unfiltered
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.2
      movement_time_allowance: 20.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 1.5
      max_vel_y: 1.5
      max_vel_theta: 4.5
      min_speed_xy: 0.4
      max_speed_xy: 1.5
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      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: 10
      vtheta_samples: 20
      sim_time: 1.2
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.5
      short_circuit_trajectory_evaluation: True
      stateful: True
      publish_cost_grid_pc: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 5.0
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 1.0
      RotateToGoal.slowing_factor: 1.0
      RotateToGoal.lookahead_time: -1.0

Thank you!