def get_obj_msg_from_detection(cloud_msg, bounding_box, category, confidence,
frame_id):
'''Creates an mas_perception_msgs.msg.Object message from the given
information about an object detected in an image generated from the
point cloud.
Keyword arguments:
cloud_msg: sensor_msgs.msg.PointCloud2
bounding_box: BoundingBox2D
category: str
confidence: float -- detection confidence
frame_id: str -- frame of the resulting object
'''
cropped_cloud = crop_organized_cloud_msg(cloud_msg, bounding_box)
obj_coords = crop_cloud_to_xyz(
cropped_cloud,
BoundingBox2D(box_geometry=(0, 0, cropped_cloud.width,
cropped_cloud.height)))
obj_coords = np.reshape(obj_coords, (-1, 3))
min_coord = np.nanmin(obj_coords, axis=0)
max_coord = np.nanmax(obj_coords, axis=0)
mean_coord = np.nanmean(obj_coords, axis=0)
# fill object geometry info
detected_obj = Object()
detected_obj.bounding_box.center.x = mean_coord[0]
detected_obj.bounding_box.center.y = mean_coord[1]
detected_obj.bounding_box.center.z = mean_coord[2]
detected_obj.bounding_box.dimensions.x = max_coord[0] - min_coord[0]
detected_obj.bounding_box.dimensions.y = max_coord[1] - min_coord[1]
detected_obj.bounding_box.dimensions.z = max_coord[2] - min_coord[2]
box_msg = detected_obj.bounding_box
detected_obj.name = category
detected_obj.category = category
detected_obj.probability = confidence
object_view = ObjectView()
object_view.point_cloud = cropped_cloud
object_view.image = cloud_msg_to_image_msg(cropped_cloud)
detected_obj.views.append(object_view)
detected_obj.pose.header.frame_id = frame_id
detected_obj.pose.header.stamp = rospy.Time.now()
detected_obj.pose.pose.position = box_msg.center
detected_obj.pose.pose.position.x = mean_coord[0]
detected_obj.pose.pose.orientation.w = 1.0
detected_obj.roi.x_offset = int(bounding_box.x)
detected_obj.roi.y_offset = int(bounding_box.y)
detected_obj.roi.width = int(bounding_box.width)
detected_obj.roi.height = int(bounding_box.height)
return detected_obj
def _get_action_result(cloud_msg, plane_list, bounding_boxes, classes,
confidences):
"""
TODO(minhnh) adapt to multiple planes
:type cloud_msg: PointCloud2
:type plane_list: PlaneList
:type bounding_boxes: list
:type classes: list
:type confidences: list
:rtype: DetectSceneResult
"""
result = DetectSceneResult()
plane = plane_list.planes[0]
for index, box in enumerate(bounding_boxes):
# TODO(minhnh): BoundingBox code returns bad positions and dimensions
# check if object in on plane
cropped_cloud = crop_organized_cloud_msg(cloud_msg, box)
obj_coords = crop_cloud_to_xyz(
cropped_cloud,
BoundingBox2D(box_geometry=(0, 0, cropped_cloud.width,
cropped_cloud.height)))
obj_coords = np.reshape(obj_coords, (-1, 3))
min_coord = np.nanmin(obj_coords, axis=0)
max_coord = np.nanmax(obj_coords, axis=0)
mean_coord = np.nanmean(obj_coords, axis=0)
# fill object geometry info
detected_obj = Object()
detected_obj.bounding_box.center.x = mean_coord[0]
detected_obj.bounding_box.center.y = mean_coord[1]
detected_obj.bounding_box.center.z = mean_coord[2]
detected_obj.bounding_box.dimensions.x = max_coord[0] - min_coord[0]
detected_obj.bounding_box.dimensions.y = max_coord[1] - min_coord[1]
detected_obj.bounding_box.dimensions.z = max_coord[2] - min_coord[2]
box_msg = detected_obj.bounding_box
if not SceneDetectionActionServer.is_object_on_plane(
plane, box_msg):
rospy.logdebug(
"skipping object '%s', position (%.3f, %.3f, %.3f)" %
(classes[index], box_msg.center.x, box_msg.center.y,
box_msg.center.z))
continue
rospy.loginfo(
"adding object '%s', position (%.3f, %.3f, %.3f), dimensions (%.3f, %.3f, %.3f)"
% (classes[index], box_msg.center.x, box_msg.center.y,
box_msg.center.z, box_msg.dimensions.x,
box_msg.dimensions.y, box_msg.dimensions.z))
detected_obj.name = classes[index]
detected_obj.category = classes[index]
detected_obj.probability = confidences[index]
detected_obj.pointcloud = cropped_cloud
detected_obj.rgb_image = cloud_msg_to_image_msg(cropped_cloud)
detected_obj.pose.header = plane.header
detected_obj.pose.pose.position = box_msg.center
detected_obj.pose.pose.position.x = mean_coord[0]
detected_obj.pose.pose.orientation.w = 1.0
plane.object_list.objects.append(detected_obj)
result.planes.append(plane)
return result