Port simplified benchmarking tutorial (moveit#772)

* Add benchmarking tutorial

* Update tutorial text

* Apply suggestions from code review

Co-authored-by: Sebastian Castro <[email protected]>

* Address review

* Fix link

---------

Co-authored-by: Sebastian Castro <[email protected]>

CMakeLists.txt

@@ -62,6 +62,7 @@ add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/planning_scene_ros_api)
 add_subdirectory(doc/examples/realtime_servo)
 add_subdirectory(doc/examples/robot_model_and_robot_state)
+add_subdirectory(doc/how_to_guides/benchmarking)
 add_subdirectory(doc/how_to_guides/isaac_panda)
 add_subdirectory(doc/how_to_guides/kinematics_cost_function)
 add_subdirectory(doc/how_to_guides/parallel_planning)

doc/examples/examples.rst

@@ -81,7 +81,6 @@ Miscellaneous
 .. toctree::
    :maxdepth: 1
 
-   benchmarking/benchmarking_tutorial
    dual_arms/dual_arms_tutorial
    hybrid_planning/hybrid_planning_tutorial
    realtime_servo/realtime_servo_tutorial

doc/how_to_guides/benchmarking/CMakeLists.txt

@@ -0,0 +1,6 @@
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)

doc/how_to_guides/benchmarking/benchmarking_tutorial.rst

@@ -0,0 +1,61 @@
+How to benchmark your planning pipeline
+=======================================
+
+Getting Started
+---------------
+If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started </doc/tutorials/getting_started/getting_started>`.
+
+The :moveit_codedir:`benchmarking package <moveit_ros/benchmarks>` provides methods to benchmark a MoveIt planning pipeline and aggregate/plot statistics using the OMPL Planner Arena.
+The example below demonstrates how the benchmarking can be run.
+
+Example
+-------
+
+To run the example you need to install git lfs by running ``git lfs install`` and clone [moveit_benchmark_resources](https://github.com/ros-planning/moveit_benchmark_resources.git) into your workspace.
+
+Start the benchmarks by running: ::
+
+    ros2 launch moveit2_tutorials run_benchmarks.launch.py
+
+
+This will take a while depending on the settings in ``benchmarks.yaml``. The benchmark results will be saved in ``/tmp/moveit_benchmarks/``.
+To introspect the benchmark data, the log files need to be converted into a database. This can be done using a script provided in the moveit_ros_benchmarks package: ::
+
+    ros2 run moveit_ros_benchmarks moveit_benchmark_statistics.py LOG_FILE_1 ... LOG_FILE_N
+
+This command will create a database containing the data form all benchmark log files included. An easier way to create the database is to create it with all log files from a given repository.
+For example, the argument ``/tmp/moveit_benchmarks/*`` can be used to collect all log files in the given directory into a single database. This database is created in the location where the command
+above is run in a file names ``benchmark.db``.
+
+The database can be visualized by uploading the the file to `plannerarena.org <http://plannerarena.org>`_ and interactively visualizing the results.
+
+
+.. image:: planners_benchmark.png
+   :width: 700px
+
+ROS 2 parameters to configure a benchmark
+-----------------------------------------
+
+The benchmarks are configured by a set of ROS 2 parameters. You can learn more about these parameters in the :moveit_codedir:`BenchmarkOptions.h <moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h>` file.
+
+
+The BenchmarkExecutor Class
+---------------------------
+
+This class creates a set of ``MotionPlanRequests`` that respect the parameters given in the supplied instance of ``BenchmarkOptions`` and then executes the requests on each of the planners specified.  From the ``BenchmarkOptions``, queries, ``goal_constraints``, and ``trajectory_constraints`` are treated as separate queries.  If a set of ``start_states`` is specified, each query, ``goal_constraint``, and ``trajectory_constraint`` is attempted with each start state (existing start states from a query are ignored).  Similarly, the (optional) set of path constraints is combined combinatorially with the start query and start ``goal_constraint`` pairs (existing ``path_constraint`` from a query are ignored).  The workspace, if specified, overrides any existing workspace parameters.
+
+The benchmarking pipeline does not utilize ``MoveGroup``.
+Instead, the planning pipelines are initialized and run directly including all specified ``PlanningRequestAdapters``.
+This is especially useful for benchmarking the effects of smoothing adapters.
+
+It is possible to customize a benchmark run by deriving a class from ``BenchmarkExecutor`` and overriding one or more of the virtual functions.
+For instance, overriding the functions ``initializeBenchmarks()`` or ``loadBenchmarkQueryData()`` allows to specify the benchmark queries directly and to provide a custom planning scene without using ROS warehouse.
+
+Additionally, a set of functions exists for ease of customization in derived classes:
+
+- ``preRunEvent``: invoked immediately before each call to solve
+- ``postRunEvent``: invoked immediately after each call to solve
+- ``plannerSwitchEvent``: invoked when the planner changes during benchmarking
+- ``querySwitchEvent``: invoked before a new benchmark problem begin execution
+
+Note, in the above, a benchmark is a concrete instance of a ``PlanningScene``, start state, goal constraints / ``trajectory_constraints``, and (optionally) ``path_constraints``.  A run is one attempt by a specific planner to solve the benchmark.

doc/how_to_guides/benchmarking/config/benchmarking_moveit_cpp.yaml

@@ -0,0 +1,3 @@
+planning_pipelines:
+  # MoveIt cpp
+  pipeline_names: ["ompl", "ompl_rrtc", "stomp", "pilz_industrial_motion_planner"]

doc/how_to_guides/benchmarking/config/benchmarks.yaml

@@ -0,0 +1,32 @@
+# A detailed description for all parameters can be found in BenchmarkOptions.h
+
+parameters:
+    name: FullBenchmark
+    runs: 1
+    group: panda_arm      # Required
+    timeout: 1.5
+    output_directory: /tmp/moveit_benchmarks/
+    start_states_regex: ready
+planning_pipelines:
+  # Benchmark tool
+  pipelines: [OMPL_ANY, OMPL_RRTC, CHOMP, PILZ_LIN]
+  OMPL_ANY:
+     name: ompl
+     planners:
+       - APSConfigDefault
+  OMPL_RRTC:
+    name: ompl_rrtc
+    planners:
+      - RRTConnectkConfigDefault
+  CHOMP:
+     name: stomp
+     planners:
+       - stomp
+  PILZ_LIN:
+     name: pilz_industrial_motion_planner
+     planners:
+       - LIN
+  parallel_pipelines: [ompl_pilz_chomp]
+  ompl_pilz_chomp:
+    pipelines: [ompl_rrtc, stomp, pilz_industrial_motion_planner]
+    planner_ids: [RRTConnectkConfigDefault, stomp, LIN]

doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py

@@ -0,0 +1,97 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+from launch_param_builder import ParameterBuilder
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+
+    moveit_ros_benchmarks_config = (
+        ParameterBuilder("moveit2_tutorials")
+        .yaml(
+            parameter_namespace="benchmark_config",
+            file_path="config/benchmarks.yaml",
+        )
+        .to_dict()
+    )
+
+    moveit_configs = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .planning_pipelines("ompl", ["ompl", "stomp", "pilz_industrial_motion_planner"])
+        .moveit_cpp(
+            os.path.join(
+                get_package_share_directory("moveit2_tutorials"),
+                "config",
+                "benchmarking_moveit_cpp.yaml",
+            )
+        )
+        .to_moveit_configs()
+    )
+
+    # Load additional OMPL pipeline
+    ompl_planning_pipeline_config = {
+        "ompl_rrtc": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": """\
+                default_planner_request_adapters/AddTimeOptimalParameterization \
+                default_planner_request_adapters/FixWorkspaceBounds \
+                default_planner_request_adapters/FixStartStateBounds \
+                default_planner_request_adapters/FixStartStateCollision \
+                default_planner_request_adapters/FixStartStatePathConstraints \
+              """,
+            "start_state_max_bounds_error": 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
+    )
+    ompl_planning_pipeline_config["ompl_rrtc"].update(ompl_planning_yaml)
+
+    sqlite_database = (
+        get_package_share_directory("moveit_benchmark_resources")
+        + "/databases/panda_kitchen_test_db.sqlite"
+    )
+
+    warehouse_ros_config = {
+        "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
+        "benchmark_config": {
+            "warehouse": {
+                "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
+                "host": sqlite_database,
+                "port": 33828,
+                "scene_name": "",  # If scene name is empty, all scenes will be used
+                "queries_regex": ".*",
+            },
+        },
+    }
+
+    # moveit_ros_benchmark demo executable
+    moveit_ros_benchmarks_node = Node(
+        name="moveit_run_benchmark",
+        package="moveit_ros_benchmarks",
+        executable="moveit_run_benchmark",
+        output="screen",
+        parameters=[
+            moveit_ros_benchmarks_config,
+            moveit_configs.to_dict(),
+            warehouse_ros_config,
+            ompl_planning_pipeline_config,
+        ],
+    )
+
+    return LaunchDescription([moveit_ros_benchmarks_node])