Remove template paramete to action base class

The action base class can only be used with the action msg
GraspPlanning.action from grasping_msgs.

Author: bostoncleek

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

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

@@ -41,23 +41,15 @@
 #include <limits>
 
 #include <actionlib/client/simple_action_client.h>
+#include <grasping_msgs/GraspPlanningAction.h>
 
 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 ActionSpec>
+/** @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<actionlib::SimpleActionClient<ActionSpec>> clientPtr_;  // action client
+	std::unique_ptr<actionlib::SimpleActionClient<grasping_msgs::GraspPlanningAction>> clientPtr_;  // action client
 	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));
-
-	// Negative timeouts are set to zero
-	server_timeout_ = server_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : server_timeout_;
-	goal_timeout_ = goal_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : goal_timeout_;
-}
-
-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/grasp_provider.h

@@ -37,6 +37,8 @@
 #pragma once
 
 #include <functional>
+#include <mutex>
+
 #include <moveit/task_constructor/stages/generate_pose.h>
 #include <moveit/task_constructor/stages/action_base.h>
 #include <moveit/task_constructor/storage.h>
@@ -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 ActionSpec>
-class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
+/** @brief Generate grasp candidates using deep learning approaches */
+class GraspProvider : public GeneratePose, ActionBase
 {
-private:
-	typedef ActionBase<ActionSpec> ActionBaseT;
-	ACTION_DEFINITION(ActionSpec);
-
 public:
 	/**
 	 * @brief Constructor
@@ -71,7 +62,7 @@ class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
 	 * @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<ActionSpec>
 	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<ActionSpec>
 	void onNewSolution(const SolutionBase& s) override;
 
 private:
+	std::mutex grasp_mutex_;
 	bool found_candidates_;
-	std::vector<geometry_msgs::PoseStamped> grasp_candidates_;
-	std::vector<double> costs_;
+	std::vector<moveit_msgs::Grasp> grasp_candidates_;
 };
-
-template <class ActionSpec>
-GraspProvider<ActionSpec>::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<std::string>("eef", "name of end-effector");
-	p.declare<std::string>("object");
-	p.declare<boost::any>("pregrasp", "pregrasp posture");
-	p.declare<boost::any>("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 <class ActionSpec>
-void GraspProvider<ActionSpec>::composeGoal() {
-	Goal goal;
-	goal.action_name = ActionBaseT::action_name_;
-	ActionBaseT::clientPtr_->sendGoal(
-	    goal, std::bind(&GraspProvider<ActionSpec>::doneCallback, this, std::placeholders::_1, std::placeholders::_2),
-	    std::bind(&GraspProvider<ActionSpec>::activeCallback, this),
-	    std::bind(&GraspProvider<ActionSpec>::feedbackCallback, this, std::placeholders::_1));
-
-	ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str());
-}
-
-template <class ActionSpec>
-bool GraspProvider<ActionSpec>::monitorGoal() {
-	// monitor timeout
-	const bool monitor_timeout = ActionBaseT::goal_timeout_ > std::numeric_limits<double>::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 <class ActionSpec>
-void GraspProvider<ActionSpec>::activeCallback() {
-	ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active");
-	found_candidates_ = false;
-}
-
-template <class ActionSpec>
-void GraspProvider<ActionSpec>::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 <class ActionSpec>
-void GraspProvider<ActionSpec>::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 <class ActionSpec>
-void GraspProvider<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 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 GraspProvider<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()) {
-		// 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 <class ActionSpec>
-void GraspProvider<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(LOGNAME, msg);
-		}
-		return;
-	}
-
-	upstream_solutions_.push(&s);
-}
-
 }  // namespace stages
 }  // namespace task_constructor
 }  // namespace moveit

core/package.xml

@@ -14,6 +14,7 @@
 
 	<depend>tf2_eigen</depend>
 	<depend>actionlib</depend>
+	<depend>grasping_msgs</depend>
 	<depend>geometry_msgs</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_planning</depend>

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