From 2e305354fa547a6a72fb92639b9fa927f73a6d3f Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Wed, 22 Jul 2020 11:32:32 -0500 Subject: [PATCH 1/5] New grasp pose generator stage 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 | 1 + .../task_constructor/stages/action_base.h | 125 ++++++++ .../task_constructor/stages/deep_grasp_pose.h | 299 ++++++++++++++++++ core/package.xml | 1 + core/src/stages/CMakeLists.txt | 2 + msgs/CMakeLists.txt | 14 +- msgs/action/SampleGraspPoses.action | 11 + msgs/package.xml | 5 + 8 files changed, 456 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 5a0c704f0..9d83f6cfe 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -4,6 +4,7 @@ project(moveit_task_constructor_core) find_package(Boost REQUIRED) find_package(catkin REQUIRED COMPONENTS tf2_eigen + actionlib geometry_msgs moveit_core moveit_ros_planning 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..147afd377 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/action_base.h @@ -0,0 +1,125 @@ +/********************************************************************* + * 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 8fbadef25..f1d5b5015 100644 --- a/core/package.xml +++ b/core/package.xml @@ -13,6 +13,7 @@ roscpp tf2_eigen + actionlib geometry_msgs moveit_core moveit_ros_planning diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 2becaa638..de174d38c 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/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 From 8220806a82120c462d80102aaad017740796c0c7 Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Wed, 28 Apr 2021 16:03:28 -0600 Subject: [PATCH 2/5] Rename stage to Grasp Provider --- .../{deep_grasp_pose.h => grasp_provider.h} | 37 +++++++++---------- core/src/stages/CMakeLists.txt | 2 +- 2 files changed, 18 insertions(+), 21 deletions(-) rename core/include/moveit/task_constructor/stages/{deep_grasp_pose.h => grasp_provider.h} (90%) diff --git a/core/include/moveit/task_constructor/stages/deep_grasp_pose.h b/core/include/moveit/task_constructor/stages/grasp_provider.h similarity index 90% rename from core/include/moveit/task_constructor/stages/deep_grasp_pose.h rename to core/include/moveit/task_constructor/stages/grasp_provider.h index 9b69f94b7..b48f4be39 100644 --- a/core/include/moveit/task_constructor/stages/deep_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -31,11 +31,12 @@ *********************************************************************/ /* Author: Boston Cleek - Desc: Grasp generator stage using deep learning based grasp synthesizers + Desc: Grasp generator stage */ #pragma once +#include #include #include #include @@ -43,15 +44,11 @@ #include #include -// #include -#include -// #include - namespace moveit { namespace task_constructor { namespace stages { -constexpr char LOGNAME[] = "deep_grasp_generator"; +constexpr char LOGNAME[] = "grasp_provider"; /** * @brief Generate grasp candidates using deep learning approaches @@ -59,7 +56,7 @@ constexpr char LOGNAME[] = "deep_grasp_generator"; * @details Interfaces with a deep learning based grasp library using a action client */ template -class DeepGraspPose : public GeneratePose, ActionBase +class GraspProvider : public GeneratePose, ActionBase { private: typedef ActionBase ActionBaseT; @@ -74,7 +71,7 @@ class DeepGraspPose : public GeneratePose, ActionBase * @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", + GraspProvider(const std::string& action_name, const std::string& stage_name = "generate grasp pose", double goal_timeout = 0.0, double server_timeout = 0.0); /** @@ -115,7 +112,7 @@ class DeepGraspPose : public GeneratePose, ActionBase }; template -DeepGraspPose::DeepGraspPose(const std::string& action_name, const std::string& stage_name, +GraspProvider::GraspProvider(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(); @@ -130,19 +127,19 @@ DeepGraspPose::DeepGraspPose(const std::string& action_name, const s } template -void DeepGraspPose::composeGoal() { +void GraspProvider::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)); + goal, std::bind(&GraspProvider::doneCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GraspProvider::activeCallback, this), + std::bind(&GraspProvider::feedbackCallback, this, std::placeholders::_1)); ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str()); } template -bool DeepGraspPose::monitorGoal() { +bool GraspProvider::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_; @@ -166,13 +163,13 @@ bool DeepGraspPose::monitorGoal() { } template -void DeepGraspPose::activeCallback() { +void GraspProvider::activeCallback() { ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active"); found_candidates_ = false; } template -void DeepGraspPose::feedbackCallback(const FeedbackConstPtr& feedback) { +void GraspProvider::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"); @@ -190,7 +187,7 @@ void DeepGraspPose::feedbackCallback(const FeedbackConstPtr& feedbac } template -void DeepGraspPose::doneCallback(const actionlib::SimpleClientGoalState& state, +void GraspProvider::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()); @@ -201,7 +198,7 @@ void DeepGraspPose::doneCallback(const actionlib::SimpleClientGoalSt } template -void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model) { +void GraspProvider::init(const core::RobotModelConstPtr& robot_model) { InitStageException errors; try { GeneratePose::init(robot_model); @@ -233,7 +230,7 @@ void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model } template -void DeepGraspPose::compute() { +void GraspProvider::compute() { if (upstream_solutions_.empty()) { return; } @@ -272,7 +269,7 @@ void DeepGraspPose::compute() { } template -void DeepGraspPose::onNewSolution(const SolutionBase& s) { +void GraspProvider::onNewSolution(const SolutionBase& s) { planning_scene::PlanningSceneConstPtr scene = s.end()->scene(); const auto& props = properties(); diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index de174d38c..281cd3c8b 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,7 +4,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/action_base.h ${PROJECT_INCLUDE}/stages/current_state.h - ${PROJECT_INCLUDE}/stages/deep_grasp_pose.h + ${PROJECT_INCLUDE}/stages/grasp_provider.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h ${PROJECT_INCLUDE}/stages/generate_pose.h From e671e06af386ff6dc8301e4c4083a4ad1f646173 Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Wed, 28 Apr 2021 16:35:55 -0600 Subject: [PATCH 3/5] Remove template paramete to action base class The action base class can only be used with the action msg GraspPlanning.action from grasping_msgs. --- core/CMakeLists.txt | 3 + .../task_constructor/stages/action_base.h | 37 +--- .../task_constructor/stages/grasp_provider.h | 209 +----------------- core/package.xml | 1 + core/src/stages/CMakeLists.txt | 5 +- core/src/stages/action_base.cpp | 70 ++++++ core/src/stages/grasp_provider.cpp | 203 +++++++++++++++++ msgs/CMakeLists.txt | 18 +- msgs/action/SampleGraspPoses.action | 11 - msgs/package.xml | 5 - 10 files changed, 301 insertions(+), 261 deletions(-) create mode 100644 core/src/stages/action_base.cpp create mode 100644 core/src/stages/grasp_provider.cpp delete mode 100644 msgs/action/SampleGraspPoses.action diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 9d83f6cfe..1c4a04afa 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_eigen actionlib geometry_msgs + grasping_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface @@ -23,7 +24,9 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS + actionlib geometry_msgs + grasping_msgs moveit_core moveit_task_constructor_msgs rviz_marker_tools diff --git a/core/include/moveit/task_constructor/stages/action_base.h b/core/include/moveit/task_constructor/stages/action_base.h index 147afd377..bb3c51533 100644 --- a/core/include/moveit/task_constructor/stages/action_base.h +++ b/core/include/moveit/task_constructor/stages/action_base.h @@ -41,23 +41,15 @@ #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 +/** @brief Interface allowing stages to use a simple action client */ class ActionBase { -protected: - ACTION_DEFINITION(ActionSpec); - public: /** * @brief Constructor @@ -87,39 +79,22 @@ class ActionBase * @brief Called every time feedback is received for the goal * @param feedback - pointer to the feedback message */ - virtual void feedbackCallback(const FeedbackConstPtr& feedback) = 0; + virtual void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& 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; + virtual void doneCallback(const actionlib::SimpleClientGoalState& state, + const grasping_msgs::GraspPlanningResultConstPtr& result) = 0; protected: ros::NodeHandle nh_; std::string action_name_; // action name space - std::unique_ptr> clientPtr_; // action client + 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/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index b48f4be39..3af040b72 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -37,6 +37,8 @@ #pragma once #include +#include + #include #include #include @@ -48,20 +50,9 @@ namespace moveit { namespace task_constructor { namespace stages { -constexpr char LOGNAME[] = "grasp_provider"; - -/** - * @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 GraspProvider : public GeneratePose, ActionBase +/** @brief Generate grasp candidates using deep learning approaches */ +class GraspProvider : public GeneratePose, ActionBase { -private: - typedef ActionBase ActionBaseT; - ACTION_DEFINITION(ActionSpec); - public: /** * @brief Constructor @@ -71,7 +62,7 @@ class GraspProvider : public GeneratePose, ActionBase * @param server_timeout - connection to server time out (0 is considered infinite timeout) * @details Initialize the client and connect to server */ - GraspProvider(const std::string& action_name, const std::string& stage_name = "generate grasp pose", + GraspProvider(const std::string& action_name, const std::string& stage_name = "grasp provider", double goal_timeout = 0.0, double server_timeout = 0.0); /** @@ -88,8 +79,9 @@ class GraspProvider : public GeneratePose, ActionBase bool monitorGoal(); void activeCallback() override; - void feedbackCallback(const FeedbackConstPtr& feedback) override; - void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) override; + void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) override; + void doneCallback(const actionlib::SimpleClientGoalState& state, + const grasping_msgs::GraspPlanningResultConstPtr& result) override; void init(const core::RobotModelConstPtr& robot_model) override; void compute() override; @@ -106,191 +98,10 @@ class GraspProvider : public GeneratePose, ActionBase void onNewSolution(const SolutionBase& s) override; private: + std::mutex grasp_mutex_; bool found_candidates_; - std::vector grasp_candidates_; - std::vector costs_; + std::vector grasp_candidates_; }; - -template -GraspProvider::GraspProvider(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 GraspProvider::composeGoal() { - Goal goal; - goal.action_name = ActionBaseT::action_name_; - ActionBaseT::clientPtr_->sendGoal( - goal, std::bind(&GraspProvider::doneCallback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GraspProvider::activeCallback, this), - std::bind(&GraspProvider::feedbackCallback, this, std::placeholders::_1)); - - ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str()); -} - -template -bool GraspProvider::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 GraspProvider::activeCallback() { - ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active"); - found_candidates_ = false; -} - -template -void GraspProvider::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 GraspProvider::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 GraspProvider::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 GraspProvider::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 GraspProvider::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 f1d5b5015..22122c345 100644 --- a/core/package.xml +++ b/core/package.xml @@ -14,6 +14,7 @@ tf2_eigen actionlib + grasping_msgs geometry_msgs moveit_core moveit_ros_planning diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 281cd3c8b..51e9b885d 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,12 +4,12 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/action_base.h ${PROJECT_INCLUDE}/stages/current_state.h - ${PROJECT_INCLUDE}/stages/grasp_provider.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h + ${PROJECT_INCLUDE}/stages/grasp_provider.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/passthrough.h ${PROJECT_INCLUDE}/stages/predicate_filter.h @@ -24,12 +24,14 @@ add_library(${PROJECT_NAME}_stages modify_planning_scene.cpp fix_collision_objects.cpp + action_base.cpp current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp + grasp_provider.cpp compute_ik.cpp passthrough.cpp predicate_filter.cpp @@ -41,6 +43,7 @@ add_library(${PROJECT_NAME}_stages simple_grasp.cpp pick.cpp ) + target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) add_library(${PROJECT_NAME}_stage_plugins diff --git a/core/src/stages/action_base.cpp b/core/src/stages/action_base.cpp new file mode 100644 index 000000000..319386585 --- /dev/null +++ b/core/src/stages/action_base.cpp @@ -0,0 +1,70 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021 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. +*/ + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +constexpr char LOGNAME[] = "action_base"; + +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_; + + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for connection to grasp generation action server..."); + clientPtr_->waitForServer(ros::Duration(server_timeout_)); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Connected to server"); +} + +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)); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for connection to grasp generation action server..."); + clientPtr_->waitForServer(ros::Duration(server_timeout_)); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Connected to server"); +} +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp new file mode 100644 index 000000000..95767bfe3 --- /dev/null +++ b/core/src/stages/grasp_provider.cpp @@ -0,0 +1,203 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021 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 +*/ + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +constexpr char LOGNAME[] = "grasp_provider"; + +GraspProvider::GraspProvider(const std::string& action_name, const std::string& stage_name, double goal_timeout, + double server_timeout) + : GeneratePose(stage_name), ActionBase(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"); +} + +void GraspProvider::composeGoal() { + const auto& props = properties(); + grasping_msgs::GraspPlanningGoal goal; + goal.object.name = props.get("object"); + clientPtr_->sendGoal(goal, + std::bind(&GraspProvider::doneCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GraspProvider::activeCallback, this), + std::bind(&GraspProvider::feedbackCallback, this, std::placeholders::_1)); + + ROS_DEBUG_NAMED(LOGNAME, "Goal sent to server to grasp object: %s", goal.object.name.c_str()); +} + +bool GraspProvider::monitorGoal() { + // monitor timeout + const bool monitor_timeout = goal_timeout_ > std::numeric_limits::epsilon(); + const double timeout_time = ros::Time::now().toSec() + goal_timeout_; + + while (nh_.ok()) { + ros::spinOnce(); + + // timeout reached + if (ros::Time::now().toSec() > timeout_time && monitor_timeout) { + 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; +} + +void GraspProvider::activeCallback() { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Generate grasp goal now active"); + found_candidates_ = false; +} + +void GraspProvider::feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) { + const std::lock_guard lock(grasp_mutex_); + grasp_candidates_ = feedback->grasps; + found_candidates_ = true; +} + +void GraspProvider::doneCallback(const actionlib::SimpleClientGoalState& state, + const grasping_msgs::GraspPlanningResultConstPtr& result) { + if (state == actionlib::SimpleClientGoalState::SUCCEEDED) { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Found grasp candidates"); + } else { + ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found (state): %s", clientPtr_->getState().toString().c_str()); + } +} + +void GraspProvider::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; + } +} + +void GraspProvider::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 + const std::lock_guard lock(grasp_mutex_); + if (monitorGoal()) { + for (unsigned int i = 0; i < grasp_candidates_.size(); i++) { + InterfaceState state(scene); + state.properties().set("target_pose", grasp_candidates_.at(i).grasp_pose); + props.exposeTo(state.properties(), { "pregrasp", "grasp" }); + + SubTrajectory trajectory; + trajectory.setCost(grasp_candidates_.at(i).grasp_quality); + trajectory.setComment(std::to_string(i)); + + // add frame at target pose + rviz_marker_tools::appendFrame(trajectory.markers(), grasp_candidates_.at(i).grasp_pose, 0.1, "grasp frame"); + + spawn(std::move(state), std::move(trajectory)); + } + } +} + +void GraspProvider::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/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index 28a12c175..8ceb9cafa 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -1,12 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_msgs) -set(MSG_DEPS moveit_msgs std_msgs visualization_msgs) +set(MSG_DEPS moveit_msgs visualization_msgs) find_package(catkin REQUIRED COMPONENTS - actionlib_msgs - genmsg - geometry_msgs message_generation ${MSG_DEPS} ) @@ -30,19 +27,12 @@ add_service_files(DIRECTORY srv FILES add_action_files(DIRECTORY action FILES ExecuteTaskSolution.action - SampleGraspPoses.action ) -generate_messages(DEPENDENCIES - actionlib_msgs - geometry_msgs - ${MSG_DEPS} -) +generate_messages(DEPENDENCIES ${MSG_DEPS}) catkin_package( CATKIN_DEPENDS - actionlib_msgs - geometry_msgs - message_runtime - ${MSG_DEPS} + message_runtime + ${MSG_DEPS} ) diff --git a/msgs/action/SampleGraspPoses.action b/msgs/action/SampleGraspPoses.action deleted file mode 100644 index 218664cf4..000000000 --- a/msgs/action/SampleGraspPoses.action +++ /dev/null @@ -1,11 +0,0 @@ -# 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 3e00fbdd5..f0bf4ec57 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -9,13 +9,8 @@ catkin - genmsg - - actionlib_msgs - geometry_msgs message_generation moveit_msgs - std_msgs visualization_msgs message_runtime From 5145e0c692685bf6533338af71d7a421743e711e Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Sun, 9 Jan 2022 14:59:45 -0500 Subject: [PATCH 4/5] Handle asynchronous feedback The grasp detection flag is an atomic bool. The scope of the grasp candidate's mutex has been moved. --- .../moveit/task_constructor/stages/grasp_provider.h | 6 +++--- core/src/stages/grasp_provider.cpp | 7 +++++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 3af040b72..709a26e4d 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -98,9 +98,9 @@ class GraspProvider : public GeneratePose, ActionBase void onNewSolution(const SolutionBase& s) override; private: - std::mutex grasp_mutex_; - bool found_candidates_; - std::vector grasp_candidates_; + std::mutex grasp_mutex_; // Protects grasp candidates + std::atomic_bool found_candidates_; // Flag indicates the discovery of grasps + std::vector grasp_candidates_; // Grasp Candidates }; } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 95767bfe3..196020f54 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -94,9 +94,11 @@ void GraspProvider::activeCallback() { } void GraspProvider::feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) { + found_candidates_ = true; + + // Protect grasp candidate incase feedback is sent asynchronously const std::lock_guard lock(grasp_mutex_); grasp_candidates_ = feedback->grasps; - found_candidates_ = true; } void GraspProvider::doneCallback(const actionlib::SimpleClientGoalState& state, @@ -158,8 +160,9 @@ void GraspProvider::compute() { // monitor feedback/results // blocking function untill timeout reached or results received - const std::lock_guard lock(grasp_mutex_); if (monitorGoal()) { + // Protect grasp candidate incase feedback is being recieved asynchronously + const std::lock_guard lock(grasp_mutex_); for (unsigned int i = 0; i < grasp_candidates_.size(); i++) { InterfaceState state(scene); state.properties().set("target_pose", grasp_candidates_.at(i).grasp_pose); From 7c4753225009b20f08fe757d1cd3de99151c3377 Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Thu, 2 Jun 2022 16:49:53 -0400 Subject: [PATCH 5/5] Fix Jafar's change requests --- core/src/stages/action_base.cpp | 13 +++---------- core/src/stages/grasp_provider.cpp | 2 +- msgs/CMakeLists.txt | 4 ++-- 3 files changed, 6 insertions(+), 13 deletions(-) diff --git a/core/src/stages/action_base.cpp b/core/src/stages/action_base.cpp index 319386585..a14e52e77 100644 --- a/core/src/stages/action_base.cpp +++ b/core/src/stages/action_base.cpp @@ -44,8 +44,8 @@ constexpr char LOGNAME[] = "action_base"; 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)); + clientPtr_ = std::make_unique>(nh_, action_name_, + spin_thread); // Negative timeouts are set to zero server_timeout_ = server_timeout_ < std::numeric_limits::epsilon() ? 0.0 : server_timeout_; @@ -57,14 +57,7 @@ ActionBase::ActionBase(const std::string& action_name, bool spin_thread, double } 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)); - - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for connection to grasp generation action server..."); - clientPtr_->waitForServer(ros::Duration(server_timeout_)); - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Connected to server"); -} + : ActionBase::ActionBase(action_name, spin_thread, 0.0, 0.0) {} } // namespace stages } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 196020f54..a791631aa 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -159,7 +159,7 @@ void GraspProvider::compute() { composeGoal(); // monitor feedback/results - // blocking function untill timeout reached or results received + // blocking function until timeout reached or results received if (monitorGoal()) { // Protect grasp candidate incase feedback is being recieved asynchronously const std::lock_guard lock(grasp_mutex_); diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index 8ceb9cafa..b72f07873 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -33,6 +33,6 @@ generate_messages(DEPENDENCIES ${MSG_DEPS}) catkin_package( CATKIN_DEPENDS - message_runtime - ${MSG_DEPS} + message_runtime + ${MSG_DEPS} )