def img_callback(self, left_msg, right_msg):
rospy.loginfo(" Received Image message")
##Decoding image ##
left_np_arr = np.fromstring(left_msg.data, np.uint8)
left_image_np = cv2.imdecode(left_np_arr, cv2.IMREAD_GRAYSCALE)
right_np_arr = np.fromstring(right_msg.data, np.uint8)
right_image_np = cv2.imdecode(right_np_arr, cv2.IMREAD_GRAYSCALE)
##Contrast Enhancement ##
left_enhanced_img = self.histenhance.apply(left_image_np)
right_enhanced_img = self.histenhance.apply(right_image_np)
#### Create CompressedIamge ####
enhanced_left_msg = CompressedImage()
enhanced_left_msg.header.stamp = left_msg.header.stamp
enhanced_left_msg.format = "jpeg"
enhanced_left_msg.data = np.array(
cv2.imencode('.jpg', left_enhanced_img)[1]).tostring()
enhanced_right_msg = CompressedImage()
enhanced_right_msg.header.stamp = right_msg.header.stamp
enhanced_right_msg.format = "jpeg"
enhanced_right_msg.data = np.array(
cv2.imencode('.jpg', right_enhanced_img)[1]).tostring()
# Publish new image
self.left_image_pub.publish(enhanced_left_msg)
self.right_image_pub.publish(enhanced_right_msg)
def main():
cap = cv2.VideoCapture(int(sys.argv[1]))
cap.set(cv2.CAP_PROP_CONVERT_RGB, False)
image_pub_left = rospy.Publisher("/camera/left/compressed",
CompressedImage,
queue_size=10)
image_pub_right = rospy.Publisher("/camera/right/compressed",
CompressedImage,
queue_size=10)
rospy.init_node('image_feature', anonymous=True)
rate = rospy.Rate(40)
while not rospy.is_shutdown():
ret, frame = cap.read()
left = frame[:, :, 0]
right = frame[:, :, 1]
#### Create CompressedIamge ####
msg_left = CompressedImage()
msg_right = CompressedImage()
msg_left.header.stamp = rospy.Time.now()
msg_right.header.stamp = rospy.Time.now()
msg_left.format = "jpeg"
msg_right.format = "jpeg"
msg_left.data = np.array(cv2.imencode('.jpg', left)[1]).tostring()
msg_right.data = np.array(cv2.imencode('.jpg', right)[1]).tostring()
# Publish new image
image_pub_left.publish(msg_left)
image_pub_right.publish(msg_right)
rate.sleep()
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 _image_cb(self, imageMsg):
# Load in image
np_arr = np.fromstring(imageMsg.data, np.uint8)
original_img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# Process image
canny_mask = self._canny_filter(original_img)
#filtered_img = canny_mask
temp_img = cv2.bitwise_and(original_img, original_img, mask=canny_mask)
filtered_img = self._hsv_vision_image_filter(temp_img)
# Publish image after filtering
imgFMsg = CompressedImage()
imgFMsg.header.stamp = rospy.Time.now()
imgFMsg.format = "jpeg"
imgFMsg.data = np.array(cv2.imencode('.jpg',
filtered_img)[1]).tostring()
self.img_f_pub.publish(imgFMsg)
# detect bloks from image
final_img, poses = self._contour_based_detection(
temp_img, filtered_img)
# Publish object poses
poseMsg = BlockPose2DArray()
poseMsg.blocks = poses
self.pose_pub.publish(poseMsg)
# Publish final image
imgCMsg = CompressedImage()
imgCMsg.header.stamp = rospy.Time.now()
imgCMsg.format = "jpeg"
imgCMsg.data = np.array(cv2.imencode('.jpg', final_img)[1]).tostring()
self.img_c_pub.publish(imgCMsg)
def pub_img():
'''Callback function of subscribed topic.
Here images get converted and features detected'''
global stamp
# cap = cv2.VideoCapture(1)
pipe = rs.pipeline()
config = rs.config()
width = 640; height = 480;
config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30)
config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30)
profile = pipe.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
pub_rgb = rospy.Publisher("/output/image_raw/compressed_rgb", CompressedImage, queue_size=1)
pub_depth = rospy.Publisher("/output/image_raw/compressed_depth", CompressedImage, queue_size=1)
print('Hey! I started publishing images.')
while(True):
temp = pipe.wait_for_frames()
aligned_frames = align.process(temp)
aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image
color_frame = aligned_frames.get_color_frame()
if not aligned_depth_frame or not color_frame:
pass
buffer = np.zeros((height,width,4))
buffer[:,:,0:3] = (np.asanyarray(color_frame.get_data(),dtype=np.uint8))
buffer[:,:,3] = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint8)
#### Create CompressedImage ####
msg_rgb = CompressedImage()
msg_depth = CompressedImage()
msg_rgb.header.stamp = rospy.Time.now()
msg_depth.header.stamp = msg_rgb.header.stamp#rospy.Time.now()
msg_rgb.format = "rgb"#f'{stamp}'
msg_depth.format = "depth"
msg_rgb.data = np.array(cv2.imencode('.jpg', buffer[:,:,0:3])[1]).tostring()
msg_depth.data = np.array(cv2.imencode('.jpg', buffer[:,:,3])[1]).tostring()
# Publish depth frame
# pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1)
# r = rospy.Rate(10)
# try:
pub_rgb.publish(msg_rgb)
pub_depth.publish(msg_depth)
# r.sleep()
# except:
# print('Could not publish')
stamp += 1
if stamp >= 100000:
stamp = 0
def procesarImagen_(self, imagen_entrada):
np_array = np.fromstring(imagen_entrada.data, np.uint8)
np_image = cv2.imdecode(np_array, cv2.IMREAD_COLOR)
# Crear y pulicar imagen procesada
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', np_image)[1]).tostring()
self.pub_imagen_procesada.publish(msg)
# Detecciones
## Deteccion de bordes ##
imagen_bordes = cv2.Canny(np_image, 100, 200)
# Crear y pulicar imagen de bordes
msg2 = CompressedImage()
msg2.header.stamp = rospy.Time.now()
msg2.format = "jpeg"
msg2.data = np.array(cv2.imencode('.jpg', imagen_bordes)[1]).tostring()
self.pub_detecciones.publish(msg2)
## Deteccion de colores ##
imagen_color = cv2.resize(np_image, (128, 96),
interpolation=cv2.INTER_AREA)
hsv_img = cv2.cvtColor(imagen_color, cv2.COLOR_RGB2HSV)
#Se extraen por separado cada color en distintas mascaras
blanco = cv2.inRange(hsv_img, BLANCO_MIN, BLANCO_MAX)
amarillo = cv2.inRange(hsv_img, AMARILLO_MIN, AMARILLO_MAX)
rojo1 = cv2.inRange(hsv_img, ROJO_MIN, ROJO_MAX)
rojo2 = cv2.inRange(hsv_img, ROJO2_MIN, ROJO2_MAX)
rojo = rojo1 + rojo2
resultado = blanco + amarillo + rojo
#Resultados
resultado_amarillo = cv2.bitwise_and(imagen_color,
imagen_color,
mask=amarillo)
resultado_blanco = cv2.bitwise_and(imagen_color,
imagen_color,
mask=blanco)
resultado_rojo = cv2.bitwise_and(imagen_color, imagen_color, mask=rojo)
resultado_final = cv2.bitwise_and(imagen_color,
imagen_color,
mask=resultado)
# Crear y pulicar imagen de colores detectados
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg',
resultado_final)[1]).tostring()
self.pub_detecciones_color.publish(msg)
def callback_right_with_3D(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
self.right_header = ros_data.header.stamp.secs
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, 1)
# convert np image to grayscale
gray_image = cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY)
if self.right_header == self.left_header and self.group_4_recieved:
s_t = time.clock()
print self.right_header, self.left_header
self.group_4_recieved = False
left_gray = cv2.cvtColor(self.left_image, cv2.COLOR_BGR2GRAY)
group_4_out = self.group_4
# PointCloudPose()
# group_4_out.header = self.group_4.header
#std_msgs/Int16 pose_id
#std_msgs/Int16 pose_id_max
#sensor_msgs/CompressedImage image_left
#sensor_msgs/CompressedImage image_right
#geometry_msgs/Pose spin_center_pose
#sensor_msgs/PointCloud2 carmine_pointcloud
#geometry_msgs/Pose carmine_pose
#sensor_msgs/PointCloud2 bumblebee_pointcloud
#geometry_msgs/Pose bumblebee_pose_left
#geometry_msgs/Pose bumblebee_pose_right
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', left_gray)[1]).tostring()
# Publish new image
group_4_out.image_left = msg
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', gray_image)[1]).tostring()
# Publish new image
group_4_out.image_right = msg
#group_4_out.bumblebee_pointcloud = pcloud
self.pub_group.publish(group_4_out)
print "save time", time.clock() - s_t
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
if VERBOSE:
print 'received image of type: "%s"' % ros_data.format
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
# image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
if self.line == None:
self.line = Line(img)
time1 = rospy.Time.now()
try:
cte, outImg, roiImg = self.line.find_offset(img)
except:
cte = 0
outImg = img
roiImg = img
time2 = rospy.Time.now()
dur = time2 - time1
# if VERBOSE :
# print '%s detector found: %s points in: %s sec.'%(method,
# len(featPoints),time2-time1)
# Debug info
rospy.loginfo("Cross track error: %s", cte)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', outImg)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', roiImg)[1]).tostring()
# Publish new image
self.image_roi_pub.publish(msg)
twist = Twist()
twist.linear.x = 1.0
twist.angular.z = -cte * 0.015
self.twist_pub.publish(twist)
def _Img_callback(ros_data):
global center, size, flage, pub_detections
np_arr = np.fromstring(ros_data.data, np.uint8)
frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
try:
# while True:
# window_name = 'Image'
if flage == True:
flage = False
for ii in range(len(center[0, :])):
left = int((center[0, ii] - size[0, ii] / 2.0))
right = int((center[0, ii] + size[0, ii] / 2.0))
top = int((center[1, ii] - size[1, ii] / 2.0))
bottom = int((center[1, ii] + size[1, ii] / 2.0))
start_point = (left, top)
end_point = (right, bottom)
color = (255, 0, 0)
thickness = 2
image = cv2.rectangle(frame, start_point, end_point, color,
thickness)
circle_thickness = -1
radius = 15
circle_center = (int(center[0, ii]), int(center[1, ii]))
image = cv2.circle(frame, circle_center, radius, color,
circle_thickness)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image)[1]).tostring()
pub_detections.publish(msg)
rospy.sleep(0.2)
# cv2.imshow(window_name, image)
# cv2.waitKey(1)
# #cv2.destroyAllWindows()
# break
else:
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
pub_detections.publish(msg)
except:
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
pub_detections.publish(msg)
def publish(self, raw_img):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode(".jpg", raw_img)[1]).tostring()
# TODO compile sensor_msg error , use String instead
self.image_pub.publish(msg.data)
def __init__(self):
# Get the ~private namespace parameters from command line or launch file.
self.UDP_IP = rospy.get_param('~UDP_IP', '192.168.50.37') #get_ip.get_local()
#if self.UDP_IP != 'localhost':
# self.UDP_IP = int(self.UDP_IP)
self.UDP_PORT = int(rospy.get_param('~UDP_PORT', '49155'))
# Create a dynamic reconfigure server.
#self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
# create ros::Publisher to send Odometry messages
cam_pub = rospy.Publisher('image_raw/compressed', CompressedImage)
#crappy test
t = True
while not rospy.is_shutdown():
#crappy test continued
if t:
print "Camera Running"
t = False
udp_string = self.receive_packet()
jpg_string = self.parse_string(udp_string)
cam_msg = CompressedImage()
cam_msg.header.stamp = rospy.Time.now() #YOYOYOYO - should be from robot time
cam_msg.format = "jpeg"
cam_msg.data = jpg_string
cam_pub.publish(cam_msg)
def cv2_to_compressed_imgmsg(cv_image):
# Create CompressedImage
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', cv_image)[1]).tostring()
return msg
def grabAndPublish(self,stream,publisher):
while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown():
yield stream
# Construct image_msg
# Grab image from stream
stamp = rospy.Time.now()
stream.seek(0)
stream_data = stream.getvalue()
# Generate compressed image
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = stream_data
image_msg.header.stamp = stamp
image_msg.header.frame_id = self.frame_id
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
rospy.sleep(rospy.Duration.from_sec(0.001))
def ros_compressed_from_numpygray(np_arr):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
np_arr = 255*(1.0*np_arr/np.max(np_arr))
msg.data = np.array(cv2.imencode('.jpg', np.array(np_arr, dtype = np.uint8))[1]).tostring()
return msg
def webcam_run():
"""
Run people detection on webcam
and publish the data
"""
video_capture = cv2.VideoCapture(0)
ros_publisher = rospy.Publisher("/people_detection", String, queue_size=10)
processed_image = rospy.Publisher("/processed_image",
CompressedImage,
queue_size=10)
people_detect = PeopleDetection()
while (True):
# Get image from webcam
ret, frame = video_capture.read()
details = people_detect.detect(frame)
if len(details) == 0:
continue
rospy.loginfo(details)
ros_publisher.publish(json.dumps(details))
# frame = cv2.resize(frame, (0,0), fx=0.3, fy=0.3)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
processed_image.publish(msg)
def publish_debug_image(self, img, pub):
img_msg = CompressedImage()
img_msg.header.stamp = rospy.Time.now()
img_msg.format = ".jpeg"
img_msg.data = RiptideVision().compressed_img_msg_data(".jpeg", img)
pub.publish(img_msg)
def callback(self, ros_data):
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
#(rows,cols,channels) = image_np.shape
#image_np = image_np[0:64, 0:480]
#equ = cv2.equalizeHist(image_np)
#res = np.hstack((image_np,equ))
#### Feature detectors using CV2 ####
# "","Grid","Pyramid" +
# "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF", "GridFAST"
#method = "GridFAST"
#feat_det = cv2.FeatureDetector_create(method)
#time1 = time.time()
# convert np image to grayscale
#featPoints = feat_det.detect( cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
#time2 = time.time()
#if VERBOSE :
# print '%s detector found: %s points in: %s sec.'%(method, len(featPoints),time2-time1)
#for featpoint in featPoints:
# x,y = featpoint.pt
# cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def zed_camera():
rospy.init_node("camera_publisher", anonymous=True)
rate = rospy.Rate(10)
device = rospy.get_param("~device")
ns = rospy.get_namespace()
cap = cv2.VideoCapture(device)
publisher_left = rospy.Publisher(ns + "left_image", CompressedImage, queue_size=10)
publisher_right = rospy.Publisher(ns + "right_image", CompressedImage, queue_size=10)
while not rospy.is_shutdown():
_, frame = cap.read()
left_right_image = np.split(frame, 2, axis=1)
for i in range(2):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "png"
msg.data = np.array(cv2.imencode('.png', cv2.resize(left_right_image[i],(672,188)))[1]).tostring()
[publisher_left,publisher_right][i].publish(msg)
rate.sleep()
def get_next_stream(self):
"""Generates a new 'stream' sequence and then waits for data.
Once the data is received, it is Published and a new 'stream' sequence is generated."""
while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown(
):
yield self.stream # publish a new 'stream' sequence to capture the next image into
self.stream.seek(
0) # reset the stream to extract the captured image
# Generate compressed image
image_msg = CompressedImage()
image_msg.header.stamp = rospy.Time.now()
image_msg.header.frame_id = self.frame_id
image_msg.format = "jpeg"
image_msg.data = self.stream.getvalue()
self.pub_img.publish(image_msg)
# Clear stream
self.stream.seek(0)
self.stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %
(self.node_name))
self.has_published = True
rospy.sleep(rospy.Duration.from_sec(0.001))
def image_publisher(device_id=0, frame_rate=10, width=320, height=240, brightness=0.5):
topic_name = "python_image" + str(device_id)
node_name = 'python_image_publisher' + str(device_id)
pub1 = rospy.Publisher(topic_name, Image, queue_size=10)
pub2 = rospy.Publisher(topic_name + "/compressed", CompressedImage, queue_size=10)
rospy.init_node(node_name)
rate = rospy.Rate(frame_rate)
cap = cv2.VideoCapture(device_id)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
cap.set(cv2.CAP_PROP_FPS, frame_rate)
cap.set(cv2.CAP_PROP_BRIGHTNESS, brightness)
# cap.set(cv2.CAP_PROP_WHITE_BALANCE, False)
br = CvBridge()
while not rospy.is_shutdown():
status, img = cap.read()
if status == True:
pub1.publish(br.cv2_to_imgmsg(img))
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg" # "jpeg" or "png"
msg.data = np.array(cv2.imencode(".jpg", img)[1]).tostring()
pub2.publish(msg)
rate.sleep()
def main():
image_pub = rospy.Publisher("/output/image_raw/compressed",
CompressedImage,
queue_size=1)
rospy.init_node('ROSPiCam')
rate = rospy.Rate(100) # 10hz
# Set publisher node to 15 Hz
# Publisher node loop
while not rospy.is_shutdown():
msg = CompressedImage()
msg.format = "jpeg"
# Open new BytesIO stream
stream = io.BytesIO()
# Capture frame from camera
camera.capture(stream, format='jpeg')
# camera.capture("/home/pi/temp.jpg
msg.header.stamp = rospy.Time.now()
msg.data = np.array(Image.open(stream)).tostring()
# Publish image
image_pub.publish(msg)
# Close stream
stream.close()
rate.sleep()
def handle_pkt(pkt=None):
ts_begin_loc = pkt.find(begin_timestamp_marker)
ts_end_loc = pkt.find(end_timestamp_marker)
if ts_begin_loc == -1 or ts_end_loc == -1:
#something's fishy, discard jpeg
print "JPEG discarded, malformed data"
return
ts = float(pkt[ts_begin_loc+len(begin_timestamp_marker):ts_end_loc])
# if ts > last_ts:
# last_ts = ts
# else:
# return
pkt = pkt[ts_end_loc+len(end_timestamp_marker):]
print "{} bytes of JPEG recvd".format(len(pkt))
msg = CompressedImage()
msg.header.stamp = rospy.Time(tango_clock_offset + float(ts))
msg.header.frame_id = camera_name
msg.data = pkt
msg.format = 'jpeg'
pub_camera.publish(msg)
def subscribe_callback(data): # called whenever get subscribed data
# get current time
rospytimenow = rospy.Time.now()
# CompressedImage (ros msg) to cv2
np_arr = np.fromstring(data.data, np.uint8)
img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# detect door
pred, img_with_bboxresult = detector.detect_image(img)
# convert img_with_bboxresult to ros msg
img_msg = CompressedImage()
img_msg.header.stamp = rospytimenow
img_msg.format = "jpg"
img_msg.data = np.array(cv2.imencode('.jpg',
img_with_bboxresult)[1]).tostring()
pub_img.publish(img_msg)
# convert detection result (pred) to ros msg
det_msg = BoundingBoxes()
det_msg.header.stamp = rospytimenow
bounding_boxes = []
for i, (x1, y1, x2, y2, conf) in enumerate(pred):
one_det_msg = BoundingBox()
one_det_msg.id = int(i)
one_det_msg.xmin = int(x1)
one_det_msg.xmax = int(x2)
one_det_msg.ymin = int(y1)
one_det_msg.ymax = int(y2)
one_det_msg.probability = conf
bounding_boxes.append(one_det_msg)
det_msg.bounding_boxes = bounding_boxes
pub_det.publish(det_msg)
def pubIm(im):
img_show = np.array(im, np.uint8)
msg1 = CompressedImage()
msg1.format = "jpeg"
msg1.header.stamp = rospy.Time.now()
msg1.data = np.array(cv2.imencode('.jpg', img_show)[1]).tostring()
pub.publish(msg1)
def grabAndPublish(self,stream,publisher):
# Grab image from stream
stream.seek(0)
img_data = stream.getvalue()
if self.uncompress:
# Publish raw image
data = np.fromstring(img_data, dtype=np.uint8)
image = cv2.imdecode(data, 1)
image_msg = self.bridge.cv2_to_imgmsg(image)
else:
# Publish compressed image only
image_msg = CompressedImage()
image_msg.data = img_data
image_msg.format = "jpeg"
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
def _publish_img(self, obs, wait_for_callback=False):
"""
Publishes the image to the compressed_image topic, which triggers the lane following loop
For training: if wait_for_callback is true, we'll also publish the camera info, and then wait until the action
has been calculated
"""
# XXX: make this into a function (there were a few of these conversions around...)
img_msg = CompressedImage()
time = rospy.get_rostime()
img_msg.header.stamp.secs = time.secs
img_msg.header.stamp.nsecs = time.nsecs
img_msg.format = "jpeg"
contig = cv2.cvtColor(np.ascontiguousarray(obs), cv2.COLOR_BGR2RGB)
img_msg.data = np.array(cv2.imencode(".jpg", contig)[1]).tostring()
self.callback_processed = False
self.latest_obs = obs
self.cam_pub.publish(img_msg)
if wait_for_callback:
self.r = rospy.Rate(1000)
self._publish_info()
while not self.callback_processed:
from time import sleep
sleep(
0.0001
) # TODO if you, yes YOU!, dear coder, have a better idea to synchronize the callback
def grabAndPublish(self, stream, publisher):
while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown(
):
yield stream
# Construct image_msg
# Grab image from stream
stamp = rospy.Time.now()
stream.seek(0)
stream_data = stream.getvalue()
# Generate compressed image
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = stream_data
image_msg.header.stamp = stamp
image_msg.header.frame_id = self.frame_id
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %
(self.node_name))
self.has_published = True
rospy.sleep(rospy.Duration.from_sec(0.001))
def kcfTracker(self, frame):
# setting initial value of bbox_obtained
bbox_obtained = self.bbox
# if inital flag is true then set the initial frame in the tracker
if (self.init_flag):
track_status = self.tracker.init(frame, self.bbox)
self.init_flag = False
if not track_status:
rospy.logwarn("Unable to initalize tracker")
else:
# updating the frames in tracker
track_status, bbox_obtained = self.tracker.update(frame)
if track_status:
p1 = (int(bbox_obtained[0]), int(bbox_obtained[1]))
p2 = (int(bbox_obtained[0] + bbox_obtained[2]),
int(bbox_obtained[1] + bbox_obtained[3]))
cv2.rectangle(frame, p1, p2, (255, 0, 0), 2, 1)
else:
cv2.putText(frame, "Tracking failure detected", (60, 80),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
# Publish new feature image
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
self.image_pub.publish(msg)
def stream(self):
url = 'http://%s:%s@%s/mjpg/video.mjpg' % (self.axis.username, self.axis.password, self.axis.hostname)
#url = url + "?resolution=%dx%d" % (self.axis.width, self.axis.height)
fp = urllib.urlopen(url)
while True:
boundary = fp.readline()
header = {}
while 1:
line = fp.readline()
if line == "\r\n": break
line = line.strip()
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis_frame_id
msg.format = "jpeg"
msg.data = img
self.axis.pub.publish(msg)
"""
def callback1(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
#print('received image of type: "%s"' % ros_data.encoding)
#### 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:
#try:
# image_np = self.bridge.imgmsg_to_cv2(ros_data, "u16")
#except CvBridgeError as e:
# print(e)
image_np = self.bridge.imgmsg_to_cv2(ros_data)
rmax = np.quantile(image_np, 0.95)
image_u8 = image_np.copy()
image_u8[image_u8 > rmax] = rmax
image_u8 = (image_u8 * (256. / rmax)).astype('uint8')
im_color = cv2.applyColorMap(image_u8, cv2.COLORMAP_JET)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', im_color)[1]).tobytes()
# Publish new image
self.image_pub1.publish(msg)
def grabAndPublish(self,stream,publisher):
# Grab image from stream
stream.seek(0)
img_data = stream.getvalue()
if self.uncompress:
# Publish raw image
data = np.fromstring(img_data, dtype=np.uint8)
image = cv2.imdecode(data, 1)
image_msg = self.bridge.cv2_to_imgmsg(image)
else:
# Publish compressed image only
image_msg = CompressedImage()
image_msg.data = img_data
image_msg.format = "jpeg"
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
# Clear stream
stream.seek(0)
stream.truncate()
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
def startCapturing(self):
rospy.loginfo("[%s] Start capturing." % (self.node_name))
while not self.is_shutdown and not rospy.is_shutdown():
#gen = self.grabAndPublish(self.stream,self.pub_img)
try:
image = self.camera.read()
self.stream = np.array(bgr8_to_jpeg(image)).tostring()
stamp = rospy.Time.now()
stream_data = self.stream
# Generate compressed image
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = stream_data
image_msg.header.stamp = stamp
image_msg.header.frame_id = self.frame_id
self.pub_img.publish(image_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %
(self.node_name))
self.has_published = True
rospy.sleep(rospy.Duration.from_sec(0.001))
except StopIteration:
pass
#print("updating framerate")
self.capture_fps = self.capture_fps
self.update_framerate = False
#self.camera.close()
rospy.loginfo("[%s] Capture Ended." % (self.node_name))
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
img = self.bridge.imgmsg_to_cv2(ros_data)
if VERBOSE:
pass
# print('received image of type: "%s"' % ros_data.format)
#### direct conversion to CV2 ####
image_np = np.array(img[:, :, :3], 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:
print(image_np.shape)
res_img = self.find_ars(image_np)
print(res_img.shape)
cv2.imshow("found stones", res_img)
if cv2.waitKey(1) & 0xFF == ord('q'):
pass
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def imageCallback(msg):
global imageSum, count, zero
# rospy.loginfo("imagaCallback called")
#### direct conversion to CV2 ####
np_arr = np.fromstring(msg.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_float = image_np.astype(np.float)
count += 1
if count == 1:
imageSum = image_float
# imageSum = imageSum
else:
beta = 1 / count
alpha = 1 - beta
# print "for count = %s, (alpha, beta) = (%s, %s)"%(count, alpha, beta)
print "----"
imageSum = cv2.addWeighted(imageSum, alpha, image_float, beta, 0)
imageAverage = imageSum
print "first imageSum pixel: (%s, %s, %s)" % tuple(imageSum[0][0])
# print "averaged"
image_np_out = array(imageAverage)
#### Create CompressedIamge ####
out = CompressedImage()
out.header.stamp = rospy.Time.now()
out.format = "jpeg"
np_arr_out = cv2.imencode('.jpg', image_np_out)[1]
out.data = np.array(np_arr_out).tostring()
# Publish new image
publisher.publish(out)
def publish(self, raw_img):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = 'jpeg'
msg.data = np.array(cv2.imencode('.jpg', raw_img)[1]).tostring()
#TODO compile sensor_msg error , use String instead
self.image_pub.publish(msg.data)
def publish_image(self):
""" publish images by publishing_names """
# # rospy.loginfo('publishers: {}, {}'%(self.publishers, self.publishers['original']))
# msg = CompressedImage()
# msg.header.stamp = rospy.Time.now()
# msg.format = "jpeg"
# for src in self.publishing_names:
# # rospy.loginfo("data on : " + str(src))
# msg.data = np.array(cv2.imencode(".jpg", self.images[src])[0]).tostring()
# self.publishers[str(src)].publish(msg)
# rospy.loginfo("published")
# for src in self.publishing_names:
# msg = CvImage(Header(), "bgr8", self.images[src]).toImageMsg()
# rospy.loginfo(msg)
# try:
# cv_image = self.bridge.cv2_to_imgmsg(self.images['original'], "bgr8")
# self.images['original'].publish(cv_image)
# for src in self.publishing_names:
# cv_image = self.bridge.imgmsg_to_cv2(self.images[src], "bgr8")
# print("successfully published")
# except CvBridgeError as e:
# print(e)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
for str in self.publishing_names:
if not str in self.images: continue
msg.data = np.array(cv2.imencode('.jpg',
self.images[str])[1]).tostring()
self.publishers[str].publish(msg)
def displayvideo(image_np, mask, cnts, center):
pts = deque(maxlen=64)
# Overlays the identified contours on the received image frame.
if len(cnts) > 0:
cv2.drawContours(image_np, [cnts], -1, (0, 255, 0), 3)
epsilon = 0.08 * cv2.arcLength(cnts, True)
approx = cv2.approxPolyDP(cnts, epsilon, True)
cv2.drawContours(image_np, [approx], -1, (255, 255, 0), 3)
((x, y), radius) = cv2.minEnclosingCircle(cnts)
if radius > 10:
cv2.circle(image_np, (int(x), int(y)), int(radius), (0, 255, 255),
2)
cv2.circle(image_np, center, 5, (0, 0, 255), -1)
# Marks the center point of the contours on the frame.
pts.appendleft(center)
for i in xrange(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5)
cv2.line(image_np, pts[i - 1], pts[i], (0, 0, 255), thickness)
# Constructs compressed image msg to be sent to roscore.
msg_vid = CompressedImage()
msg_vid.header.stamp = rospy.Time.now()
msg_vid.format = "jpeg"
msg_vid.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
pub_debug_vid.publish(msg_vid)
def pubOriginal(self, msg):
compressed_img_msg = CompressedImage()
compressed_img_msg.format = "png"
compressed_img_msg.data = np.array(cv2.imencode(".png", self.testImageOrig)[1]).tostring()
# compressed_img_msg.header.stamp = msg.header.stamp
# compressed_img_msg.header.frame_id = msg.header.frame_id
self.pub_raw.publish(compressed_img_msg)
def sendTestImage(self):
img_name = '0'+str(self.sent_image_count)+'_orig.'+self.test_image_format
test_img = cv2.imread(self.test_image_path+img_name)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = self.test_image_format
msg.data = np.array(cv2.imencode('.'+self.test_image_format, test_img)[1]).tostring()
self.pub_test_image.publish(msg)
def stream(self):
"""
Reads and process the streams from the camera
"""
try:
#If flag self.enable_auth is 'True' then use the user/password to access the camera. Otherwise use only self.url
if self.enable_auth:
req = urllib2.Request(self.url, None, {"Authorization": self.auth })
else:
req = urllib2.Request(self.url)
fp = urllib2.urlopen(req, timeout = self.timeout)
#fp = urllib2.urlopen(req)
while self.run_camera and not rospy.is_shutdown():
boundary = fp.readline()
header = {}
while not rospy.is_shutdown():
line = fp.readline()
#print 'read line %s'%line
if line == "\r\n": break
line = line.strip()
#print line
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
#print 'Length = %d'%content_length
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis_frame_id
msg.format = "jpeg"
msg.data = img
# publish image
self.compressed_image_publisher.publish(msg)
cimsg = self.cinfo.getCameraInfo()
cimsg.header.stamp = msg.header.stamp
cimsg.header.frame_id = self.axis_frame_id
# publish camera info
self.caminfo_publisher.publish(cimsg)
#print self.real_fps
self.last_update = datetime.datetime.now()
self.error_reading = False
self.image_pub_freq.tick()
except urllib2.URLError,e:
self.error_reading = True
self.error_reading_msg = e
rospy.logerr('Axis:stream: Camera %s (%s:%d). Error: %s'%(self.camera_id, self.hostname, self.camera_number, e))
def pubOrig(self, args=None):
image_msg_out = CompressedImage()
image_msg_out.header.stamp = rospy.Time.now()
image_msg_out.format = "png"
image_msg_out.data = np.array(cv2.imencode('.png',
self.original_image)[1]).tostring()
self.pub_image.publish(image_msg_out)
rospy.loginfo("Publishing original image")
def pubImage(self):
filename = '0'+str(self.num_sent)+'_orig.png'
image = cv2.imread(self.im_dir+filename)
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = 'png'
msg.data = np.array(cv2.imencode('.png', image)[1]).tostring()
self.pub_fake_images.publish(msg)
rospy.loginfo("[%s] Image %s published" %(self.node_name,filename))
def cv2_to_compressed_ros(self, img):
try:
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', img)[1]).tostring()
return msg
except CvBridgeError as e:
rospy.logerr(e)
def publish_image(pub, image_handle):
image = CompressedImage()
image.data = image_handle.read()
image.format = 'jpeg'
image.header.seq = 0
image.header.stamp.secs = math.floor(time.time())
image.header.stamp.nsecs = 0
image.header.frame_id = "0"
pub.publish(image)
def processImage(self, image_msg):
image_cv = self.bridge.imgmsg_to_cv2(image_msg)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_cv)[1]).tostring()
self.pub_image_comp.publish(msg)
def stream(self):
url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname
url = url + "?fps=0&resolultion=%dx%d" % (self.axis.width, self.axis.height)
rospy.logdebug('opening ' + str(self.axis))
# only try to authenticate if user/pass configured
if self.axis.password != '' and self.axis.username != '':
# create a password manager
password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm()
# Add the username and password, use default realm.
top_level_url = "http://" + self.axis.hostname
password_mgr.add_password(None, top_level_url,
self.axis.username,
self.axis.password)
handler = urllib2.HTTPBasicAuthHandler(password_mgr)
# create "opener" (OpenerDirector instance)
opener = urllib2.build_opener(handler)
# ...and install it globally so it can be used with urlopen.
urllib2.install_opener(opener)
fp = urllib2.urlopen(url)
while True:
boundary = fp.readline()
header = {}
while 1:
line = fp.readline()
if line == "\r\n": break
line = line.strip()
parts = line.split(": ", 1)
header[parts[0]] = parts[1]
content_length = int(header['Content-Length'])
img = fp.read(content_length)
line = fp.readline()
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.axis.frame_id
msg.format = "jpeg"
msg.data = img
self.axis.pub.publish(msg)
cimsg = self.axis.cinfo.getCameraInfo()
cimsg.header.stamp = msg.header.stamp
cimsg.header.frame_id = self.axis.frame_id
cimsg.width = self.axis.width
cimsg.height = self.axis.height
self.axis.caminfo_pub.publish(cimsg)
def flipImageNP(self,msg):
np_array = np.fromstring(msg.data, np.uint8)
image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR)
#flip_arr = np.fliplr(np_arr)
imageflip_np=cv2.flip(image_np,1)
flip_im = CompressedImage()
flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
flip_im.header.stamp = msg.header.stamp
self.pub_image.publish(flip_im)
def callback(self,ros_data):
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_np = cv2.flip(image_np, self.flip_code)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
self.image_pub.publish(msg)
def publish(self):
img = cv2.imread("/home/qian/dev/catkin_ws/src/smart_ar/script/aurora.png", cv2.CV_LOAD_IMAGE_COLOR)
M = cv2.getRotationMatrix2D((img.shape[1]/2,img.shape[0]/2),self.ga*100, 1.0 / self.dist)
img = cv2.warpAffine(img,M,(img.shape[1], img.shape[0]))
bridge = CvBridge()
msg = CompressedImage()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode(".jpg",img)[1]).tostring()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'image_compressed_publisher'
msg.header.seq = self.seq
self.seq += 1
self.imgPublisher.publish(msg)
print self.seq, 'image published'
def cropCallback(msg):
time_stamp = msg.header.stamp
img_bytes = BytesIO(msg.data)
img = PIL.Image.open(img_bytes)
resize = (80, 60)
img = img.resize(resize, resample=PIL.Image.BILINEAR)
np_img = np.asarray(img)
msg = CompressedImage()
msg.header.stamp = time_stamp
msg.format = "png"
msg.data = np.array(cv2.imencode('.png', np_img)[1]).tostring()
publish_cropimg.publish(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 receive_image(self):
"""
Retrives image data coming from the rover and then
transforms it to numpy and string data for further
manupulation with OpenCV and Kivy and returns the final
data
"""
data = ''
ldata = array.array('c')
image_buffer = CompressedImage()
image_buffer.format = "jpeg"
# start = ''
found_start = False
found_end = False
start_pos = 0 #current position in ldata
#position of the end of frame in the current array of data
end_pos = 0
while (not found_end):
data = self.video_socket.recv(self.max_tcp_buffer)
if(data == ''):
continue
#check for the message start token 'MO_V'
for i in range(0, len(data)-2):
if (data[i:(i+4)] == 'MO_V'):
if not found_start:
found_start = True
start_pos = i
#crop the data only include stuff after the
#start token.
data = data[start_pos:len(data)]
break
elif not found_end:
found_end = True
end_pos = i
break
#if you have found the start but not the end (in the
#middle of a image frame)
if (found_start and not found_end):
#add the recent data to ldata
ldata.extend(list(data))
if found_end:
ldata.extend(list(data[0:end_pos]))
data = ''
l_len = len(ldata)
#### Create CompressedIamge ####
# convert to numpy to use with OpenCV, etc.
image_buffer.header.stamp = rospy.Time.now()
image_buffer.data = np.array(ldata[36:l_len]).tostring()
return image_buffer
def cbImage(self, img):
# decode image
np_arr = np.fromstring(img.data, np.uint8)
color = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
self.overlayInfo(color)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', color)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
def publishTestImage(self):
original_img = cv2.imread(self.original_image_file, cv2.CV_LOAD_IMAGE_COLOR)
#### Create CompressedIamge ####
out = CompressedImage()
out.header.stamp = rospy.Time.now()
out.format = "png"
np_arr_out = cv2.imencode('.png', original_img)[1]
out.data = np.array(np_arr_out).tostring()
# Publish new image
rospy.sleep(1)
self.pub_mirror.publish(out)
rospy.loginfo("[%s] published image"%self.node_name)
def cam_callback(msg):
np_arr = np.fromstring(msg.data, np.uint8)
# Decode
image_rgb = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
# Mirror image laterally
image_rgb_rev = image_rgb[:,::-1,:]
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_rgb_rev)[1]).tostring()
publisher.publish(msg)
rospy.loginfo("Re Published ")
def set_scene(index):
image = CompressedImage()
# image.header.frame_id = '/odom_combined'
# If you ever want to drive the base, you will want to use the line above, or /map or something.
# Using /base_link means the robot will always stay exactly in the middle of the backdrop.
image.header.frame_id = '/base_link'
image.format = 'jpeg'
try:
scene = scenes[index]
except TypeError:
index = main_window.scene_comboBox.findText(index)
scene = scenes[index]
image.data = open(scene).read()
image.header.stamp = rospy.Time.now()
backdrop_publisher.publish(image)
def _publish_image(self, header, image, timestamp):
"""Publish JPG image as a ROS message.
:param header: The HTTP-like header read from the stream.
:type header: dict
:param image: The video frame in JPG.
:type image: :py:obj:`basestring`
:param timestamp: The time when the frame was captured (or its best estimation).
:type timestamp: :py:class:`rospy.Time`
"""
msg = CompressedImage()
msg.header.stamp = timestamp
msg.header.frame_id = self._axis._frame_id
msg.format = "jpeg"
msg.data = image
self._axis._video_publisher.publish(msg)
def on_image(self, ros_data):
compressed_in = np.fromstring(ros_data.data, np.uint8)
image_in = cv2.imdecode(compressed_in, cv2.CV_LOAD_IMAGE_COLOR)
if self.prvs == None :
self.prvs = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height))
else:
next = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height))
image_out = cv2.resize(image_in, (width,height))
if self.should_be_controlled() : self.pub.publish(self.apply_control(next, image_out))
self.prvs = next
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_out)[1]).tostring()
self.ipub.publish(msg)
