def __init__(self):
self.camera_frame = rospy.get_param('~camera_frame')
self.detection_topic = rospy.get_param(
'~detection_topic', default='yolo_predict/detection')
self.odom_topic = rospy.get_param('~odom_topic', default='odom')
self.horiz_fov = rospy.get_param('~horiz_fov', default=96.0)
self.image_width = 1
self.last_detection = Detection2DArray()
self.last_odom = Odometry()
self.bridge = CvBridge()
self.detection_sub = rospy.Subscriber(self.detection_topic,
Detection2DArray,
self._detection_cb)
self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry,
self._odom_cb)
rate = rospy.Rate(10)
last_detection = Detection2DArray()
while not rospy.is_shutdown():
cur_detection = self.last_detection
if cur_detection.header.stamp != last_detection.header.stamp:
x, y = self.get_cone_pose(cur_detection)
rospy.loginfo('x: {} y: {}'.format(x, y))
if x and y != None:
self._handle_transform(self.camera_frame, x, y)
last_detection = cur_detection
rate.sleep()
def __init__(self):
self.camera_type = rospy.get_param('~camera_type', default='zed')
self.camera_frame = rospy.get_param('~camera_frame')
self.detection_topic = rospy.get_param(
'~detection_topic', default='yolo_predict/detection')
self.odom_topic = rospy.get_param('~odom_topic', default='odom')
self.pose_topic = rospy.get_param('~pose_topic',
default='vision_poses')
self.horiz_fov = rospy.get_param('~horiz_fov', default=85.2)
self.vert_fov = rospy.get_param('~vert_fov', default=58.0)
self.image_width = 1
self.last_detection = Detection2DArray()
self.last_odom = Odometry()
self.bridge = CvBridge()
self.tf_listener = tf.TransformListener()
self.rate = 30
self.tracker = Tracker(1,
100,
200,
255,
self.rate,
ra=1.0,
sv=100000.0)
self.detection_sub = rospy.Subscriber(self.detection_topic,
Detection2DArray,
self._detection_cb)
self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry,
self._odom_cb)
self.pose_pub = rospy.Publisher(self.pose_topic,
PoseArray,
queue_size=1)
self.path_pub = rospy.Publisher('paths', MarkerArray, queue_size=1)
rate = rospy.Rate(self.rate)
last_detection = Detection2DArray()
while not rospy.is_shutdown():
cur_detection = self.last_detection
if cur_detection.header.stamp != last_detection.header.stamp:
vision_poses = self.get_vision_pose(cur_detection)
if vision_poses is not None:
tracked_poses, tracked_paths = self.tracker.update(
vision_poses)
self._handle_pose_broadcast(tracked_poses,
self.camera_frame)
self._handle_path_vis_broadcast(tracked_paths,
self.camera_frame)
# rospy.loginfo(len(self.tracker.update(vision_poses)))
# if x and y and z != None:
# self._handle_transform(self.camera_frame, x, y, z)
last_detection = cur_detection
rate.sleep()
def _construct_detection_msg_and_update_detection_image(
self, detection_res, channel_id, stamp):
if channel_id == Side.PORT:
multiplier = -1
else:
multiplier = 1
detection_array_msg = Detection2DArray()
detection_array_msg.header.frame_id = self.pub_frame_id
detection_array_msg.header.stamp = stamp
for object_id, detection in detection_res.items():
detection_msg = Detection2D()
detection_msg.header = detection_array_msg.header
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = object_id.value
object_hypothesis.score = detection['confidence']
object_hypothesis.pose.pose = self._detection_to_pose(
detection['pos'], channel_id)
# Filter out object detection outliers
if abs(object_hypothesis.pose.pose.position.y) > self.water_depth:
continue
else:
pos = self.channel_size + multiplier * detection['pos']
self.detection_image[
0,
max(pos -
10, 0):min(pos + 10, self.channel_size *
2), :] = self.detection_colors[object_id]
detection_msg.results.append(object_hypothesis)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
def detect(self, image):
detection_array_msg = Detection2DArray()
found, corners = cv.findChessboardCorners(image,
self.checkerboard_shape)
if not found:
return detection_array_msg
# Refines corners based on criteria
corners = cv.cornerSubPix(image,
corners=corners,
winSize=self.win_size,
zeroZone=(-1, -1),
criteria=CRITERIA)
# Contains N x D (N = number of corners, D = dimensions)
corners = np.array(corners).squeeze()
detection_msg = util.corners_to_detection_2d(corners)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
def listener_callback(self, data):
self.get_logger().info("Received an image! ")
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
self.timer.start()
boxes, labels, probs = self.predictor.predict(image, 10, 0.4)
interval = self.timer.end()
print('Time: {:.2f}s, Detect Objects: {:d}.'.format(
interval, labels.size(0)))
detection_array = Detection2DArray()
for i in range(boxes.size(0)):
box = boxes[i, :]
label = f"{self.class_names[labels[i]]}: {probs[i]:.2f}"
print("Object: " + str(i) + " " + label)
cv2.rectangle(cv_image, (box[0], box[1]), (box[2], box[3]),
(255, 255, 0), 4)
# Definition of 2D array message and ading all object stored in it.
object_hypothesis_with_pose = ObjectHypothesisWithPose()
object_hypothesis_with_pose.id = str(self.class_names[labels[i]])
object_hypothesis_with_pose.score = float(probs[i])
bounding_box = BoundingBox2D()
bounding_box.center.x = float((box[0] + box[2]) / 2)
bounding_box.center.y = float((box[1] + box[3]) / 2)
bounding_box.center.theta = 0.0
bounding_box.size_x = float(2 * (bounding_box.center.x - box[0]))
bounding_box.size_y = float(2 * (bounding_box.center.y - box[1]))
detection = Detection2D()
detection.header = data.header
detection.results.append(object_hypothesis_with_pose)
detection.bbox = bounding_box
detection_array.header = data.header
detection_array.detections.append(detection)
cv2.putText(
cv_image,
label,
(box[0] + 20, box[1] + 40),
cv2.FONT_HERSHEY_SIMPLEX,
1, # font scale
(255, 0, 255),
2) # line type
# Displaying the predictions
cv2.imshow('object_detection', cv_image)
# Publishing the results onto the the Detection2DArray vision_msgs format
self.detection_publisher.publish(detection_array)
ros_image = self.bridge.cv2_to_imgmsg(cv_image)
ros_image.header.frame_id = 'camera_frame'
self.result_publisher.publish(ros_image)
cv2.waitKey(1)
def callback(self, ros_data):
'''Callback function of subscribed topic. '''
if ros_data.header.stamp < self.latest_finish_time:
return
#### direct conversion to CV2 ####
now = rospy.Time.now()
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# dim = (int(len(image_np / 20)), int(len(image_np[0]) / 20))
# cv2.resize(image_np, dim)
# image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
results = detect(self.net, self.meta, image_np)
detections = Detection2DArray()
for result in results:
detection = Detection2D()
detection.header = ros_data.header
res = ObjectHypothesisWithPose()
res.id = self.get_id(result[0])
res.score = result[1]
detection.results.append(res)
detection.bbox.size_x = result[2][2]
detection.bbox.size_y = result[2][3]
detection.bbox.center.x = result[2][0]
detection.bbox.center.y = result[2][1]
detections.detections.append(detection)
self.res_pub.publish(detections)
rospy.loginfo_throttle(
1, 'Took yolo %s to process image' %
((rospy.Time.now() - now).to_sec()))
self.latest_finish_time = rospy.Time.now()
def __init__(self):
# init Node
rospy.init_node("distance_node")
rospy.loginfo("Starting DistanceNode.")
# Subscriber/Publisher
rospy.Subscriber("/scan", LaserScan, self.lidar_calback)
rospy.Subscriber("/objectDedector/objects", Detection2DArray,
self.obj_callback)
self.distance_pub = rospy.Publisher("/objectDedector/3Dobjects",
Detection3DArray,
queue_size=1)
# get params
self.cam_width = float(rospy.get_param("/usb_cam/image_width"))
self.flip_image = bool(
rospy.get_param("/usb_cam/flip_image", default="True"))
cam_fov = float(rospy.get_param("/usb_cam/fov"))
self.angleFactor = cam_fov / self.cam_width
# init messages
self.lastScan = LaserScan()
self.lastScan.angle_increment = 1
self.lastObj = Detection2DArray()
self.lastPub = Detection3DArray()
# publish image with distances
if True:
rospy.Subscriber("/objectDedector/overlayImage", Image,
self.img_callback)
self.img_pub = rospy.Publisher("/objectDedector/overlayImage3D",
Image,
queue_size=1)
self.bridge = CvBridge()
self.font = cv2.FONT_HERSHEY_SIMPLEX
def callbackPoseRCNN(self, data):
global obj, VecNeural #,obj_hypothesis
# recive data
objArray = Detection2DArray()
obj = Detection2D()
#obj_hypothesis= ObjectHypothesisWithPose()
VecNeural = Vector3()
# rcnn_pose
objArray = data
obj = objArray.detections
# rospy.loginfo(" lenth obj: %f", len(obj))
if len(obj) != 0:
# obj_hypothesis.id = obj[0].results[0].id
# obj_hypothesis.score = obj[0].results[0].score
# obj_hypothesis.pose.pose.position.x = obj[0].results[0].pose.pose.position.x
# obj_hypothesis.pose.pose.position.y = obj[0].results[0].pose.pose.position.y
# obj_hypothesis.pose.pose.position.z = obj[0].results[0].pose.pose.position.z
VecNeural.x = self.kalman.x[0] = obj[0].results[
0].pose.pose.position.x
VecNeural.y = self.kalman.x[1] = obj[0].results[
0].pose.pose.position.y
VecNeural.z = self.kalman.x[2] = obj[0].results[
0].pose.pose.position.z
def __init__(self):
#initialize the node
rospy.init_node("droneControllerNode")
#subscribe to input info
rospy.Subscriber("/mavros/state", State, self.ardupilotStateChange)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.poseUpdate)
rospy.Subscriber("/detectnet/detections", Detection2DArray, self.detectionUpdate)
#publishes position commands to the flight controller
self.positionPub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1);
#services that can be carried out by the flight controller
rospy.wait_for_service('/mavros/cmd/arming')
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool);
rospy.wait_for_service('/mavros/cmd/takeoff')
self.takeoffService = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL);
rospy.wait_for_service('/mavros/setMode')
self.changeModeService = rospy.ServiceProxy('/mavros/setMode', SetMode);
#class variables
self.pose = Pose()
self.timestamp = rospy.Time()
self.theta = 0
self.ardupilotState = stabilizeMode
self.detectionArray = Detection2DArray()
rospy.loginfo("droneControllerNode has initialized")
def image_cb(self, data):
objArray = Detection2DArray()
try:
rgb_img = self.bridge.imgmsg_to_cv2(data, "bgr8")
rgb_img_crop = self.get_roi(rgb_img)
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(rgb_img_crop, cv2.COLOR_BGR2RGB)
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
image_np = np.asarray(image)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Each box represents a part of the image where a particular object was detected.
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
scores = detection_graph.get_tensor_by_name('detection_scores:0')
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
#print('begin detect ...')
(boxes, scores, classes, num_detections) = self.sess.run(
[boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
#print('end detect ....')
#print(category_index)
#print(classes)
objects = vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
category_index,
use_normalized_coordinates=True,
line_thickness=2)
objArray.detections = []
objArray.header = data.header
object_count = 1
for i in range(len(objects)):
object_count += 1
objArray.detections.append(
self.object_predict(objects[i], data.header, image_np,
cv_image))
self.object_pub.publish(objArray)
img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(img, "bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)
def image_depth(self, data):
objArray = Detection2DArray()
if self.is_initialized == 0:
return 1
try:
if not GAZEBO_SIMULATION:
cv_image = self.bridge.imgmsg_to_cv2(data, "32FC1")
else:
cv_image = self.bridge.imgmsg_to_cv2(data, "mono8")
except CvBridgeError as e:
print(e)
self.depth_img = np.array(cv_image, dtype=np.float32)
if (self.flag == 1):
distance = self.depth_img[
int(self.pos_img[1]) -
self.size_img[1] / 4:int(self.pos_img[1]) +
self.size_img[1] / 4,
int(self.pos_img[0]) -
self.size_img[0] / 4:int(self.pos_img[0]) +
self.size_img[0] / 4]
if GAZEBO_SIMULATION:
distance = distance[(distance > 0.2)]
distance = np.nanmin(distance)
self.target_depth = distance
else:
distance = distance[(distance > 200)]
distance = np.nanmin(distance)
self.target_depth = distance / 1000.0
def image_cb(self, data):
objectArray = Detection2DArray()
try:
# Recordar que es necesario el uso de cv_bridge
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
# Representamos y guardamos la imagen como un array. Se usara mas tarde para presentar el resultado
# con las cajas y etiquetas
npArrayImage = np.asarray(image)
npArrayImageExpanded = np.expand_dims(npArrayImage, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Cada caja representa una seccion de la imagen donde un objeto fue detectado
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Obtenemos los scores o nivel de confianza de la prediccion
scores = detection_graph.get_tensor_by_name('detection_scores:0')
# Obtenemos la clase que tambien sera mostrada junto con el score
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
print("Detectando!")
(boxes, scores, classes, num_detections) = self.sess.run(
[boxes, scores, classes, num_detections],
feed_dict={image_tensor: npArrayImageExpanded})
objects = vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
categoryIndex,
use_normalized_coordinates=True,
line_thickness=2)
# NOTA: esto podria ser interesante. Leer el comentario largo en la funcion object_predict mas abajo.
objectArray.detections = []
objectArray.header = data.header
object_count = 1
for i in range(len(objects)):
object_count += 1
objectArray.detections.append(
self.object_predict(objects[i], data.header, npArrayImage,
cv_image))
self.object_pub.publish(objectArray)
img = cv2.cvtColor(npArrayImage, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
# Volvemos a convertir la imagen de OpenCV a mensaje de ROS
image_out = self.bridge.cv2_to_imgmsg(img, "bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)
def callback(self):
image = rospy.wait_for_message(
"/zed2/zed_node/left/image_rect_color/compressed", CompressedImage)
time1 = rospy.Time.now().to_sec()
self.timer.header = image.header
self.timer.header.frame_id = "zed2_left_camera_frame"
self.timer.time_ref = rospy.Time.now()
self.timer_pub.publish(self.timer)
cv_image = self.bridge.compressed_imgmsg_to_cv2(image, "bgr8")
_, bboxes = detect_image(self.yolo,
cv_image,
"",
input_size=YOLO_INPUT_SIZE,
show=False,
CLASSES=TRAIN_CLASSES,
score_threshold=0.3,
iou_threshold=0.1,
rectangle_colors=(255, 0, 0))
detect = Detection2DArray()
detect.header = image.header
for Object in bboxes:
detection = Detection2D()
hypo = ObjectHypothesisWithPose()
#Start x
x1 = Object[0]
#End x
x2 = Object[2]
#Start y
y1 = Object[1]
#end y
y2 = Object[3]
#Size x
Sx = x2 - x1
#Size y
Sy = y2 - y1
#Center x
Cx = x1 + Sx / 2
#Center y
Cy = y1 + Sy / 2
detection.bbox.center.x = Cx
detection.bbox.center.y = Cy
detection.bbox.size_x = Sx
detection.bbox.size_y = Sy
hypo.id = int(Object[5])
hypo.score = Object[4]
detection.results = [
hypo,
]
detection.is_tracking = False
detect.detections.append(detection)
self.boxes_pub.publish(detect)
self.callback()
def detect(self, image):
# Convert to grayscale if needed
if image.ndim == 3:
image = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
image_height, image_width = image.shape
image_area = image_height * image_width
# Apply (inverse) binary threshold to input image
mask = cv.threshold(image, THRESHOLD, THRESHOLD_MAX,
cv.THRESH_BINARY_INV)[1]
# Dilate mask to find more reliable contours
# kernel = np.ones((5, 5), np.uint8)
# mask_dilated = cv.dilate(mask, kernel, iterations=1)
# Find external approximate contours in dilated mask
contours, hierarchy = cv.findContours(mask, cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_SIMPLE)
# Filter out contours that don't qualify as a detection
detections = []
for contour in contours:
# Filer out if the contour touches the image border
x, y, w, h = cv.boundingRect(contour)
if x == 0 or y == 0 or x + w == image_width or y + h == image_height:
continue
# Filter out if the contour is too small
if cv.contourArea(contour) < 1e-4 * image_area:
continue
detections.append((x, y, w, h))
# Fill detections msg
detection_array_msg = Detection2DArray()
for detection in detections:
x, y, w, h = detection
center_x = x + w / 2.
center_y = y + h / 2.
bbox = BoundingBox2D()
bbox.center = Pose2D(x=center_x, y=center_y, theta=0)
bbox.size_x = w
bbox.size_y = h
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = 0
object_hypothesis.score = 1.0
detection_msg = Detection2D()
detection_msg.bbox = bbox
detection_msg.results.append(object_hypothesis)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
def image_cb(self, data):
objArray = Detection2DArray()
try:
cv_image = self.bridge.imgmsg_to_cv2(data)
except CvBridgeError as e:
print(e)
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
image = cv_image[:, :, 0]
mod_image = np.zeros((600, 600, 3))
mod_image[:, :, 0] = image
mod_image[:, :, 1] = image
mod_image[:, :, 2] = image
image_np = mod_image.astype(np.uint8)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Each box represents a part of the image where a particular object was detected.
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
scores = detection_graph.get_tensor_by_name('detection_scores:0')
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
(boxes, scores, classes, num_detections) = self.sess.run(
[boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
objects, array = vis_util.visualize_boxes_and_labels_on_image_array(
image_np,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
category_index,
use_normalized_coordinates=True,
line_thickness=2)
if (array != None):
array[0] = array[0] * 640 #450
array[1] = array[1] * 640 #450
array[2] = array[2] * 480 #300
array[3] = array[3] * 480 #300
print(array)
self.drawBoundingBox(self.img_array, array)
#display(img.fromarray(image_np))
#cv2.imshow("window",image_np))
h1, w1 = image_np.shape[:2]
h2, w2 = self.img_array.shape[:2]
#create empty matrix
vis = np.zeros((max(h1, h2), w1 + w2, 3), np.uint8)
#combine 2 images
vis[:h1, :w1, :3] = image_np
vis[:h2, w1:w1 + w2, :3] = self.img_array
cv2.imshow("window", vis)
cv2.waitKey(1)
def image_cb(self, data):
objectArray = Detection2DArray()
try:
# Recordar que es necesario el uso de cv_bridge
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
# Representamos y guardamos la imagen como un array. Se usara mas tarde para presentar el resultado
# con las cajas y etiquetas
npArrayImage = np.asarray(image)
# Cosas de numpy. El modelo necesita que el array tenga shape: [1, None, None, 3]
# Repasar todo el tema de los shapes de numpy
npArrayImageExpanded = np.expand_dims(npArrayImage, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Cada caja representa una seccion de la imagen donde un objeto fue detectado
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Obtenemos los scores o nivel de confianza de la prediccion
scores = detection_graph.get_tensor_by_name('detection_scores:0')
# Obtenemos la clase que tambien sera mostrada junto con el score
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
print("Detectando3!")
(boxes, scores, classes, num_detections) = self.sess.run(
[boxes, scores, classes, num_detections],
feed_dict={image_tensor: npArrayImageExpanded})
# Nota: averiguar como hacer el texto mas grande porque ahora mismo practicamente no se lee.
# Hay que modificar el fichero visualization_utils, la propiedad font-size, pero justo en Ubuntu 16.04 ocurre un problema
# y es que la fuente utilizada no esta en el sistema (pero no se puede usar otra).
# No obstante, hay soluciones y parecen muy, muy sencillas. Debo probarlo.
objects = vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
categoryIndex,
use_normalized_coordinates=True,
line_thickness=2)
self.try_pub.publish(objects)
# NOTA: esto podria ser interesante. Leer el comentario largo en la funcion object_predict mas abajo.
objectArray.detections = []
objectArray.header = data.header
object_count = 1
for i in range(len(objects)):
object_count += 1
objectArray.detections.append(
self.object_predict(objects[i], data.header, npArrayImage,
cv_image))
def _handle_yolo_detect(self, req):
cv_image = None
detection_array = Detection2DArray()
detections = []
boxes = None
try:
cv_image = self.bridge.imgmsg_to_cv2(req.image, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
try:
boxes = self.yolo.predict(cv_image)
except SystemError:
pass
# rospy.loginfo('Found {} boxes'.format(len(boxes)))
for box in boxes:
detection = Detection2D()
results = []
bbox = BoundingBox2D()
center = Pose2D()
detection.header = Header()
detection.header.stamp = rospy.get_rostime()
# detection.source_img = deepcopy(req.image)
labels = box.get_all_labels()
for i in range(0,len(labels)):
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = i
object_hypothesis.score = labels[i]
results.append(object_hypothesis)
detection.results = results
x, y = box.get_xy_center()
center.x = x
center.y = y
center.theta = 0.0
bbox.center = center
size_x, size_y = box.get_xy_extents()
bbox.size_x = size_x
bbox.size_y = size_y
detection.bbox = bbox
detections.append(detection)
detection_array.header = Header()
detection_array.header.stamp = rospy.get_rostime()
detection_array.detections = detections
return YoloDetectResponse(detection_array)
def track(self, image):
detections = Detection2DArray()
success, boxes = self.tracker.update(image)
if not success:
return detections
for box in boxes:
x, y, w, h = box
detection = Detection2D()
detection.bbox.center.x = x
detection.bbox.center.y = y
detection.bbox.size_x = w
detection.bbox.size_y = h
detections.detections.append(detection)
return detections
def _publish_marker_detection(self, marker, cos_sim):
"""Publish detected marker"""
distance = self._get_distance_to_marker(marker)
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = 1
# the higher the cos_sim (further away from 90 degree angle between current_pose
# and the marker), the lower the score
object_hypothesis.score = (-cos_sim + (self.buoy_radius * 2)) / (
self.buoy_radius * 2)
marker_transformed = self._transform_pose(
marker, from_frame=marker.header.frame_id)
object_hypothesis.pose.pose = marker_transformed.pose
# Add random noise to pose of object
object_hypothesis.pose.pose.position.x += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.position.y += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.position.z += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.x += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.y += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.z += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.w += np.random.randn(
) * self.noise_sigma
# Wrap ObjectHypothesisWithPose msg into Detection2D msg
detection_msg = Detection2D()
detection_msg.header.frame_id = self.published_frame_id
detection_msg.header.stamp = rospy.Time.now()
detection_msg.results.append(object_hypothesis)
# Wrap Detection2D msg into Detection2DArray msg
detection_array_msg = Detection2DArray()
detection_array_msg.header = detection_msg.header
detection_array_msg.detections.append(detection_msg)
self.pub.publish(detection_array_msg)
# Publish detection as a Marker for visualization in rviz
detected_marker = copy.deepcopy(marker)
detected_marker.header.stamp = rospy.Time.now()
detected_marker.ns = 'detected_{}'.format(detected_marker.ns)
detected_marker.color = ColorRGBA(0, 1, 0, 1)
detected_marker.lifetime.secs = 1
self.pub_detected_markers.publish(detected_marker)
def callback_yolo(self, image):
image = self.image
cv_image = self.bridge.compressed_imgmsg_to_cv2(image, "bgr8")
_, bboxes = detect_image(self.yolo,
cv_image,
"",
input_size=YOLO_INPUT_SIZE,
show=False,
rectangle_colors=(255, 0, 0))
detect = Detection2DArray()
detect.header = image.header
for Object in bboxes:
detection = Detection2D()
hypo = ObjectHypothesisWithPose()
#Start x
x1 = Object[0]
#End x
x2 = Object[2]
#Start y
y1 = Object[1]
#end y
y2 = Object[3]
#Size x
Sx = x2 - x1
#Size y
Sy = y2 - y1
#Center x
Cx = x1 + Sx / 2
#Center y
Cy = y1 + Sy / 2
detection.bbox.center.x = Cx
detection.bbox.center.y = Cy
detection.bbox.size_x = Sx
detection.bbox.size_y = Sy
hypo.id = int(Object[5])
hypo.score = Object[4]
detection.results = [
hypo,
]
detect.detections.append(detection)
self.bbox_pub.publish(detect)
def image_cb(self, data):
objArray = Detection2DArray()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
image_np = np.asarray(image)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
output_dict = self.sess.run(self.tensor_dict,
feed_dict={self.image_tensor: image_np_expanded})
objects=vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(output_dict["detection_boxes"]),
np.squeeze(output_dict["detection_classes"]).astype(np.int32),
np.squeeze(output_dict["detection_scores"]),
self.category_index,
use_normalized_coordinates=True,
line_thickness=2)
objArray.detections =[]
objArray.header=data.header
object_count=1
for i in range(len(objects)):
object_count+=1
objArray.detections.append(self.object_predict(objects[i],data.header,image_np,cv_image))
self.object_pub.publish(objArray)
img=cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(img,"bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)
def detect(self, image):
detection_array_msg = Detection2DArray()
detection_results = self.detector.detect(image)
if len(detection_results) == 0:
return detection_array_msg
for detection_result in detection_results:
# Contains N x D (N = number of corners, D = dimensions)
corners = detection_result.corners.squeeze()
detection_msg = util.corners_to_detection_2d(corners)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
def create_detections_msg(self, image_np, output_dict):
img_height = image_np.shape[0]
img_width = image_np.shape[1]
boxes = output_dict['detection_boxes']
classes = output_dict['detection_classes']
scores = output_dict['detection_scores']
detections = Detection2DArray()
detections.header.stamp = self.get_clock().now().to_msg()
detections.detections = []
for i in range(len(boxes)):
det = Detection2D()
det.header = detections.header
det.results = []
detected_object = ObjectHypothesisWithPose()
detected_object.id = classes[i].item()
detected_object.score = scores[i].item()
det.results.append(detected_object)
# box is ymin, xmin, ymax, xmax in normalized coordinates
box = boxes[i]
det.bbox.size_y = (box[2] - box[0]) * img_height
det.bbox.size_x = (box[3] - box[1]) * img_width
det.bbox.center.x = (box[1] + box[3]) * img_height / 2
det.bbox.center.y = (box[0] + box[2]) * img_width / 2
if (self.republish_image):
box_img = image_np[int(box[0] * img_height):int(box[2] *
img_height),
int(box[1] * img_width):int(box[3] *
img_width)]
det.source_img = img_utils.image_np_to_image_msg(box_img)
detections.detections.append(det)
return detections
def detect_objects(self, data):
"""
This function detects and classifies the objects in the image provided through a Service Request by running on her the provided
detection model. Returns a vision_msgs/Detection2DArray, for which each detection is populated only with the bbox and results fields. Moreover,
for what it concerns the results field, each result is populated only with the id and score fields.
All the other fields are not significant for this application, so they have been ignored.
Args:
data (sensor_msgs/Image): image to perform object detection on.
Returns:
pepper_object_detectionResponse: response encapsulating data regarding detected objects, structured as in service definition.
"""
# Convert image from sensor_msgs/Image to numpy array
img_np = ros_numpy.numpify(data.img)
rospy.loginfo('Object detection server computing predictions...')
detections = self._detection_model(img_np)
rospy.loginfo('Predictions computed!')
message = Detection2DArray()
# Insert all the predictions into the message and return them
for class_id, score, box in zip(detections['detection_classes'],
detections['detection_scores'],
detections['detection_boxes']):
detection = Detection2D()
detection.bbox.size_x = box[3] - box[1]
detection.bbox.size_y = box[2] - box[0]
detection.bbox.center.x = box[1] + detection.bbox.size_x / 2
detection.bbox.center.y = box[0] + detection.bbox.size_y / 2
detected_object = ObjectHypothesisWithPose()
detected_object.score = score
detected_object.id = class_id
detection.results.append(detected_object)
message.detections.append(detection)
# Create a response object
response = pepper_object_detectionResponse()
response.detections = message
return response
def publish_markers(self, fid_data_array):
fidarray = FiducialTransformArray()
fidarray.header.stamp = rospy.Time.now()
vis = Detection2DArray()
vis.header.stamp = rospy.Time.now()
for fid in fid_data_array:
if VIS_MSGS:
obj = Detection2D()
oh = ObjectHypothesisWithPose()
oh.id = fid.id
oh.pose.pose.position.x = fid.translation.x
oh.pose.pose.position.y = fid.translation.y
oh.pose.pose.position.z = fid.translation.z
oh.pose.pose.orientation.w = fid.rotation.w
oh.pose.pose.orientation.x = fid.rotation.x
oh.pose.pose.orientation.y = fid.rotation.y
oh.pose.pose.orientation.z = fid.rotation.z
oh.score = math.exp(-2 * OBJECT_ERROR)
obj.results.append(oh)
vis.detections.append(obj)
else:
data = FiducialTransform()
data.fiducial_id = fid.id
data.transform.translation = fid.translation
data.transform.rotation = fid.rotation
data.image_error = IMAGE_ERROR
data.object_error = OBJECT_ERROR
data.fiducial_area = FIDUCIAL_AREA
fidarray.transforms.append(data)
if VIS_MSGS:
self.fid_pub.publish(vis)
else:
self.fid_pub.publish(fidarray)
def encode_detection_msg(self,detections):
detections_msg = Detection2DArray()
if (len(detections)>0):
i=0
detstring='Detected:'
for det in detections:
detection = Detection2D()
detection.header.seq = self.detection_seq
detection.header.stamp = rospy.Time.now()
detection.header.frame_id = self.camera_frame
result = [ObjectHypothesisWithPose()]
result[0].id = det[0]
result[0].score = det[1]
detection.results = result
detection.bbox.center.x = (det[2]+det[4])/2
detection.bbox.center.y = (det[3]+det[5])/2
detection.bbox.size_x = det[4]-det[2]
detection.bbox.size_y = det[5]-det[3]
detections_msg.detections.append(detection)
detstring=detstring+' '+self.classes[int(det[0])]+', p=%.2f.'%(det[1])
i+=1
rospy.logwarn(detstring)
self.detection_seq += 1
return detections_msg
def callback_boundbox(color_img):
if not has_color_mask:
return 'Failed to unpack color mask.'
print "Triggered callback_boundbox."
img = bridge.imgmsg_to_cv2(color_img, "bgr8") # BGR OpenCV image
rgb = img[:, :, ::-1] # flip to RGB for display
hsv = cv2.cvtColor(img,
cv2.COLOR_BGR2HSV) # convert to HSV for color masking
bg_mask = cv2.inRange(hsv, np.array(floor), np.array(ceiling))
fg_mask = cv2.bitwise_not(bg_mask)
fg_mask = cv2.morphologyEx(fg_mask, cv2.MORPH_OPEN,
np.ones((3, 3), np.uint8))
# Apply final mask to show sherds and black out background
objects = cv2.bitwise_and(rgb, rgb, mask=fg_mask)
# Display original image and segmented objects
plt.subplot(121), plt.imshow(rgb)
plt.title('Original Image'), plt.xticks([]), plt.yticks([])
plt.subplot(122), plt.imshow(objects)
plt.title('Sherds'), plt.xticks([]), plt.yticks([])
plt.show()
# Find contours in gray 'objects' image. Finding contours in 'color_mask' image returns false positives.
gray_objects = cv2.cvtColor(np.array(objects), cv2.COLOR_BGR2GRAY)
_, contours, _ = cv2.findContours(gray_objects, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
# Set minimum rectangle area to be recognized as sherd
min_height_px = 30
min_width_px = 30
print("Found %d objects in this frame - may or may not all be sherds." %
(len(contours)))
print(
"Recognizing only rectangles larger than %d by %d pixels as sherds." %
(min_height_px, min_width_px))
# Construct Detection2DArray ROS message to contain all valid bounding boxes
BoundingBoxes_Array = Detection2DArray()
BoundingBoxes_Array.header = color_img.header # meta-data
# For each detected contour, find bounding box
for cnt in contours:
rect = cv2.minAreaRect(cnt)
# box dimensions in pixels
height_px = rect[1][1]
width_px = rect[1][0]
# If rectangle >= specified area...
if height_px >= min_height_px and width_px >= min_width_px:
print("This sherd's bounding box: " + str(rect))
# Extract (x,y) coordinates of box corners in order to draw rectangles, starting at "lowest" corner (largest y-coordinate) and moving CW
# Note: height is distance between 0th and 1st corner. Width is distance between 1st and 2nd corner.
box = cv2.boxPoints(rect)
box = np.int0(box)
print("Box corner coordinates: " + str(box))
# Debugging: draw bounding boxes around sherds
# Convert original RGB image to np.array to draw contours as boxes
rgb_contours = cv2.drawContours(np.array(rgb), [box], 0,
(255, 0, 0), 3)
# x,y of bounding box centers
x_center_pos = rect[0][0]
y_center_pos = rect[0][1]
# transform rotation angle to read between long side and y-axis
angle = rect[2]
if width_px < height_px:
angle += 180
else:
angle += 90
# transform rotation angle so that gripper never rotates more than 90deg CW or CCW
if angle > 90:
grip_angle = angle - 180
else:
grip_angle = angle
# Debugging: print
print("x center is " + str(x_center_pos))
print("y center is " + str(y_center_pos))
print("Box rotation angle is %f." % angle)
print("Gripper rotates %f degrees." % grip_angle)
plt.figure("Figure 2")
plt.imshow(rgb_contours)
# plt.imshow(img[:, :, ::-1])
plt.title("Bounding Box around Sherd")
plt.show()
# Construct a Detection2D() msg to add this bounding box to BoundingBoxes_Array
BoundingBoxes_Element = Detection2D()
BoundingBoxes_Element.header = BoundingBoxes_Array.header
BoundingBoxes_Element.bbox.center.x = x_center_pos
BoundingBoxes_Element.bbox.center.y = y_center_pos
BoundingBoxes_Element.bbox.center.theta = np.radians(
grip_angle) # angle converted to radians
BoundingBoxes_Element.bbox.size_x = width_px
BoundingBoxes_Element.bbox.size_y = height_px
BoundingBoxes_Array.detections.append(BoundingBoxes_Element)
else:
pass
# Publish the BoundingBoxes_Array message to the 'Bounding_Boxes' topic
boundbox_pub.publish(BoundingBoxes_Array)
print "BoundingBoxes_Array msg published to Bounding_Boxes."
def image_cb(self, data):
objArray = Detection2DArray()
time_i = time.time()
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(data, "bgr8")
cv_image = cv2.undistort(cv_image, self.cam_mtx, self.dst_mtx)
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
# ROI
ypixel_offset_l = image.shape[0]//2+50
ypixel_offset_u = image.shape[0] + 200
roi_image = image[ypixel_offset_l:ypixel_offset_u,:,:]
image_np = np.asarray(roi_image)
image_np_real = np.asarray(image)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np_real, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Each box represents a part of the image where a particular object was detected.
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
scores = detection_graph.get_tensor_by_name('detection_scores:0')
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
min_score = 0.2
# category_id = 3 # 3 for mobilnet/ 1 for kitte
category_id = 1
(boxes, scores, classes, num_detections) = self.sess.run([boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
(boxes, scores, classes) = self.filter_boxes(min_score, np.squeeze(boxes),
np.squeeze(scores), np.squeeze(classes).astype(np.int32), [category_id])
# ht, wdt, chn = image[ypixel_offset_l:ypixel_offset_u, :, :].shape
# (boxes[:,0], boxes[:,1], boxes[:,2], boxes[:,3]) = ((boxes[:,0]*ht+ypixel_offset_l), boxes[:,1]*image.shape[1],
# (boxes[:,2]*ht+ypixel_offset_l), boxes[:,3]*image.shape[1]) # [y_min, x_min, y_max, x_max]
objects=vis_util.visualize_boxes_and_labels_on_image_array(
image,
boxes,
classes,
scores,
category_index,
use_normalized_coordinates=True,
line_thickness=2)
############# new logic
ht, wdt, chn = image.shape
#############
objArray.detections = []
objArray.header = data.header
object_count = 1
bbox = []
x_mid = []
y_mid = []
image_loc = []
xy_range = []
for i in range(len(objects)):
object_count+=1
objArray.detections.append(self.object_predict(objects[i],data.header,image_np,cv_image))
bbox.append(objects[i][2]) # [y_min, x_min, y_max, x_max]
x_mid.append(int((bbox[i][3] - bbox[i][1])*wdt/2) + int((bbox[i][1])*wdt))
y_mid.append(int((bbox[i][2])*ht))
image_loc = np.hstack((np.array(x_mid), np.array(y_mid))).reshape((-1, 2))
for i in range(len(objects)):
xy_range.append(np.array(self.m.imageToVehicle(np.array([x_mid[i],y_mid[i]]))))
xy_range = np.array(xy_range).reshape((-1,2))
fps = 1/(time.time() - time_i)
#############################################
# Inserting range value into the images
# convert array into image to draw
font = cv2.FONT_HERSHEY_SIMPLEX
for i in range(len(objects)):
cv2.putText(image, "x= %.2f" % xy_range[i][0] + " y= %.2f" % xy_range[i][1],
(x_mid[i], y_mid[i]), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(image, "fps=%.2f" % fps,
(0, ht - 20), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
homography = self.m.tformToImage()
horizon = [int(homography[0, 0] / homography[0, 2]), int(homography[0, 1] / homography[0, 2])]
cv2.line(image, (0, horizon[1]), (wdt, horizon[1]), (255, 255, 255),
1) # horizon line ######################
cv2.line(image, (horizon[0], 0), (horizon[0], ht), (255, 255, 255),
1) # horizon line ######################
cv2.line(image, (0, ht),(horizon[0], horizon[1]), (255, 255, 255))
cv2.line(image, (wdt, ht), (horizon[0], horizon[1]), (255, 255, 255))
cv2.line(image, (0, ht//2), (wdt, ht//2), (0, 0, 0),
1)
msg = Lanepoints()
if len(objects):
msg.rows = image_loc.shape[0]
msg.cols = image_loc.shape[1]
msg.loc = xy_range.reshape((-1))
#############################################
# else:
# msg.rows = 1
# msg.cols = 2
# msg.loc = [-1,-1]
self.loc_pub.publish(msg)
self.object_pub.publish(objArray)
img=cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
###############################################
# # world cordinate plot
# fig = plt.figure()
# plt.plot(world[:, 1], world[:, 0], world[:, 3], world[:, 2])
# plt.plot(xy_range[:,1],xy_range[:,0],'ro',markersize=22)
# plt.xlim((5, -5))
# fig.canvas.draw()
# # convert canvas to image
# plot = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8,
# sep='')
# plot = plot.reshape(fig.canvas.get_width_height()[::-1] + (3,))
#
# # img is rgb, convert to opencv's default bgr
# plot = cv2.cvtColor(plot, cv2.COLOR_RGB2BGR)
# plt.close(fig)
# # plt.show(fig)
# cv2.fillPoly(img, [pts0], (0, 255, 0))
##############################################
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(image,"bgr8")
# map_out = self.bridge.cv2_to_imgmsg(plot,"bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)
def step(self):
stamp = super().step()
if not stamp:
return
# Publish camera data
if self._image_publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
image = self._wb_device.getImage()
if image is None:
return
# Image data
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = self._wb_device.getHeight()
msg.width = self._wb_device.getWidth()
msg.is_bigendian = False
msg.step = self._wb_device.getWidth() * 4
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = image
msg.encoding = 'bgra8'
self._image_publisher.publish(msg)
self.__message_info.header.stamp = Time(
seconds=self._node.robot.getTime()).to_msg()
self._camera_info_publisher.publish(self.__message_info)
if self._wb_device.hasRecognition(
) and (self._recognition_publisher.get_subscription_count() > 0 or
self._recognition_webots_publisher.get_subscription_count()
> 0):
self._wb_device.recognitionEnable(self._timestep)
objects = self._wb_device.getRecognitionObjects()
if objects is None:
return
# Recognition data
reco_msg = Detection2DArray()
reco_msg_webots = WbCameraRecognitionObjects()
reco_msg.header.stamp = stamp
reco_msg_webots.header.stamp = stamp
reco_msg.header.frame_id = self._frame_id
reco_msg_webots.header.frame_id = self._frame_id
for obj in objects:
# Getting Object Info
position = Point()
orientation = Quaternion()
position.x = obj.get_position()[0]
position.y = obj.get_position()[1]
position.z = obj.get_position()[2]
axangle = obj.get_orientation()
quat = axangle2quat(axangle[:-1], axangle[-1])
orientation.w = quat[0]
orientation.x = quat[1]
orientation.y = quat[2]
orientation.z = quat[3]
obj_model = obj.get_model().decode('UTF-8')
obj_center = [
float(i) for i in obj.get_position_on_image()
]
obj_size = [float(i) for i in obj.get_size_on_image()]
obj_id = obj.get_id()
obj_colors = obj.get_colors()
# Object Info -> Detection2D
reco_obj = Detection2D()
hyp = ObjectHypothesisWithPose()
hyp.id = obj_model
hyp.pose.pose.position = position
hyp.pose.pose.orientation = orientation
reco_obj.results.append(hyp)
reco_obj.bbox.center.x = obj_center[0]
reco_obj.bbox.center.y = obj_center[1]
reco_obj.bbox.size_x = obj_size[0]
reco_obj.bbox.size_y = obj_size[1]
reco_msg.detections.append(reco_obj)
# Object Info -> WbCameraRecognitionObject
reco_webots_obj = WbCameraRecognitionObject()
reco_webots_obj.id = obj_id
reco_webots_obj.model = obj_model
reco_webots_obj.pose.pose.position = position
reco_webots_obj.pose.pose.orientation = orientation
reco_webots_obj.bbox.center.x = obj_center[0]
reco_webots_obj.bbox.center.y = obj_center[1]
reco_webots_obj.bbox.size_x = obj_size[0]
reco_webots_obj.bbox.size_y = obj_size[1]
for i in range(0, obj.get_number_of_colors()):
color = ColorRGBA()
color.r = obj_colors[3 * i]
color.g = obj_colors[3 * i + 1]
color.b = obj_colors[3 * i + 2]
reco_webots_obj.colors.append(color)
reco_msg_webots.objects.append(reco_webots_obj)
self._recognition_webots_publisher.publish(reco_msg_webots)
self._recognition_publisher.publish(reco_msg)
else:
self._wb_device.recognitionDisable()
else:
self._wb_device.disable()
def image_cb(self, data):
objArray = Detection2DArray()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
time_start = time.clock()
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
image_np = np.asarray(image)
image_np_expanded = np.expand_dims(image_np, axis=0)
with self.detection_graph.as_default():
image_tensor = self.detection_graph.get_tensor_by_name(
'image_tensor:0')
# Each box represents a part of the image where a particular object was detected.
boxes = self.detection_graph.get_tensor_by_name(
'detection_boxes:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
scores = self.detection_graph.get_tensor_by_name(
'detection_scores:0')
classes = self.detection_graph.get_tensor_by_name(
'detection_classes:0')
num_detections = self.detection_graph.get_tensor_by_name(
'num_detections:0')
(boxes, scores, classes, num_detections) = self.sess.run(
[boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
objects = vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
self.category_index,
use_normalized_coordinates=True,
line_thickness=2)
time_elapsed = (time.clock() - time_start)
#print("time : ",time_elapsed)
objArray.detections = []
objArray.header = data.header
# print len(objects)
object_count = 1
self.flag = 0
self.tar_image = [
self.flag, self.size_img[0], self.size_img[1], self.pos_img[0],
self.pos_img[1]
]
for i in range(len(objects)):
#print ("Object Number: %d" %object_count)
object_count += 1
objArray.detections.append(
self.object_predict(objects[i], data.header, image_np,
cv_image))
if (objects[i][0] == 1):
self.flag = 1
dimensions = objects[i][2]
self.pos_img = [
int((dimensions[1] + dimensions[3]) * image_width / 2),
int((dimensions[0] + dimensions[2]) * image_height / 2)
]
self.size_img = [
int((dimensions[3] - dimensions[1]) * image_width),
int((dimensions[2] - dimensions[0]) * image_height)
]
self.tar_image = [
self.flag, self.size_img[0], self.size_img[1],
self.pos_img[0], self.pos_img[1]
]
print(round(1 / time_elapsed, 2), "[Hz]", self.tar_image)
self.detect_pub.publish(data=self.tar_image)
self.object_pub.publish(objArray)
img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(img, "bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)