Create template class PickPlaceBase

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

@@ -39,6 +39,8 @@
 #include <pluginlib/class_loader.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit/task_constructor/container.h>
+#include <moveit/task_constructor/stages/move_relative.h>
+#include <moveit/task_constructor/stages/compute_ik.h>
 #include <moveit/task_constructor/stages/grasp_provider.h>
 #include <moveit/task_constructor/stages/place_provider.h>
 #include <moveit/task_constructor/stages/modify_planning_scene.h>
@@ -75,6 +77,7 @@ namespace stages {
  * - detach the object
  * - linearly retract end effector
  */
+template <class C>
 class PickPlaceBase : public SerialContainer
 {
 
@@ -83,25 +86,21 @@ class PickPlaceBase : public SerialContainer
 	solvers::CartesianPathPtr cartesian_solver_;
 	solvers::PipelinePlannerPtr sampling_planner_;
 
-	Stage* move_there_stage_ = nullptr;
-	Stage* compute_ik_stage_ = nullptr;
+	MoveRelative* move_there_stage_ = nullptr;
+	ComputeIK* compute_ik_stage_ = nullptr;
 	ModifyPlanningScene* set_collision_object_hand_stage_ = nullptr;
 	ModifyPlanningScene* allow_collision_object_support_stage_ = nullptr;
 	ModifyPlanningScene* forbid_collision_object_support_stage_ = nullptr;
-	Stage* move_back_stage_ = nullptr;
+	MoveRelative* move_back_stage_ = nullptr;
 
 	std::string provider_stage_plugin_name_;
 
-	pluginlib::ClassLoader<GraspProviderBase>* grasp_provider_class_loader_;
-	pluginlib::ClassLoader<PlaceProviderBase>* place_provider_class_loader_;
-
 protected:
-	GraspProviderBase* grasp_stage_ = nullptr;
-	PlaceProviderBase* place_stage_ = nullptr;
+	C* provider_plugin_stage_ = nullptr;
 	ModifyPlanningScene* attach_detach_stage_ = nullptr;
 
 public:
-	PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader<GraspProviderBase>* grasp_class_loader, pluginlib::ClassLoader<PlaceProviderBase>* place_class_loader);
+	PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader<C>* class_loader);
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
@@ -157,15 +156,15 @@ class PickPlaceBase : public SerialContainer
 	solvers::PipelinePlannerPtr samplingPlanner() {return sampling_planner_;}
 
 	// Use this to retrieve a pointer to the GraspProviderPlugin object, to set its custom properties
-	moveit::task_constructor::Stage* GraspProviderPlugin() {return grasp_stage_;}
+	C* ProviderPlugin() {return provider_plugin_stage_;}
 };
 
 /// specialization of PickPlaceBase to realize picking
-class Pick : public PickPlaceBase
+class Pick : public PickPlaceBase <GraspProviderBase>
 {
 public:
 	Pick(const std::string& name = "pick", const std::string& provider_stage_plugin_name = "moveit_task_constructor/GraspProviderDefault", pluginlib::ClassLoader<GraspProviderBase>* class_loader = nullptr)
-	  : PickPlaceBase(name, provider_stage_plugin_name, true, class_loader, nullptr) {}
+	  : PickPlaceBase(name, provider_stage_plugin_name, true, class_loader) {}
 
 	void setMonitoredStage(Stage* monitored);
 
@@ -193,11 +192,11 @@ class Pick : public PickPlaceBase
 };
 
 /// specialization of PickPlaceBase to realize placing
-class Place : public PickPlaceBase
+class Place : public PickPlaceBase <PlaceProviderBase>
 {
 public:
 	Place(const std::string& name = "place", const std::string& provider_stage_plugin_name = "moveit_task_constructor/PlaceProviderDefault", pluginlib::ClassLoader<PlaceProviderBase>* class_loader = nullptr)
-	  : PickPlaceBase(name, provider_stage_plugin_name, false, nullptr, class_loader) {}
+	  : PickPlaceBase(name, provider_stage_plugin_name, false, class_loader) {}
 
 	void setMonitoredStage(Stage* monitored);
 

core/src/stages/pick.cpp

@@ -21,7 +21,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
-PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader<GraspProviderBase>* grasp_class_loader, pluginlib::ClassLoader<PlaceProviderBase>* place_class_loader)
+template <class C>
+PickPlaceBase<C>::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader<C>* class_loader)
   : SerialContainer(name) {
 	PropertyMap& p = properties();
 	p.declare<std::string>("object", "name of object to grasp/place");
@@ -35,9 +36,6 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide
 	p.declare<std::string>("eef_group", "JMG of eef");
 	p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
 
-	grasp_provider_class_loader_ = grasp_class_loader;
-	place_provider_class_loader_ = place_class_loader;
-
 	cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
 	sampling_planner_ = std::make_shared<solvers::PipelinePlanner>();
 
@@ -68,20 +66,20 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide
 	{
 		std::unique_ptr<ComputeIK> wrapper;
 		if (is_pick_) {
-			std::unique_ptr<GraspProviderBase> provider_stage_plugin(grasp_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_));
+			std::unique_ptr<C> provider_stage_plugin(class_loader->createUnmanagedInstance(provider_stage_plugin_name_));
 			provider_stage_plugin->properties().configureInitFrom(Stage::PARENT);
 			provider_stage_plugin->properties().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose");
 			provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose");
 			provider_stage_plugin->properties().set("marker_ns", "grasp_pose");
-			grasp_stage_ = provider_stage_plugin.get();
+			provider_plugin_stage_ = provider_stage_plugin.get();
 
 			wrapper = std::make_unique<ComputeIK>("grasp pose IK", std::move(provider_stage_plugin));
 		}
 		else {
-			std::unique_ptr<PlaceProviderBase> provider_stage_plugin(place_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_));
+			std::unique_ptr<C> provider_stage_plugin(class_loader->createUnmanagedInstance(provider_stage_plugin_name_));
 			provider_stage_plugin->properties().configureInitFrom(Stage::PARENT);
 			provider_stage_plugin->properties().set("marker_ns", "place_pose");
-			place_stage_ = provider_stage_plugin.get();
+			provider_plugin_stage_ = provider_stage_plugin.get();
 
 			wrapper = std::make_unique<ComputeIK>("place pose IK", std::move(provider_stage_plugin));
 		}
@@ -144,7 +142,11 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide
 	}
 }
 
-void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
+template class PickPlaceBase<GraspProviderBase>;
+template class PickPlaceBase<PlaceProviderBase>;
+
+template <class C>
+void PickPlaceBase<C>::init(const moveit::core::RobotModelConstPtr& robot_model) {
 	// inherit properties from parent
 	PropertyMap* p = &properties();
 	p->performInitFrom(Stage::PARENT, parent()->properties());
@@ -180,7 +182,8 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
 /// -------------------------
 /// setters of own properties
 
-void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
+template <class C>
+void PickPlaceBase<C>::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
 	geometry_msgs::PoseStamped pose_msg;
 	pose_msg.header.frame_id = link;
 	tf::poseEigenToMsg(pose, pose_msg.pose);
@@ -194,62 +197,71 @@ void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string&
 
 /// approach / place
 
-void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance,
+template <class C>
+void PickPlaceBase<C>::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance,
                                        double max_distance) {
 	auto& p = move_there_stage_->properties();
 	p.set("direction", motion);
 	p.set("min_distance", min_distance);
 	p.set("max_distance", max_distance);
 }
 
-void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance,
+template <class C>
+void PickPlaceBase<C>::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance,
                                        double max_distance) {
 	auto& p = move_there_stage_->properties();
 	p.set("direction", direction);
 	p.set("min_distance", min_distance);
 	p.set("max_distance", max_distance);
 }
 
-void PickPlaceBase::setApproachPlace(const std::map<std::string, double>& joints) {
+template <class C>
+void PickPlaceBase<C>::setApproachPlace(const std::map<std::string, double>& joints) {
 	auto& p = move_there_stage_->properties();
 	p.set("joints", joints);
 }
 
 /// IK computation
 
-void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) {
+template <class C>
+void PickPlaceBase<C>::setMaxIKSolutions(const uint32_t& max_ik_solutions) {
 	auto& p = compute_ik_stage_->properties();
 	p.set("max_ik_solutions", max_ik_solutions);
 }
 
-void PickPlaceBase::setMinIKSolutionDistance(const double& min_ik_solution_distance) {
+template <class C>
+void PickPlaceBase<C>::setMinIKSolutionDistance(const double& min_ik_solution_distance) {
 	auto& p = compute_ik_stage_->properties();
 	p.set("min_solution_distance", min_ik_solution_distance);
 }
 
-void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) {
+template <class C>
+void PickPlaceBase<C>::setIgnoreIKCollisions(const bool& ignore_ik_collisions) {
 	auto& p = compute_ik_stage_->properties();
 	p.set("ignore_collisions", ignore_ik_collisions);
 }
 
 
 /// lift / retract
 
-void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
+template <class C>
+void PickPlaceBase<C>::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
 	auto& p = move_back_stage_->properties();
 	p.set("direction", motion);
 	p.set("min_distance", min_distance);
 	p.set("max_distance", max_distance);
 }
 
-void PickPlaceBase::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) {
+template <class C>
+void PickPlaceBase<C>::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) {
 	auto& p = move_back_stage_->properties();
 	p.set("direction", direction);
 	p.set("min_distance", min_distance);
 	p.set("max_distance", max_distance);
 }
 
-void PickPlaceBase::setLiftRetract(const std::map<std::string, double>& joints) {
+template <class C>
+void PickPlaceBase<C>::setLiftRetract(const std::map<std::string, double>& joints) {
 	auto& p = move_back_stage_->properties();
 	p.set("joints", joints);
 }
@@ -259,19 +271,19 @@ void PickPlaceBase::setLiftRetract(const std::map<std::string, double>& joints)
 /// setters of pick properties
 
 void Pick::setMonitoredStage(Stage* monitored) {
-	grasp_stage_->setMonitoredStage(monitored);
+	provider_plugin_stage_->setMonitoredStage(monitored);
 }
 
 
 /// -------------------------
 /// setters of place properties
 
 void Place::setMonitoredStage(Stage* monitored) {
-	place_stage_->setMonitoredStage(monitored);
+	provider_plugin_stage_->setMonitoredStage(monitored);
 }
 
 void Place::setPlacePose(const geometry_msgs::PoseStamped& pose) {
-	place_stage_->setPose(pose);
+	provider_plugin_stage_->setPose(pose);
 }
 
 }  // namespace stages

core/src/tasks/pick_place_task.cpp

@@ -147,7 +147,7 @@ void PickPlaceTask::init(const Parameters& parameters)
     stage->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_);
     stage->setSupportSurfaces(parameters.support_surfaces_);
     stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
-    stage->GraspProviderPlugin()->properties().set("angle_delta", M_PI / 12);  // Set plugin-specific properties
+    stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12);  // Set plugin-specific properties
     stage->setMonitoredStage(current_state_stage_);
     stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_);
     stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_);