diff --git a/doc/persistent_scenes_and_states/move_group.launch.py b/doc/persistent_scenes_and_states/move_group.launch.py new file mode 100644 index 0000000000..15b3519a28 --- /dev/null +++ b/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 diff --git a/doc/persistent_scenes_and_states/persistent_scenes_and_states.rst b/doc/persistent_scenes_and_states/persistent_scenes_and_states.rst new file mode 100644 index 0000000000..b1ec84e07f --- /dev/null +++ b/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 `_) are available: + +* `warehouse_ros_mongo `_, which uses MongoDB as backend +* `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". diff --git a/doc/persistent_scenes_and_states/rviz_connect.png b/doc/persistent_scenes_and_states/rviz_connect.png new file mode 100644 index 0000000000..ba1be11fc5 Binary files /dev/null and b/doc/persistent_scenes_and_states/rviz_connect.png differ diff --git a/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst b/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst index a0910b3fbc..cb8c28789b 100644 --- a/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst +++ b/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>`_ diff --git a/index.rst b/index.rst index 238387d2a5..adc43b67fb 100644 --- a/index.rst +++ b/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 ----------------------------