diff --git a/core/include/moveit/task_constructor/stages/action_base.h b/core/include/moveit/task_constructor/stages/action_base.h index 67c5f9429..ac11305b0 100644 --- a/core/include/moveit/task_constructor/stages/action_base.h +++ b/core/include/moveit/task_constructor/stages/action_base.h @@ -41,7 +41,6 @@ #include - namespace moveit { namespace task_constructor { namespace stages { @@ -52,80 +51,72 @@ namespace stages { * @details Some stages may require an action client. This class wraps the * simple client action interface and exposes event based execution callbacks. */ -template +template class ActionBase { protected: - ACTION_DEFINITION(ActionSpec); + 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; + /** + * @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 + 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, +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)); + : action_name_(action_name), server_timeout_(server_timeout), goal_timeout_(goal_timeout) { + clientPtr_.reset(new actionlib::SimpleActionClient(nh_, action_name_, spin_thread)); - if (server_timeout_ < 0.0 || goal_timeout < 0.0){ - ROS_WARN("Timeouts cannot be negative"); - } + if (server_timeout_ < 0.0 || goal_timeout < 0.0) { + ROS_WARN("Timeouts cannot be negative"); + } } - -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)); +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 diff --git a/core/include/moveit/task_constructor/stages/deep_grasp_pose.h b/core/include/moveit/task_constructor/stages/deep_grasp_pose.h index 7b3fb52bd..f8a8d0853 100644 --- a/core/include/moveit/task_constructor/stages/deep_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/deep_grasp_pose.h @@ -30,9 +30,9 @@ * 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 - */ +/* Author: Boston Cleek + Desc: Grasp generator stage using deep learning based grasp synthesizers +*/ #pragma once @@ -47,23 +47,20 @@ #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 +template class DeepGraspPose : public GeneratePose, ActionBase { - private: typedef ActionBase ActionBaseT; ACTION_DEFINITION(ActionSpec); @@ -77,10 +74,8 @@ 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", - double goal_timeout = 0.0, - double server_timeout = 0.0); + 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 @@ -96,8 +91,8 @@ class DeepGraspPose : 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 FeedbackConstPtr& feedback) override; + void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) override; void init(const core::RobotModelConstPtr& robot_model) override; void compute() override; @@ -119,83 +114,70 @@ class DeepGraspPose : public GeneratePose, ActionBase 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("angle_delta", 0.1, "angular steps (rad)"); - - 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 +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("angle_delta", 0.1, "angular steps (rad)"); + + 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(){ +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)); + 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(){ - +template +bool DeepGraspPose::monitorGoal() { // monitor timeout bool monitor_timeout = ActionBaseT::goal_timeout_ > std::numeric_limits::epsilon() ? true : false; double timeout_time = ros::Time::now().toSec() + ActionBaseT::goal_timeout_; - while(ActionBaseT::nh_.ok()) - { + while (ActionBaseT::nh_.ok()) { ros::spinOnce(); - if (found_candidates_){ + if (found_candidates_) { break; } - if (ros::Time::now().toSec() > timeout_time && monitor_timeout){ - ActionBaseT::clientPtr_->cancelGoal(); - ROS_ERROR_NAMED(LOGNAME, "Grasp pose generator time out reached"); - return false; + if (ros::Time::now().toSec() > timeout_time && monitor_timeout) { + ActionBaseT::clientPtr_->cancelGoal(); + ROS_ERROR_NAMED(LOGNAME, "Grasp pose generator time out reached"); + return false; } } return true; } - -template -void DeepGraspPose::activeCallback(){ +template +void DeepGraspPose::activeCallback() { ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active"); found_candidates_ = false; } - -template -void DeepGraspPose::feedbackCallback(const FeedbackConstPtr &feedback){ +template +void DeepGraspPose::feedbackCallback(const FeedbackConstPtr& feedback) { // each candidate should have a cost - if (feedback->grasp_candidates.size() != feedback->costs.size()){ + if (feedback->grasp_candidates.size() != feedback->costs.size()) { ROS_ERROR_NAMED(LOGNAME, "Invalid input: each grasp candidate needs an associated cost"); - } - else{ + } else { ROS_INFO_NAMED(LOGNAME, "Grasp generated feedback received %lu candidates: ", feedback->grasp_candidates.size()); found_candidates_ = true; @@ -208,21 +190,19 @@ void DeepGraspPose::feedbackCallback(const FeedbackConstPtr &feedbac } } - -template +template void DeepGraspPose::doneCallback(const actionlib::SimpleClientGoalState& state, - const ResultConstPtr &result){ - if (state == actionlib::SimpleClientGoalState::SUCCEEDED){ + 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()); + } 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){ +template +void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model) { InitStageException errors; try { GeneratePose::init(robot_model); @@ -233,7 +213,7 @@ void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model const auto& props = properties(); // check angle_delta - if (props.get("angle_delta") == 0.){ + if (props.get("angle_delta") == 0.) { errors.push_back(*this, "angle_delta must be non-zero"); } @@ -241,30 +221,28 @@ void DeepGraspPose::init(const core::RobotModelConstPtr& robot_model props.get("object"); // check availability of eef const std::string& eef = props.get("eef"); - if (!robot_model->hasEndEffector(eef)){ + if (!robot_model->hasEndEffector(eef)) { errors.push_back(*this, "unknown end effector: " + eef); - } - else { + } 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)){ + if (!jmg->getVariableDefaultPositions(name, m)) { errors.push_back(*this, "unknown end effector pose: " + name); } } - if (errors){ + if (errors) { throw errors; } } - -template -void DeepGraspPose::compute(){ - if (upstream_solutions_.empty()){ - return; - } +template +void DeepGraspPose::compute() { + if (upstream_solutions_.empty()) { + return; + } planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); // set end effector pose @@ -280,29 +258,27 @@ void DeepGraspPose::compute(){ // monitor feedback/results // blocking function untill timeout reached or results received - if (monitorGoal()){ + 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" }); + 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)); + 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"); + // 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)); - } + spawn(std::move(state), std::move(trajectory)); + } } } - -template -void DeepGraspPose::onNewSolution(const SolutionBase& s){ +template +void DeepGraspPose::onNewSolution(const SolutionBase& s) { planning_scene::PlanningSceneConstPtr scene = s.end()->scene(); const auto& props = properties(); @@ -315,7 +291,7 @@ void DeepGraspPose::onNewSolution(const SolutionBase& s){ solution.markAsFailure(); solution.setComment(msg); spawn(std::move(state), std::move(solution)); - } else{ + } else { ROS_WARN_STREAM_NAMED(LOGNAME, msg); } return;