def decode_image(self, frame, orig_msg):
msg = Image()
msg.data = frame.to_rgb().planes[0].to_bytes()
msg.width = frame.width
msg.height = frame.height
msg.step = frame.width * 3
msg.is_bigendian = 0
msg.encoding = 'rgb8'
msg.header = Header()
msg.header = orig_msg.header
self.pub.publish(msg)
def detect(self):
# call detectron2 model
outputs = self.predictor(self.img)
# process pointcloud to get 3D position and size
detections = self.process_points(outputs)
# publish detection result
obstacle_array = ObstacleArray()
obstacle_array.header = self.header
if self.detect_obj_pub.get_subscription_count() > 0:
obstacle_array.obstacles = detections
self.detect_obj_pub.publish(obstacle_array)
# visualize detection with detectron API
if self.detect_img_pub.get_subscription_count() > 0:
v = Visualizer(self.img[:, :, ::-1],
MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]),
scale=1)
out = v.draw_instance_predictions(outputs["instances"].to("cpu"))
out_img = out.get_image()[:, :, ::-1]
out_img_msg = Image()
out_img_msg.header = self.header
out_img_msg.height = out_img.shape[0]
out_img_msg.width = out_img.shape[1]
out_img_msg.encoding = 'rgb8'
out_img_msg.step = 3 * out_img.shape[1]
out_img_msg.data = out_img.flatten().tolist()
self.detect_img_pub.publish(out_img_msg)
def record_rgb():
"""Recording 2D visuals
"""
# Get the RGB image from the Kinect sensor
# Note: There are issues with wait_for_message not getting the
# most up to date message from /camera/rgb/image_raw.
# This is a hack to wait for the updated message
current_time = rospy.get_rostime()
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
while orig_rgb.header.stamp < current_time:
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
# Copy the recevied Image message to a new Image message
copy_rgb = Image()
copy_rgb.header = orig_rgb.header
copy_rgb.height = orig_rgb.height
copy_rgb.width = orig_rgb.width
copy_rgb.encoding = orig_rgb.encoding
copy_rgb.is_bigendian = orig_rgb.is_bigendian
copy_rgb.step = orig_rgb.step
copy_rgb.data = orig_rgb.data
# Publish the new Image to be saved with the specific name
rgb_pub = rospy.Publisher('rgb_image', Image, queue_size=1)
rospy.loginfo('-------------------------------------------------')
rospy.loginfo('Generating RGB')
rospy.loginfo('-------------------------------------------------')
rospy.sleep(3)
rgb_pub.publish(copy_rgb)
def StreamCameraSensor(self, request, context):
"""
Takes in a gRPC SensorStreamingRequest containing
all the data needed to create and publish a sensor_msgs/Image
ROS message.
"""
img_string = request.data
cv_image = np.fromstring(img_string, np.uint8)
# NOTE, the height is specifiec as a parameter before the width
cv_image = cv_image.reshape(request.height, request.width, 3)
cv_image = cv2.flip(cv_image, 0)
bgr_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
msg = Image()
header = std_msgs.msg.Header()
try:
# RGB
# msg = self.bridge.cv2_to_imgmsg(cv_image, 'rgb8')
# BGR
msg = self.bridge.cv2_to_imgmsg(bgr_image, 'bgr8')
header.stamp = rospy.Time.from_sec(request.timeStamp)
msg.header = header
except CvBridgeError as e:
print(e)
camera_pubs[request.frame_id].publish(msg)
return sensor_streaming_pb2.CameraStreamingResponse(success=True)
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 camera_node():
cap = cv2.VideoCapture(Configs.video_path)
fps = cap.get(cv2.CAP_PROP_FPS)
rospy.init_node('camera', anonymous=True)
pub = rospy.Publisher('/camera_image', Image, queue_size=10)
rate = rospy.Rate(Configs.camera_rate) # how many in one minute
i = 0
while not rospy.is_shutdown():
try:
ret, frame = cap.read()
i += 1
if not ret and Configs.video_start:
cap = cv2.VideoCapture(Configs.video_path)
print("*************** Restart ****************")
i = 0
time.sleep(2)
continue
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'camera'
image_temp.height = frame.shape[0]
image_temp.width = frame.shape[1]
image_temp.encoding = 'bgr8'
image_temp.data = frame.tostring()
image_temp.header = header
image_temp.step = frame.shape[1] * 3
pub.publish(image_temp)
rate.sleep()
print("Frame " + str(i) + ", Time " + str(int(i / fps)) +
" s, Image: " + str(frame.shape[0]) + "*" + str(frame.shape[1]))
except Exception as ex:
print(ex)
def hoof(self, image):
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
self.curr_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
self.count = self.count + 1
ff = self.curr_image
frame = cv2.resize(ff, None, fx=0.5, fy=0.5)
height, width = frame.shape[:2]
celh = int(height / self.h_num)
celw = int(width / self.w_num)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
hsv = np.zeros_like(frame)
hsv[:, :, 1] = 255
bgr = frame
d8 = np.zeros((self.h_num, self.w_num, 8))
d16 = np.zeros((self.h_num, self.w_num, 16))
if (self.count == 1): self.prev_image = gray
self.curr_image = gray
edge = cv2.Canny(self.curr_image, 300, 300)
flow = cv2.calcOpticalFlowFarneback(self.prev_image, self.curr_image,
None, 0.5, 3, 15, 3, 5, 1.2, 0)
mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1])
hsv[:, :, 0] = ang * 180 / np.pi / 2
hsv[:, :, 2] = cv2.min(mag * 30, 255)
a2 = np.floor((ang) * 15.9999995 / (2 * np.pi))
for j in range(0, self.h_num):
for i in range(0, self.w_num):
for k in range(0, 16):
aa = a2[(j - 1) * celh:j * celh - 1,
(i - 1) * celw:i * celw - 1]
mm = mag[(j - 1) * celh:j * celh - 1,
(i - 1) * celw:i * celw - 1]
d16[j, i, k] = np.sum(mm[np.where(aa == k)])
d8[j, i, 0] = d16[j, i, 0] + d16[j, i, 15]
for k in range(1, 8):
d8[j, i, k] = np.sum(d16[j, i, 2 * k - 1:2 * k])
g = hsv[:, :, 2]
hsv[:, :, 2] = cv2.max(g, edge)
hsv[:, :, 1] = 255 - edge
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
self.prev_image = self.curr_image
self.dd = np.append(self.dd, np.ravel(d8))
for i in range(1, self.h_num):
cv2.line(bgr, (0, celh * i), (width, celh * i), (0, 0, 255), 2)
for j in range(1, self.w_num):
cv2.line(bgr, (celw * j, 0), (celw * j, height), (0, 0, 255),
2)
img = cv2.cvtColor(bgr, 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 = image.header
self.hoof_pub.publish(image_out)
def timer_callback(self):
ret, frame = self.cap.read()
msg = Image()
header = Header()
header.frame_id = str(self.counter)
header.stamp = self.get_clock().now().to_msg()
if not ret:
self.get_logger().warning('Publishing RANDOM IMAGE "%s"' %
str(header.stamp))
frame = np.random.randint(255, size=(480, 640, 3), dtype=np.uint8)
if self.stream_video:
cv2.imshow('frame', frame)
cv2.waitKey(1)
frame = cv2.resize(frame,
dsize=(64, 64),
interpolation=cv2.INTER_CUBIC)
frame = np.flipud(frame)
frame = np.fliplr(frame)
msg.header = header
msg.height = frame.shape[0]
msg.width = frame.shape[1]
msg.encoding = "bgr8"
value = self.bridge.cv2_to_imgmsg(frame.astype(np.uint8))
msg.data = value.data
self.publisher_.publish(msg)
self.get_logger().info('Publishing Image "%s"' % str(header.stamp))
self.counter += 1
def _build_result_msg(self, msg, result):
result_msg = Result()
result_msg.header = msg.header
for i, (y1, x1, y2, x2) in enumerate(result['rois']):
box = RegionOfInterest()
box.x_offset = x1#np.asscalar(x1)
box.y_offset = y1#np.asscalar(y1)
box.height = y2-y1#np.asscalar(y2 - y1)
box.width = x2-x1#np.asscalar(x2 - x1)
result_msg.boxes.append(box)
class_id = result['class_ids'][i]
result_msg.class_ids.append(class_id)
class_name = self._class_names[class_id]
result_msg.class_names.append(class_name)
score = result['scores'][i]
result_msg.scores.append(score)
mask = Image()
mask.header = msg.header
mask.height = result['masks'].shape[0]
mask.width = result['masks'].shape[1]
mask.encoding = "mono8"
mask.is_bigendian = False
mask.step = mask.width
mask.data = (result['masks'][:, :, i] * 255).tobytes()
result_msg.masks.append(mask)
return result_msg
def image_cb(self, data):
#Convert Image to cv2 bgr8
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
image_np = np.asarray(image)
image_np_expanded = np.expand_dims(image_np, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
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')
#Run model detection on Image
(boxes, scores, classes, num_detections) = 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),
category_index,
use_normalized_coordinates=True,
line_thickness=2)
img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
image_out = self.bridge.cv2_to_imgmsg(img, "bgr8")
image_out.header = data.header
self.image_pub.publish(image_out)
def publish_processed_image(self):
# draw box and caption string
self.rgb = cv2.rectangle(self.rgb, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), (0,0,0xFF), 2)
self.rgb = cv2.putText(self.rgb, self.caption, (self.bbox[0]+2, self.bbox[1]-5), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0,0,0xFF), thickness=2)
#construct sensor_msgs/Image object
self.imgmsg.data = self.rgb.tobytes()
# publish frame
rospy.loginfo('publishing processed frame (with bbox) %s' % rospy.get_time())
self.rgb1_pub.publish(self.imgmsg)
# hack to republish frame
msk = np.zeros( (self.rgb.shape[0], self.rgb.shape[1]))
msk = cv2.rectangle(msk, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), 0xFF, -1)
rgb_msked = np.zeros_like(self.rgb)
rgb_msked[msk==0xFF, :] = self.rgb[msk==0xFF, :]
# construct new imgmsg
msg = Image()
msg.header = self.imgmsg.header
msg.height, msg.width, msg.step = self.imgmsg.height, self.imgmsg.width, self.imgmsg.step
msg.encoding = self.imgmsg.encoding
msg.is_bigendian = self.imgmsg.is_bigendian
msg.data = rgb_msked.tobytes()
self.rgb2_pub.publish(msg)
self.rate.sleep()
# release GPU memory for next data
del self.img
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 default(self, ci="unused"):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data["image"]
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += "/base_image"
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = "rgba8"
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data["intrinsic_matrix"]
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = "plumb_bob"
camera_info.K = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
]
camera_info.R = R
camera_info.P = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
Tx,
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
Ty,
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
0,
]
self.publish(image)
self.topic_camera_info.publish(camera_info)
def visualizacion_ros(self, img):
image_out = Image()
try:
image_out = bridge.cv2_to_imgmsg(img, 'bgr8')
except CvBridgeError as e:
print(e)
image_out.header = current_image.header
self.img_publisher.publish(image_out)
def copy_Image_empty_data(image_old):
image_new = Image()
image_new.header = copy(image_old.header)
image_new.height = copy(image_old.height)
image_new.width = copy(image_old.width)
image_new.encoding = copy(image_old.encoding)
image_new.is_bigendian = copy(image_old.is_bigendian)
image_new.step = copy(image_old.step)
return image_new
def record_rgb(object_name):
"""Recording 2D visuals
Spawn the object, record the visuals and delete the object
Parameters
----------
object_name : str
The name of the tool or object to be recorded
"""
# Spawn the object
rospack = rospkg.RosPack()
object_urdf_path = str(
rospack.get_path('affordance_experiment') + '/models/' + object_name +
'.urdf')
pitch = pi / 2
yaw = 0
if object_name == 'cylinder':
yaw = pi / 2
spawn_object(object_name, object_urdf_path, 0.0, 0.35, 1.05, pi / 2, pitch,
yaw)
# Wait for the object to rest
rospy.sleep(2)
# Get the RGB image from the Kinect sensor
# Note: There are issues with wait_for_message not getting the
# most up to date message from /camera/rgb/image_raw.
# This is a hack to wait for the updated message
current_time = rospy.get_rostime()
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
while orig_rgb.header.stamp < current_time:
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
# Copy the recevied Image message to a new Image message
copy_rgb = Image()
copy_rgb.header = orig_rgb.header
copy_rgb.height = orig_rgb.height
copy_rgb.width = orig_rgb.width
copy_rgb.encoding = orig_rgb.encoding
copy_rgb.is_bigendian = orig_rgb.is_bigendian
copy_rgb.step = orig_rgb.step
copy_rgb.data = orig_rgb.data
# Publish the new Image to be saved with the specific name
rgb_pub = rospy.Publisher(object_name + '_rgb', Image, queue_size=1)
rospy.loginfo('-------------------------------------------------')
rospy.loginfo('Generating RGB')
rospy.loginfo('-------------------------------------------------')
rospy.sleep(3)
rgb_pub.publish(copy_rgb)
# Delete object
delete_object(object_name)
rospy.sleep(3)
def callback_depth_img(data):
global depth_data, depth_img, msg_bool
depth_data = bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
depth_data.setflags(write=1)
depth_data[(depth_data < depth_range[0]) | (depth_data > depth_range[1])] = 0
depth_img = bridge.cv2_to_imgmsg(depth_data, encoding="passthrough")
depth_img = Image()
depth_img.header = copy(data.header)
depth_img = copy(data)
msg_bool[2] = True
def construct_image(header: Header, response: ImageResponse, encoding: str) -> Image:
msg = Image()
msg.header = header
msg.encoding = encoding
msg.height = response.height
msg.width = response.width
msg.data = response.image_data_uint8 if response.image_type != ImageType.DepthPlanar else response.image_data_float
msg.is_bigendian = 0
msg.step = response.width * 3
return msg
def imageCallback(self, msg):
'''!
Recebe a imagem, seg,emta e publica
'''
img = np.array(msg.data, dtype=np.uint8)
img = np.array(np.split(img, msg.height))
img = np.array(np.split(img, msg.width, axis=1))
img = np.rot90(img)
img = tf.keras.preprocessing.image.array_to_img(img)
image_array = tf.keras.preprocessing.image.img_to_array(img)
result = self.bodypix_model.predict_single(image_array)
mask = result.get_mask(threshold=0.75)
colored_mask = result.get_colored_part_mask(mask)
mask = mask.numpy().astype(np.uint8)
colored_mask = colored_mask.astype(np.uint8)
maskMsg = Image()
maskMsg._data = mask.flatten().tolist()
maskMsg.height = mask.shape[0]
maskMsg.width = mask.shape[1]
maskMsg.encoding = "8UC1"
maskMsg.is_bigendian = 0
maskMsg.step = maskMsg.width
colMsg = Image()
colMsg._data = colored_mask.flatten().tolist()
colMsg.height = colored_mask.shape[0]
colMsg.width = colored_mask.shape[1]
colMsg.encoding = "8UC3"
colMsg.is_bigendian = 0
colMsg.step = colMsg.width * 3
maskMsg.header = msg.header
colMsg.header = msg.header
self.pubMask.publish(maskMsg)
self.colMask.publish(colMsg)
def live_depth_analysis(self):
#Live stream video analysis
while not rospy.is_shutdown():
#print("Looking for a frame")
current_frame = self.frame
current_depth_frame = self.depth_frame
if (current_frame != [] and current_depth_frame != []):
#print("got frame")
#image = cv2.imread("/home/pmcrivet/catkin_ws/src/Mask_RCNN/script/images/frame0057.jpg")
#img = numpy.asarray(frame,dtype='uint8')
#cv2.imshow("image",image)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#print image.shape
#print image.dtype
#Dilatation effect on masks for better feature masking
dilatation = 10
threshold = 0.3
results = self.model.detect([current_frame],
dilatation,
threshold,
verbose=1)
r = results[0]
#selected_class = self.class_selection(r['masks'], r['class_ids'])
selected_class = r['masks']
#Comment for faster code run
#result_image = visualize_cv.cv_img_masked(current_frame, r['rois'], selected_class, r['class_ids'],
# self.class_names, r['scores'])
result_depth_image = visualize_cv.cv_depth_img_masked(
current_depth_frame, r['rois'], selected_class,
r['class_ids'], self.class_names, r['scores'])
DITS = Image()
#ITS = Image()
DITS = self.bridge.cv2_to_imgmsg(result_depth_image, '16UC1')
#ITS = self.bridge.cv2_to_imgmsg(result_image,'bgr8')
DITS.header = self.depth_msg_header
#ITS.header = self.msg_header
#self.image_pub.publish(ITS)
self.depth_image_pub.publish(DITS)
#print("Publishing img")
else:
print("Waiting for frame")
def publish_image(image, name):
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height = image.shape[0]
image_temp.width = image.shape[1]
image_temp.encoding = 'rgb8'
image_temp.data = np.array(image).tostring()
image_temp.header = header
image_temp.step = image.shape[0] * 3
image_pubulish.publish(image_temp)
def img_cb(self, data):
if self.stop_line == 0 or self.ncs_set == 0:
return
try:
print "start detection"
start = time.time()
#for SR300 image
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
#for compressed image
#np_arr = np.fromstring(data.data, np.uint8)
#self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img_gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY)
mser = cv2.MSER_create(20, 300, 20000, 0.25, 0.2, 200, 1.01, 0.003,
5)
regions, boxes = mser.detectRegions(img_gray)
hulls = [cv2.convexHull(p.reshape(-1, 1, 2)) for p in regions]
#cv2.polylines(gray_img, hulls, 1, (255, 0, 0), 2)
imgContours = self.cv_image
contour_list = []
point_list = [[0, 0]]
point_check = True
for i, contour in enumerate(hulls):
x, y, w, h = cv2.boundingRect(contour)
if 2 * h < w and h * w < 10000 and h * w > 1000:
point_check = True
for p in point_list:
#print p
if p[0] - 20 < x < p[0] + 20 and p[1] - 20 < y < p[
1] + 20:
point_check = False
if point_check == True:
point_list.append([x, y])
cv2.rectangle(imgContours, (x, y), (x + w, y + h),
(0, 255, 0), 3)
img_region = self.cv_image[y:y + h, x:x + w]
self.cv_img_crop.append(img_region)
#publish image_with_box
image_all_mser_image = Image()
image_all_mser_image.header = rospy.Time.now
image_all_mser_image = self.bridge.cv2_to_imgmsg(
imgContours, "bgr8")
self.image_pub.publish(image_all_mser_image)
print "detection time:", time.time() - start
#call prediction
self.cnn()
except CvBridgeError as e:
print(e)
def publish_raw_image(publisher, header, img):
if len(img.shape) < 3:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
msg_img = Image()
try:
msg_img = CvBridge().cv2_to_imgmsg(img, "bgr8")
except CvBridgeError as e:
print(e)
return
msg_img.header = header
publisher.publish(msg_img)
def publish_image(imgdata):
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height = IMAGE_HEIGHT
image_temp.width = IMAGE_WIDTH
image_temp.encoding = 'rgb8'
image_temp.data = np.array(imgdata).tostring()
#print(imgdata)
#image_temp.is_bigendian=True
image_temp.header = header
image_temp.step = 1241 * 3
image_pub.publish(image_temp)
def apriltag_callback(data):
if len(data.detections) >=1:
id = Image()
for detect in data.detections:
id.header = detect.pose.header
if(detect.id == 0):
id.step = detect.id
break
id.step = detect.id
#print id.data
#print " "
id_pub = rospy.Publisher("/id_output", Image, queue_size=1)
id_pub.publish(id)
def publish_image(imgdata):
image_temp = Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height = np.shape(imgdata)[0]
image_temp.width = np.shape(imgdata)[1]
image_temp.encoding = 'rgb8'
image_temp.data = np.array(imgdata).tostring()
#print(imgdata)
#image_temp.is_bigendian=True
image_temp.header = header
image_temp.step = np.shape(imgdata)[1] * 3
img_pub.publish(image_temp)
def map_image(values):
"""
Map the values generated with the hypothesis-ros image strategy to a rospy Image type.
"""
if not isinstance(values, _Image):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_image = Image()
ros_image.header = values.header
ros_image.height = values.height
ros_image.width = values.width
ros_image.encoding = values.encoding
ros_image.is_bigendian = values.is_bigendian
ros_image.data = values.data
return ros_image
def timer_callback(self):
image = cv2.imread(str(self.images[self.counter]))
msg = Image()
header = Header()
header.frame_id = str(self.counter)
header.stamp = self.get_clock().now().to_msg()
msg.header = header
msg.height = image.shape[0]
msg.width = image.shape[1]
msg.encoding = "bgr8"
value = self.bridge.cv2_to_imgmsg(image.astype(np.uint8))
msg.data = value.data
self.publisher_.publish(msg)
self.counter += 1
def publish_image(image_data, ts):
image = bridge.cv2_to_imgmsg(image_data, encoding='bgr8')
image_publisher.publish(image)
return
image = Image()
# ts = rospy.Time.from_sec(float(ts) / 1000)
ts = rospy.Time.now()
header = Header(stamp=ts)
header.frame_id = 'world'
image.height = image_data.shape[0]
image.width = image_data.shape[1]
image.encoding = 'bgr8'
image.data = np.array(image_data).tostring()
image.header = header
image_publisher.publish(image)
def image_rgb_callback(msg):
frame = bridge.imgmsg_to_cv2(msg)
global image_header
#UD = UserData()
#UD.type = 16
#UD.rows = frame.shape[0]
#UD.cols = frame.shape[0]
#UD.data = frame
IRGBTS = Image()
IRGBTS = msg
IRGBTS.encoding = "bgr8"
IRGBTS.header = image_header
#UD_pub.publish(UD)
IRGBTS_pub.publish(IRGBTS)
def img_cb(self, data):
print "image received"
if self.stop_line == 0:
return
try:
print "start detection"
###caffe.set_mode_gpu()
#self.start = data.header.stamp.secs
start = time.time()
#for SR300 image
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
#for compressed image
#np_arr = np.fromstring(data.data, np.uint8)
#self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img_gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY)
mser = cv2.MSER_create(25, 100, 250000, 0.25, 0.2, 200, 1.01,
0.003, 5)
regions, _ = mser.detectRegions(img_gray)
hulls = [cv2.convexHull(p.reshape(-1, 1, 2)) for p in regions]
#cv2.polylines(gray_img, hulls, 1, (255, 0, 0), 2)
imgContours = self.cv_image
contour_list = []
for i, contour in enumerate(hulls):
x, y, w, h = cv2.boundingRect(contour)
#repeat = self.checkContourRepeat(contour_list, x, y, w, h)
#img_region = img_cv[y:y+h, x:x+w]
if 2 * h < w and h * w < 10000 and h * w > 1000:
cv2.rectangle(imgContours, (x, y), (x + w, y + h),
(0, 255, 0), 3)
img_region = self.cv_image[y:y + h, x:x + w]
self.cv_img_crop.append(img_region)
self.cv_crop_region.append([x, y, w, h])
#publish image_with_box
image_all_mser_image = Image()
image_all_mser_image.header = rospy.Time.now
image_all_mser_image = self.bridge.cv2_to_imgmsg(
imgContours, "bgr8")
self.image_pub.publish(image_all_mser_image)
print "detection time:", time.time() - start
#call prediction
###self.cnn()
except CvBridgeError as e:
print(e)
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = self.encoding
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.D = [0]
camera_info.K = [
intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0],
intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1],
intrinsic[2][2]
]
camera_info.R = R
camera_info.P = [
intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0
]
if self.pub_tf:
self.publish_with_robot_transform(image)
else:
self.publish(image)
self.topic_camera_info.publish(camera_info)
def OccupancyGridToNavImage(self, grid_map):
img = Image()
img.header = grid_map.header
img.height = grid_map.info.height
img.width = grid_map.info.width
img.is_bigendian = 1
img.step = grid_map.info.width
img.encoding = "mono8"
maxindex = img.height*img.width
numpy.uint
for i in range(0, maxindex, 1):
if int(grid_map.data[i]) < 20:
#img.data[i] = "0"
data.append(0)
else:
img.data[i] = "255"
return imgding
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = self.encoding
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.D = [0]
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
if self.pub_tf:
self.publish_with_robot_transform(image)
else:
self.publish(image)
self.topic_camera_info.publish(camera_info)
def find_object(self):
with self.lock_depth:
image_data = self.image_data
depth_data = self.depth_data
pub_data = rospy.Publisher('/racecar/image/data', Float64MultiArray, queue_size=1)
arrayData = Float64MultiArray()
#print "received image"
if image_data:
newImage = Image()
newImage.encoding = image_data.encoding
newImage.header = image_data.header
newImage.is_bigendian = image_data.is_bigendian
newData = bytearray()
image = image_data.data
clusters = []
white = chr(255) *3
black = chr(0) * 3
red = chr(0) + chr(0) + chr(255)
blue = chr(255) + chr(0) * 2
#defining color search thresholds
redThreshold = 230
blueThreshold = 20
greenLowerThreshold = 50
greenUpperThreshold = 200
height = 0
starttime = time.time()
for h in xrange(0, image_data.height, self.resolution):
height += 1
for w in xrange(0, image_data.width, self.resolution):
index = w * 3 + image_data.width * 3 * h
b = ord(image[index])
g = ord(image[index + 1])
r = ord(image[index + 2])
#filtering for the color
if r > redThreshold and b < blueThreshold and g <= greenUpperThreshold and g >= greenLowerThreshold:
for char in white:
newData.append(char)
inCluster = False
for cluster in clusters:
if self.checkAdjacency((w, h), cluster):
cluster.add((w, h))
inCluster = True
if not inCluster:
newCluster = set()
newCluster.add((w, h))
clusters.append(newCluster)
else:
for char in black:
newData.append(char)
newImage.height = height
newImage.width = len(newData)/height/3
newImage.step = len(newData)/height
newImage.data = str(newData)
#print 'time taken: ', time.time() - starttime
clusters = self.reduceClusters(clusters)
#print len(clusters), float(len(candidates_x))/len(newData)
#print [len(cluster) for cluster in clusters]
if len(clusters) > 1:
print "multiple objects found"
print [len(cluster) for cluster in clusters]
if len(clusters):
publishableClusters = [max(clusters, key=lambda x:len(x))]
data_to_publish = []
for cluster in publishableClusters:
centerX = sum(x for (x, y) in cluster)/len(cluster)
centerY = sum(y for (x, y) in cluster)/len(cluster)
width = max(cluster, key=lambda (x, y):x)[0] - min(cluster, key=lambda (x, y):x)[0]
height = max(cluster, key=lambda (x, y):y)[1] - min(cluster, key=lambda (x, y):y)[1]
relativeLocation = float(centerX)/image_data.width - 0.5
relativeAngle = relativeLocation * 100 * math.pi / 180
data_to_publish += [centerX, centerY, width, height, relativeAngle]
#print 'center ', centerX, centerY, len(candidates_x)
#print 'distance ', depth1, depth2
#print 'dim ', height, width
'''depth_index = centerX * 2 + image_data.width * 2 * centerY
depth1 = ord(depth_data.data[depth_index])
depth2 = ord(depth_data.data[depth_index + 1])'''
arrayData.data = data_to_publish
else:
arrayData.data = [-1, -1, -1, -1, -1]
print 'no cone found'
pub_data.publish(arrayData)
#print "publishing"
pub_img = rospy.Publisher('/racecar/image/newImage', Image, queue_size=1)
pub_img.publish(newImage)
else:
arrayData.data = [-1, -1, -1, -1]
print 'image not ready'
def default(self, ci='unused'):
""" Publish the data of the Camera as a ROS Image message. """
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += '/base_image'
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = 'rgba8'
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ]
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
self.publish(image)
self.topic_camera_info.publish(camera_info)
