diff --git a/CMakeLists.txt b/CMakeLists.txt index 54b3c16cec..4d71088afd 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index c378f631b1..ea6b1f19ea 100644 --- a/doc/examples/examples.rst +++ b/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 ------------------------------------------ diff --git a/doc/examples/move_group_capabilities/CMakeLists.txt b/doc/examples/move_group_capabilities/CMakeLists.txt new file mode 100644 index 0000000000..c5709429dc --- /dev/null +++ b/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} +) diff --git a/doc/examples/move_group_capabilities/list_collision_objects.gif b/doc/examples/move_group_capabilities/list_collision_objects.gif new file mode 100644 index 0000000000..d28eb5ae3f Binary files /dev/null and b/doc/examples/move_group_capabilities/list_collision_objects.gif differ diff --git a/doc/examples/move_group_capabilities/move_group_capabilities.rst b/doc/examples/move_group_capabilities/move_group_capabilities.rst new file mode 100644 index 0000000000..4eda36845e --- /dev/null +++ b/doc/examples/move_group_capabilities/move_group_capabilities.rst @@ -0,0 +1,85 @@ +Creating a new MoveGroup capability +=================================== + +In :doc:`Move Group C++ Interface ` we saw how MoveGroup node make it easy to use MoveIt through ROS interfaces, this node have a list of `default capabilities `_ 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 `. + +You should also have gone through the steps in :doc:`Visualization with MoveIt RViz Plugin ` + +Creating a MoveGroup Capability +------------------------------- +`This Page `_ 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 `. + +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 + + + + + MoveGroup capability to get a list of collision objects in the planning scene + + + + +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 diff --git a/doc/examples/move_group_capabilities/plugin_description.xml b/doc/examples/move_group_capabilities/plugin_description.xml new file mode 100644 index 0000000000..458414fcbe --- /dev/null +++ b/doc/examples/move_group_capabilities/plugin_description.xml @@ -0,0 +1,7 @@ + + + + MoveGroup capability to get a list of collision objects in the planning scene + + + diff --git a/doc/examples/move_group_capabilities/src/get_collision_objects_service_capability.cpp b/doc/examples/move_group_capabilities/src/get_collision_objects_service_capability.cpp new file mode 100644 index 0000000000..0122f61f6c --- /dev/null +++ b/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 +#include +#include +#include + +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( + "~/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::SharedPtr get_collision_objects_service_; +}; +} // namespace move_group + +#include +/* Register the class as a plugins */ +PLUGINLIB_EXPORT_CLASS(move_group::GetCollisionObjectsCapability, move_group::MoveGroupCapability) + +// END_TUTORIAL diff --git a/doc/examples/move_group_capabilities/srv/GetCollisionObjects.srv b/doc/examples/move_group_capabilities/srv/GetCollisionObjects.srv new file mode 100644 index 0000000000..c43866a57d --- /dev/null +++ b/doc/examples/move_group_capabilities/srv/GetCollisionObjects.srv @@ -0,0 +1,2 @@ +--- +moveit_msgs/CollisionObject[] collision_objects diff --git a/package.xml b/package.xml index a6080877ba..b183af68c1 100644 --- a/package.xml +++ b/package.xml @@ -39,6 +39,7 @@ geometric_shapes moveit_common moveit_ros_planning + rosidl_default_generators @@ -54,9 +55,12 @@ rviz2 warehouse_ros_mongo xacro + rosidl_default_runtime ament_cmake_gtest + rosidl_interface_packages + ament_cmake