mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
moved "attach object" from "pick" to "grasp" stage
This commit is contained in:
parent
bff6cc569c
commit
ea6cc4b6bf
@ -56,23 +56,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
|
|||||||
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
|
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
|
||||||
insert(std::move(grasp_stage));
|
insert(std::move(grasp_stage));
|
||||||
|
|
||||||
{
|
|
||||||
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
|
|
||||||
PropertyMap& p = attach->properties();
|
|
||||||
p.declare<std::string>("eef");
|
|
||||||
p.declare<std::string>("object");
|
|
||||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
|
||||||
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
|
||||||
const std::string& eef = p.get<std::string>("eef");
|
|
||||||
moveit_msgs::AttachedCollisionObject obj;
|
|
||||||
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
|
||||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
|
||||||
obj.object.id = p.get<std::string>("object");
|
|
||||||
scene->processAttachedCollisionObjectMsg(obj);
|
|
||||||
});
|
|
||||||
insert(std::move(attach));
|
|
||||||
}
|
|
||||||
|
|
||||||
{
|
{
|
||||||
auto lift = std::make_unique<MoveRelative>("lift object", cartesian);
|
auto lift = std::make_unique<MoveRelative>("lift object", cartesian);
|
||||||
PropertyMap& p = lift->properties();
|
PropertyMap& p = lift->properties();
|
||||||
|
|||||||
@ -86,7 +86,6 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
|
|||||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||||
|
|
||||||
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
|
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
|
||||||
move->restrictDirection(MoveTo::FORWARD);
|
|
||||||
PropertyMap& p = move->properties();
|
PropertyMap& p = move->properties();
|
||||||
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
||||||
const std::string& eef = parent_map.get<std::string>("eef");
|
const std::string& eef = parent_map.get<std::string>("eef");
|
||||||
@ -96,6 +95,22 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
|
|||||||
insert(std::move(move));
|
insert(std::move(move));
|
||||||
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
|
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
|
||||||
}
|
}
|
||||||
|
{
|
||||||
|
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
|
||||||
|
PropertyMap& p = attach->properties();
|
||||||
|
p.declare<std::string>("eef");
|
||||||
|
p.declare<std::string>("object");
|
||||||
|
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
||||||
|
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||||
|
const std::string& eef = p.get<std::string>("eef");
|
||||||
|
moveit_msgs::AttachedCollisionObject obj;
|
||||||
|
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
||||||
|
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||||
|
obj.object.id = p.get<std::string>("object");
|
||||||
|
scene->processAttachedCollisionObjectMsg(obj);
|
||||||
|
});
|
||||||
|
insert(std::move(attach));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
|
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user