void demoPick(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::Grasp> grasps;
for (std::size_t i = 0 ; i < 20 ; ++i)
{
geometry_msgs::PoseStamped p = group.getRandomPose();
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::Grasp g;
g.grasp_pose = p;
g.approach.direction.vector.x = 1.0;
g.retreat.direction.vector.z = 1.0;
g.retreat.direction.header = p.header;
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.27;
g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
g.pre_grasp_posture.position.resize(1);
g.pre_grasp_posture.position[0] = 1;
g.grasp_posture.name.resize(1, "r_gripper_joint");
g.grasp_posture.position.resize(1);
g.grasp_posture.position[0] = 0;
grasps.push_back(g);
}
group.pick("bubu", grasps);
}
void pick(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::Grasp> grasps;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.32;
p.pose.position.y = -0.7;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::Grasp g;
g.grasp_pose = p;
g.approach.direction.vector.x = 1.0;
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.direction.header.frame_id = "base_footprint";
g.retreat.direction.vector.z = 1.0;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
g.pre_grasp_posture.position.resize(1);
g.pre_grasp_posture.position[0] = 1;
g.grasp_posture.name.resize(1, "r_gripper_joint");
g.grasp_posture.position.resize(1);
g.grasp_posture.position[0] = 0;
grasps.push_back(g);
group.setSupportSurfaceName("table");
group.pick("part", grasps);
}
int pickup() {
/*
// ----- pick up object near point (in meter) relative to base_footprint
geometry_msgs::PointStamped pickup_point;
pickup_point.header.frame_id = KURTANA_BASE_LINK;
pickup_point.point.x = desired_pickup_point.x;
pickup_point.point.y = desired_pickup_point.y;
pickup_point.point.z = desired_pickup_point.z;
*/
// ----- call the tabletop detection
ROS_INFO("Calling tabletop detector");
tabletop_object_detector::TabletopDetection detection_call;
//we want recognized database objects returned
//set this to false if you are using the pipeline without the database
detection_call.request.return_models = false;
//we want the individual object point clouds returned as well
detection_call.request.return_clusters = false;
if (!object_detection_srv.call(detection_call)) {
ROS_ERROR("Tabletop detection service failed");
return -1;
}
if (detection_call.response.detection.result
!= detection_call.response.detection.SUCCESS) {
ROS_ERROR(
"Tabletop detection returned error code %d", detection_call.response.detection.result);
return -1;
}
if (detection_call.response.detection.clusters.empty()
&& detection_call.response.detection.models.empty()) {
ROS_ERROR(
"The tabletop detector detected the table, but found no objects");
return -1;
}
//Remove the table because there are convex hull problems if adding the table to envirnonment
//detection_call.response.detection.table.convex_hull = shape_msgs::Mesh();
tabletop_collision_map_processing::TabletopCollisionMapProcessing process_call;
process_call.request.detection_result = detection_call.response.detection;
process_call.request.reset_collision_models = false;
process_call.request.reset_attached_models = false;
if (!collision_processing_srv.call(process_call)) {
ROS_ERROR("Tabletop Collision Map Processing failed");
return -1;
}
ROS_INFO_STREAM("Found objects count: " << process_call.response.collision_object_names.size());
if (process_call.response.collision_object_names.empty()) {
ROS_ERROR("Tabletop Collision Map Processing error");
return -1;
}
/*
ROS_INFO("Add table to planning scene");
moveit_msgs::CollisionObject collision_object;
collision_object.header.stamp = ros::Time::now();
collision_object.header.frame_id =
detection_call.response.detection.table.pose.header.frame_id;
ROS_INFO_STREAM("Add clusters");
object_positions.clear();
//add objects to planning scene
for (unsigned int i = 0;
i < detection_call.response.detection.clusters.size(); i++) {
sensor_msgs::PointCloud2 pc2;
sensor_msgs::convertPointCloudToPointCloud2(
detection_call.response.detection.clusters[i], pc2);
geometry_msgs::PoseStamped poseStamped;
geometry_msgs::Vector3 dimension;
getClusterBoundingBox3D(pc2, poseStamped, dimension);
geometry_msgs::PointStamped point;
point.header.frame_id = poseStamped.header.frame_id;
point.point = poseStamped.pose.position;
object_positions.push_back(point);
ostringstream id;
id << "object " << i;
collision_object.id = id.str().c_str();
ROS_INFO_STREAM("Object id: " << collision_object.id);
collision_object.operation = moveit_msgs::CollisionObject::REMOVE;
pub_collision_object.publish(collision_object);
collision_object.operation = moveit_msgs::CollisionObject::ADD;
collision_object.primitives.resize(1);
collision_object.primitives[0].type =
shape_msgs::SolidPrimitive::BOX;
collision_object.primitives[0].dimensions.resize(
shape_tools::SolidPrimitiveDimCount<
shape_msgs::SolidPrimitive::BOX>::value);
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] =
dimension.x;
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] =
dimension.y;
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] =
dimension.z;
collision_object.primitive_poses.resize(1);
collision_object.primitive_poses[0] = poseStamped.pose;
pub_collision_object.publish(collision_object);
}
*/
//group->setSupportSurfaceName("table");
//call object pickup
ROS_INFO("Calling the pickup action");
generated_pickup_grasp = generateGrasp();
ROS_INFO("Get nearest object");
object_to_manipulate = nearest_object();
//test
/*
[DEBUG] [1378456372.242171284]: IK pose: position:
x: 0.44866
y: -0.185202
z: 0.392208
orientation:
x: 0.00125435
y: -0.00632681
z: -0.194471
w: 0.980887
[DEBUG] [1378456372.242351955]: Found 4 solutions from IKFast
[DEBUG] [1378456372.242443678]: Sol 0: -3.914825e-01 1.548975e+00 -1.572923e+00 -3.587233e-02 -4.921995e-03 2.618548e-322
[DEBUG] [1378456372.243036938]: Solution passes callback
geometry_msgs::PoseStamped p;
p.header.frame_id = ARM_BASE_LINK;
p.pose.position.x = 0.44866;
p.pose.position.y = -0.185202;
p.pose.position.z = 0.392208;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
group->setPoseTarget(p,GRIPPER_FRAME);
group->move();
*/
//group->pick(object_to_manipulate);
ROS_INFO_STREAM("Picking up Object: " << object_to_manipulate);
group->pick(object_to_manipulate, generated_pickup_grasp);
ROS_INFO("Pick returned!!!!!11111 OMGWTFIT");
//group->place(object_to_manipulate);
std::vector<double> rpy = group->getCurrentRPY(
group->getEndEffectorLink());
ROS_INFO_STREAM(
"End effector link: " << rpy.at(0)<< rpy.at(1) << rpy.at(2));
object_in_gripper = 1;
move_arm_out_of_the_way();
return 0;
}
