add list of default capabilities (#359)

Functionality in MoveIt!'s move_group node is implemented in terms of
MoveGroupCapability plugins. But until now the choice of which plugins
are loaded was solely left to the user who in many cases is not familiar
with these plugins at all. This entails that whenever we introduce or change
a basic capability we have to annoy the user with warnings on startup that
bug him to change and/or update his move_group.launch file.

This commit changes the startup behavior of move_group to load a specified
list of default capabilities and provides a new ROS parameter `disable_capabilities`
that can be used to disable default plugins.

Fixes #125.

moveit_ros/move_group/include/moveit/move_group/capability_names.h

@@ -41,6 +41,25 @@
 
 namespace move_group
 {
+// These capabilities are loaded unless listed in disable_capabilities
+// clang-format off
+static const char* DEFAULT_CAPABILITIES[] = {
+   "move_group/MoveGroupCartesianPathService",
+   "move_group/MoveGroupKinematicsService",
+   "move_group/MoveGroupCartesianPathService",
+   "move_group/MoveGroupExecuteTrajectoryAction",
+   "move_group/MoveGroupKinematicsService",
+   "move_group/MoveGroupMoveAction",
+   "move_group/MoveGroupPickPlaceAction",
+   "move_group/MoveGroupPlanService",
+   "move_group/MoveGroupQueryPlannersService",
+   "move_group/MoveGroupStateValidationService",
+   "move_group/MoveGroupGetPlanningSceneService",
+   "move_group/ApplyPlanningSceneService",
+   "move_group/ClearOctomapService",
+};
+// clang-format on
+
 static const std::string PLANNER_SERVICE_NAME =
     "plan_kinematic_path";  // name of the advertised service (within the ~ namespace)
 static const std::string EXECUTE_SERVICE_NAME =

moveit_ros/move_group/src/move_group.cpp

@@ -36,12 +36,14 @@
 
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <tf/transform_listener.h>
+#include <moveit/move_group/capability_names.h>
 #include <moveit/move_group/move_group_capability.h>
 #include <boost/algorithm/string/join.hpp>
 #include <boost/tokenizer.hpp>
 #include <moveit/macros/console_colors.h>
 #include <moveit/move_group/node_name.h>
 #include <memory>
+#include <set>
 
 static const std::string ROBOT_DESCRIPTION =
     "robot_description";  // name of the robot description (a param name, so it can be changed externally)
@@ -102,32 +104,50 @@ class MoveGroupExe
       return;
     }
 
-    // add individual capabilities move_group supports
+    std::set<std::string> capabilities;
+
+    // add default capabilities
+    for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
+      capabilities.insert(DEFAULT_CAPABILITIES[i]);
+
+    // add capabilities listed in ROS parameter
     std::string capability_plugins;
     if (node_handle_.getParam("capabilities", capability_plugins))
     {
       boost::char_separator<char> sep(" ");
       boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
-      for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
+      capabilities.insert(tok.begin(), tok.end());
+    }
+
+    // drop capabilities that have been explicitly disabled
+    if (node_handle_.getParam("disable_capabilities", capability_plugins))
+    {
+      boost::char_separator<char> sep(" ");
+      boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
+      for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
+           ++cap_name)
+        capabilities.erase(*cap_name);
+    }
+
+    for (std::set<std::string>::iterator plugin = capabilities.cbegin(); plugin != capabilities.cend(); ++plugin)
+    {
+      try
       {
-        std::string plugin = *beg;
-        try
-        {
-          printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin.c_str());
-          MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(plugin);
-          cap->setContext(context_);
-          cap->initialize();
-          capabilities_.push_back(MoveGroupCapabilityPtr(cap));
-        }
-        catch (pluginlib::PluginlibException& ex)
-        {
-          ROS_ERROR_STREAM("Exception while loading move_group capability '"
-                           << plugin << "': " << ex.what() << std::endl
-                           << "Available capabilities: "
-                           << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
-        }
+        printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
+        MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(*plugin);
+        cap->setContext(context_);
+        cap->initialize();
+        capabilities_.push_back(MoveGroupCapabilityPtr(cap));
+      }
+      catch (pluginlib::PluginlibException& ex)
+      {
+        ROS_ERROR_STREAM("Exception while loading move_group capability '"
+                         << *plugin << "': " << ex.what() << std::endl
+                         << "Available capabilities: "
+                         << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
       }
     }
+
     std::stringstream ss;
     ss << std::endl;
     ss << std::endl;

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch

@@ -46,19 +46,21 @@
     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-    <!-- MoveGroup capabilities to load -->
-    <param name="capabilities" value="move_group/MoveGroupCartesianPathService
-				      move_group/MoveGroupExecuteTrajectoryAction
-				      move_group/MoveGroupKinematicsService
-				      move_group/MoveGroupMoveAction
-				      move_group/MoveGroupPickPlaceAction
-				      move_group/MoveGroupPlanService
-				      move_group/MoveGroupQueryPlannersService
-				      move_group/MoveGroupStateValidationService
-				      move_group/MoveGroupGetPlanningSceneService
-				      move_group/ApplyPlanningSceneService
-				      move_group/ClearOctomapService
-				      " />
+    <!-- load these non-default MoveGroup capabilities -->
+    <!--
+    <param name="capabilities" value="
+                  a_package/AwsomeMotionPlanningCapability
+                  another_package/GraspPlanningPipeline
+                  " />
+    -->
+
+    <!-- inhibit these default MoveGroup capabilities -->
+    <!--
+    <param name="disable_capabilities" value="
+                  move_group/MoveGroupKinematicsService
+                  move_group/ClearOctomapService
+                  " />
+    -->
 
     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
     <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />