Add tutorial for warehouse_ros(_sqlite) (#101)

Co-authored-by: Michael Görner <[email protected]>

doc/persistent_scenes_and_states/move_group.launch.py

@@ -0,0 +1,230 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory
+import xacro
+
+
+def load_file(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 file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+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():
+
+    # planning_context
+    robot_description_config = xacro.process_file(
+        os.path.join(
+            get_package_share_directory("moveit_resources_panda_moveit_config"),
+            "config",
+            "panda.urdf.xacro",
+        )
+    )
+    robot_description = {"robot_description": robot_description_config.toxml()}
+
+    robot_description_semantic_config = load_file(
+        "moveit_resources_panda_moveit_config", "config/panda.srdf"
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
+    )
+    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
+
+    # Planning Functionality
+    ompl_planning_pipeline_config = {
+        "move_group": {
+            "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["move_group"].update(ompl_planning_yaml)
+
+    # Trajectory Execution Functionality
+    moveit_simple_controllers_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
+    )
+    moveit_controllers = {
+        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
+        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+    }
+
+    trajectory_execution = {
+        "moveit_manage_controllers": True,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        "publish_planning_scene": True,
+        "publish_geometry_updates": True,
+        "publish_state_updates": True,
+        "publish_transforms_updates": True,
+    }
+
+    ## BEGIN_SUB_TUTORIAL add_config
+    ## * Add a dictionary with the warehouse_ros options
+    warehouse_ros_config = {
+        # For warehouse_ros_sqlite
+        "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
+        "warehouse_host": "/path/to/my/warehouse_db.sqlite",
+        # For warehouse_ros_mongodb use the following instead
+        # "warehouse_port": 33829,
+        # "warehouse_host": "localhost",
+        # "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
+    }
+    ## END_SUB_TUTORIAL
+
+    # Start the actual move_group node/action server
+    ## BEGIN_SUB_TUTORIAL set_config_move_group
+    ## * Add it to the Move Group config
+    run_move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            # here
+            warehouse_ros_config,
+        ],
+    )
+    ## END_SUB_TUTORIAL
+
+    # RViz
+    rviz_config_file = (
+        get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz"
+    )
+
+    ## BEGIN_SUB_TUTORIAL set_config_rviz
+    ## * and to the RViz config
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+            # here
+            warehouse_ros_config,
+        ],
+    )
+    ## END_SUB_TUTORIAL
+
+    # Static TF
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "panda_ros_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, ros2_controllers_path],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in [
+        "panda_arm_controller",
+        "panda_hand_controller",
+        "joint_state_broadcaster",
+    ]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
+
+    # Warehouse mongodb server
+    ## BEGIN_SUB_TUTORIAL start_mongodb_server
+    ## * Optionally, start the MongoDB server (uncomment if necessary)
+    # mongodb_server_node = Node(
+    #    package="warehouse_ros_mongo",
+    #    executable="mongo_wrapper_ros.py",
+    #    parameters=[
+    #        warehouse_ros_config,
+    #    ],
+    #    output="screen",
+    # )
+    ## END_SUB_TUTORIAL
+
+    return LaunchDescription(
+        [
+            rviz_node,
+            static_tf,
+            robot_state_publisher,
+            run_move_group_node,
+            ros2_control_node,
+            mongodb_server_node,
+        ]
+        + load_controllers
+    )
+
+
+## BEGIN_TUTORIAL
+## CALL_SUB_TUTORIAL add_config
+## CALL_SUB_TUTORIAL set_config_move_group
+## CALL_SUB_TUTORIAL set_config_rviz
+## CALL_SUB_TUTORIAL start_mongodb_server
+## END_TUTORIAL

doc/persistent_scenes_and_states/persistent_scenes_and_states.rst

@@ -0,0 +1,60 @@
+Warehouse - Persistent Scenes and States
+===========================================
+
+The "MotionPlanning" plugin of RViz offers the possibility to save
+complete planning scenes and robot states persistently.
+Currently, two storage plugins (based on
+`warehouse_ros <https://github.com/ros-planning/warehouse_ros>`_) are available:
+
+* `warehouse_ros_mongo <https://github.com/ros-planning/warehouse_ros_mongo>`_, which uses MongoDB as backend
+* `warehouse_ros_sqlite <https://github.com/ros-planning/warehouse_ros_sqlite>`_, which uses SQLite as backend
+
+You can install both of them with your favourite package manager
+(e.g. ``apt-get install ros-foxy-warehouse-ros-mongo``) or
+`build them from source <../getting_started/getting_started.html>`_
+(of course, you'll have to check out the corresponding repository into your ``src`` folder for that).
+
+Storage plugin selection
+------------------------
+
+The warehouse plugin and settings have to be specified in the launch files of your MoveIt configuration.
+You should adapt ``move_group.launch.py`` if you do not wish to use the MongoDB plugin.
+The storage plugin is determined by the parameter ``warehouse_plugin``.
+Valid options are ``warehouse_ros_mongo::MongoDatabaseConnection`` for MongoDB and
+``warehouse_ros_sqlite::DatabaseConnection`` for SQLite.
+Furthermore, the parameters ``warehouse_host`` and ``warehouse_port`` configure the connection details.
+In case of the SQLite plugin, ``warehouse_host`` contains the path to the database file,
+and ``warehouse_port`` is unused.
+If the database file does not exist yet, an empty database will be created.
+
+.. tutorial-formatter:: ./move_group.launch.py
+
+Connecting to the storage backend
+---------------------------------
+
+After choosing the storage plugin and configuring the launch.py file,
+run RViz using ::
+
+   ros2 launch moveit2_tutorials demo.launch.py
+
+In RViz, navigate to the "Context" tab of the "MotionPlanning" window.
+Verify the connection details (host/port for MongoDB, file path for SQLite)
+and click on "Connect".
+
+.. image:: rviz_connect.png
+    :width: 600px
+
+After that, a dialogue box will appear and ask you whether you'd like to erase all current
+states and scenes in RViz (not in the database, the persistent data is not affected by that).
+As you just started RViz, you can safely select "yes".
+
+Saving/Loading scenes and states
+--------------------------------
+
+Now that you connected successfully,
+you can save and restore robot states and planned scenes.
+This can be done in the "Stored Scenes" resp. "Stored States" tab in RViz.
+
+To save a start state, drag the green manipulator to the correct position and click "Save Start".
+The goal state (orange manipulator) can be saved with the "Save Goal" button.
+To restore a state, select it in the list and click on "Set as Start" resp. "Set as Goal".

doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst

@@ -226,3 +226,5 @@ Next Tutorials
 * To easily control your robot using Python, check out the `Move Group Python Interface <../move_group_python_interface/move_group_python_interface_tutorial.html>`_
 
 * To create your own ``*_moveit_config`` package, check out the `Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_
+
+* Save and restore robot states from a database or from disk using `warehouse_ros <../persistent_scenes_and_states/persistent_scenes_and_states.html>`_

index.rst

@@ -83,6 +83,7 @@ Configuration
    doc/trajopt_planner/trajopt_planner_tutorial
    doc/pilz_industrial_motion_planner/pilz_industrial_motion_planner
    doc/planning_adapters/planning_adapters_tutorial.rst
+   doc/persistent_scenes_and_states/persistent_scenes_and_states
 
 Miscellaneous
 ----------------------------