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

ros2 nav2 costmap not working

asked 2020-09-01 07:21:06 -0600

Joe28965 gravatar image

updated 2020-09-03 03:43:13 -0600

So I made my own robot, and I wanted to use nav2 to control it. I have copied the turtlebot3 files and changed them to work with my robot. Everything works, I can even give it commands via rviz. However, the costmap doesn't work. I have tried everything and yet can't seem to get it working. All I changed was the world that was used by gazebo, the robot itself and I changed the map to my map and made small changes to the yaml file

This is the yaml file that I use:

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: "differential"
    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

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    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_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    debug_trajectory_details: True
    min_vel_x: 0.0
    min_vel_y: 0.0
    max_vel_x: 0.26
    max_vel_y: 0.0
    max_vel_theta: 1.0
    min_speed_xy: 0.0
    max_speed_xy: 0.26
    min_speed_theta: 0.0
    min_x_velocity_threshold: 0.001
    # Add high threshold velocity for turtlebot 3 issue.
    # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    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: 5
    vtheta_samples: 20
    sim_time: 1.7
    linear_granularity: 0.05
    xy_goal_tolerance: 0.25
    transform_tolerance: 0.2
    critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
    BaseObstacle.scale: 0.02
    PathAlign.scale: 0.0
    GoalAlign.scale: 0.0
    PathDist.scale: 32.0
    GoalDist.scale: 24.0
    RotateToGoal.scale: 32.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: True
      global_frame: odom
      plugin_names: ["obstacle_layer", "inflation_layer"]
      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      footprint_padding: 0.02
      footprint: "[[0.7, 0.375], [0.7, -0.375], [-0.2, -0.375], [-0.2, 0.375]]"
      inflation_layer:
        cost_scaling_factor: 3.0
      obstacle_layer:
        enabled: True
        observation_sources: scan1 scan2
        scan1:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
        scan2:
          topic: /scan2
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: True
      plugin_names: ["static_layer", "obstacle_layer", "inflation_layer"]
      plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
      footprint_padding: 0.02
      footprint: "[[0.7, 0.375], [0.7, -0.375], [-0.2, -0.375], [-0.2, 0.375]]"
      obstacle_layer:
        enabled: True
        observation_sources: scan1 scan2
        scan1:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True ...
(more)
edit retag flag offensive close merge delete

Comments

Which ROS distribution do you use? If it is old, it may be related to this PR: https://github.com/ros2/rviz/pull/375

lukicdarkoo gravatar image lukicdarkoo  ( 2020-09-01 12:27:28 -0600 )edit

I'm using Eloquent, so I don't think it's related, but thank you for taking the time to answer regardless!

Joe28965 gravatar image Joe28965  ( 2020-09-01 12:30:00 -0600 )edit

I am actually not sure what exactly is not working in your case. Could you be a little more specific about that?

There has been a change in the plugin interface (see here). Which version of navigation2 are you using? From what i can see, your yaml file still uses the old interface.

Phgo gravatar image Phgo  ( 2020-09-02 07:58:07 -0600 )edit

First, let me say thank you for taking the time to help me. Second, I have updated the question to hopefully explain my issue better. I am using the eloquent-devel branch of navigation2, with its latest changes. I have used the turtlebot yaml file in the eloquent-devel branch to make my yaml file, so I'm not sure if I'm using the old interface to be honest (and your link doesn't work for me, for some reason)

Joe28965 gravatar image Joe28965  ( 2020-09-02 09:03:26 -0600 )edit

Looks like the new plugin interface does not effect the eloquent branch. Anyway, sorry for the broken link. For the sake of completeness: https://github.com/ros-planning/navigation2/blob/foxy-devel/doc/parameters/param_list.md#node-costmap_2d_ros

Are you sure, that the map file ./map/custom_map.yaml exists and is published correctly (especially with matching QoS)? Are there any relevant Log messages from the map server/ controller server?

Phgo gravatar image Phgo  ( 2020-09-03 02:49:50 -0600 )edit

I updated my question again, I hope I answered your questions.

Joe28965 gravatar image Joe28965  ( 2020-09-03 03:44:19 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-09-10 06:13:12 -0600

Joe28965 gravatar image

updated 2021-06-10 07:06:50 -0600

So I found out what the problem was. The turtlebot is included in the world file. My robot was spawned in using the gazebo_ros spawner. This means I had to use gazebo_ros_factory.so when starting Gazebo. It seems I didn't start that properly. It started well enough not to report any errors, and it spawned my robot correctly. However it did break my costmap somehow

ETA: Just in case somebody has this problem. I start the gzserver using:

start_gazebo_server_cmd = ExecuteProcess(
    cmd=['gzserver', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
    cwd=[this_share_dir], output='screen')

Not including the '-s', 'libgazebo_ros_factory.so' part seemed to break it. I have no idea how or why, but that seemed to be my issue.

edit flag offensive delete link more

Comments

I am facing the same issue. Were you able to fix it?

divyam.rastogi gravatar image divyam.rastogi  ( 2021-03-04 15:21:29 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2020-09-01 07:21:06 -0600

Seen: 1,379 times

Last updated: Jun 10 '21