ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Robot bumping into obstacles and not following planned path

asked 2021-05-17 18:08:13 -0500

spoluri gravatar image

updated 2021-05-25 16:34:20 -0500

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 ... (more)

edit retag flag offensive close merge delete

Comments

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.

kscottz gravatar image kscottz  ( 2021-05-21 14:22:35 -0500 )edit

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)

spoluri gravatar image spoluri  ( 2021-05-21 15:21:31 -0500 )edit

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?

spoluri gravatar image spoluri  ( 2021-05-24 14:14:27 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-06-10 18:40:04 -0500

spoluri gravatar image

I resolved both issues. Obstacle bumping and following global path. For following global path: I used the TEB parameters of global_plan_viapoint_sep set to a positive value and set the weight_viapoint at a slightly lower value to the weight_obstacle.

For obstacle bumping issue: Two things of note: The global planner does not consider your robot radius or footprint. It will assume it is a point. Which is why the inflation settings exist. Make sure that the inflation radius is slightly larger than your robot radius. This way, the global planner will plan with sufficient padding. The TEB local planner uses it's own settings instead of the settings from your local_costmap. I played with inflation_dist and min_obstacle_dist settings to get the optimal behavior I needed.

If anyone is stuck dealing with ackermann robots, reach out to me. I think I learned a lot by failing.

edit flag offensive delete link more

Comments

I have the same problem but I am using non-ackermann robots. Can you help me ???

Staredeath gravatar image Staredeath  ( 2021-11-07 02:46:06 -0500 )edit

@Staredeath I'm deleting your answer to this question because it is not an answer, it is another question. If you have a question, please open a new question. Best of luck!

jarvisschultz gravatar image jarvisschultz  ( 2021-11-07 17:35:14 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2021-05-17 18:08:13 -0500

Seen: 366 times

Last updated: Jun 10 '21