From bdb25521dd52defae40323ff0a053c1f5f4f70ed Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Wed, 22 Jul 2020 11:33:05 -0500 Subject: [PATCH] This stage uses action messages to communicate with an action server external to MTC. The action server can use machine learning pipelines to sample grasps given a point cloud/depth image. This removes complex dependencies from MTC. The stage sends a request for grasps. If a time out is reached or no grasps are found then planning fails. This stage uses a template parameter which is the action message and inherits from an action base class. This allows other stages to inherit these properties and use action messages. --- core/CMakeLists.txt | 2 + .../task_constructor/stages/action_base.h | 126 +++++ .../task_constructor/stages/deep_grasp_pose.h | 299 ++++++++++++ core/package.xml | 1 + core/src/stages/CMakeLists.txt | 2 + demo/CMakeLists.txt | 11 +- .../gpd_pick_place_task.h | 124 ----- demo/package.xml | 1 + demo/src/gpd_pick_place_demo.cpp | 135 ------ demo/src/gpd_pick_place_task.cpp | 452 ------------------ msgs/CMakeLists.txt | 14 +- msgs/action/SampleGraspPoses.action | 11 + msgs/package.xml | 5 + 13 files changed, 463 insertions(+), 720 deletions(-) create mode 100644 core/include/moveit/task_constructor/stages/action_base.h create mode 100644 core/include/moveit/task_constructor/stages/deep_grasp_pose.h delete mode 100644 demo/include/moveit_task_constructor_demo/gpd_pick_place_task.h delete mode 100644 demo/src/gpd_pick_place_demo.cpp delete mode 100644 demo/src/gpd_pick_place_task.cpp create mode 100644 msgs/action/SampleGraspPoses.action diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7032b57dc..323cb657c 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -3,6 +3,7 @@ project(moveit_task_constructor_core) find_package(Boost REQUIRED) find_package(catkin REQUIRED COMPONENTS + actionlib eigen_conversions geometry_msgs moveit_core @@ -24,6 +25,7 @@ catkin_package( geometry_msgs moveit_core moveit_task_constructor_msgs + rviz_marker_tools visualization_msgs ) diff --git a/core/include/moveit/task_constructor/stages/action_base.h b/core/include/moveit/task_constructor/stages/action_base.h new file mode 100644 index 000000000..47836d4e4 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/action_base.h @@ -0,0 +1,126 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2020 PickNik 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 copyright holder 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 HOLDER 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: Boston Cleek + Desc: Abstact class for stages using a simple action client. +*/ + +#pragma once + +#include +#include +#include + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/** + * @brief Interface allowing stages to use a simple action client + * @param ActionSpec - action message (action message name + "ACTION") + * @details Some stages may require an action client. This class wraps the + * simple client action interface and exposes event based execution callbacks. + */ +template +class ActionBase +{ +protected: + ACTION_DEFINITION(ActionSpec); + +public: + /** + * @brief Constructor + * @param action_name - action namespace + * @param spin_thread - spins a thread to service this action's subscriptions + * @param goal_timeout - goal to completed time out (0 is considered infinite timeout) + * @param server_timeout - connection to server time out (0 is considered infinite timeout) + * @details Initialize the action client and time out parameters + */ + ActionBase(const std::string& action_name, bool spin_thread, double goal_timeout, double server_timeout); + + /** + * @brief Constructor + * @param action_name - action namespace + * @param spin_thread - spins a thread to service this action's subscriptions + * @details Initialize the action client and time out parameters to infinity + */ + ActionBase(const std::string& action_name, bool spin_thread); + + /* @brief Destructor */ + virtual ~ActionBase() = default; + + /* @brief Called when goal becomes active */ + virtual void activeCallback() = 0; + + /** + * @brief Called every time feedback is received for the goal + * @param feedback - pointer to the feedback message + */ + virtual void feedbackCallback(const FeedbackConstPtr& feedback) = 0; + + /** + * @brief Called once when the goal completes + * @param state - state info for goal + * @param result - pointer to result message + */ + virtual void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) = 0; + +protected: + ros::NodeHandle nh_; + std::string action_name_; // action name space + std::unique_ptr> clientPtr_; // action client + double server_timeout_, goal_timeout_; // connection and goal completed time out +}; + +template +ActionBase::ActionBase(const std::string& action_name, bool spin_thread, double goal_timeout, + double server_timeout) + : action_name_(action_name), server_timeout_(server_timeout), goal_timeout_(goal_timeout) { + clientPtr_.reset(new actionlib::SimpleActionClient(nh_, action_name_, spin_thread)); + + // Negative timeouts are set to zero + server_timeout_ = server_timeout_ < std::numeric_limits::epsilon() ? 0.0 : server_timeout_; + goal_timeout_ = goal_timeout_ < std::numeric_limits::epsilon() ? 0.0 : goal_timeout_; +} + +template +ActionBase::ActionBase(const std::string& action_name, bool spin_thread) + : action_name_(action_name), server_timeout_(0.0), goal_timeout_(0.0) { + clientPtr_.reset(new actionlib::SimpleActionClient(nh_, action_name_, spin_thread)); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit + diff --git a/core/include/moveit/task_constructor/stages/deep_grasp_pose.h b/core/include/moveit/task_constructor/stages/deep_grasp_pose.h new file mode 100644 index 000000000..9b69f94b7 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/deep_grasp_pose.h @@ -0,0 +1,299 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2020 PickNik 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 copyright holder 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 HOLDER 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: Boston Cleek + Desc: Grasp generator stage using deep learning based grasp synthesizers +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +// #include +#include +// #include + +namespace moveit { +namespace task_constructor { +namespace stages { + +constexpr char LOGNAME[] = "deep_grasp_generator"; + +/** + * @brief Generate grasp candidates using deep learning approaches + * @param ActionSpec - action message (action message name + "ACTION") + * @details Interfaces with a deep learning based grasp library using a action client + */ +template +class DeepGraspPose : public GeneratePose, ActionBase +{ +private: + typedef ActionBase ActionBaseT; + ACTION_DEFINITION(ActionSpec); + +public: + /** + * @brief Constructor + * @param action_name - action namespace + * @param stage_name - name of stage + * @param goal_timeout - goal to completed time out (0 is considered infinite timeout) + * @param server_timeout - connection to server time out (0 is considered infinite timeout) + * @details Initialize the client and connect to server + */ + DeepGraspPose(const std::string& action_name, const std::string& stage_name = "generate grasp pose", + double goal_timeout = 0.0, double server_timeout = 0.0); + + /** + * @brief Composes the action goal and sends to server + */ + void composeGoal(); + + /** + * @brief Monitors status of action goal + * @return true if grasp candidates are received within (optional) timeout + * @details This is a blocking call. It will wait until either grasp candidates + * are received or the timeout has been reached. + */ + bool monitorGoal(); + + void activeCallback() override; + void feedbackCallback(const FeedbackConstPtr& feedback) override; + void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) override; + + void init(const core::RobotModelConstPtr& robot_model) override; + void compute() 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& pregrasp) { properties().set("pregrasp", pregrasp); } + void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); } + void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); } + +protected: + void onNewSolution(const SolutionBase& s) override; + +private: + bool found_candidates_; + std::vector grasp_candidates_; + std::vector costs_; +}; + +template +DeepGraspPose::DeepGraspPose(const std::string& action_name, const std::string& stage_name, + double goal_timeout, double server_timeout) + : GeneratePose(stage_name), ActionBaseT(action_name, false, goal_timeout, server_timeout), found_candidates_(false) { + auto& p = properties(); + p.declare("eef", "name of end-effector"); + p.declare("object"); + p.declare("pregrasp", "pregrasp posture"); + p.declare("grasp", "grasp posture"); + + ROS_INFO_NAMED(LOGNAME, "Waiting for connection to grasp generation action server..."); + ActionBaseT::clientPtr_->waitForServer(ros::Duration(ActionBaseT::server_timeout_)); + ROS_INFO_NAMED(LOGNAME, "Connected to server"); +} + +template +void DeepGraspPose::composeGoal() { + Goal goal; + goal.action_name = ActionBaseT::action_name_; + ActionBaseT::clientPtr_->sendGoal( + goal, std::bind(&DeepGraspPose::doneCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&DeepGraspPose::activeCallback, this), + std::bind(&DeepGraspPose::feedbackCallback, this, std::placeholders::_1)); + + ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str()); +} + +template +bool DeepGraspPose::monitorGoal() { + // monitor timeout + const bool monitor_timeout = ActionBaseT::goal_timeout_ > std::numeric_limits::epsilon() ? true : false; + const double timeout_time = ros::Time::now().toSec() + ActionBaseT::goal_timeout_; + + while (ActionBaseT::nh_.ok()) { + ros::spinOnce(); + + // timeout reached + if (ros::Time::now().toSec() > timeout_time && monitor_timeout) { + ActionBaseT::clientPtr_->cancelGoal(); + ROS_ERROR_NAMED(LOGNAME, "Grasp pose generator time out reached"); + return false; + } else if (found_candidates_) { + // timeout not reached (or not active) and grasps are found + // only way return true + break; + } + } + + return true; +} + +template +void DeepGraspPose::activeCallback() { + ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active"); + found_candidates_ = false; +} + +template +void DeepGraspPose::feedbackCallback(const FeedbackConstPtr& feedback) { + // each candidate should have a cost + if (feedback->grasp_candidates.size() != feedback->costs.size()) { + ROS_ERROR_NAMED(LOGNAME, "Invalid input: each grasp candidate needs an associated cost"); + } else { + ROS_INFO_NAMED(LOGNAME, "Grasp generated feedback received %lu candidates: ", feedback->grasp_candidates.size()); + + grasp_candidates_.resize(feedback->grasp_candidates.size()); + costs_.resize(feedback->costs.size()); + + grasp_candidates_ = feedback->grasp_candidates; + costs_ = feedback->costs; + + found_candidates_ = true; + } +} + +template +void DeepGraspPose::doneCallback(const actionlib::SimpleClientGoalState& state, + const ResultConstPtr& result) { + if (state == actionlib::SimpleClientGoalState::SUCCEEDED) { + ROS_INFO_NAMED(LOGNAME, "Found grasp candidates (result): %s", result->grasp_state.c_str()); + } else { + ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found (state): %s", + ActionBaseT::clientPtr_->getState().toString().c_str()); + } +} + +template +void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model) { + InitStageException errors; + try { + GeneratePose::init(robot_model); + } catch (InitStageException& e) { + errors.append(e); + } + + const auto& props = properties(); + + // check availability of object + props.get("object"); + // check availability of eef + const std::string& eef = props.get("eef"); + if (!robot_model->hasEndEffector(eef)) { + errors.push_back(*this, "unknown end effector: " + eef); + } else { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) { + errors.push_back(*this, "unknown end effector pose: " + name); + } + } + + if (errors) { + throw errors; + } +} + +template +void DeepGraspPose::compute() { + if (upstream_solutions_.empty()) { + return; + } + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + + // set end effector pose + const auto& props = properties(); + const std::string& eef = props.get("eef"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); + + robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + robot_state.setToDefaultValues(jmg, props.get("pregrasp")); + + // compose/send goal + composeGoal(); + + // monitor feedback/results + // blocking function untill timeout reached or results received + if (monitorGoal()) { + // ROS_WARN_NAMED(LOGNAME, "number %lu: ",grasp_candidates_.size()); + for (unsigned int i = 0; i < grasp_candidates_.size(); i++) { + InterfaceState state(scene); + state.properties().set("target_pose", grasp_candidates_.at(i)); + props.exposeTo(state.properties(), { "pregrasp", "grasp" }); + + SubTrajectory trajectory; + trajectory.setCost(costs_.at(i)); + trajectory.setComment(std::to_string(i)); + + // add frame at target pose + rviz_marker_tools::appendFrame(trajectory.markers(), grasp_candidates_.at(i), 0.1, "grasp frame"); + + spawn(std::move(state), std::move(trajectory)); + } + } +} + +template +void DeepGraspPose::onNewSolution(const SolutionBase& s) { + planning_scene::PlanningSceneConstPtr scene = s.end()->scene(); + + const auto& props = properties(); + const std::string& object = props.get("object"); + if (!scene->knowsFrameTransform(object)) { + const std::string msg = "object '" + object + "' not in scene"; + if (storeFailures()) { + InterfaceState state(scene); + SubTrajectory solution; + solution.markAsFailure(); + solution.setComment(msg); + spawn(std::move(state), std::move(solution)); + } else { + ROS_WARN_STREAM_NAMED(LOGNAME, msg); + } + return; + } + + upstream_solutions_.push(&s); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/package.xml b/core/package.xml index bb6e89b06..801900fc6 100644 --- a/core/package.xml +++ b/core/package.xml @@ -9,6 +9,7 @@ catkin + actionlib eigen_conversions geometry_msgs roscpp diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 857a4977c..4f1c3f67d 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -2,7 +2,9 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/modify_planning_scene.h ${PROJECT_INCLUDE}/stages/fix_collision_objects.h + ${PROJECT_INCLUDE}/stages/action_base.h ${PROJECT_INCLUDE}/stages/current_state.h + ${PROJECT_INCLUDE}/stages/deep_grasp_pose.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h ${PROJECT_INCLUDE}/stages/generate_pose.h diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index b85e6b919..7eb7d1e20 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -4,10 +4,10 @@ project(moveit_task_constructor_demo) add_compile_options(-std=c++14) find_package(catkin REQUIRED COMPONENTS + roscpp moveit_core - moveit_ros_planning_interface moveit_task_constructor_core - roscpp + moveit_ros_planning_interface rosparam_shortcuts ) @@ -27,15 +27,12 @@ target_link_libraries(modular ${catkin_LIBRARIES}) add_executable(pick_place_demo src/pick_place_demo.cpp src/pick_place_task.cpp) target_link_libraries(pick_place_demo ${catkin_LIBRARIES}) -add_executable(gpd_pick_place_demo src/gpd_pick_place_demo.cpp src/gpd_pick_place_task.cpp) -target_link_libraries(gpd_pick_place_demo ${catkin_LIBRARIES}) - -install(TARGETS cartesian gpd_pick_place_demo modular pick_place_demo +install(TARGETS cartesian modular pick_place_demo ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY config launch +install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/demo/include/moveit_task_constructor_demo/gpd_pick_place_task.h b/demo/include/moveit_task_constructor_demo/gpd_pick_place_task.h deleted file mode 100644 index 7dadb774e..000000000 --- a/demo/include/moveit_task_constructor_demo/gpd_pick_place_task.h +++ /dev/null @@ -1,124 +0,0 @@ -/********************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019 PickNik LLC. - * 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 copyright holder 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 HOLDER 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, Simon Goldstein, Boston Cleek - Desc: A demo to show MoveIt Task Constructor using GPD in action -*/ - -// ROS -#include - -// MoveIt -#include -#include -#include - -// MTC -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#pragma once - -namespace moveit_task_constructor_demo { -using namespace moveit::task_constructor; - -class PickPlaceTask -{ -public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); - ~PickPlaceTask() = default; - - void loadParameters(); - - void init(); - - bool plan(); - - bool execute(); - -private: - ros::NodeHandle nh_; - - std::string task_name_; - moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_group_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Execution - actionlib::SimpleActionClient execute_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::Pose place_pose_; - double place_surface_offset_; -}; -} // namespace moveit_task_constructor_demo diff --git a/demo/package.xml b/demo/package.xml index 27ef23921..1624a6c02 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -11,6 +11,7 @@ BSD catkin + roscpp moveit_task_constructor_core moveit_ros_planning_interface moveit_core diff --git a/demo/src/gpd_pick_place_demo.cpp b/demo/src/gpd_pick_place_demo.cpp deleted file mode 100644 index 257937a16..000000000 --- a/demo/src/gpd_pick_place_demo.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/********************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019 PickNik LLC. - * 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 copyright holder 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 HOLDER 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, Simon Goldstein, Boston Cleek - Desc: A demo to show MoveIt Task Constructor using GPD in action - */ - -// ROS -#include - -// MTC pick/place demo implementation -#include - -#include -#include -#include -#include - -constexpr char LOGNAME[] = "moveit_task_constructor_demo"; - -void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { - if (!psi.applyCollisionObject(object)) - throw std::runtime_error("Failed to spawn object: " + object.id); -} - -moveit_msgs::CollisionObject createTable() { - ros::NodeHandle pnh("~"); - std::string table_name, table_reference_frame; - std::vector table_dimensions; - geometry_msgs::Pose pose; - std::size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); - - moveit_msgs::CollisionObject object; - object.id = table_name; - object.header.frame_id = table_reference_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object.primitives[0].dimensions = table_dimensions; - pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world - object.primitive_poses.push_back(pose); - return object; -} - -moveit_msgs::CollisionObject createObject() { - ros::NodeHandle pnh("~"); - std::string object_name, object_reference_frame; - std::vector object_dimensions; - geometry_msgs::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); - - moveit_msgs::CollisionObject object; - object.id = object_name; - object.header.frame_id = object_reference_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = object_dimensions; - pose.position.z += 0.5 * object_dimensions[0]; - object.primitive_poses.push_back(pose); - return object; -} - -int main(int argc, char** argv) { - ROS_INFO_NAMED(LOGNAME, "Init moveit_task_constructor_demo"); - ros::init(argc, argv, "moveit_task_constructor_demo"); - ros::NodeHandle nh; - ros::AsyncSpinner spinner(1); - spinner.start(); - - // Add table and object to planning scene - ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service - moveit::planning_interface::PlanningSceneInterface psi; - ros::NodeHandle pnh("~"); - if (pnh.param("spawn_table", true)) - spawnObject(psi, createTable()); - spawnObject(psi, createObject()); - - // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); - pick_place_task.loadParameters(); - pick_place_task.init(); - if (pick_place_task.plan()) { - ROS_INFO_NAMED(LOGNAME, "Planning succeded"); - if (pnh.param("execute", false)) { - pick_place_task.execute(); - ROS_INFO_NAMED(LOGNAME, "Execution complete"); - } else { - ROS_INFO_NAMED(LOGNAME, "Execution disabled"); - } - } else { - ROS_INFO_NAMED(LOGNAME, "Planning failed"); - } - - // Keep introspection alive - ros::waitForShutdown(); - return 0; -} diff --git a/demo/src/gpd_pick_place_task.cpp b/demo/src/gpd_pick_place_task.cpp deleted file mode 100644 index 2a6907c29..000000000 --- a/demo/src/gpd_pick_place_task.cpp +++ /dev/null @@ -1,452 +0,0 @@ -/********************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019 PickNik LLC. - * 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 copyright holder 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 HOLDER 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, Simon Goldstein, Boston Cleek - Desc: A demo to show MoveIt Task Constructor using GPD in action - */ - -#include -#include - -namespace moveit_task_constructor_demo { -constexpr char LOGNAME[] = "pick_place_task"; -PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} - -void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); - ros::NodeHandle pnh("~"); - - // Planning group properties - size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); - - // Predefined pose targets - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); - - // Target object - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; - - // Pick/Place metrics - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); -} - -void PickPlaceTask::init() { - ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); - const std::string object = object_name_; - - // Reset ROS introspection before constructing the new object - // TODO(henningkayser): verify this is a bug, fix if possible - task_.reset(); - task_.reset(new moveit::task_constructor::Task()); - - Task& t = *task_; - t.stages()->setName(task_name_); - t.loadRobotModel(); - - // Sampling planner - auto sampling_planner = std::make_shared(); - sampling_planner->setProperty("goal_joint_tolerance", 1e-5); - - // Cartesian planner - auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); - cartesian_planner->setStepSize(.01); - - // Set task properties - t.setProperty("group", arm_group_name_); - t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_group_name_); - t.setProperty("hand_grasping_frame", hand_frame_); - t.setProperty("ik_frame", hand_frame_); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator - { - auto current_state = std::make_unique("current state"); - - // Verify that object is not attached - auto applicability_filter = - std::make_unique("applicability test", std::move(current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { - if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { - comment = "object with id '" + object + "' is already attached and cannot be picked"; - return false; - } - return true; - }); - - current_state_ptr = applicability_filter.get(); - t.add(std::move(applicability_filter)); - } - - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ - { // Open Hand - auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ - { // Move-to pre-grasp - auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator - { - auto grasp = std::make_unique("pick object"); - t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); - grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - - /**************************************************** - ---- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique("approach object", cartesian_planner); - stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); - - // Set hand forward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** - ---- * Generate Grasp Pose * - ***************************************************/ - { - // Sample grasp pose - auto stage = std::make_unique("generate grasp pose"); - stage->properties().configureInitFrom(Stage::PARENT); - stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(hand_open_pose_); - stage->setObject(object); - stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state_ptr); // Hook into current state - - // Compute IK - auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); - wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - grasp->insert(std::move(wrapper)); - } - - /**************************************************** - ---- * Allow Collision (hand object) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (hand,object)"); - stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), - true); - grasp->insert(std::move(stage)); - } - - /**************************************************** - ---- * Close Hand * - ***************************************************/ - { - auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_close_pose_); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Attach Object * - ***************************************************/ - { - auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Allow collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Lift object * - ***************************************************/ - { - auto stage = std::make_unique("lift object", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "lift_object"); - - // Set upward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - grasp->insert(std::move(stage)); - } - - // Add grasp container to task - t.add(std::move(grasp)); - } - - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ - { - auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ - { - auto place = std::make_unique("place object"); - t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); - place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); - - /****************************************************** - ---- * Lower Object * - *****************************************************/ - { - auto stage = std::make_unique("lower object", cartesian_planner); - stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.03, .13); - - // Set downward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Generate Place Pose * - *****************************************************/ - { - // Generate Place Pose - auto stage = std::make_unique("generate place pose"); - stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); - stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); - - // Set target pose - geometry_msgs::PoseStamped p; - p.header.frame_id = object_reference_frame_; - p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; - stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage - - // Compute IK - auto wrapper = std::make_unique("place pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - place->insert(std::move(wrapper)); - } - - /****************************************************** - ---- * Open Hand * - *****************************************************/ - { - auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_open_pose_); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Forbid collision (hand, object) * - *****************************************************/ - { - auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions( - object_name_, - t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Detach Object * - *****************************************************/ - { - auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Retreat Motion * - *****************************************************/ - { - auto stage = std::make_unique("retreat after place", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - // Add place container to task - t.add(std::move(place)); - } - - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ - { - auto stage = std::make_unique("move home", sampling_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(arm_home_pose_); - stage->restrictDirection(stages::MoveTo::FORWARD); - t.add(std::move(stage)); - } -} - -bool PickPlaceTask::plan() { - ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); - ros::NodeHandle pnh("~"); - int max_solutions = pnh.param("max_solutions", 10); - - try { - task_->plan(max_solutions); - } catch (InitStageException& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); - return false; - } - if (task_->numSolutions() == 0) { - ROS_ERROR_NAMED(LOGNAME, "Planning failed"); - return false; - } - return true; -} - -bool PickPlaceTask::execute() { - ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - task_->solutions().front()->fillMessage(execute_goal.solution); - execute_.sendGoal(execute_goal); - execute_.waitForResult(); - moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; - - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); - return false; - } - - return true; -} -} // namespace moveit_task_constructor_demo diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index b72f07873..28a12c175 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -1,9 +1,12 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_msgs) -set(MSG_DEPS moveit_msgs visualization_msgs) +set(MSG_DEPS moveit_msgs std_msgs visualization_msgs) find_package(catkin REQUIRED COMPONENTS + actionlib_msgs + genmsg + geometry_msgs message_generation ${MSG_DEPS} ) @@ -27,12 +30,19 @@ add_service_files(DIRECTORY srv FILES add_action_files(DIRECTORY action FILES ExecuteTaskSolution.action + SampleGraspPoses.action ) -generate_messages(DEPENDENCIES ${MSG_DEPS}) +generate_messages(DEPENDENCIES + actionlib_msgs + geometry_msgs + ${MSG_DEPS} +) catkin_package( CATKIN_DEPENDS + actionlib_msgs + geometry_msgs message_runtime ${MSG_DEPS} ) diff --git a/msgs/action/SampleGraspPoses.action b/msgs/action/SampleGraspPoses.action new file mode 100644 index 000000000..218664cf4 --- /dev/null +++ b/msgs/action/SampleGraspPoses.action @@ -0,0 +1,11 @@ +# goal sent to client +string action_name +--- +# result sent to server +string grasp_state +--- +# feedback sent to server +# grasp poses +geometry_msgs/PoseStamped[] grasp_candidates +# cost of each grasp +float64[] costs diff --git a/msgs/package.xml b/msgs/package.xml index f0bf4ec57..3e00fbdd5 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -9,8 +9,13 @@ catkin + genmsg + + actionlib_msgs + geometry_msgs message_generation moveit_msgs + std_msgs visualization_msgs message_runtime