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; }