def publish(self):
print("pi4 initiated, time: " + str(time.time()))
while True:
if rospy.is_shutdown():
break
#with picamera.PiCamera() as camera:
# camera.resolution=(1280,720)
## camera.framerate=Fraction(1,6)
## camera.shutter_speed=6000000
## camera.exposure_mode='off'
## camera.iso=800
# camera.start_preview()
# time.sleep(2)
# with picamera.array.PiRGBArray(camera) as stream:
# camera.capture(stream, format='bgr')
# image=stream.array
# bridge=CvBridge()
# msg=bridge.cv2_to_compressed_imgmsg(image)
# pub.publish(msg)
bridge = CvBridge()
_, img = self.cam.read()
_, img2 = self.cam2.read()
msg1 = bridge.cv2_to_compressed_imgmsg(img)
msg2 = bridge.cv2_to_compressed_imgmsg(img2)
self.pub.publish(msg1)
self.pub2.publish(msg2)
self.rate.sleep()
class image_capture:
'''
Image capture and ROS publisher
'''
def __init__(self, node_name="image_capture"):
'''
Initialize the image capturer
Parameters:
N/A
Returns:
N/A
'''
self.node_name = node_name
rospy.init_node(node_name)
cam_topic = rospy.get_namespace() + "cv2_capture"
self.image_pub = rospy.Publisher(cam_topic, CompressedImage, queue_size=1)
self.bridge = CvBridge()
self.cv_capture = cv2.VideoCapture('v4l2src device=/dev/video0 ! video/x-raw, framerate=30/1, width=640, height=360 ! videoconvert ! appsink')
#self.cv_capture = cv2.VideoCapture(0)
print('Initiated cam connection')
def run(self):
try:
while not rospy.is_shutdown():
ret, cv_img = self.cv_capture.read()
if ret: # Publish the resulting frame
try:
self.image_pub.publish(self.bridge.cv2_to_compressed_imgmsg(cv_img))
except CvBridgeError as e:
print(e)
except rospy.ROSInterruptException:
pass
def start(self):
env = gym_duckietown.simulator.Simulator(
seed=123, # random seed
map_name="loop_empty",
max_steps=500001, # we don't want the gym to reset itself
domain_rand=0,
camera_width=640,
camera_height=480,
accept_start_angle_deg=4, # start close to straight
full_transparency=True,
distortion=True,
)
bridge = CvBridge()
while (True):
action = self.action
observation, reward, done, misc = env.step(action)
env.render()
if done:
env.reset()
#Generate the compressed image
image = cv2.cvtColor(numpy.asarray(observation), cv2.COLOR_RGB2BGR)
image_message = bridge.cv2_to_compressed_imgmsg(image,
dst_format='jpeg')
#Publish the compressed image
self.pub_img.publish(image_message)
def spin(self):
time.sleep(1.0)
started = time.time()
counter = 0
cvim = numpy.zeros((480, 640, 3), numpy.uint8)
ball_xv = 10
ball_yv = 10
ball_x = 100
ball_y = 100
cvb = CvBridge()
while not rospy.core.is_shutdown():
cvim.fill(0)
cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
ball_x += ball_xv
ball_y += ball_yv
if ball_x in [10, 630]:
ball_xv = -ball_xv
if ball_y in [10, 470]:
ball_yv = -ball_yv
self.pub.publish(cvb.cv2_to_imgmsg(cvim, "bgr8"))
self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
time.sleep(0.03)
class ROSWrapper(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(ROSWrapper, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.bridge = CvBridge()
self.pub = rospy.Publisher('/fakebot/camera_node/image/compressed',
CompressedImage,
queue_size=1)
self.sub = rospy.Subscriber('/fakebot/wheels_driver_node/wheels_cmd',
WheelsCmdStamped, self.callback)
self.action = [0.1, 0.1]
def imagepublisher(self, observation):
#rate = rospy.Rate(5)
b, g, r = cv2.split(observation)
img = cv2.merge([r, g, b])
#img_msg=self.bridge.cv2_to_compressed_imgmsg(observation)
img_msg = self.bridge.cv2_to_compressed_imgmsg(img)
img_msg.header.stamp = rospy.Time.now()
#rospy.loginfo("Get an image...")
self.pub.publish(img_msg)
#rospy.loginfo("Published to topic camera_node/image/compressed")
#rate.sleep()
def callback(self, wheelcommand):
rospy.loginfo("get wheel command")
self.action = [wheelcommand.vel_left, wheelcommand.vel_right]
class Gray_Scale():
def __init__(self):
self.enabled = False
rospy.init_node("gray_scale", anonymous=True)
# services
s = rospy.Service('perception_service', SetBool, self.perception_service_cb)
# subscribers
# subscriber for camera feed
self.subVideo = rospy.Subscriber("/camera/image_raw/compressed", CompressedImage, self.video_cb)
# publishers
self.pub_pose = rospy.Publisher("/gray_scale_out/compressed", CompressedImage, queue_size=1)
self.bridge = CvBridge()
def perception_service_cb(self, req):
self.enabled = req.data
return SetBoolResponse(True, "Yay!")
def convert_to_grayscale(self, compressed_image_msg):
cv_image = self.bridge.compressed_imgmsg_to_cv2(compressed_image_msg)
cv_gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
gray_image_msg = self.bridge.cv2_to_compressed_imgmsg(cv_gray_image)
return gray_image_msg
def video_cb(self, msg):
if(self.enabled):
gray_msg = self.convert_to_grayscale(msg)
self.pub_pose.publish(gray_msg)
def run(self):
rate = rospy.Rate(10) # 10hz default
while not rospy.is_shutdown():
rospy.loginfo_throttle(0.5,'enabled: ' + str(self.enabled))
rate.sleep()
def test_encode_decode_cv2_compressed(self):
import numpy as np
# from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm",
"jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in formats:
for channels in ([], 1, 3):
if channels == []:
original = np.uint8(np.random.randint(0, 255, size=(h, w)))
else:
original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f)
newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg)
self.assert_(original.dtype == newimg.dtype)
if channels == 1:
# in that case, a gray image has a shape of size 2
self.assert_(original.shape[:2] == newimg.shape[:2])
else:
self.assert_(original.shape == newimg.shape)
self.assert_(len(original.tostring()) == len(newimg.tostring()))
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args=args)
node = rclpy.create_node("Source")
node_logger = node.get_logger()
topic = args[1]
filenames = args[2:]
pub_img = node.create_publisher(sensor_msgs.msg.Image, topic)
pub_img_compressed = node.create_publisher(sensor_msgs.msg.CompressedImage,
topic + "/compressed")
time.sleep(1.0)
cvb = CvBridge()
while rclpy.ok():
try:
cvim = cv2.imread(filenames[0])
pub_img.publish(cvb.cv2_to_imgmsg(cvim))
pub_img_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
filenames = filenames[1:] + [filenames[0]]
time.sleep(1)
except KeyboardInterrupt:
node_logger.info("shutting down: keyboard interrupt")
break
node_logger.info("test_completed")
node.destroy_node()
rclpy.shutdown()
def spin(self):
time.sleep(1.0)
started = time.time()
counter = 0
cvim = numpy.zeros((480, 640, 1), numpy.uint8)
ball_xv = 10
ball_yv = 10
ball_x = 100
ball_y = 100
cvb = CvBridge()
while not rospy.core.is_shutdown():
cvim.fill(0)
cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
ball_x += ball_xv
ball_y += ball_yv
if ball_x in [10, 630]:
ball_xv = -ball_xv
if ball_y in [10, 470]:
ball_yv = -ball_yv
self.pub.publish(cvb.cv2_to_imgmsg(cvim))
self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
time.sleep(0.03)
def test_encode_decode_cv2_compressed(self):
import numpy as np
# from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm",
"jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in formats:
for channels in ([], 1, 3):
if channels == []:
original = np.uint8(np.random.randint(0, 255, size=(h, w)))
else:
original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f)
newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg)
self.assert_(original.dtype == newimg.dtype)
if channels == 1:
# in that case, a gray image has a shape of size 2
self.assert_(original.shape[:2] == newimg.shape[:2])
else:
self.assert_(original.shape == newimg.shape)
self.assert_(len(original.tostring()) == len(newimg.tostring()))
class DetectionImagePublisher(object):
def __init__(self, detection_images_qdic):
self.detection_images_qdic = detection_images_qdic
self.image_publishers = {}
self.bridge = CvBridge()
for camera in self.detection_images_qdic.keys():
topic = '/cameras/' + camera + '/ar_detection_images/compressed'
self.image_publishers[camera] = rospy.Publisher(topic,
CompressedImage,
queue_size=10)
self._detection_image_available = Event()
def notify_detection_image_available(self):
self._detection_image_available.set()
def run(self):
while not rospy.is_shutdown():
self._detection_image_available.wait()
for camera in self.detection_images_qdic.keys():
for _ in range(self.detection_images_qdic[camera].qsize()):
try:
self.image_publishers[camera].publish(
self.bridge.cv2_to_compressed_imgmsg(
self.detection_images_qdic[camera].get()))
except CvBridgeError as e:
print e
self._detection_image_available.clear()
def video_to_msg():
rospy.init_node("video_to_msg_node", anonymous=True)
pub_image = rospy.Publisher("/axis/image_raw", Image, queue_size=10)
pub_compr = rospy.Publisher("/axis/image_raw/compressed",
CompressedImage,
queue_size=10)
pub_info = rospy.Publisher("/axis/camera_info", CameraInfo, queue_size=10)
filename = sys.argv[1]
video = cv2.VideoCapture(filename)
if not video.isOpened():
print "Error: Can't open: " + str(filename)
exit(0)
print "Correctly opened, starting to publish."
fps = video.get(cv2.CAP_PROP_FPS)
rate = rospy.Rate(fps)
count_max = int(video.get(cv2.CAP_PROP_FRAME_COUNT))
resolution = video.get(cv2.CAP_PROP_FRAME_HEIGHT)
bridge = CvBridge()
msg_header = std.Header()
msg_header.frame_id = "/axis"
if resolution == 720.0:
msg_info = info_720p()
elif resolution == 480.0:
msg_info = info_480p()
success, frame = video.read()
count = 1
while success and not rospy.is_shutdown():
msg_image = bridge.cv2_to_imgmsg(np.uint8(frame), encoding="rgb8")
msg_compr = bridge.cv2_to_compressed_imgmsg(frame, dst_format="jpeg")
msg_header.stamp = rospy.Time.now()
msg_header.seq = count
msg_image.header = msg_header
msg_compr.header = msg_header
msg_info.header = msg_header
#print msg_compr
#time.sleep(10)
pub_image.publish(msg_image)
pub_compr.publish(msg_compr)
pub_info.publish(msg_info)
print str(count) + "/" + str(count_max)
rate.sleep()
success, frame = video.read()
count += 1
def spin(self):
time.sleep(1.0)
started = time.time()
counter = 0
cvim = numpy.zeros((480, 640, 1), numpy.uint8)
ball_xv = 10
ball_yv = 10
ball_x = 100
ball_y = 100
cvb = CvBridge()
cv2.namedWindow("source", cv2.WINDOW_NORMAL)
src = cv2.imread(
"/home/ganga/Documents/rover-control/src/opencv_tests/ball.jpg", 1)
cv2.imshow("source", src)
cv2.waitKey(0)
while not rospy.core.is_shutdown():
cvim.fill(0)
cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
ball_x += ball_xv
ball_y += ball_yv
if ball_x in [10, 630]:
ball_xv = -ball_xv
if ball_y in [10, 470]:
ball_yv = -ball_yv
self.pub.publish(cvb.cv2_to_imgmsg(cvim))
self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
time.sleep(0.03)
class camera_Worker(threading.Thread):
def __init__(self):
self.flag = False
self.wait_flag = threading.Event()
threading.Thread.__init__(self)
self.image_pub = rospy.Publisher("/camera_topic",
CompressedImage,
queue_size=1) #encoding defult jpg
self.bridge = CvBridge()
def run(self):
self.camera()
def camera(self):
self.cap = cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER)
if self.cap.isOpened():
print("Start Camera")
while not rospy.is_shutdown():
if not self.flag:
self.wait_flag.wait()
self.ret_val, self.img = self.cap.read()
self.image_pub.publish(
self.bridge.cv2_to_compressed_imgmsg(
self.img)) #encoding defult jpg
print("Close Camera")
else:
print("Unable to open camera")
self.cap.release()
print(self.cap.isOpened())
def sync_callback(self, _img1, _img2, _img3, _img4):
_time = time.time()
self.is_initiated = True
img1, time1 = self.callback_undistort1(_img1)
img2, time2 = self.callback_undistort2(_img2)
img3, time3 = self.callback_undistort3(_img3)
img4, time4 = self.callback_undistort4(_img4)
print("1: " + str(time.time() - _time))
im_mask_inv1, im_mask1 = self.find_mask(img1)
im_mask_inv3, im_mask3 = self.find_mask(img3)
print("2: " + str(time.time() - _time))
img1_masked = np.multiply(img1, im_mask_inv1)
img2_masked = np.multiply(np.multiply(img2, im_mask1), im_mask3)
print("3: " + str(time.time() - _time))
img3_masked = np.multiply(img3, im_mask_inv3)
img4_masked = np.multiply(np.multiply(img4, im_mask1), im_mask3)
summed_image = img1_masked + img2_masked + img3_masked + img4_masked
print("4: " + str(time.time() - _time))
#cv2.imshow('summed.png', summed_image)
#cv2.waitKey(3)
bridge = CvBridge()
summed_msg = bridge.cv2_to_compressed_imgmsg(summed_image)
print("5: " + str(time.time() - _time))
self.sum_pub.publish(summed_msg)
print("img1: " + str(time1) + ", img2: " + str(time2 - time1) +
", img3: " + str(time3 - time1) + ", img4: " +
str(time4 - time1) + ", summ: " + str(time.time() - time1))
def run(self):
'''
Continuously run the wrapper to map ROS interface with simulator.
This method runs the node, and begins an iterative process where the
image is retrieved from the simulator and published in the camera image topic,
and the wheels command received by subscribing to the wheels_cmd topic is translated
into actions executed in the simulator. The process iterates until the node is terminated
'''
while not rospy.is_shutdown():
# Update the simulator
# the action message is updated every time the wheels_cmd_cb is called
# that is, every time a message arrives.
observation, reward, done, misc = self.env.step(self.action)
# Render the new state
self.env.render()
# Set again action to stop, in case no more messages are being received
self.action = [0.0, 0.0]
if done:
# Reset the simulator when the robot goes out of the road.
self.env.reset()
# Transalte the observation into a CompressedImage message to publish it
bgr_obs = cv2.cvtColor(observation, cv2.COLOR_RGB2BGR)
bridge = CvBridge()
img_msg = bridge.cv2_to_compressed_imgmsg(cvim=bgr_obs,
dst_format="jpg")
# Publish the image in the /image/compressed topic
self.camera_pub.publish(img_msg)
def spin(self):
time.sleep(1.0)
started = time.time()
counter = 0
# imv = np.zeros((height, width, bpp), np.uint8)
cvim = numpy.zeros((500, 500, 3), numpy.uint8)
ball_xv = 10
ball_yv = 10
ball_x = 100
ball_y = 100
cvb = CvBridge()
while not rospy.core.is_shutdown():
# cvim.fill(0)
# cv2.circle(img, (center_x, center_y), radius, (b, g, r), thickness)
cv2.circle(cvim, (ball_x, ball_y), 100, (0,0,255), 1)
ball_x += ball_xv
ball_y += ball_yv
if ball_x in [10, 630]:
ball_xv = -ball_xv
if ball_y in [10, 470]:
ball_yv = -ball_yv
self.pub.publish(cvb.cv2_to_imgmsg(cvim, 'bgr8'))
self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
time.sleep(0.03)
def CreateVideoBag(videopath, bagname):
'''Creates a bag file with a video file'''
bag = rosbag.Bag(bagname, 'w')
cap = cv2.VideoCapture(videopath)
cb = CvBridge()
prop_fps = cap.get(cv2.CAP_PROP_FPS)
if prop_fps != prop_fps or prop_fps <= 1e-2:
print "Warning: can't get FPS. Assuming 24."
prop_fps = 24
ret = True
frame_id = 0
while (ret):
ret, frame = cap.read()
if not ret:
break
stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps)
frame_id += 1
# image = cb.cv2_to_imgmsg(frame, encoding='bgr8')
image = cv2.resize(frame,
dsize=(1280, 720),
interpolation=cv2.INTER_LINEAR)
image = cb.cv2_to_compressed_imgmsg(frame)
image.header.stamp = stamp
image.header.frame_id = "camera"
bag.write(TOPIC, image, stamp)
cap.release()
bag.close()
class VehicleDetectionTestNode(object):
def __init__(self):
self.node_name = "Vehcile Detection Test"
self.bridge = CvBridge()
self.pub_image = rospy.Publisher("~image", CompressedImage, queue_size=1)
self.sub_image = rospy.Subscriber("~corners", VehicleCorners,
self.processCorners, queue_size=1)
self.original_filename = rospy.get_param('~original_image_file')
self.original_image = cv2.imread(self.original_filename)
rospy.loginfo("Initialization of [%s] completed" % (self.node_name))
pub_period = rospy.get_param("~pub_period", 1.0)
rospy.Timer(rospy.Duration.from_sec(pub_period), self.pubOrig)
def pubOrig(self, args=None):
np_arr = np.fromstring(np.array(cv2.imencode('.png',
self.original_image)[1]).tostring(), np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img_msg = self.bridge.cv2_to_compressed_imgmsg(cv_image, "jpg")
img_msg.header.stamp = rospy.Time.now()
self.pub_image.publish(img_msg)
rospy.loginfo("Publishing original image")
def processCorners(self, corners_msg):
for i in np.arange(len(corners_msg.corners)):
rospy.loginfo('Corners received : (x = %.2f, y = %.2f)' %
(corners_msg.corners[i].x, corners_msg.corners[i].y))
def talker():
pub = rospy.Publisher('/howard17/camera_node/image/compressed',
CompressedImage,
queue_size=10)
rospy.init_node('aruco_talker')
rate = rospy.Rate(10) # 10hz
br = CvBridge()
#cap = cv2.VideoCapture('test1.mp4')
cap = cv2.VideoCapture(0)
#print('Got the cap')
#cap = cv2.VideoCapture(0)
while not rospy.is_shutdown():
ret, frame = cap.read()
if ret == True:
#print('ret is true')
# Our operations on the frame come here
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Display the resulting frame
#cv2.imshow('frame',gray)
#if cv2.waitKey(1) & 0xFF == ord('q'):
#break
#rospy.loginfo(hello_str)
# Convert the video to a ros Compressed image
message = br.cv2_to_compressed_imgmsg(frame)
pub.publish(message)
#rospy.loginfo(message)
#rate.sleep()
# Upon shutdown
cap.release()
cv2.destroyAllWindows()
class FaceRecognitionNode:
def __init__(self):
self.__bridge = CvBridge()
self.__image_pub = rospy.Publisher(
'face_recognition_node/image/compressed',
CompressedImage,
queue_size=1)
self.__image_sub = rospy.Subscriber('raspicam_node/image/compressed',
CompressedImage, self.callback)
confidence_level = rospy.get_param('/face_rec_python/confidence_level',
20)
rospy.loginfo("FaceRecognitionNode: Confidence level %s",
str(confidence_level))
# Create the face_recognition_lib class instance
self.__frc = face_recognition_lib.FaceRecognition(
roslib.packages.get_pkg_dir('face_recognition', required=True),
confidence_level)
# Create the Action server
self.__as = actionlib.SimpleActionServer('face_recognition',
scan_for_facesAction,
self.do_action, False)
self.__as.start()
def do_action(self, goal):
# Scan the current image for faces recognised
# The image may show more than one face.
# The returned dictionary will contain the unique IDs
# and Names of any subjects recognised.
# If no detection/recognition dictionary will be empty
image = self.__bridge.compressed_imgmsg_to_cv2(self.__current_image)
# In the next call image will be altered if faces are recognised
detected_dict = self.__frc.scan_for_faces(image)
try:
self.__image_pub.publish(
self.__bridge.cv2_to_compressed_imgmsg(image))
except CvBridgeError as e:
print(e)
# Now post a message with the list of IDs and names
ids = []
names = []
for k, v in detected_dict.items():
ids.append(k)
names.append(v)
# Set result for the action
result = scan_for_facesResult()
result.ids_detected = ids
result.names_detected = names
self.__as.set_succeeded(result)
# Callback for new image received
def callback(self, data):
# Each time we receive an image we store it ready in case then asked to scan it
self.__current_image = data
def spin(self):
time.sleep(1.0)
cvb = CvBridge()
while not rospy.core.is_shutdown():
cvim = cv2.imload(self.filenames[0])
self.pub.publish(cvb.cv2_to_imgmsg(cvim))
self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
self.filenames = self.filenames[1:] + [self.filenames[0]]
time.sleep(1)
class OpenMVCamNode:
def __init__(self):
rospy.init_node('openmv_cam_node', argv=sys.argv)
self.device = rospy.get_param('~device', default='/dev/ttyACM0')
self.topic = rospy.get_param(
'~topic', default='openmv_cam/camera_main/image_raw')
self.compressed = rospy.get_param('~compressed', default=False)
self.image_type = Image
if self.compressed:
self.topic += '/compressed'
self.image_type = CompressedImage
try:
self.openmv_cam = OpenMVCam(self.device)
except Exception:
rospy.logerr('Serial connection to OpenMV Cam failed: {}'.format(
self.device))
exit()
self.bridge = CvBridge()
self.publisher = rospy.Publisher(self.topic,
self.image_type,
queue_size=1)
self.seq = 0
rospy.loginfo("Relaying '{}' to '{}'".format(self.device, self.topic))
def read_and_publish_image(self):
# Read image from camera
image = self.openmv_cam.read_image()
# Deduce color/grayscale from image dimensions
channels = 1 if image.ndim == 2 else 3
encoding = 'bgr8' if channels == 3 else 'mono8'
# Convert from BGR to RGB by reversing the channel order
if channels == 3:
image = image[..., ::-1]
# Convert numpy image to (commpressed) ROS image message
if self.compressed:
image_msg = self.bridge.cv2_to_compressed_imgmsg(image)
else:
image_msg = self.bridge.cv2_to_imgmsg(image, encoding=encoding)
# Add timestamp and sequence number (empty by default)
image_msg.header.stamp = rospy.Time.now()
image_msg.header.seq = self.seq
self.seq += 1
self.publisher.publish(image_msg)
class ObjectDetectionNode:
def __init__(self):
self.__bridge = CvBridge()
# Publisher to publish update image
self.__image_pub = rospy.Publisher(
'tf_object_detection_node/image/compressed',
CompressedImage,
queue_size=1)
# Subscribe to the topic which will supply the image fom the camera
self.__image_sub = rospy.Subscriber('raspicam_node/image/compressed',
CompressedImage,
self.Imagecallback)
# Read the path for models/research/object_detection directory from the parameter server or use this default
object_detection_path = rospy.get_param(
'/object_detection/path',
'/home/ubuntu/git/models/research/object_detection')
# Read the confidence level, any object with a level below this will not be used
confidence_level = rospy.get_param(
'/object_detection/confidence_level', 0.50)
# Create the object_detection_lib class instance
self.__odc = object_detection_lib.ObjectDetection(
object_detection_path, confidence_level)
# Create the Action server
self.__as = actionlib.SimpleActionServer('object_detection',
scan_for_objectsAction,
self.do_action, False)
self.__as.start()
# Callback for new image received
def Imagecallback(self, data):
# Each time we receive an image we store it in case we are asked to scan it
self.__current_image = data
def do_action(self, goal):
# Scan the current image for objects
# Convert the ROS compressed image to an OpenCV image
image = self.__bridge.compressed_imgmsg_to_cv2(self.__current_image)
# The supplied image will be modified if known objects are detected
object_names_detected = self.__odc.scan_for_objects(image)
# publish the image, it may have been modified
try:
self.__image_pub.publish(
self.__bridge.cv2_to_compressed_imgmsg(image))
except CvBridgeError as e:
print(e)
# Set result for the action
result = scan_for_objectsResult()
result.names_detected = object_names_detected
self.__as.set_succeeded(result)
class ImageConverter:
def __init__(self):
self.image_pub = rospy.Publisher("/image/compressed",
CompressedImage,
queue_size=1)
self.brige = CvBridge()
self.image_sub = rospy.Subscriber("/raspicam_node/image/compressed",
CompressedImage, self.callback)
def callback(self, data):
try:
cv_image = self.brige.compressed_imgmsg_to_cv2(data, "passthrough")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
cv2.circle(cv_image, (50, 50), 10, 255)
# cv2.imshow("Image Window", cv_image)
# cv2.waitKey(3)
# insert jays code
image = cv_image
#try:
image_copy = np.copy(image)
# Edge Detection using Canny E.D
img_canny_detection = canny_ed(image_copy)
img_region_extract = region_extract(img_canny_detection)
lines = cv2.HoughLinesP(img_region_extract,
2,
np.pi / 180,
100,
np.array([]),
minLineLength=40,
maxLineGap=5)
# num of pixels in the bin, 1 deg, thereshold(min), placeholder, tracd below 40 lines rejected, max distance btw pixels)
lines_avg = avg_slope(image_copy, lines)
img_lines = line_display(image_copy, lines)
#img_combined = img_canny_detection
#img_combined = img_region_extract
img_combined = cv2.addWeighted(image_copy, 0.7, img_lines, 1, 1)
rows, cols, ch = img_combined.shape
#psts1 = np.float32([[155, 240], [463, 243], [0, 290], [600, 290]])
#psts2 = np.float32([[0, 0], [640, 0], [0, 450], [640, 450]])
#M = cv2.getPerspectiveTransform(psts1, psts2)
#dst = cv2.warpPerspective(img_combined, M, (640, 450))
try:
# changed this from cv_image to img_combined
self.image_pub.publish(
self.brige.cv2_to_compressed_imgmsg(img_combined))
except CvBridgeError as e:
print(e)
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic", CompressedImage)
self.bridge = CvBridge()
try:
img = cv2.imread('25.png')
self.image_pub.publish(cmprsmsg = self.bridge.cv2_to_compressed_imgmsg(img, 'png'))
except CvBridgeError as e:
print(e)
def operator():
rospy.init_node('image_file_publisher', anonymous=True)
pub_c = rospy.Publisher('image_data/compressed',
CompressedImage,
queue_size=10)
pub_r = rospy.Publisher('image_data/raw', Image, queue_size=10)
dir_name = rospy.get_param('~dir_name')
file_name = rospy.get_param('~file_name')
sequence_no = int(rospy.get_param('~sequence_no'))
if sequence_no < 1 and file_name != "":
filepath = os.path.abspath(
os.path.join(os.path.dirname(dir_name), file_name))
elif sequence_no >= 1 and sequence_no <= 999 and file_name == "":
qr_file_name = "MarkerData_%03d.png" % sequence_no
filepath = os.path.abspath(
os.path.join(os.path.dirname(dir_name), qr_file_name))
else:
raise SyntaxError(
"sequence_no is 1 to 999 and file_name is not empty.")
print filepath
output_width_px = int(rospy.get_param("~output_width_px", "320"))
output_height_px = int(rospy.get_param("~output_height_px", "240"))
pub_rate = int(rospy.get_param("~pub_rate", "1"))
# make bridge
bridge = CvBridge()
rate = rospy.Rate(pub_rate)
while not rospy.is_shutdown():
# read image
try:
im = cv2.imread(filepath, cv2.IMREAD_COLOR)
tmp = im[:, :]
input_height_px, input_width_px = im.shape[:2]
new_img = cv2.resize(np.full((1, 1, 3), 255, np.uint8),
(output_width_px, output_height_px))
# Add Margin(Side || Side)
tmp = cv2.resize(
tmp,
dsize=None,
fx=(float(output_height_px) / float(input_height_px)),
fy=(float(output_height_px) / float(input_height_px)))
start = int((output_width_px - tmp.shape[1]) / 2)
fin = int((tmp.shape[1] + output_width_px) / 2)
new_img[0:output_height_px, start:fin] = tmp
# make msg
msg_c = bridge.cv2_to_compressed_imgmsg(new_img, dst_format='jpg')
msg_r = bridge.cv2_to_imgmsg(new_img, encoding="bgr8")
pub_c.publish(msg_c)
pub_r.publish(msg_r)
except:
pass
rate.sleep()
class Camera:
# DO_NOTHING_CMD = CFMotion()
def __init__(self, ID):
self.id = ID
self.bridge = CvBridge()
self.mat = None
#need to facilitate a set of publishers per cf node
self.image_pub = rospy.Publisher('cf/%d/image' % self.id,
CompressedImage,
queue_size=10)
## CALLBACKS ##
## THREADS ##
def run(self):
try:
cap = cv2.VideoCapture(
0) # TODO: multiple vid captures in parallel
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 192)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 144)
# cap.set(cv2.CAP_PROP_BRIGHTNESS, 0.8)
# cap.set(cv2.CAP_PROP_CONTRAST, 0.2)
# cap.set(cv2.CAP_PROP_EXPOSURE, 0.08)
# cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
while not rospy.is_shutdown():
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#ret, gray = cap.read()
self.image_pub.publish(
self.bridge.cv2_to_compressed_imgmsg(gray))
cv2.imshow('frame', gray)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
except Exception as e:
print "CAMERA %d STREAM FAILED -- CHECK INPUTS" % self.id
print "Error: " + str(e)
print " -- Camera %d Finished -- " % self.id
class CamCapture():
def __init__(self):
self.camID = rospy.get_param("~camID", 0)
self.width = rospy.get_param("~width", 640)
self.height = rospy.get_param("~height", 480)
self.rate = rospy.get_param("~rate", 30)
self.flipH = rospy.get_param("~flipH", False)
self.flipV = rospy.get_param("~flipV", False)
rospy.loginfo("use camera id=%d, w=%d, h=%d, rate=%f" %
(self.camID, self.width, self.height, self.rate))
self.cap = cv2.VideoCapture(self.camID)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)
self.br = CvBridge()
self.pub = rospy.Publisher("/topower_v1/camera/image_raw/compressed",
CompressedImage,
queue_size=1)
self.sub = rospy.Subscriber("/topower_v1/camera/capture", Empty,
self.CaptureImage)
self.savePath = rospy.get_param("~savePath", "captureImage/")
self.frame = None
def CaptureImage(self, msg):
if self.frame is not None:
if not os.path.exists(self.savePath):
os.makedirs(self.savePath)
now = datetime.datetime.now()
filename = self.savePath + now.strftime(
"%Y-%m-%d_%H-%M-%S") + ".jpg"
rospy.loginfo("save image " + filename)
cv2.imwrite(filename, self.frame)
def Run(self):
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
ret, self.frame = self.cap.read()
if self.flipH and self.flipV:
self.frame = cv2.flip(self.frame, -1)
elif self.flipH:
self.frame = cv2.flip(self.frame, 1)
elif self.flipV:
self.frame = cv2.flip(self.frame, 0)
msg = self.br.cv2_to_compressed_imgmsg(self.frame)
self.pub.publish(msg)
#cv2.imshow('frame',self.frame)
#cv2.waitKey(1)
rate.sleep()
class RosTensorFlow():
def __init__(self):
in_img_topic = get_param('~in_img_topic', '/image_in')
in_geo_img_topic = get_param('~in_geo_img_topic', '/geo_image_in')
out_img_topic = get_param('~out_img_topic', '/image_detected')
out_geo_img_topic = get_param('~out_geo_img_topic',
'/geo_image_detected')
self.m_cv_bridge = CvBridge()
self.m_geo_img = GeoImageCompressed()
self.m_sub_img = rospy.Subscriber(in_img_topic,
Image,
self.detectImgCb,
queue_size=50)
self.m_sub_geo_img = rospy.Subscriber(in_geo_img_topic,
GeoImageCompressed,
self.detectGeoImgCb,
queue_size=50)
self.m_pub_img = rospy.Publisher(out_img_topic, Image, queue_size=50)
self.m_pub_geo_img = rospy.Publisher(out_geo_img_topic,
GeoImageCompressed,
queue_size=50)
def detectGeoImgCb(self, geo_image_msg):
self.m_geo_img = geo_image_msg # First copy the full msg and then update only the detected image later.
print("Image recieved.")
frame = self.m_cv_bridge.compressed_imgmsg_to_cv2(
geo_image_msg.imagedata, "bgr8")
#frame = cv2.resize(frame,(0,0), fx=0.2, fy=0.2)
t = time.time()
detected_image = detect_objects(frame, sess, detection_graph)
self.m_geo_img.imagedata = self.m_cv_bridge.cv2_to_compressed_imgmsg(
detected_image, dst_format='jpg')
self.m_pub_geo_img.publish(self.m_geo_img)
self.m_pub_img.publish(
self.m_cv_bridge.cv2_to_imgmsg(detected_image, "bgr8"))
print('[INFO] elapsed time: {:.2f}'.format(time.time() - t))
def detectImgCb(self, image_msg):
print("Image recieved.")
frame = self.m_cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
#frame = cv2.resize(frame,(0,0), fx=0.2, fy=0.2)
t = time.time()
detected_image = detect_objects(frame, sess, detection_graph)
self.m_pub_img.publish(
self.m_cv_bridge.cv2_to_imgmsg(detected_image, "bgr8"))
print('[INFO] elapsed time: {:.2f}'.format(time.time() - t))
def main(self):
rospy.spin()
class AcaptureCameraNode(Node):
def __init__(self):
super().__init__('acapture_camera_node',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
self.node_name = self.get_name()
self.log = self.get_logger()
self.cap = acapture.open(0) #/dev/video0
#self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
#self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
#self.cap.set(cv2.CAP_PROP_FPS, 5)
self.log.info("successfully opened camera device!")
# init ROS opencv bridge
self.bridge = CvBridge()
self.log.info("sucessfully created CvBridge.")
self.pub_img = self.create_publisher(CompressedImage,
"image/compressed", 1)
self.image_msg = CompressedImage()
def start_capturing(self):
self.log.info("Start capturing.")
while True:
try:
check, frame = self.cap.read() # non-blocking
if check:
self.log.info("sucessfully read a frame")
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
self.image_msg = self.bridge.cv2_to_compressed_imgmsg(
frame)
self.image_msg.header.stamp = self.get_clock().now(
).to_msg()
# self.image_msg.frame_id = "camera_optical_frame"
self.pub_img.publish(self.image_msg)
self.log.info("publishing image")
# cv2.waitKey(100)
sleep(0.05)
except StopIteration:
pass
self.log.info("Capture Ended.")
class ImageConverter:
def __init__(self):
self.brige = CvBridge()
self.brige2 = CvBridge()
self.image_pub = rospy.Publisher("/image/compressed2", CompressedImage, queue_size=1)
self.top_image_pub = rospy.Publisher("/top_image/compressed", CompressedImage, queue_size=1)
#self.image_pub2 = rospy.Publisher("/image/compressed2", CompressedImage, queue_size=1)
self.image_sub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, self.callback)
def callback(self,data):
try:
cv_image = self.brige.compressed_imgmsg_to_cv2(data, "passthrough")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
#cv2.circle(cv_image, (50,50), 10, 255)
# print center of lane line bisector angle average intersection
if CENTER_LANE_X <= 640 and CENTER_LANE_X>= 0:
cv2.circle(cv_image, (int(CENTER_LANE_X), 480), 20, (0, 0, 255))
elif CENTER_LANE_X > 640:
cv2.circle(cv_image, (640, 480), 20, (0, 0, 255))
elif CENTER_LANE_X < 0:
cv2.circle(cv_image, (0, 480), 20, (0, 0, 255))
# print target
cv2.circle(cv_image, (int(640/2), 480), 20, (255, 0, 0))
# insert jays code
global LAST_LINES_AVG
image = cv_image
#try:
image_copy = np.copy(image)
img_combined = image_copy
# Edge Detection using Canny E.D
img_canny_detection = canny_ed(image_copy)
img_region_extract = region_extract(img_canny_detection)
img_combined = img_canny_detection
try:
# changed this from cv_image to img_combined
self.image_pub.publish(self.brige.cv2_to_compressed_imgmsg(img_combined))
# self.top_image_pub.publish(self.brige.cv2_to_compressed_imgmsg(dst))
#self.image_pub.publish(self.brige.cv2_to_compressed_imgmsg(dst))
#self.image_pub2.publish(CvBridge().cv2_to_compressed_imgmsg(dst))
except CvBridgeError as e:
print(e)
