From ff9ab714b76d9b1c499133d56100b10d2584a3cb Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 8 Apr 2021 18:47:24 +0200 Subject: [PATCH] Create template class PickPlaceBase --- .../moveit/task_constructor/stages/pick.h | 27 +++++---- core/src/stages/pick.cpp | 56 +++++++++++-------- core/src/tasks/pick_place_task.cpp | 2 +- 3 files changed, 48 insertions(+), 37 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index b958bb3e3..178637bf8 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -75,6 +77,7 @@ namespace stages { * - detach the object * - linearly retract end effector */ +template 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* grasp_provider_class_loader_; - pluginlib::ClassLoader* 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* grasp_class_loader, pluginlib::ClassLoader* place_class_loader); + PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* 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 { public: Pick(const std::string& name = "pick", const std::string& provider_stage_plugin_name = "moveit_task_constructor/GraspProviderDefault", pluginlib::ClassLoader* 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 { public: Place(const std::string& name = "place", const std::string& provider_stage_plugin_name = "moveit_task_constructor/PlaceProviderDefault", pluginlib::ClassLoader* 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); diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index e779020d5..91e5e7614 100644 --- a/core/src/stages/pick.cpp +++ b/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* grasp_class_loader, pluginlib::ClassLoader* place_class_loader) +template +PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* class_loader) : SerialContainer(name) { PropertyMap& p = properties(); p.declare("object", "name of object to grasp/place"); @@ -35,9 +36,6 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide p.declare("eef_group", "JMG of eef"); p.declare("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(); sampling_planner_ = std::make_shared(); @@ -68,20 +66,20 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide { std::unique_ptr wrapper; if (is_pick_) { - std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); + std::unique_ptr 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("grasp pose IK", std::move(provider_stage_plugin)); } else { - std::unique_ptr provider_stage_plugin(place_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); + std::unique_ptr 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("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; +template class PickPlaceBase; + +template +void PickPlaceBase::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 +void PickPlaceBase::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,7 +197,8 @@ void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& /// approach / place -void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, +template +void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = move_there_stage_->properties(); p.set("direction", motion); @@ -202,7 +206,8 @@ void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, p.set("max_distance", max_distance); } -void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, +template +void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { auto& p = move_there_stage_->properties(); p.set("direction", direction); @@ -210,24 +215,28 @@ void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direct p.set("max_distance", max_distance); } -void PickPlaceBase::setApproachPlace(const std::map& joints) { +template +void PickPlaceBase::setApproachPlace(const std::map& joints) { auto& p = move_there_stage_->properties(); p.set("joints", joints); } /// IK computation -void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) { +template +void PickPlaceBase::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 +void PickPlaceBase::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 +void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { auto& p = compute_ik_stage_->properties(); p.set("ignore_collisions", ignore_ik_collisions); } @@ -235,21 +244,24 @@ void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { /// lift / retract -void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { +template +void PickPlaceBase::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 +void PickPlaceBase::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& joints) { +template +void PickPlaceBase::setLiftRetract(const std::map& joints) { auto& p = move_back_stage_->properties(); p.set("joints", joints); } @@ -259,7 +271,7 @@ void PickPlaceBase::setLiftRetract(const std::map& joints) /// setters of pick properties void Pick::setMonitoredStage(Stage* monitored) { - grasp_stage_->setMonitoredStage(monitored); + provider_plugin_stage_->setMonitoredStage(monitored); } @@ -267,11 +279,11 @@ void Pick::setMonitoredStage(Stage* 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 diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 9aeec9cec..e746baf58 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/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_);