C++ (Cpp) Descriptor::GetShape Examples

Example #1

File: CollisionInterface.cpp Project: code-iai/ias_perception

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

Example #2

File: CollisionInterface.cpp Project: code-iai/ias_perception

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;

}