search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.42k stars 1.24k forks source link

Planner benchmark not functional (buggy) #3873

Closed Manouselis closed 10 months ago

Manouselis commented 10 months ago

Bug report

Required Info:

Steps to reproduce issue

Follow README instructions in navigation2/tools/planner_benchmarking

Expected behavior

Run a set of planners over randomly generated maps, with randomly generated goals for objective benchmarking.

Actual behavior

NAV2 stack is not initialized (properly). More specifically, I get the following warnings and errors:

[map_server-1] [ERROR] [map_io]: Failed processing YAML file /home/ros2_oa/install/nav2_bringup/share/nav2_bringup/maps/map.yaml at position (-1:-1) for reason: bad file: /home/ros2_oa/install/nav2_bringup/share/nav2_bringup/maps/map.yaml
[map_server-1] [ERROR] [1697121114.123512494] []: Caught exception in callback for transition 10
[map_server-1] [ERROR] [1697121114.123517150] []: Original error: Failed to load map yaml file: /home/ros2_oa/install/nav2_bringup/share/nav2_bringup/maps/map.yaml
[map_server-1] [WARN] [1697121114.123524238] []: Error occurred while doing error handling.
[map_server-1] [FATAL] [1697121114.123529140] [map_server]: Lifecycle node map_server does not have error state implemented
[lifecycle_manager-5] [ERROR] [1697121114.123858557] [lifecycle_manager]: Failed to change state for node: map_server
[lifecycle_manager-5] [ERROR] [1697121114.123879620] [lifecycle_manager]: Failed to bring up all requested nodes. Aborting bringup.

I suspect that the problem arises in the following line of code in the planning_benchmark_bringup.py file: map_file = os.path.join(nav2_bringup_dir, 'maps', 'map.yaml') (line 26) I believe this happens because the map.yaml file does not exist in that directory anymore.

Additional information

If I disregard the errors and warnings and proceed to follow the rest of the instructions (i.e., run metrics.py), I get the following: [INFO] [1697121448.525634049] [basic_navigator]: change map service not available, waiting...

SteveMacenski commented 10 months ago

There is no such thing as map.yaml https://github.com/ros-planning/navigation2/tree/main/nav2_bringup/maps

It also addresses that same yaml file in the bringup for the benchmark https://github.com/ros-planning/navigation2/blob/main/tools/planner_benchmarking/planning_benchmark_bringup.py#L26

How did you obtain map.yaml? I don't see that in the codebase

SteveMacenski commented 10 months ago

Ah I see it in the Humble branch… that’s interesting. I’ll need to look into that! Please confirm though that’s where you see it so I know I’m barking up the right tree

Manouselis commented 10 months ago

Yes, my ROS2 version is humble

Edit: If I manually proceed to change line 26 from: map_file = os.path.join(nav2_bringup_dir, 'maps', 'map.yaml') to: map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml')

The nav2 stack starts normally, but metrics.py still outputs warnings and errors and does not work as expected:

[WARN] [1697177854.325154615] [basic_navigator]: Getting path failed with status code: 6
[INFO] [1697177854.325691131] [basic_navigator]: Getting path...
One of the planners was invalid
Cycle:  0 out of:  100
Start geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697177826, nanosec=349415534), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=47.300000704824924, y=63.10000094026327, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=-0.0, y=0.0, z=0.9982530813236248, w=-0.05908287085009112)))
Goal geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697177826, nanosec=349415534), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=43.200000643730164, y=9.500000141561031, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.4738281739100532, w=0.8806173184812256)))
One of the planners was invalid
Cycle:  0 out of:  100
Start geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697177826, nanosec=349415534), frame_id='map'),
SteveMacenski commented 10 months ago

https://github.com/ros-planning/navigation2/commit/5b87fabadbdb5ad6c1cc56066e77e4de1a8b26df The default is updated if you pull in that update locally.

Did you follow the instructions in the readme so that you have all the planners? Did you configure them for the behavior you'd like them to have? This is not a zero-setup solution, you need to specify what tests you'd like to configure the planners and which planners you'd like to test.

https://github.com/ros-planning/navigation2/tree/humble/tools/planner_benchmarking#planning-benchmark

Manouselis commented 10 months ago

I can confirm that after configuring the planners, the planner_benchmarking now works.

Edit: The smoother_benchmark does not however. After following the README file and running the smoother_benchmark_bringup.py script, I get a bunch of warnings:

[python3-8] [WARN] [1697464805.453648820] [basic_navigator]: Getting path failed with status code: 6
[python3-8] simple_smoother  failed to smooth the path
[python3-8] Cycle:  0 out of:  100
[python3-8] Start geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697464761, nanosec=633598136), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=4.6000000685453415, y=10.200000151991844, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=-0.0, y=0.0, z=0.9861561501187913, w=-0.16581932210355946)))
[python3-8] Goal geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697464761, nanosec=633598136), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=5.300000078976154, y=5.800000086426735, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.730479469361706, w=0.6829346563479118)))
[python3-8] [INFO] [1697464805.454327603] [basic_navigator]: Getting path...
[python3-8] [INFO] [1697464805.481543826] [basic_navigator]: Smoothing path...
[smoother_server-5] [INFO] [1697464805.482533099] [smoother_server]: Received a path to smooth.
[smoother_server-5] [ERROR] [1697464805.482595290] [smoother_server]: Costmap is not available
[smoother_server-5] [WARN] [1697464805.482604965] [smoother_server]: [smooth_path] [ActionServer] Aborting handle.
[python3-8] [WARN] [1697464805.491386615] [basic_navigator]: Getting path failed with status code: 6
[python3-8] simple_smoother  failed to smooth the path
[python3-8] Cycle:  0 out of:  100
[python3-8] Start geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697464761, nanosec=633598136), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=6.550000097602606, y=12.450000185519457, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.9415382162528875, w=0.33690619960952173)))
[python3-8] Goal geometry_msgs.msg.PoseStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1697464761, nanosec=633598136), frame_id='map'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=10.350000154227018, y=3.0000000447034836, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=-0.0, y=0.0, z=0.9943544671032563, w=-0.10610934808865485)))
[python3-8] [INFO] [1697464805.492111545] [basic_navigator]: Getting path...
[planner_server-4] [WARN] [1697464805.679556110] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 5.3466 Hz
[python3-8] [INFO] [1697464805.692776132] [basic_navigator]: Smoothing path...
[smoother_server-5] [INFO] [1697464805.694196229] [smoother_server]: Received a path to smooth.
[smoother_server-5] [ERROR] [1697464805.694268690] [smoother_server]: Costmap is not available
[smoother_server-5] [WARN] [1697464805.694279316] [smoother_server]: [smooth_path] [ActionServer] Aborting handle.
[python3-8] [WARN] [1697464805.706420814] [basic_navigator]: Getting path failed with status code: 6
[python3-8] simple_smoother  failed to smooth the path
[python3-8] Cycle:  0 out of:  100
SteveMacenski commented 10 months ago

I imagine its the same analog problems. Configure them.

Manouselis commented 10 months ago

I imagine its the same analog problems. Configure them.

Excuse my naivete, but even configured it does not seem to work with the following parameters:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["SmacHybrid"]
    SmacHybrid:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.5
      motion_model_for_search: "DUBIN" # default, non-reverse motion
      smooth_path: false # should be disabled for experiment
      analytic_expansion_max_length: 0.3 # decreased to avoid robot jerking

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother", "constrained_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
    constrained_smoother:
      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
      w_smooth: 1.0 # tuned

Have you tried the code (for Humble) and it works for you Steven?