Check collision between objects and arrow label

getAnnotionCenter.cpp

  // get corner vertices of the label
  std::vector<geometry_msgs::Point> label_rect;
  geometry_msgs::Point pt;
  pt.x = center.position.x - side_length / 2;
  pt.y = center.position.y - side_length / 2;
  label_rect.push_back(pt);

  pt.x = center.position.x + side_length / 2;
  pt.y = center.position.y - side_length / 2;
  label_rect.push_back(pt);

  pt.x = center.position.x + side_length / 2;
  pt.y = center.position.y + side_length / 2;
  label_rect.push_back(pt);

  pt.x = center.position.x - side_length / 2;
  pt.y = center.position.y + side_length / 2;
  label_rect.push_back(pt);
  
  // Marker
  visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/table";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = 2;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = .1;
    marker.scale.y = .1;
    marker.scale.z = .1;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();
    marker_pub.publish(marker);

  // Check collisions with other objects.
  // TODO check collisions with occlusions / projectable space.
  bool has_collision = false;
  for (int i = 0; i < objects.poses.size(); i++) {
    // Get pose and dimensions of object
    geometry_msgs::Pose obj_center = objects.poses[i];
    // geometry_msgs::Pose obj_orientation = objects.orientation[i];
    geometry_msgs::Vector3 obj_dims = objects.dimensions[i];
    
    // Get rectangle vertices of object
    // std::vector<geometry_msgs::Point> object_rect;
    // label_rect.push_back(geometry_msgs::Point(obj_center.position.x - side_length / 2, obj_center.position.y - side_length / 2));
    // label_rect.push_back(geometry_msgs::Point(obj_center.position.x + side_length / 2, obj_center.position.y - side_length / 2));
    // label_rect.push_back(geometry_msgs::Point(obj_center.position.x + side_length / 2, obj_center.position.y + side_length / 2));
    // label_rect.push_back(geometry_msgs::Point(obj_center.position.x - side_length / 2, obj_center.position.y + side_length / 2));
    // if (RectA.X1 < RectB.X2 && RectA.X2 > RectB.X1 &&
    //     RectA.Y1 > RectB.Y2 && RectA.Y2 < RectB.Y1) {

    //     }
    // std::cout << objects.poses[i] << objects.dimensions[i] << std::endl;
  }