moved "attach object" from "pick" to "grasp" stage

This commit is contained in:
llach 2018-02-24 16:51:04 +01:00 committed by Robert Haschke
parent bff6cc569c
commit ea6cc4b6bf
2 changed files with 16 additions and 18 deletions

View File

@ -56,23 +56,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
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);
PropertyMap& p = lift->properties();

View File

@ -86,7 +86,6 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
move->restrictDirection(MoveTo::FORWARD);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
const std::string& eef = parent_map.get<std::string>("eef");
@ -96,6 +95,22 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
insert(std::move(move));
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)