developing impl using action client

Author: bostoncleek

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

@@ -37,24 +37,40 @@
 #pragma once
 
 #include <moveit/task_constructor/stages/generate_pose.h>
+#include <moveit/task_constructor/stages/action_base.h>
+#include <moveit/task_constructor/storage.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 <actionlib/client/simple_action_client.h>
-
-#include <moveit/task_constructor/action_base.h>
 
 
 namespace moveit {
 namespace task_constructor {
 namespace stages {
 
 
-class DeepGraspPose : public GeneratePose
+template<class ActionSpec>
+class DeepGraspPose : public GeneratePose, ActionBase<ActionSpec>
 {
+private:
+	typedef ActionBase<ActionSpec> ActionBaseT;
+	ACTION_DEFINITION(ActionSpec);
+
 public:
-	DeepGraspPose(const std::string& name = "generate grasp pose");
+	DeepGraspPose(const std::string& action_name,
+		            const std::string& stage_name = "generate grasp pose");
+
+	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;
@@ -70,8 +86,138 @@ class DeepGraspPose : public GeneratePose
 protected:
 	void onNewSolution(const SolutionBase& s) override;
 
-  actionlib::SimpleActionClient<moveit_task_constructor_msgs::GenerateDeepGraspPoseAction> client_;
+private:
+	bool found_candidates_;
+	std::vector<geometry_msgs::PoseStamped> grasp_candidates_;
+	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) {
+ auto& p = properties();
+ p.declare<std::string>("eef", "name of end-effector");
+ p.declare<std::string>("object");
+ p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
+
+ p.declare<boost::any>("pregrasp", "pregrasp posture");
+ p.declare<boost::any>("grasp", "grasp posture");
+
+ ROS_INFO("Waiting for connection to grasp generation action server...");
+ ActionBaseT::clientPtr_->waitForServer(ros::Duration(ActionBaseT::server_timeout_));
+}
+
+
+template<class ActionSpec>
+void DeepGraspPose<ActionSpec>::activeCallback(){
+	ROS_INFO("Generate grasp goal now active ");
+	// found_candidates_ = false;
+}
+
+
+template<class ActionSpec>
+void DeepGraspPose<ActionSpec>::feedbackCallback(const FeedbackConstPtr &feedback){
+	// TODO: Check if costs are positive ?
+
+	// 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");
+	}
+	else{
+		grasp_candidates_.resize(feedback->grasp_candidates.size());
+		costs_.resize(feedback->costs.size());
+
+		grasp_candidates_ = feedback->grasp_candidates;
+		costs_ = feedback->costs;
+	}
+}
+
+
+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());
+	}
+	else{
+		ROS_WARN("No grasp candidates found (state): %s", ActionBaseT::clientPtr_->getState().toString().c_str());
+	}
+}
+
+
+template<class ActionSpec>
+void DeepGraspPose<ActionSpec>::init(const core::RobotModelConstPtr& robot_model){
+	InitStageException errors;
+	try {
+		GeneratePose::init(robot_model);
+	} catch (InitStageException& e) {
+		errors.append(e);
+	}
+
+	const auto& props = properties();
+
+	// check angle_delta
+	if (props.get<double>("angle_delta") == 0.){
+		errors.push_back(*this, "angle_delta must be non-zero");
+	}
+
+	// check availability of object
+	props.get<std::string>("object");
+	// check availability of eef
+	const std::string& eef = props.get<std::string>("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<std::string>("pregrasp");
+		std::map<std::string, double> m;
+		if (!jmg->getVariableDefaultPositions(name, m)){
+			errors.push_back(*this, "unknown end effector pose: " + name);
+		}
+	}
+
+	if (errors){
+		throw errors;
+	}
+}
+
+
+template<class ActionSpec>
+void DeepGraspPose<ActionSpec>::compute(){
+
+
+}
+
+
+template<class ActionSpec>
+void DeepGraspPose<ActionSpec>::onNewSolution(const SolutionBase& s){
+	planning_scene::PlanningSceneConstPtr scene = s.end()->scene();
+
+	const auto& props = properties();
+	const std::string& object = props.get<std::string>("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("GenerateGraspPose", msg);
+		}
+		return;
+	}
+
+	upstream_solutions_.push(&s);
+}
+
 }  // namespace stages
 }  // namespace task_constructor
 }  // namespace moveit

core/src/CMakeLists.txt

@@ -1,5 +1,4 @@
 add_library(${PROJECT_NAME}
-	${PROJECT_INCLUDE}/action_base.h
 	${PROJECT_INCLUDE}/container.h
 	${PROJECT_INCLUDE}/container_p.h
 	${PROJECT_INCLUDE}/cost_queue.h

core/src/stages/CMakeLists.txt

@@ -2,6 +2,7 @@ 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/gpd_generator.h
 	${PROJECT_INCLUDE}/stages/fixed_state.h
@@ -28,7 +29,6 @@ add_library(${PROJECT_NAME}_stages
 	generate_pose.cpp
 	generate_grasp_pose.cpp
 	generate_place_pose.cpp
-	gpd_generator.cpp
 	compute_ik.cpp
 	predicate_filter.cpp