From 916d26db25c540e61ed6b3d667417cf8b60a96db Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:52 +0100 Subject: [PATCH] Create empty plan_pick_place_capability --- capabilities/CMakeLists.txt | 3 + .../capabilities_plugin_description.xml | 6 ++ capabilities/package.xml | 1 + .../src/plan_pick_place_capability.cpp | 74 +++++++++++++++++++ capabilities/src/plan_pick_place_capability.h | 71 ++++++++++++++++++ 5 files changed, 155 insertions(+) create mode 100644 capabilities/src/plan_pick_place_capability.cpp create mode 100644 capabilities/src/plan_pick_place_capability.h 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