Add tutorial on how to add a new move group capability

CMakeLists.txt

@@ -78,10 +78,12 @@ add_subdirectory(doc/examples/moveit_cpp)
 # add_subdirectory(doc/examples/visualizing_collisions)
 # add_subdirectory(doc/examples/bullet_collision_checker)
 add_subdirectory(doc/examples/realtime_servo)
+add_subdirectory(doc/examples/move_group_capabilities)
 
 ament_export_dependencies(
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
+pluginlib_export_plugin_description_file(moveit_ros_move_group doc/examples/move_group_capabilities/plugin_description.xml)
 
 ament_export_include_directories(include)
 

doc/examples/examples.rst

@@ -15,6 +15,7 @@ The simplest way to use MoveIt through scripting is using the ``move_group_inter
    move_group_interface/move_group_interface_tutorial
    move_group_python_interface/move_group_python_interface_tutorial
    moveit_commander_scripting/moveit_commander_scripting_tutorial
+   move_group_capabilities/move_group_capabilities
 
 Using MoveIt Directly Through the C++ API
 ------------------------------------------

doc/examples/move_group_capabilities/CMakeLists.txt

@@ -0,0 +1,22 @@
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    srv/GetCollisionObjects.srv
+    DEPENDENCIES
+    builtin_interfaces
+    moveit_msgs)
+
+add_library(get_collision_objects_service_capability SHARED
+    src/get_collision_objects_service_capability.cpp)
+rosidl_get_typesupport_target(${PROJECT_NAME}_target ${PROJECT_NAME} rosidl_typesupport_cpp)
+target_link_libraries(get_collision_objects_service_capability ${${PROJECT_NAME}_target})
+ament_target_dependencies(get_collision_objects_service_capability ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+    TARGETS
+    get_collision_objects_service_capability
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+)

doc/examples/move_group_capabilities/move_group_capabilities.rst

@@ -0,0 +1,85 @@
+Creating a new MoveGroup capability
+===================================
+
+In :doc:`Move Group C++ Interface </doc/examples/move_group_interface/move_group_interface_tutorial>` we saw how MoveGroup node make it easy to use MoveIt through ROS interfaces, this node have a list of `default capabilities <https://github.com/ros-planning/moveit2/blob/main/moveit_ros/move_group/default_capabilities_plugin_description.xml>`_ that provide common used action/services to interact with its internal components, sometimes we need to extend it for our own specific applications which could be done easily by creating MoveGroup capability.
+
+
+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>`.
+
+You should also have gone through the steps in :doc:`Visualization with MoveIt RViz Plugin </doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial>`
+
+Creating a MoveGroup Capability
+-------------------------------
+`This Page <http://wiki.ros.org/pluginlib>`_ gives a detailed explanation of how to add plugins in ROS in general. The two necessary elements are base and plugin classes. The plugin class inherits from the base class and overrides its virtual functions. The main library used for this purpose is pluginlib.
+
+In this section, we will show how to add a new MoveGroup capability. For demonstration purposes, we will create a capability which provide a service to query the collision objects in MoveGroup's planning scene. The final source files designed in this tutorial are available :codedir:`here <move_group_capabilities/src>`.
+
+First we start by creating a class GetCollisionObjects by inheriting from move_group::MoveGroupCapability, to do so we create a file named ``get_collision_objects_service_capability.cpp`` in src folder. the minimal requirement is to pass the plugin name to the base class constructor ``MoveGroupCapability``, override the ``initialize`` function, and register the class as a plugin by using ``PLUGINLIB_EXPORT_CLASS``.
+
+.. tutorial-formatter:: ./src/get_collision_objects_service_capability.cpp
+
+Exporting the capability
+^^^^^^^^^^^^^^^^^^^^^^^^
+To export the plugin we create a plugin description xml file ``plugin_description.xml``
+
+.. code-block:: xml
+
+  <library path="move_group_tool_changing_capability">
+    <class name="move_group/ToolChangingCapability" type="move_group::ToolChangingCapability" base_class_type="move_group::MoveGroupCapability">
+        <description>
+            MoveGroup capability to get a list of collision objects in the planning scene
+        </description>
+    </class>
+  </library>
+
+The ``plugin_description.xml`` need to be added to the ament index to be found at runtime, in ``CMakeLists.txt`` add
+
+**NOTE** Make sure to add it to the top-level ``CMakeLists.txt`` file otherwise it will not be detected bu ROS
+
+.. code-block:: cmake
+
+  pluginlib_export_plugin_description_file(moveit_ros_move_group doc/examples/move_group_capabilities/plugin_description.xml)
+
+Checking the capability
+^^^^^^^^^^^^^^^^^^^^^^^
+To get a list of the all available capabilities run
+
+.. code-block:: bash
+
+  ros2 run moveit_ros_move_group list_move_group_capabilities
+
+you should see ``move_group/GetCollisionObjectsCapability`` as a capability
+
+Capability usage
+^^^^^^^^^^^^^^^^
+To use the capability we need first to tell move_group to use it as follow
+
+.. code-block:: python
+
+  run_move_group_node = Node(
+      package="moveit_ros_move_group",
+      executable="move_group",
+      parameters=[
+          ...
+          {"capabilities": """move_group/GetCollisionObjects"""},
+      ],
+  )
+
+Now launch move_group
+
+.. code-block:: bash
+
+  ros2 launch moveit2_tutorials move_group.launch.py
+
+To use the new capability run the following line, it should return empty list of objects
+
+.. code-block:: bash
+
+  ros2 service call /move_group/get_collision_objects moveit2_tutorials/srv/GetCollisionObjects {}
+
+Now try to add different objects and use the capability again
+
+.. image:: list_collision_objects.gif
+   :width: 500pt

doc/examples/move_group_capabilities/plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="get_collision_objects_service_capability">
+    <class name="move_group/GetCollisionObjectsCapability" type="move_group::GetCollisionObjectsCapability" base_class_type="move_group::MoveGroupCapability">
+        <description>
+            MoveGroup capability to get a list of collision objects in the planning scene
+        </description>
+    </class>
+</library>

doc/examples/move_group_capabilities/src/get_collision_objects_service_capability.cpp

@@ -0,0 +1,73 @@
+// Copyright 2022 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/move_group/move_group_capability.h>
+#include <moveit2_tutorials/srv/get_collision_objects.hpp>
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.move_group.get_collision_objects_capability");
+
+// BEGIN_TUTORIAL
+//
+namespace move_group
+{
+class GetCollisionObjectsCapability : public MoveGroupCapability
+{
+public:
+  /* Pass the plugin name as a parameter to the base class */
+  GetCollisionObjectsCapability() : MoveGroupCapability("GetCollisionObjectsCapability")
+  {
+  }
+
+  /* Override this function to do the necessary initializations, here we only need a service server */
+  void initialize() override
+  {
+    get_collision_objects_service_ =
+        context_->moveit_cpp_->getNode()->create_service<moveit2_tutorials::srv::GetCollisionObjects>(
+            "~/get_collision_objects", std::bind(&GetCollisionObjectsCapability::getCollisionObjects, this,
+                                                 std::placeholders::_1, std::placeholders::_2));
+  }
+
+  void getCollisionObjects(const moveit2_tutorials::srv::GetCollisionObjects::Request::SharedPtr& /*req*/,
+                           moveit2_tutorials::srv::GetCollisionObjects::Response::SharedPtr res) const
+  {
+    planning_scene_monitor::LockedPlanningSceneRO scene(context_->moveit_cpp_->getPlanningSceneMonitor());
+    scene->getCollisionObjectMsgs(res->collision_objects);
+  }
+
+private:
+  rclcpp::Service<moveit2_tutorials::srv::GetCollisionObjects>::SharedPtr get_collision_objects_service_;
+};
+}  // namespace move_group
+
+#include <pluginlib/class_list_macros.hpp>
+/* Register the class as a plugins */
+PLUGINLIB_EXPORT_CLASS(move_group::GetCollisionObjectsCapability, move_group::MoveGroupCapability)
+
+// END_TUTORIAL

doc/examples/move_group_capabilities/srv/GetCollisionObjects.srv

@@ -0,0 +1,2 @@
+---
+moveit_msgs/CollisionObject[] collision_objects

package.xml

@@ -39,6 +39,7 @@
   <build_depend>geometric_shapes</build_depend>
   <build_depend>moveit_common</build_depend>
   <build_depend>moveit_ros_planning</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
 
   <!-- <exec_depend>franka_description</exec_depend> -->
   <!-- <exec_depend>moveit_commander</exec_depend> -->
@@ -54,9 +55,12 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>warehouse_ros_mongo</exec_depend>
   <exec_depend>xacro</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <export>
     <build_type>ament_cmake</build_type>
     <!-- An example controller manager plugin for MoveIt. This is not functional code. -->