def image_callback(self, ros_data):
if self.input_compressed:
np_arr = np.fromstring(ros_data.data, np.uint8)
img_input = cv2.imdecode(np_arr, 1)
else:
img_input = bridge.imgmsg_to_cv2(ros_data)
with Tick('prediction'):
img_resized = self.warpper.resize_keeping_aspect_ratio(img_input)
semantic = self.warpper.predict(
self.warpper.project(img_resized))[0]
detections = self.warpper.resize_back(
voc.get_label_colormap(semantic))
print(voc.semantic_report(semantic), end='')
overlap = cv2.addWeighted(detections, 0.5, img_input, 0.5, 20)
cv2.imshow('overlap', overlap)
cv2.waitKey(1)
if self.output_compressed:
msg = CompressedImage()
msg.header = ros_data.header
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', detections)[1]).tostring()
self.pub.publish(msg)
else:
msg = Image()
msg.header = ros_data.header
msg.data = detections.tostring()
self.pub.publish(msg)
def compress_image(msg):
"""
Take a sensor_msgs/Image
return a sensor_msgs/CompressedImage
"""
# fromstring is not available on precise python-imaging v 1.1.7
# but has been deprecated in v 2.0 using it for now while we need
# to support precise
# img = Image.frombytes("L", (msg.width, msg.height), msg.data,
# 'raw', "L", 0, 1)
img = Image.fromstring("L", (msg.width, msg.height), msg.data, 'raw', "L",
0, 1)
# img.show()
output = BytesIO()
img.save(output, format='png')
output.flush()
output_msg = CompressedImage()
output_msg.header = msg.header
output_msg.format = 'png'
output_msg.data = output.getvalue()
encoding_msg = String()
encoding_msg.data = msg.encoding
return output_msg, encoding_msg
def _callback(self, ros_data):
''' callback function for mask detection '''
rospy.loginfo("[DETECTOR] image received!")
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
#image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
rospy.loginfo(image_np.shape)
boxes, image = self.inference(image_np, target_shape=(360, 360))
# output_info.append([class_id, conf, xmin, ymin, xmax, ymax])
face_msg = MaskFrame()
face_msg.header.stamp = rospy.Time.now()
for bbox in boxes:
face_msg.bboxes.append(BoxToMessage(bbox))
class_id, conf, xmin, ymin, xmax, ymax = bbox
color = (0, 255, 0) if class_id == 0 else (255, 0, 0)
cv2.rectangle(image_np, (xmin, ymin), (xmax, ymax), color, 2)
cv2.putText(image_np, "%s: %.2f" % (self.id2class[class_id], conf), (xmin + 2, ymin - 2),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, color)
#### Create Compressed Image ####
msg = CompressedImage()
#msg.header.stamp = rospy.Time.now()
msg.header = ros_data.header
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
self.face_pub.publish(face_msg)
def publish_image(self, stream):
image_msg = CompressedImage()
image_msg.header = Header()
image_msg.format = 'jpeg'
image_msg.data = stream.getvalue()
self.image_pub.publish(image_msg)
return rospy.is_shutdown()
def transform_image_and_repub(self, msg):
img = cv2.imdecode(np.fromstring(msg.data, np.uint8), cv2.IMREAD_COLOR)
img_rotated = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE)
repub_msg = CompressedImage()
repub_msg.header = msg.header
repub_msg.format = "jpeg"
repub_msg.data = np.array(cv2.imencode('.jpg',
img_rotated)[1]).tostring()
self.image_repub.publish(repub_msg)
def transform_image_and_repub(self, msg):
img = cv2.imdecode(np.fromstring(msg.data, np.uint8), cv2.IMREAD_COLOR)
img_rotated = np.flip(np.transpose(img, (1, 0, 2)),
0)[CHOP_TOP:-CHOP_BOT]
repub_msg = CompressedImage()
repub_msg.header = msg.header
repub_msg.format = "jpeg"
repub_msg.data = np.array(cv2.imencode('.jpg',
img_rotated)[1]).tostring()
self.image_repub.publish(repub_msg)
def callback(self, image, index):
self.buffer.paste(PilImage.open(io.BytesIO(bytearray(image.data))), ((640*index), 0))
#Compose a ROS message of the buffer
outgoing = CompressedImage()
outgoing.header = image.header #TODO this is bullshit
outgoing.format = "png"
img_byte_arr = io.BytesIO()
self.buffer.save(img_byte_arr, format="png")
outgoing.data = img_byte_arr.getvalue()
# Make it everybody's problem
self.merged_pub.publish(outgoing)
def __init__(self):
self.node_name = "Virtual Mirror Nbuckman Tester"
self.fake_pub_image = rospy.Publisher("/ernie/camera_node/image/compressed", CompressedImage,queue_size=1)
img_file = rospy.get_param("~img_file","/home/noambuckman/test_images/01_horz.jpg")
rospy.loginfo(img_file)
im_cv = cv2.imread(img_file)
img_str = cv2.imencode('.jpg', im_cv)[1].tostring()
pub_im=CompressedImage()
pub_im.header=rospy.Time.now()
pub_im.data = img_str
#pub_im.data = np.array(cv2.imencode('.jpg', im_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
#flip_im.header.stamp = msg.header.stamp
self.fake_pub_image.publish(pub_im)
def subscribe(self, message):
file_path = get_latest_modified_file_path("/tmp/ros/camera/")
header = Header()
header.seq = self.sequence
self.sequence += 1
header.stamp.secs = int(os.path.getmtime(file_path))
header.frame_id = "0"
response = CompressedImage()
response.format = "jpeg"
f = open(file_path)
response.data = f.read()
f.close()
response.header = header
rospy.loginfo('PUBLISH JPG IMAGE')
self.stream_topic.publish(response)
def visualize_cb(self, event):
if (not self.visualize or self.img is None or self.header is None
or self.bboxes is None or self.labels is None
or self.scores is None):
return
with self.lock:
vis_img = self.img.copy()
header = copy.deepcopy(self.header)
bboxes = self.bboxes.copy()
labels = self.labels.copy()
scores = self.scores.copy()
# bbox
cmap = matplotlib.cm.get_cmap('hsv')
n = max(len(bboxes) - 1, 10)
for i, (bbox, label, score) in enumerate(zip(bboxes, labels, scores)):
rgba = np.array(cmap(1. * i / n))
color = rgba[:3] * 255
label_text = '{}, {:.2f}'.format(self.label_names[label], score)
cv2.rectangle(vis_img, (bbox[1], bbox[0]), (bbox[3], bbox[2]),
color,
thickness=3)
cv2.putText(vis_img,
label_text, (bbox[1], max(bbox[0] - 10, 0)),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
color,
thickness=2)
if self.pub_image.get_num_connections() > 0:
vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8')
# BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA
vis_msg.step = int(vis_msg.step)
vis_msg.header = header
self.pub_image.publish(vis_msg)
if self.pub_image_compressed.get_num_connections() > 0:
# publish compressed http://wiki.ros.org/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber # NOQA
vis_compressed_msg = CompressedImage()
vis_compressed_msg.header = header
vis_compressed_msg.format = "jpeg"
vis_img_rgb = cv2.cvtColor(vis_img, cv2.COLOR_BGR2RGB)
vis_compressed_msg.data = np.array(
cv2.imencode('.jpg', vis_img_rgb)[1]).tostring()
self.pub_image_compressed.publish(vis_compressed_msg)
def __init__(self):
self.node_name = "Virtual Mirror Nbuckman Tester"
self.fake_pub_image = rospy.Publisher(
"/ernie/camera_node/image/compressed",
CompressedImage,
queue_size=1)
img_file = rospy.get_param(
"~img_file", "/home/noambuckman/test_images/01_horz.jpg")
rospy.loginfo(img_file)
im_cv = cv2.imread(img_file)
img_str = cv2.imencode('.jpg', im_cv)[1].tostring()
pub_im = CompressedImage()
pub_im.header = rospy.Time.now()
pub_im.data = img_str
#pub_im.data = np.array(cv2.imencode('.jpg', im_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
#flip_im.header.stamp = msg.header.stamp
self.fake_pub_image.publish(pub_im)
def visualize_cb(self, event):
if (not self.visualize or self.img is None or self.header is None
or self.bboxes is None or self.labels is None
or self.scores is None):
return
with self.lock:
img = self.img.copy()
header = copy.deepcopy(self.header)
bboxes = self.bboxes.copy()
labels = self.labels.copy()
scores = self.scores.copy()
fig = plt.figure(tight_layout={'pad': 0})
ax = plt.Axes(fig, [0., 0., 1., 1.])
ax.axis('off')
fig.add_axes(ax)
vis_bbox(img.transpose((2, 0, 1)),
bboxes,
labels,
scores,
label_names=self.label_names,
ax=ax)
fig.canvas.draw()
w, h = fig.canvas.get_width_height()
vis_img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8)
vis_img.shape = (h, w, 3)
fig.clf()
plt.close()
if self.pub_image.get_num_connections() > 0:
vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8')
# BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA
vis_msg.step = int(vis_msg.step)
vis_msg.header = header
self.pub_image.publish(vis_msg)
if self.pub_image_compressed.get_num_connections() > 0:
# publish compressed http://wiki.ros.org/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber # NOQA
vis_compressed_msg = CompressedImage()
vis_compressed_msg.header = header
vis_compressed_msg.format = "jpeg"
vis_img_rgb = cv2.cvtColor(vis_img, cv2.COLOR_BGR2RGB)
vis_compressed_msg.data = np.array(
cv2.imencode('.jpg', vis_img_rgb)[1]).tostring()
self.pub_image_compressed.publish(vis_compressed_msg)
def visualize_cb(self, event):
if (not self.visualize or self.img is None or self.header is None
or self.label is None):
return
with self.lock:
img = self.img.copy()
header = copy.deepcopy(self.header)
label = self.label.copy()
fig = plt.figure(tight_layout={'pad': 0})
ax = plt.subplot(1, 1, 1)
ax.axis('off')
ax, legend_handles = vis_semantic_segmentation(
img.transpose((2, 0, 1)),
label,
label_names=self.label_names,
alpha=0.7,
all_label_names_in_legend=True,
ax=ax)
ax.legend(handles=legend_handles, bbox_to_anchor=(1, 1), loc=2)
fig.canvas.draw()
w, h = fig.canvas.get_width_height()
vis_img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8)
vis_img.shape = (h, w, 3)
fig.clf()
plt.close()
if self.pub_image.get_num_connections() > 0:
vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8')
# BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA
vis_msg.step = int(vis_msg.step)
vis_msg.header = header
self.pub_image.publish(vis_msg)
if self.pub_image_compressed.get_num_connections() > 0:
# publish compressed http://wiki.ros.org/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber # NOQA
vis_compressed_msg = CompressedImage()
vis_compressed_msg.header = header
vis_compressed_msg.format = "jpeg"
vis_img_rgb = cv2.cvtColor(vis_img, cv2.COLOR_BGR2RGB)
vis_compressed_msg.data = np.array(
cv2.imencode('.jpg', vis_img_rgb)[1]).tostring()
self.pub_image_compressed.publish(vis_compressed_msg)
def _send_image(self, img_data, encoding):
# send uncompressed
# if (self._img_pub.get_num_connections() > 0):
if (self._compressed_img_pub.get_num_connections() > 0
or self._img_pub.get_num_connections() > 0):
msg = self._cv_bridge.cv2_to_imgmsg(img_data, encoding=encoding)
# send uncompressed
if (self._img_pub.get_num_connections() > 0):
self._img_pub.publish(msg)
# send compressed
if (self._compressed_img_pub.get_num_connections() > 0):
compressed_msg = CompressedImage()
compressed_msg.header = msg.header
compressed_msg.format = 'jpeg'
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 95]
result, data = cv2.imencode('.jpg', img_data, encode_param)
if result:
compressed_msg.data = data.flatten().tolist()
self._compressed_img_pub.publish(compressed_msg)
def visualize_cb(self, event):
if (not self.visualize or self.img is None or self.points is None
or self.visibles is None):
return
with self.lock:
vis_img = self.img.copy()
header = copy.deepcopy(self.header)
points = self.points.copy()
visibles = self.visibles.copy()
# keypoints
cmap = matplotlib.cm.get_cmap('hsv')
for i, (point, visible) in enumerate(zip(points, visibles)):
n = len(point) - 1
for j, (pp, vis) in enumerate(zip(point, visible)):
if vis:
rgba = np.array(cmap(1. * j / n))
color = rgba[:3] * 255
cv2.circle(vis_img, (pp[1], pp[0]), 8, color, thickness=-1)
if self.pub_image.get_num_connections() > 0:
vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8')
# BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA
vis_msg.step = int(vis_msg.step)
vis_msg.header = header
self.pub_image.publish(vis_msg)
if self.pub_image_compressed.get_num_connections() > 0:
# publish compressed http://wiki.ros.org/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber # NOQA
vis_compressed_msg = CompressedImage()
vis_compressed_msg.header = header
vis_compressed_msg.format = "jpeg"
vis_img_rgb = cv2.cvtColor(vis_img, cv2.COLOR_BGR2RGB)
vis_compressed_msg.data = np.array(
cv2.imencode('.jpg', vis_img_rgb)[1]).tostring()
self.pub_image_compressed.publish(vis_compressed_msg)
def loop(self):
r = rospy.Rate(
self.max_detection_rate
) # Hz to check if there is a new detection being requested
while not rospy.is_shutdown():
if self.current_image_msg is not None:
# copy to prevent being changed from the callback
img_msg = deepcopy(self.current_image_msg)
self.current_image_msg = None
if not self.using_compressed:
try:
image = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
return
else: # compressed, cant use cv_bridge in python
# get image from topic
np_arr = np.fromstring(img_msg.data, np.uint8)
# get OpenCV version
(major_version, _minor_version,
_) = cv2.__version__.split(".")
if major_version >= 3:
image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
else:
image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
publish_image = self.image_pub.get_num_connections() > 0
initOnly = False
detections = performDetect(image, self.threshold,
self.cfg_file, self.weights_file,
self.data_file, initOnly)
recog_obj_array = RecognizedObjectArray()
image_h, image_w, _channels = image.shape
if self.show_only_best_detection:
best_object = None
best_confidence = -1
best_obj_details = -1
for obj_id, detection in enumerate(detections):
data = detection.split()
class_name, confidence = data[:2]
x_center, y_center, width, height = [
int(float(val)) for val in data[2:]
]
confidence = float(confidence)
# draw one detection on the image to be published
if publish_image:
color = (255, 0, 0) # blue
image = self.draw_info(image, class_name, confidence,
x_center, y_center, width,
height, color)
if self.debug:
rospy.loginfo(
'''\n'''
'''class_name: %s\n'''
'''confidence: %s\n'''
'''x_center: %s\n'''
'''y_center: %s\n'''
'''width: %s\n'''
'''height: %s''', class_name, confidence,
x_center, y_center, width, height)
recog_obj = RecognizedObject()
recog_obj.class_name = class_name
recog_obj.confidence = confidence
x_offset = min(max(0, x_center - width / 2), image_w)
y_offset = min(max(0, y_center - height / 2), image_h)
recog_obj.bounding_box = RegionOfInterest(
x_offset=x_offset,
y_offset=y_offset,
width=min(width, image_w - x_offset),
height=min(height, image_h - y_offset))
if self.debug:
rospy.loginfo("recog_obj.bounding_box")
rospy.loginfo(recog_obj.bounding_box)
if self.show_only_best_detection:
if confidence > best_confidence:
best_object = recog_obj
best_confidence = confidence
best_obj_details = [
class_name, confidence, x_center, y_center,
width, height
]
else:
recog_obj_array.objects.append(recog_obj)
if self.show_only_best_detection and best_object is not None:
recog_obj_array.objects.append(best_object)
class_name, confidence, x_center, y_center, width, height = tuple(
best_obj_details)
color = (0, 255, 0) # green
image = self.draw_info(image, class_name, confidence,
x_center, y_center, width, height,
color)
recog_obj_array_stamped = RecognizedObjectArrayStamped()
recog_obj_array_stamped.header = img_msg.header
recog_obj_array_stamped.objects = recog_obj_array
self.result_publisher.publish(recog_obj_array_stamped)
# actually publish the image
if publish_image:
# Create CompressedImage and publish
msg = CompressedImage()
msg.header = img_msg.header
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg',
image)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
r.sleep()
rospy.init_node('static_virtual_camera')
# get image_trasport before ConnectionBasedTransport subscribes ~input
transport_hint = rospy.get_param('~image_transport', 'raw')
rospy.loginfo("Using transport {}".format(transport_hint))
pub_img = rospy.Publisher('~image_color', Image, queue_size=1)
pub_img_compressed = rospy.Publisher('~image_color/compressed',
CompressedImage,
queue_size=1)
pub_info = rospy.Publisher('~camera_info', CameraInfo, queue_size=1)
bridge = cv_bridge.CvBridge()
imgmsg = bridge.cv2_to_imgmsg(img, encoding='bgr8')
imgmsg.header.frame_id = 'static_virtual_camera'
compmsg = CompressedImage()
compmsg.header = imgmsg.header
compmsg.format = "jpeg"
compmsg.data = np.array(
cv2.imencode('.jpg', cv2.cvtColor(img,
cv2.COLOR_BGR2RGB))[1]).tostring()
info_msg = CameraInfo()
info_msg.header.frame_id = imgmsg.header.frame_id
height, width = img.shape[:2]
info_msg.height = height
info_msg.width = width
rospy.Timer(rospy.Duration(0.1), timer_cb)
rospy.spin()
def send(self, cpath, dpath, jp, time_ns, ind, cpose):
if self.args.time:
time_ns = int(time.time() * 1e9)
sec, nsec = divmod(time_ns, int(1e9))
now = genpy.Time(secs=sec, nsecs=nsec)
hdr = Header(stamp=now, frame_id=self.args.camera_frame)
# index of image in original list
img_index = ind
if self.args.print_file:
filename = os.path.splitext(os.path.basename(cpath))[0]
self.new_file = (self.last_filename != filename)
if self.new_file:
print("img(" + str(img_index) + "):", filename)
self.last_filename = filename
# T_cw
camera_pose = TransformStamped()
camera_pose.header.stamp = hdr.stamp
camera_pose.header.frame_id = self.args.camera_frame
camera_pose.child_frame_id = self.args.base_frame
# invert camera pose
camera_pose_mat = np.dot(translation_matrix(cpose[:3]),
quaternion_matrix(
[cpose[4], cpose[5], cpose[6],
cpose[3]])) # xyzw
camera_pose_mat = np.linalg.inv(camera_pose_mat)
p = translation_from_matrix(camera_pose_mat)
q = quaternion_from_matrix(camera_pose_mat)
camera_pose.transform.translation = Vector3(x=p[0], y=p[1],
z=p[2]) # xyzw
camera_pose.transform.rotation = Quaternion(x=q[0],
y=q[1],
z=q[2],
w=q[3])
self.broadcaster.sendTransform(camera_pose)
if self.vicon_poses is not None and img_index < len(self.vicon_poses):
# px,py,pz,qw,qx,qy,qz
vicon_pose = self.vicon_poses[img_index]
vicon_tf = TransformStamped()
vicon_tf.transform.translation = Vector3(x=vicon_pose[0],
y=vicon_pose[1],
z=vicon_pose[2])
vicon_tf.transform.rotation = Quaternion(w=vicon_pose[3],
x=vicon_pose[4],
y=vicon_pose[5],
z=vicon_pose[6])
vicon_tf.header.frame_id = self.args.base_frame
vicon_tf.child_frame_id = "vicon_end_effector"
self.broadcaster.sendTransform(vicon_tf)
with open(cpath, mode='rb') as f:
cimg_buf = f.read()
msg_cimg_compr = CompressedImage()
msg_cimg_compr.header = hdr
msg_cimg_compr.format = "png"
msg_cimg_compr.data = cimg_buf
if self.args.raw:
cimg = cv2.imdecode(np.fromstring(cimg_buf, dtype=np.uint8),
cv2.IMREAD_UNCHANGED) # bgr8
msg_cimg = self.cvbridge.cv2_to_imgmsg(cimg, encoding="bgr8")
with open(dpath, mode='rb') as f:
dimg_buf = f.read()
msg_dimg_compr = CompressedImage()
msg_dimg_compr.header = hdr
msg_dimg_compr.format = "16UC1; png compressed"
msg_dimg_compr.data = dimg_buf
msg_dimg_compr_depth = CompressedImage()
msg_dimg_compr_depth.header = hdr
msg_dimg_compr_depth.format = "16UC1; compressedDepth"
# prepend 12bit fake header
msg_dimg_compr_depth.data = "000000000000" + dimg_buf
if self.args.raw:
dimg = cv2.imdecode(np.fromstring(dimg_buf, dtype=np.uint8),
cv2.IMREAD_UNCHANGED)
msg_dimg = self.cvbridge.cv2_to_imgmsg(dimg, encoding="16UC1")
try:
self.pub_clock.publish(clock=now)
if self.args.raw:
self.pub_colour.publish(msg_cimg)
self.pub_depth.publish(msg_dimg)
self.pub_colour_compressed.publish(msg_cimg_compr)
self.pub_depth_compressed.publish(msg_dimg_compr)
self.pub_depth_compressed_depth.publish(msg_dimg_compr_depth)
self.ci.header = hdr
self.pub_ci_colour.publish(self.ci)
self.pub_ci_depth.publish(self.ci)
self.pub_joints.publish(name=self.joint_names,
position=jp,
header=hdr)
self.pub_id.publish(data=img_index)
except rospy.ROSException:
pass
