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