diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt
index df46f9ff..99a1ab58 100644
--- a/capabilities/CMakeLists.txt
+++ b/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
)
@@ -16,6 +17,7 @@ catkin_package(
LIBRARIES $PROJECT_NAME}
CATKIN_DEPENDS
moveit_task_constructor_msgs
+ moveit_task_constructor_core
std_msgs
)
@@ -25,6 +27,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})
diff --git a/capabilities/capabilities_plugin_description.xml b/capabilities/capabilities_plugin_description.xml
index eba669c8..a0ecf7ad 100644
--- a/capabilities/capabilities_plugin_description.xml
+++ b/capabilities/capabilities_plugin_description.xml
@@ -4,4 +4,10 @@
Action server to execute solutions generated through the MoveIt Task Constructor.
+
+
+
+ Action server to plan full pick and place motions using the MoveIt Task Constructor.
+
+
diff --git a/capabilities/package.xml b/capabilities/package.xml
index 11eaa306..27b5cfb5 100644
--- a/capabilities/package.xml
+++ b/capabilities/package.xml
@@ -18,6 +18,7 @@
pluginlib
std_msgs
moveit_task_constructor_msgs
+ moveit_task_constructor_core
diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp
new file mode 100644
index 00000000..0f0ffd07
--- /dev/null
+++ b/capabilities/src/plan_pick_place_capability.cpp
@@ -0,0 +1,74 @@
+/*********************************************************************
+ * 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 */
+
+#include "plan_pick_place_capability.h"
+
+#include
+#include
+
+
+namespace move_group {
+
+PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
+
+void PlanPickPlaceCapability::initialize() {
+ // configure the action server
+ as_.reset(new actionlib::SimpleActionServer(
+ root_node_handle_, "plan_pick_place",
+ std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
+ as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
+ as_->start();
+ ros::NodeHandle nh("~");
+ pick_place_task_ = std::make_unique("", nh);
+}
+
+void PlanPickPlaceCapability::goalCallback(
+ const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
+ moveit_task_constructor_msgs::PlanPickPlaceResult result;
+
+ // initialize task
+ // run plan
+ // return result
+}
+
+void PlanPickPlaceCapability::preemptCallback() {
+ // TODO(henningkayser): abort planning
+}
+
+} // namespace move_group
+
+#include
+CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)
diff --git a/capabilities/src/plan_pick_place_capability.h b/capabilities/src/plan_pick_place_capability.h
new file mode 100644
index 00000000..6df190b1
--- /dev/null
+++ b/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
+#include
+
+#include
+#include
+
+#include
+
+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> as_;
+
+ std::unique_ptr pick_place_task_;
+};
+
+} // namespace move_group