[WIP] Add pick-place capability

capabilities/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_move_group
   moveit_task_constructor_msgs
+  moveit_task_constructor_core
   pluginlib
   std_msgs
 )
@@ -17,6 +18,7 @@ catkin_package(
   CATKIN_DEPENDS
     actionlib
     moveit_task_constructor_msgs
+    moveit_task_constructor_core
     std_msgs
 )
 
@@ -36,6 +38,7 @@ include_directories(
 
 add_library(${PROJECT_NAME}
    src/execute_task_solution_capability.cpp
+   src/plan_pick_place_capability.cpp
 )
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

capabilities/capabilities_plugin_description.xml

@@ -4,4 +4,10 @@
       Action server to execute solutions generated through the MoveIt Task Constructor.
     </description>
   </class>
+
+  <class name="move_group/PlanPickPlaceCapability" type="move_group::PlanPickPlaceCapability" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Action server to plan full pick and place motions using the MoveIt Task Constructor.
+    </description>
+  </class>
 </library>

capabilities/package.xml

@@ -18,6 +18,7 @@
 	<depend>pluginlib</depend>
 	<depend>std_msgs</depend>
 	<depend>moveit_task_constructor_msgs</depend>
+	<depend>moveit_task_constructor_core</depend>
 
 	<export>
 		<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>

capabilities/src/plan_pick_place_capability.cpp

@@ -0,0 +1,98 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Kentaro Wada.
+ *  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 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: Henning Kayser, Artur Karoly*/
+
+#include "plan_pick_place_capability.h"
+
+#include <moveit/move_group/capability_names.h>
+#include <moveit/robot_state/conversions.h>
+
+
+namespace move_group {
+
+PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
+
+void PlanPickPlaceCapability::initialize() {
+	// Configure the action server
+	as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>(
+	    root_node_handle_, "plan_pick_place",
+	    std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
+	as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
+  as_->start();
+  pick_place_task_ = std::make_unique<PickPlaceTask>("pick_place_task");
+}
+
+void PlanPickPlaceCapability::goalCallback(
+    const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
+	moveit_task_constructor_msgs::PlanPickPlaceResult result;
+
+  // Fill parameters
+  PickPlaceTask::Parameters parameters;
+  parameters.task_type_ = goal->task_type;
+  parameters.arm_group_name_ = goal->arm_group_name;
+  parameters.hand_group_name_ = goal->hand_group_name;
+  parameters.eef_name_ = goal->eef_name;
+  parameters.hand_frame_ = goal->hand_frame;
+  parameters.object_name_ = goal->object_id;
+  parameters.support_surfaces_ = goal->support_surfaces;
+  parameters.grasps_ = goal->grasps;
+  parameters.grasp_provider_plugin_name_ = goal->grasp_provider_plugin_name;
+  tf::poseMsgToEigen(goal->grasp_frame_transform, parameters.grasp_frame_transform_);
+
+  parameters.place_provider_plugin_name_ = goal->place_provider_plugin_name;
+  parameters.place_locations_ = goal->place_locations;
+
+  // Initialize task and plan
+  if (pick_place_task_->init(parameters)){
+    // Compute plan
+    result.success = pick_place_task_->plan();
+    if (result.success) {
+      pick_place_task_->getSolutionMsg(result.solution);
+    }
+  } else {
+    result.success = false;
+  }
+  // Retrieve and return result
+    as_->setSucceeded(result);
+}
+
+void PlanPickPlaceCapability::preemptCallback() {
+  pick_place_task_->preempt();
+}
+
+}  // namespace move_group
+
+#include <class_loader/class_loader.hpp>
+CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)

capabilities/src/plan_pick_place_capability.h

@@ -0,0 +1,71 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Hamburg University.
+ *  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 Hamburg University 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.
+ *********************************************************************/
+
+/*
+ * Capability to plan pick and place motions using the  MoveIt Task Constructor.
+ *
+ * Author: Henning Kayser
+ * */
+
+#pragma once
+
+#include <moveit/move_group/move_group_capability.h>
+#include <actionlib/server/simple_action_server.h>
+
+#include <moveit_task_constructor_msgs/PlanPickPlaceAction.h>
+#include <moveit/task_constructor/tasks/pick_place_task.h>
+
+#include <memory>
+
+namespace move_group {
+
+using moveit::task_constructor::tasks::PickPlaceTask;
+
+class PlanPickPlaceCapability : public MoveGroupCapability
+{
+public:
+	PlanPickPlaceCapability();
+
+	virtual void initialize();
+
+private:
+	void goalCallback(const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal);
+	void preemptCallback();
+
+	std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>> as_;
+
+  std::unique_ptr<PickPlaceTask> pick_place_task_;
+};
+
+}  // namespace move_group

core/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
 	moveit_ros_planning_interface
 	moveit_task_constructor_msgs
 	roscpp
+	rosparam_shortcuts
 	visualization_msgs
 	rviz_marker_tools
 )
@@ -18,6 +19,7 @@ catkin_package(
 	LIBRARIES
 		${PROJECT_NAME}
 		${PROJECT_NAME}_stages
+		${PROJECT_NAME}_tasks
 	INCLUDE_DIRS
 		include
 	CATKIN_DEPENDS
@@ -43,7 +45,7 @@ add_compile_options(-fvisibility-inlines-hidden)
 set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
 
 add_subdirectory(src)
-add_subdirectory(test)
+# add_subdirectory(test)
 
 install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
 	PATTERN "*_p.h" EXCLUDE)

core/include/moveit/task_constructor/stages/grasp_provider.h

@@ -0,0 +1,74 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Hamburg University
+ *  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 Bielefeld University 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.
+ *********************************************************************/
+
+/* Authors: Michael Goerner, Artur Karoly
+   Desc:    Grasp provider plugins and default plugin
+*/
+
+#pragma once
+
+#include <moveit/task_constructor/stages/generate_pose.h>
+#include "grasp_provider_base.h"
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+/// Default Grasp Provider plugin implementing the functionality of the GenerateGraspPose stage
+
+class GraspProviderDefault : public GraspProviderBase
+{
+public:
+	GraspProviderDefault(const std::string& name = "generate grasp pose");
+
+	void init(const core::RobotModelConstPtr& robot_model) override;
+	void compute() override;
+};
+
+
+/// Grasp Provider plugin for setting a set of grasp poses
+
+class GraspProviderFixedPoses : public GraspProviderBase
+{
+public:
+	GraspProviderFixedPoses(const std::string& name = "set grasp poses");
+
+	void init(const core::RobotModelConstPtr& robot_model) override;
+	void compute() override;
+};
+
+
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/include/moveit/task_constructor/stages/grasp_provider_base.h

@@ -0,0 +1,73 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Bielefeld + Hamburg University
+ *  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 Bielefeld University 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.
+ *********************************************************************/
+
+/* Authors: Artur Karoly, Jafar Abdi */
+
+#ifndef MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H
+#define MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H
+
+#include "memory"
+#include "moveit/task_constructor/container.h"
+#include "grasp_provider_base.h"
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+class GraspProviderBase : public GeneratePose
+{
+public:
+	GraspProviderBase(const std::string& name = "grasp provider");
+
+	void init(const std::shared_ptr<const moveit::core::RobotModel>& robot_model) override;
+
+	void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
+	void setObject(const std::string& object) { setProperty("object", object); }
+
+	void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
+	void setPreGraspPose(const ::moveit_msgs::RobotState_<std::allocator<void>>& pregrasp) {
+		properties().set("pregrasp", pregrasp);
+	}
+	void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
+	void setGraspPose(const ::moveit_msgs::RobotState_<std::allocator<void>>& grasp) {
+		properties().set("grasp", grasp);
+	}
+
+protected:
+	void onNewSolution(const SolutionBase& s) override;
+};
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit
+#include <moveit/task_constructor/stages/generate_pose.h>
+#endif  // MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H