From 344713536d6342f9d71c46d074cd2de82ba57962 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 24 Mar 2017 12:43:43 +0100 Subject: [PATCH] demo ur5: spawn object in the beginning --- src/demo/plan_pick_ur5.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 55a4ad9e..4f3c0750 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -8,13 +8,36 @@ #include +#include + using namespace moveit::task_constructor; +void spawnObject(){ + moveit::planning_interface::PlanningSceneInterface psi; + + moveit_msgs::CollisionObject o; + o.id= "object"; + o.header.frame_id= "table_top"; + o.primitive_poses.resize(1); + o.primitive_poses[0].position.x= -0.2; + o.primitive_poses[0].position.y= 0.13; + o.primitive_poses[0].position.z= 0.12; + o.primitive_poses[0].orientation.w= 1.0; + o.primitives.resize(1); + o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].dimensions.resize(2); + o.primitives[0].dimensions[0]= 0.23; + o.primitives[0].dimensions[1]= 0.03; + psi.applyCollisionObject(o); +} + int main(int argc, char** argv){ ros::init(argc, argv, "plan_pick"); ros::AsyncSpinner spinner(1); spinner.start(); + spawnObject(); + Task t; t.addStart( std::make_shared("current state") );