move_group now uses plugins

interface/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(PythonLibs)
 find_package(catkin REQUIRED COMPONENTS
   moveit_ros_planning
   moveit_ros_warehouse
+  moveit_ros_manipulation
   moveit_ros_move_group
   eigen_conversions
   interactive_markers
@@ -39,6 +40,7 @@ catkin_package(
   CATKIN_DEPENDS
     moveit_ros_planning
     moveit_ros_warehouse 
+    moveit_ros_manipulation
     moveit_ros_move_group
     )
 

interface/move_group_interface/src/move_group.cpp

@@ -35,7 +35,8 @@
 /* Author: Ioan Sucan */
 
 #include <stdexcept>
-#include <moveit/move_group/names.h>
+#include <moveit/move_group/capability_names.h>
+#include <moveit/move_group_pick_place_capability/capability_names.h>
 #include <moveit/move_group_interface/move_group.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/planning_scene_monitor/current_state_monitor.h>

interface/package.xml

@@ -18,6 +18,7 @@
   <build_depend>moveit_ros_planning</build_depend>
   <build_depend>moveit_ros_warehouse</build_depend> 
   <build_depend>moveit_ros_move_group</build_depend>
+  <build_depend>moveit_ros_manipulation</build_depend>
   <build_depend>roscpp</build_depend> 
   <build_depend>rospy</build_depend>
   <build_depend>rosconsole</build_depend>
@@ -30,6 +31,7 @@
   <run_depend>moveit_ros_planning</run_depend>
   <run_depend>moveit_ros_warehouse</run_depend> 
   <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_ros_manipulation</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>rosconsole</run_depend>

manipulation/CMakeLists.txt

@@ -10,23 +10,27 @@ find_package(Boost REQUIRED thread system filesystem date_time program_options)
 find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_planning
-  moveit_ros_warehouse
+  moveit_ros_move_group
   roscpp
   rosconsole
   tf
+  pluginlib
+  actionlib
 )
 
 catkin_package(
   INCLUDE_DIRS
     pick_place/include
+    move_group_pick_place_capability/include
   LIBRARIES
     moveit_pick_place_planner
   CATKIN_DEPENDS
     moveit_core
     moveit_ros_planning
-    moveit_ros_warehouse)
+    )
 
 include_directories(pick_place/include)
+include_directories(move_group_pick_place_capability/include)
 
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(SYSTEM
@@ -37,3 +41,8 @@ link_directories(${Boost_LIBRARY_DIRS})
 link_directories(${catkin_LIBRARY_DIRS})
 
 add_subdirectory(pick_place)
+add_subdirectory(move_group_pick_place_capability)
+
+install(FILES
+  pick_place_capability_plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

manipulation/move_group_pick_place_capability/CMakeLists.txt

@@ -0,0 +1,15 @@
+set(MOVEIT_LIB_NAME moveit_move_group_pick_place_capability)
+
+include_directories(src)
+
+add_library(${MOVEIT_LIB_NAME}
+  src/pick_place_action_capability.cpp
+  )
+
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+install(TARGETS ${MOVEIT_LIB_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+install(DIRECTORY include/ DESTINATION include)

manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h

@@ -0,0 +1,50 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2012, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  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 Willow Garage 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 OWNER 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.
+*********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES
+#define MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES
+
+#include <string>
+
+namespace move_group
+{
+
+static const std::string PICKUP_ACTION = "pickup"; // name of 'pickup' action
+static const std::string PLACE_ACTION = "place"; // name of 'place' action
+
+}
+
+#endif

move_group/src/move_group_pick_place_action_capability.cpp → manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp

@@ -34,20 +34,25 @@
 
 /* Author: Ioan Sucan */
 
-#include <moveit/move_group/names.h>
-#include <moveit/move_group/move_group_pick_place_action_capability.h>
-#include <moveit/pick_place/pick_place.h>
+#include "pick_place_action_capability.h"
 #include <moveit/plan_execution/plan_execution.h>
 #include <moveit/plan_execution/plan_with_sensing.h>
+#include <moveit/move_group_pick_place_capability/capability_names.h>
 
 move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction() : 
-  MoveGroupCapability(),
+  MoveGroupCapability("PickPlaceAction"),
   pickup_state_(IDLE)
 {
 }
 
 void move_group::MoveGroupPickPlaceAction::initialize()
-{
+{ 
+  pick_place_.reset(new pick_place::PickPlace(context_->planning_pipeline_));
+  pick_place_->displayComputedMotionPlans(true);
+
+  if (context_->debug_)
+    pick_place_->displayProcessedGrasps(true);
+
   // start the pickup action server
   pickup_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::PickupAction>(root_node_handle_, PICKUP_ACTION,
                                                                                            boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false));
@@ -87,7 +92,7 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const
   try
   {
     planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
-    plan = context_->pick_place_->planPick(ps, *goal);
+    plan = pick_place_->planPick(ps, *goal);
   }
   catch(std::runtime_error &ex)
   {
@@ -127,7 +132,7 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const m
   try
   {
     planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
-    plan = context_->pick_place_->planPlace(ps, *goal);
+    plan = pick_place_->planPlace(ps, *goal);
   }
   catch(std::runtime_error &ex)
   {
@@ -170,7 +175,7 @@ bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const movei
   pick_place::PickPlanPtr pick_plan;
   try
   {
-    pick_plan = context_->pick_place_->planPick(plan.planning_scene_, goal);
+    pick_plan = pick_place_->planPick(plan.planning_scene_, goal);
   }
   catch(std::runtime_error &ex)
   {
@@ -212,7 +217,7 @@ bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Place(const moveit
   pick_place::PlacePlanPtr place_plan;
   try
   {
-    place_plan = context_->pick_place_->planPlace(plan.planning_scene_, goal);
+    place_plan = pick_place_->planPlace(plan.planning_scene_, goal);
   }
   catch(std::runtime_error &ex)
   {
@@ -438,3 +443,6 @@ void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& g
     goal.possible_grasps.push_back(g);
   }
 }
+
+#include <class_loader/class_loader.h> 
+CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupPickPlaceAction, move_group::MoveGroupCapability)

move_group/include/moveit/move_group/move_group_pick_place_action_capability.h → manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h

@@ -39,6 +39,7 @@
 
 #include <moveit/move_group/move_group_capability.h>
 #include <actionlib/server/simple_action_server.h>
+#include <moveit/pick_place/pick_place.h>
 #include <moveit_msgs/PickupAction.h>
 #include <moveit_msgs/PlaceAction.h>
 
@@ -79,6 +80,8 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability
   void setPlaceState(MoveGroupState state);
 
   void fillGrasps(moveit_msgs::PickupGoal& goal);
+
+  pick_place::PickPlacePtr pick_place_;
 
   boost::scoped_ptr<actionlib::SimpleActionServer<moveit_msgs::PickupAction> > pickup_action_server_;
   moveit_msgs::PickupFeedback pickup_feedback_;
@@ -87,13 +90,11 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability
   moveit_msgs::PlaceFeedback place_feedback_;
 
   boost::scoped_ptr<moveit_msgs::AttachedCollisionObject> diff_attached_object_;
-  
+
   MoveGroupState pickup_state_;
   MoveGroupState place_state_;  
 };
 
-
-
 }
 
 #endif

manipulation/package.xml

@@ -17,20 +17,28 @@
 
   <buildtool_depend>catkin</buildtool_depend> 
 
+  <build_depend>actionlib</build_depend>
   <build_depend>moveit_core</build_depend>
-  <build_depend>moveit_ros_warehouse</build_depend>
+  <build_depend>moveit_ros_move_group</build_depend>
   <build_depend>moveit_ros_planning</build_depend>
   <build_depend>moveit_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rosconsole</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>pluginlib</build_depend>
 
+  <run_depend>actionlib</run_depend>
   <run_depend>moveit_core</run_depend>
-  <run_depend>moveit_ros_warehouse</run_depend>
+  <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_ros_planning</run_depend>
   <run_depend>moveit_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rosconsole</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <export>
+    <moveit_ros_move_group plugin="${prefix}/pick_place_capability_plugin_description.xml"/>
+  </export>
 
 </package>

manipulation/pick_place_capability_plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="libmoveit_move_group_pick_place_capability">
+
+  <class name="move_group/MoveGroupPickPlaceAction" type="move_group::MoveGroupPickPlaceAction" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide pick and place capability to MoveGroup
+    </description>
+  </class>
+
+</library>

move_group/CMakeLists.txt

@@ -9,9 +9,9 @@ find_package(Boost REQUIRED system filesystem date_time program_options signals
 find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_planning
-  moveit_ros_manipulation
   actionlib
   roscpp
+  pluginlib
   tf
 )
 
@@ -22,9 +22,10 @@ catkin_package(
   CATKIN_DEPENDS
     moveit_core
     moveit_ros_planning
-    moveit_ros_manipulation)
+)
 
 include_directories(include)
+include_directories(sec/default_capabilities)
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(SYSTEM
                     ${Boost_INCLUDE_DIRS}
@@ -33,22 +34,39 @@ include_directories(SYSTEM
 link_directories(${Boost_LIBRARY_DIRS})
 link_directories(${catkin_LIBRARY_DIRS})
 
-add_executable(move_group_action_server
+add_executable(move_group
   src/move_group_context.cpp
   src/move_group_capability.cpp
-  src/move_group_move_action_capability.cpp
-  src/move_group_pick_place_action_capability.cpp
-  src/move_group_plan_service_capability.cpp
-  src/move_group_execute_service_capability.cpp
-  src/move_group_query_planners_service_capability.cpp  
-  src/move_group_kinematics_service_capability.cpp
-  src/move_group_state_validation_service_capability.cpp
-  src/move_group_cartesian_path_service_capability.cpp
   src/move_group.cpp
+)
+
+add_executable(list_move_group_capabilities src/list_capabilities.cpp)
+
+add_library(moveit_move_group_default_capabilities
+  src/default_capabilities/move_action_capability.cpp
+  src/default_capabilities/plan_service_capability.cpp
+  src/default_capabilities/execute_service_capability.cpp
+  src/default_capabilities/query_planners_service_capability.cpp  
+  src/default_capabilities/kinematics_service_capability.cpp
+  src/default_capabilities/state_validation_service_capability.cpp
+  src/default_capabilities/cartesian_path_service_capability.cpp
   )
 
-target_link_libraries(move_group_action_server ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(move_group ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(moveit_move_group_default_capabilities ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(list_move_group_capabilities ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+install(TARGETS move_group list_move_group_capabilities moveit_move_group_default_capabilities
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-install(TARGETS move_group_action_server RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 install(DIRECTORY include/ DESTINATION include)
-install(PROGRAMS scripts/load_map scripts/save_map DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(PROGRAMS
+  scripts/load_map scripts/save_map DESTINATION
+  ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(FILES
+  default_capabilities_plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})