new deep grasp generator stages uses action client

Author: bostoncleek

core/include/moveit/task_constructor/stages/action_base.h

@@ -62,22 +62,23 @@ class ActionBase
   /**
   * @brief Constructor
   * @param action_name - action namespace
-  * @param server_timeout - connection to server time out (0 is considered infinite timeout)
-  * @param goal_timeout - goal to completed time out (0 is considered infinite timeout)
   * @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,
-             double server_timeout = 0.0,
-             double goal_timeout = 0.0,
-             bool spin_thread = true)
-             : action_name_(action_name), server_timeout_(server_timeout), goal_timeout_(goal_timeout)  {
+             bool spin_thread,
+             double goal_timeout,
+             double server_timeout);
 
-  clientPtr_.reset(new actionlib::SimpleActionClient<ActionSpec>(action_name, spin_thread));
-  }
-
-  // ActionBase(const std::string &action_name) : action_name_(action_name) {};
-  // ActionBase() {}
+  /**
+  * @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;
@@ -100,13 +101,33 @@ class ActionBase
                             const ResultConstPtr &result) = 0;
 
 protected:
-  std::string action_name_;
+  ros::NodeHandle nh_;
+  std::string action_name_;                                                // action name space
   std::unique_ptr<actionlib::SimpleActionClient<ActionSpec>> clientPtr_;   // action client
-  ActionGoal goal_;                                                        // goal message
   double server_timeout_, goal_timeout_;                                   // connection and goal completed time out
 };
 
 
+template<class ActionSpec>
+ActionBase<ActionSpec>::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<ActionSpec>(nh_, action_name_, spin_thread));
+
+  if (server_timeout_ < 0.0 || goal_timeout < 0.0){
+    ROS_WARN("Timeouts cannot be negative");
+    }
+}
+
+
+template<class ActionSpec>
+ActionBase<ActionSpec>::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<ActionSpec>(nh_, action_name_, spin_thread));
+}
+
 }  // namespace stages
 }  // namespace task_constructor
 }  // namespace moveit

core/include/moveit/task_constructor/stages/gpd_generator.h

@@ -39,38 +39,63 @@
 #include <moveit/task_constructor/stages/generate_pose.h>
 #include <moveit/task_constructor/stages/action_base.h>
 #include <moveit/task_constructor/storage.h>
+#include <moveit/task_constructor/marker_tools.h>
+#include <rviz_marker_tools/marker_creation.h>
 #include <moveit/planning_scene/planning_scene.h>
 
-#include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
-
 #include <memory>
-
-#include <moveit_task_constructor_msgs/GenerateDeepGraspPoseAction.h>
+#include <functional>
 
 
 namespace moveit {
 namespace task_constructor {
 namespace stages {
 
-
+/**
+* @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 ActionSpec>
 class DeepGraspPose : public GeneratePose, ActionBase<ActionSpec>
 {
+
+const std::string LOGNAME = "deep_grasp_generator";
+
 private:
 	typedef ActionBase<ActionSpec> 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");
+		            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 (optinal) timeout
+	* @details This is a blocking call. It will wait until either grasp candidates
+	*          are received or the (optinal) 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 doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr &result) override;
 
 	void init(const core::RobotModelConstPtr& robot_model) override;
 	void compute() override;
@@ -92,10 +117,16 @@ class DeepGraspPose : public GeneratePose, ActionBase<ActionSpec>
 	std::vector<double> costs_;
 };
 
+
 template<class ActionSpec>
 DeepGraspPose<ActionSpec>::DeepGraspPose(const std::string& action_name,
-	                                       const std::string& stage_name)
-									:  GeneratePose(stage_name), ActionBaseT(action_name), found_candidates_(false) {
+	                                       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<std::string>("eef", "name of end-effector");
  p.declare<std::string>("object");
@@ -104,15 +135,58 @@ DeepGraspPose<ActionSpec>::DeepGraspPose(const std::string& action_name,
  p.declare<boost::any>("pregrasp", "pregrasp posture");
  p.declare<boost::any>("grasp", "grasp posture");
 
- ROS_INFO("Waiting for connection to grasp generation action server...");
+ 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<class ActionSpec>
+void DeepGraspPose<ActionSpec>::composeGoal(){
+	Goal goal;
+	goal.action_name = ActionBaseT::action_name_;
+	ActionBaseT::clientPtr_->sendGoal(goal,
+	                                  std::bind(&DeepGraspPose<ActionSpec>::doneCallback, this, std::placeholders::_1, std::placeholders::_2),
+																	  std::bind(&DeepGraspPose<ActionSpec>::activeCallback, this),
+																	  std::bind(&DeepGraspPose<ActionSpec>::feedbackCallback, this, std::placeholders::_1));
+
+	ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str());
+}
+
+
+template<class ActionSpec>
+bool DeepGraspPose<ActionSpec>::monitorGoal(){
+
+	// monitor timeout
+	bool monitor_timeout = ActionBaseT::goal_timeout_ > std::numeric_limits<double>::epsilon() ? true : false;
+
+	double curr_time = ros::Time::now().toSec();
+	double timeout_time = curr_time + ActionBaseT::goal_timeout_;
+
+	while(ActionBaseT::nh_.ok())
+	{
+		ros::spinOnce();
+
+		if (found_candidates_){
+			break;
+		}
+
+		curr_time = ros::Time::now().toSec();
+		if (curr_time > timeout_time && monitor_timeout){
+				ActionBaseT::clientPtr_->cancelGoal();
+				ROS_ERROR_NAMED(LOGNAME, "Grasp pose generator time out reached");
+				return false;
+		}
+	}
+
+	return true;
 }
 
 
 template<class ActionSpec>
 void DeepGraspPose<ActionSpec>::activeCallback(){
-	ROS_INFO("Generate grasp goal now active ");
-	// found_candidates_ = false;
+	ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active");
+	found_candidates_ = false;
 }
 
 
@@ -122,9 +196,11 @@ void DeepGraspPose<ActionSpec>::feedbackCallback(const FeedbackConstPtr &feedbac
 
 	// each candidate should have a cost
 	if (feedback->grasp_candidates.size() != feedback->costs.size()){
-		ROS_ERROR("Invalid input: each grasp candidate needs an associated cost");
+		ROS_ERROR_NAMED(LOGNAME, "Invalid input: each grasp candidate needs an associated cost");
 	}
 	else{
+		ROS_INFO_NAMED(LOGNAME, "Grasp generated feedback received");
+
 		grasp_candidates_.resize(feedback->grasp_candidates.size());
 		costs_.resize(feedback->costs.size());
 
@@ -137,14 +213,12 @@ void DeepGraspPose<ActionSpec>::feedbackCallback(const FeedbackConstPtr &feedbac
 template<class ActionSpec>
 void DeepGraspPose<ActionSpec>::doneCallback(const actionlib::SimpleClientGoalState& state,
 													const ResultConstPtr &result){
-	// TODO: add completion timeout
-
 	if (state == actionlib::SimpleClientGoalState::SUCCEEDED){
 		found_candidates_ = true;
-		ROS_INFO("Successfully found grasp candidates (result): %s", result->grasp_state.c_str());
+		ROS_INFO_NAMED(LOGNAME, "Found grasp candidates (result): %s", result->grasp_state.c_str());
 	}
 	else{
-		ROS_WARN("No grasp candidates found (state): %s", ActionBaseT::clientPtr_->getState().toString().c_str());
+		ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found (state): %s", ActionBaseT::clientPtr_->getState().toString().c_str());
 	}
 }
 
@@ -190,8 +264,43 @@ void DeepGraspPose<ActionSpec>::init(const core::RobotModelConstPtr& robot_model
 
 template<class ActionSpec>
 void DeepGraspPose<ActionSpec>::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<std::string>("eef");
+	const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
+
+	robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
+	robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
+
+
+	// compose/send goal
+	composeGoal();
+
+	// monitor feedback/results
+	// blocking function untill timeout reached or results received
+	if (monitorGoal()){
+		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));
+	  }
+	}
 }
 
 
@@ -210,7 +319,7 @@ void DeepGraspPose<ActionSpec>::onNewSolution(const SolutionBase& s){
 			solution.setComment(msg);
 			spawn(std::move(state), std::move(solution));
 		} else{
-			ROS_WARN_STREAM_NAMED("GenerateGraspPose", msg);
+			ROS_WARN_STREAM_NAMED(LOGNAME, msg);
 		}
 		return;
 	}