0
$\begingroup$

Rosanswers logo

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
          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
  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
    yaml_filename: "../map/custom_map.yaml"

planner_server:
  ros__parameters:
    use_sim_time: True
    tolerance: 0.0
    use_astar: false

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

I can even see the updated footprint that I created, but the costmap itself refuses to load for some reason. Has anyone had this problem before, and knows how to solve it?

Edit to add: as you can see in rviz, my robot is able to navigate. However the global and local costmaps both give a warning stating no map has been received. image description

I would expect a costmap to be made and have the overlay as I have when I run the turtlebot example.

I am using navigation2 with the eloquent-devel branch, with the latest changes (the June 30th update)

2nd edit: I checked my map server, which states the following:

[INFO] [map_server]: Loading yaml file: /home/user/eloquent_ws/install/nena_navigation2/share/nena_navigation2/params/custom_map.yaml
[INFO] [map_server]: Loading image_file: /home/user/eloquent_ws/install/nena_navigation2/share/nena_navigation2/params/custom_map.pgm

My controller server also says that it's creating the costmap and my planner server states this:

[INFO] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 414 X 621 at 0.050000 m/pix

Which is the size of my map according to the custom_map.pgm

My planner_server does however also show me this message:

[INFO] [global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'lidar1' at time 10.968 for reason 'Unknown'
[INFO] [global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'lidar2' at time 10.969 for reason 'Unknown'
[INFO] [global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'lidar2' at time 19.076 for reason 'Unknown'
[INFO] [global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'lidar1' at time 19.076 for reason 'Unknown'

With about 250 lines in between, with only the time changing. But it does stop after that last message, even though I ran the navigation for longer, and from what I could find online it was just something that hadn't been fixed yet, it wasn't anything bad, so I don't really know

Originally posted by Joe28965 on ROS Answers with karma: 1124 on 2020-09-01

Post score: 0

Original comments

Comment by lukicdarkoo on 2020-09-01:
Which ROS distribution do you use? If it is old, it may be related to this PR: https://github.com/ros2/rviz/pull/375

Comment by Joe28965 on 2020-09-01:
I'm using Eloquent, so I don't think it's related, but thank you for taking the time to answer regardless!

Comment by Phgo on 2020-09-02:
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](interface https://github.com/ros-planning/navigation2/commit/f3b7be2397f585c53d4940b0fc516d8fe4738670#diff-654f280b3f7a18a77bdf3f828c8538f1)). Which version of navigation2 are you using? From what i can see, your yaml file still uses the old interface.

Comment by Joe28965 on 2020-09-02:
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)

Comment by Phgo on 2020-09-03:
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?

Comment by Joe28965 on 2020-09-03:
I updated my question again, I hope I answered your questions.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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.

Originally posted by Joe28965 with karma: 1124 on 2020-09-10

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by divyam.rastogi on 2021-03-04:
I am facing the same issue. Were you able to fix it?

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.