From 7717231775af5471d63e9acb02392f8642a5bc67 Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Wed, 22 Jul 2020 11:32:32 -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/package.xml | 1 + msgs/CMakeLists.txt | 14 +- msgs/action/SampleGraspPoses.action | 11 + msgs/package.xml | 5 + 9 files changed, 459 insertions(+), 2 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 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/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/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