bool CollisionInterface::GetGeometricShape(vision_srvs::cop_get_object_shape::Request &msg, vision_srvs::cop_get_object_shape::Response &answer) { ROS_INFO("In CollisionInterface::GetGeometricShape"); Signature* sig = m_myCop.s_sigDb->GetSignatureByID(msg.object_id); if(sig == NULL) { ROS_ERROR("Wrong object ID\n"); return false; } ROS_INFO("Got signature"); GeometricShape shape; shape.type = 0; bool hasShape = false; for(size_t id = 0; id < sig->CountElems(); id++) { Descriptor* elem = (Descriptor*)sig->GetElement (id, ELEM); if(elem->GetShape(shape)) hasShape = true; } if(hasShape) { SetShape(answer.shape, shape); } else { ROS_INFO("No Shape found"); return false; } return true; }
std::string CollisionInterface::AddCollisionObject(Signature* sig, arm_navigation_msgs::CollisionObject &object, bool ignore_pcd) { if(sig == NULL) { ROS_ERROR("Wrong object ID\n"); return ""; } ROS_INFO("Got signature with %ld elems", sig->CountElems()); GeometricShape shape; shape.type = 0; bool hasShape = false; for(size_t id = 0; id < sig->CountElems(); id++) { Descriptor* elem = (Descriptor*)sig->GetElement (id, ELEM); printf("Descriptor: %s\n", elem->GetNodeName().c_str()); if(elem->GetShape(shape)) { if(ignore_pcd && shape.type==4) { shape.type = arm_navigation_msgs::Shape::BOX; shape.dimensions.resize(3); shape.dimensions[0] = (std::max_element(shape.vertices.begin(), shape.vertices.end(), &comparePointX)->x - std::min_element(shape.vertices.begin(), shape.vertices.end(), &comparePointX)->x) * 1.1; shape.dimensions[1] = (std::max_element(shape.vertices.begin(), shape.vertices.end(), &comparePointY)->y - std::min_element(shape.vertices.begin(), shape.vertices.end(), &comparePointY)->y) * 1.1; shape.dimensions[2] = (std::max_element(shape.vertices.begin(), shape.vertices.end(), &comparePointZ)->z - std::min_element(shape.vertices.begin(), shape.vertices.end(), &comparePointZ)->z) * 1.1; shape.triangles.clear(); shape.vertices.clear(); } hasShape = true; printf("has shape\n"); } else { printf("has NO shape (but hasSahpe = %s)\n", hasShape ? "true" : "false"); } } arm_navigation_msgs::Shape answer_shape; if(hasShape) { SetShape(answer_shape, shape); } else { ROS_INFO("No Shape found"); return ""; } char objectid[90]; sprintf(objectid, "Object%ld", sig->m_ID); object.id = objectid; object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; object.header.frame_id = "map"; object.header.stamp = ros::Time::now(); if(sig->GetObjectPose() == NULL) { ROS_ERROR("Object not localized, can not add it to collision map"); return ""; } geometry_msgs::Pose pose = RelPoseToRosPose(sig->GetObjectPose(), 1); object.poses.push_back(pose); object.shapes.push_back(answer_shape); m_object_in_map_pub.publish(object); return objectid; }