diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 31dd1b89b..feaff9088 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib eigen_conversions 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 754c471fb..671136c90 100644 --- a/core/package.xml +++ b/core/package.xml @@ -14,6 +14,7 @@ actionlib eigen_conversions + 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