def run(data):
frame = np.frombuffer(data.data,
dtype=np.uint8).reshape(data.height, data.width, -1)
tstart = time.time()
height = frame.shape[0]
width = frame.shape[1]
frame1 = cv.resize(frame, (576, 324))
frame_np = frame1[:, :, [2, 1, 0]]
# test
img = Image()
img.encoding = "bgr8"
img.height = height
img.width = width
# img.step = (width) * sizeof(float)
img.step = img.width * 8 * 3
img.is_bigendian = 0
img.data = np.asarray(frame, np.uint8).tostring()
raw_video_pub.publish(img)
print("in")
def _receive_message(self,message):
global my
rospy.loginfo(rospy.get_caller_id() + " I heard a message of %s",self._message_type)
rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s",(rospy.get_time()-my) )#edw mou leei unicode type
my=rospy.get_time()
try:
msg=Image()
msg.header.seq=message['header']['seq']
msg.header.stamp.secs=message['header']['stamp']['secs']
msg.header.stamp.nsecs=message['header']['stamp']['nsecs']
msg.header.frame_id=message['header']['frame_id']
msg.height=message['height']
msg.width=message['width']
msg.encoding=str(message['encoding'])
msg.is_bigendian=message['is_bigendian']
msg.step=message['step']
msg.data=message['data'].decode('base64')
self._rosPub=rospy.Publisher(self._local_topic_name, Image, queue_size=10) #message type is String instead of msg_stds/String
self._rosPub.publish(msg)
except:
print('Error')
def array_to_image(array):
"""Takes a NxMx3 array and converts it into a ROS image message.
"""
# Sanity check the input array shape
if len(array.shape) != 3 or array.shape[2] != 3:
raise ValueError('Array must have shape MxNx3')
# Ravel the array into a single buffer
image_data = (array.astype(np.uint8)).tostring(order='C')
# Create the image message
image_msg = Image()
image_msg.height = array.shape[0]
image_msg.width = array.shape[1]
image_msg.encoding = 'rgb8'
image_msg.is_bigendian = 0
image_msg.step = array.shape[1] * 3
image_msg.data = image_data
return image_msg
def got_img(self, img):
# init result sensor_msgs/Image msg
self.imgmsg = Image()
self.imgmsg.header.stamp = img.header.stamp
self.imgmsg.header.frame_id = img.header.frame_id
self.imgmsg.step = img.step
self.imgmsg.encoding = img.encoding
self.imgmsg.is_bigendian = img.is_bigendian
self.imgmsg.height, self.imgmsg.width = img.height, img.width
# load image
self.h, self.w = img.height, img.width
self.rgb = np.frombuffer(img.data,
dtype=np.uint8).reshape(self.h, self.w, -1)
img = cv2.resize(self.rgb, (self.resize_image, self.resize_image))
self.img = cv2_to_tensor(img, np.float_)
# predict box and publish processed data
self.inference()
self.publish_processed_image()
def __init__(self):
self.known_location = {}
self.current_img = Image()
self.heading = PoseWithCovarianceStamped()
self.filepath = os.path.dirname(os.path.realpath(__file__))
Current_loc = rospy.Service('memorize_position', memorize_position,
self.memorize_position_func)
file = rospy.Service('file_position', file_position,
self.file_position_func)
self.server = actionlib.SimpleActionServer('control_robot',
control_robotAction,
self.action_callback, False)
self.patrol_server = actionlib.SimpleActionServer(
'patrol', patrolAction, self.patrol_action_callback, False)
self.server.start()
self.patrol_server.start()
self.pose_sub = rospy.Subscriber('/amcl_pose',
PoseWithCovarianceStamped,
self.pose_callback)
self.image_sub = rospy.Subscriber('/head_camera/rgb/image_raw', Image,
self.image_callback)
def picture_from_preview_ff(self):
cv2.namedWindow("GoPro", cv2.CV_WINDOW_AUTOSIZE)
sp.Popen([
'ffmpeg',
'-i',
'udp://@' + self.ip + ':8554/',
'r',
'1',
'-f',
'image2',
'-vframes',
'1',
'-vcodec',
'mjpeg',
'captured.jpg',
],
stdin=sp.PIPE,
stdout=sp.PIPE)
return Image('captured.jpg')
def __init__(self):
"""
Definition and initialization of class attributes
"""
#parameters
self.camera_model = rospy.get_param("~camera_model", "")
self.port = rospy.get_param("~port", "")
self.racecar_name = rospy.get_param("~racecar_name", "car_1")
self.topic_pub_Image_Pub = rospy.get_param("~topic_pub_Image_Pub", "")
#publishers
self.Image_Pub_ = rospy.Publisher(self.topic_pub_, Image, queue_size=1)
self.out_Image_Pub = Image()
self.out_Image_Pub_active = bool()
# protected region user member variables begin #
self.cap = cv2.VideoCapture(-1)
self.cap.set(3, 320)
self.cap.set(4, 240)
self.cap.set(5, 10)
self.cap.set(12, 100)
self.bridge = CvBridge()
def generateDepthImageAndInfo(self):
self.msg_counter += 1
width = 640
height = 480
img = np.empty((height, width), np.uint16)
img.fill(1400)
for i in range(width):
for j in range(height / 2 - 1, height / 2 + 1):
img[j][i] = 520
depthimg = Image()
depthimg.header.frame_id = 'depthmap_test'
depthimg.header.seq = self.msg_counter
depthimg.header.stamp = rospy.Time.now()
depthimg.height = height
depthimg.width = width
depthimg.encoding = '16UC1'
depthimg.step = depthimg.width * 2
depthimg.data = img.tostring()
info = CameraInfo()
info.header = depthimg.header
info.height = height
info.width = width
info.distortion_model = 'plumb_bob'
info.K[0] = 570
info.K[2] = 314
info.K[4] = 570
info.K[5] = 239
info.K[8] = 1.0
info.R[0] = 1.0
info.R[4] = 1.0
info.R[8] = 1.0
info.P[0] = 570
info.P[2] = 314
info.P[5] = 570
info.P[6] = 235
info.P[10] = 1.0
return depthimg, info
def __init__(self):
# create lock
self.lock = Lock()
# start possible debug window
cv2.startWindowThread()
# initialize current frame and timestamp
self.cur_image = Image()
self.cur_ts = 0.0
# get pipeline name
self.name = rospy.get_namespace().split('/')[-2]
# get parameters
self.debug_hand_detect_flag = rospy.get_param("debug_hand_detect_flag")
if self.debug_hand_detect_flag:
cv2.namedWindow(self.name + " hands")
self.fovy = rospy.get_param("fovy")
self.aspect = rospy.get_param("aspect")
self.rotate = rospy.get_param("rotate")
self.hand_detect_rate = rospy.get_param("hand_detect_rate")
self.timer = rospy.Timer(rospy.Duration(1.0 / self.hand_detect_rate),
self.HandleTimer)
self.hand_detect_work_width = rospy.get_param("hand_detect_work_width")
self.hand_detect_work_height = rospy.get_param(
"hand_detect_work_height")
# start dynamic reconfigure client
self.dynparam = dynamic_reconfigure.client.Client(
"vision_pipeline", timeout=30, config_callback=self.HandleConfig)
# start subscriber and publisher
self.image_sub = rospy.Subscriber("camera/image_raw", Image,
self.HandleImage)
self.hand_pub = rospy.Publisher("raw_hand", Hand, queue_size=5)
def callback(self, image):
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
self.img = image
cl_bin = Image()
print('Subscribed photoneo_image')
# ROI抽出 [y1:y2, x1:x2]
roi_img = cv_image[2:615, 712:1330]
# roi_img = cv_image[2:640, 712:1330]
print('Successed roi')
gray_img = cv2.cvtColor(roi_img, cv2.COLOR_BGR2GRAY)
gamma = 1.5
lookUpTable = np.empty((1, 256), np.uint8)
for i in range(256):
lookUpTable[0, i] = np.clip(pow(i / 255.0, gamma) * 255.0, 0, 255)
# Look up tableを使って画像の輝度値を変更
gamma_img = cv2.LUT(gray_img, lookUpTable)
print('Successed gamma')
# hist_img = cv2.calcHist([gray_img], [0], None, [256], [0, 256])
blur_img = cv2.GaussianBlur(gray_img, (3, 3), 0)
print('Successed gaussian_filter')
ret, ohtsu_img = cv2.threshold(blur_img, 0, 255, cv2.THRESH_OTSU)
print('Processed binarization')
kernel = np.ones((10, 10), np.uint8)
op_bin = cv2.morphologyEx(ohtsu_img, cv2.MORPH_OPEN, kernel)
cl_bin = cv2.morphologyEx(op_bin, cv2.MORPH_CLOSE, kernel)
self.cable_filter.publish(self.bridge.cv2_to_imgmsg(cl_bin, "mono8"))
print('Published result_image')
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 main(args):
FPS = 60
B = 0
dir = 1
# Init ROS node
rospy.init_node('image_publisher', anonymous=True)
pub = rospy.Publisher("/ros/color/image_raw", Image, queue_size=1)
rate = rospy.Rate(FPS)
print("Begin Publishing")
count = 0
ts_start = time.perf_counter()
IMG_WIDTH = 640
IMG_HEIGHT = 480
while not rospy.is_shutdown():
image_np = np.zeros((IMG_HEIGHT, IMG_WIDTH, 3), dtype=np.uint8)
B += 1 * dir
if B == 255:
dir = -1
elif B == 0:
dir = 1
image_np[:, :, 0] = B
# Create Image
image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.height = image_np.shape[0]
msg.width = image_np.shape[1]
msg.encoding = "rgb8"
# "bgr8" is not supported by Isaac SDK
msg.data = image_np.tostring()
# Publish new image
pub.publish(msg)
count += 1
delta = time.perf_counter() - ts_start
# Log
print("Sent", count, "images in", round(delta), "seconds with",
round(count / delta, 2), "FPS")
rate.sleep()
def __init__(self, wait_time=60):
self._counter = 0
self.img = Image()
self.ubd = list()
self.change = list()
self._db = MessageStoreProxy(collection="ubd_scene_log")
# ptu
self._max_dist = 0.1
self._wait_time = wait_time
rospy.loginfo("Subcribe to /ptu/state...")
self._ptu = JointState()
self._ptu.position = [0, 0]
self._ptu_counter = 0
self._is_ptu_changing = [True for i in range(wait_time)]
rospy.Subscriber("/ptu/state", JointState, self._ptu_cb, None, 1)
# robot pose
rospy.loginfo("Subcribe to /robot_pose...")
self._robot_pose = Pose()
self._robot_pose_counter = 0
self._is_robot_moving = [True for i in range(wait_time)]
rospy.Subscriber("/robot_pose", Pose, self._robot_cb, None, 1)
# logging stuff
subs = [
message_filters.Subscriber(
rospy.get_param("~scene_topic",
"/change_detection/detections"),
ChangeDetectionMsg),
message_filters.Subscriber(
rospy.get_param("~ubd_topic", "/vision_logging_service/log"),
LoggingUBD),
message_filters.Subscriber(
rospy.get_param("~image_topic",
"/head_xtion/rgb/image_rect_color"), Image)
]
ts = message_filters.ApproximateTimeSynchronizer(subs,
queue_size=1,
slop=0.5)
ts.registerCallback(self.cb)
rospy.Timer(rospy.Duration(60), self.to_log)
def mono_cb(self, msg):
print('Received Mono Image.')
self.has_image = True
self.camera_image = msg
try:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
except CvBridgeError as e:
print(e)
print('Error reading Mono Camera image.')
# (rows,cols,channels) = cv_image.shape
# For debugging purposes
# cv2.imshow("Image window", cv_image)
# cv2.waitKey(0)
try:
# self.test_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) # For testing purposes
self.segmented_pub.publish(Image()) # For testing purposes
except CvBridgeError as e:
print(e)
def compressedCallback(self, msg):
if self.camInfo is None:
return
try:
im = PILImage.frombuffer("RGB",
(self.camInfo.width, self.camInfo.height),
bytes(msg.data), "jpeg", "RGB", "")
b, g, r = im.split()
im = PILImage.merge("RGB", (r, g, b))
except Exception as e:
print("Exception loading PILImage.frombuffer: ", e)
return
self.rawmsg = Image()
self.rawmsg.header = msg.header
self.rawmsg.width = self.camInfo.width
self.rawmsg.height = self.camInfo.height
self.rawmsg.encoding = "bgr8"
self.rawmsg.data = im.tobytes()
self.rawmsg.step = int(len(self.rawmsg.data) / self.rawmsg.height)
self.rawPublisher_.publish(self.rawmsg)
def talker(width, height, step, loop=1000):
print 'talker start'
pub = rospy.Publisher('chatter', Image, queue_size=30)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(30)
data = ' ' * (width * height * step) # initialize the data first
count = 0
while not rospy.is_shutdown():
img = Image()
img.width = width
img.height = height
img.step = step
img.data = data
img.header.stamp = rospy.get_rostime()
pub.publish(img)
rate.sleep()
count += 1
if count == loop: break
print 'talker end'
def __init__(self):
self.detector_id = "gate"
self.numBits = 8
self.imageWidth = 640
self.imageHeight = 480
self.maxVal = 2**self.numBits - 1
self.gate_dimensions = np.array(rospy.get_param("object_tags/gate/dimensions")).astype(float)
self.gate_width = self.gate_dimensions[0]
self.gate_height = self.gate_dimensions[2]
self.left_img_flag = False
self.stereo_left = Image()
self.left_camera_info = CameraInfo()
self.left_stream = rospy.Subscriber("/albatross/stereo_camera_left_front/camera_image", Image, self.left_callback)
self.left_camera_info = rospy.Subscriber("/albatross/stereo_camera_left_front/camera_info", CameraInfo, self.camera_info_callback)
self.prev = [None, None]
self.cv_bridge = CvBridge()
self.gate_detection_pub = rospy.Publisher("gate_detections", Image, queue_size=10)
rospy.wait_for_service("detector_bucket/register_object_detection")
self.registration_service = rospy.ServiceProxy("detector_bucket/register_object_detection", RegisterObjectDetections)
self.spin_callback = rospy.Timer(rospy.Duration(.010), self.spin)
def cam_grabber():
image_pub = rospy.Publisher("/image_raw",
Image,queue_size=10)
rospy.init_node('cam_grabber', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ret, frame=c.read()
#cv2.imshow("live feed",frame)
#if cv2.waitKey(1)&0xFF==ord('q'):
# break
image_np=cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frame"
msg.height = len(frame)
msg.width = len(frame[1])
msg.encoding="mono8"
msg.step=640
msg.data = image_np.tostring()
# Publish new image
image_pub.publish(msg)
rate.sleep()
def subscribe_robot_data(self):
rospy.loginfo("Subscribing to robot data.")
# Reset variables
self.reset_variables()
# Create image buffers
self.raw_image_buffer = collections.deque(self.image_buffer_size*[Image()], self.image_buffer_size)
self.compressed_image_buffer = collections.deque(self.image_buffer_size*[CompressedImage()], self.image_buffer_size)
self.last_raw_image_time = rospy.Time.now()
self.last_compressed_image_time = rospy.Time.now()
# Create robot data subscribers
self.robot_data_subscribers = []
self.robot_data_subscribers.append(rospy.Subscriber(self.robot_mode_topic, RobotMode, self.robot_mode_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.gps_fix_topic, NavSatFix, self.navsatfix_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.local_pose_topic, Pose, self.pose_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.map_topic, OccupancyGrid, self.map_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.image_topic, Image, self.image_callback))
self.robot_data_subscribers.append(rospy.Subscriber(self.image_compressed_topic, CompressedImage, self.compressed_image_callback))
# If using tf, create tf listener for local position
if self.use_tf:
self.tfBuffer = tf2_ros.Buffer(rospy.Duration(self.send_pose_period))
self.listener = tf2_ros.TransformListener(self.tfBuffer)
pass
def __init__(self):
# OpenCV Bridge
self.bridge = CvBridge()
# Initialize global messages
self.depth = Image()
self.odometry = Odometry()
self.camera_info = CameraInfo()
# ZED Image subscribers for RGB image
sub1 = rospy.Subscriber('/camera/zed_node/rgb/image_rect_color', Image, self.zed_rgb_cb)
#sub2 = rospy.Subscriber('/camera/zed_node/right/image_rect_color', Image, self.zed_right_cb)
sub3 = rospy.Subscriber('/camera/zed_node/odom', Odometry, self.odom_cb)
sub4 = rospy.Subscriber('/camera/zed_node/rgb/camera_info', CameraInfo, self.camera_info_cb)
sub5 = rospy.Subscriber('/camera/zed_node/depth/depth_registered', Image, self.depth_cb)
# Mono Camera Image Subscriber
sub = rospy.Subscriber('/mono_image_color', Image, self.mono_cb)
print('Object Detector listening for images..')
self.segmented_pub = rospy.Publisher('/segmented_img', Image, queue_size=1)
self.segmented_pub_2 = rospy.Publisher('/segmented_img_2', Image, queue_size=1)
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
self.camera_info_pub = rospy.Publisher('/camera_info', CameraInfo, queue_size=1)
self.depth_pub = rospy.Publisher('/depth_registered', Image, queue_size=1)
self.count = 0
self.gate_mission = True
self.path_marker_mission = False
self.dummy_segment = False # For testing
# Load configuration file here
# config_string = rospy.get_param("/object_detector_config")
# self.config = yaml.load(config_string)
rospy.init_node('object_detector')
rospy.spin()
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]
self.publish_with_robot_transform(image)
self.topic_camera_info.publish(camera_info)
def publish_image(self):
"""
Publisher for camera image, sends a message containing
320 x 240 greyscale image in the form of a bytes object
as well as other miscellaneous information about the image
"""
self.cozmo.camera.image_stream_enabled = True
camera_image = self.cozmo.world.latest_image
if camera_image is not None:
img = camera_image.raw_image.convert('L')
ros_img = Image()
ros_img.header.frame_id = 'cozmo_camera'
image_time = camera_image.image_recv_time
ros_img.header.stamp = rospy.Time.from_sec(image_time)
ros_img.height = img.size[1]
ros_img.width = img.size[0]
ros_img.encoding = 'mono8'
ros_img.step = ros_img.width
ros_img.data = img.tobytes()
self.camera_publisher.publish(ros_img)
def __init__(self):
rospy.init_node("lidar_to_image")
self.transformed_image = Image()
self.transformed_image.header.frame_id = "velodyne"
self.x_resolution = 10
self.y_resolution = 10
self.pixels_number = 500
self.transformed_image.height = self.pixels_number
self.transformed_image.width = self.pixels_number
self.transformed_image.encoding = "mono8"
self.max_value = 255
self.step = 25
self.size = int(self.pixels_number*self.pixels_number)
self.transformed_image.data = [255] * self.size
self.publisher = rospy.Publisher("/scan_velodyne_hack/image_raw", Image, queue_size=1)
rospy.Subscriber("/velodyne_points/filtered", PointCloud2, self.topic_cb, queue_size=1)
rospy.loginfo("Node initialized")
rospy.spin()
def image_cb(self, msg):
# image is received
self.received_img = msg
try:
self.cv_image = self.bridge.imgmsg_to_cv2(self.received_img, "bgr8")
self.screw_data, self.drawn_image = self.collect_or_detect_screw() # conduct the detection
# publish the result, which is an image (scaled)
result_image_msg = Image()
try:
result_image_msg = self.bridge.cv2_to_imgmsg(self.drawn_image, "bgr8")
#print(self.screw_data)
except CvBridgeError as e:
print("Could not make it through the cv bridge of death.")
self.result_image_pub.publish(result_image_msg)
# reset the screw data
self.screw_data = []
except CvBridgeError as e:
print("Monocular could not make it through the cv bridge of death.")
def run(self):
self.running = True
while self.running:
# read framebuffer
# rospy.loginfo("try to read fb...")
fb = pyopenmv.fb_dump()
if fb is None:
continue
w, h, data = fb
# create Image message
img = Image()
img.header.stamp = rospy.Time.now()
img.header.frame_id = "openmv_cam"
img.width = w
img.height = h
img.data = list(data.flat[0:])
img.step = w * 3
img.encoding = "rgb8"
# publish it
self.pub_image.publish(img)
def runCamera(airSimConnector, publisher):
# Get the data
responses = airSimConnector.getImage()
# Prepare the message
for response in responses:
img_rgb_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgb8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgb_string
msg.is_bigendian = 0
msg.step = msg.width * 3
# Publish the message
rospy.loginfo("Image data " + str(len(response.image_data_uint8)))
publisher.publish(msg)
def __init__(self, framerate, res_w, res_h):
self.camera = picamera.PiCamera()
self.camera.resolution = (res_w, res_h)
self.camera.framerate = framerate
self.image_data = np.empty((res_h, res_w, 3), dtype=np.uint8)
self.image_msg = Image()
self.image_msg.height = res_h
self.image_msg.width = res_w
self.image_msg.encoding = "rgb8"
self.image_msg.is_bigendian = False
self.image_msg.step = 3 * res_w
# Create publisher
self.image_pub = rospy.Publisher('/rpi_sensors/image',
Image,
queue_size=1)
# Create subscriber
rospy.Subscriber("/rpi_sensors/request_data", String,
self.request_handler)
def save_image_bag(frame_id,seq, seconds, nanoseconds, image, ros_image_config): ros_image = Image() img_topic = "/stereo/" + frame_id + "/image_raw" img_config_topic = "/stereo/" + frame_id + "/camera_info" ros_image.header.frame_id = "/stereo/" + frame_id ros_image.header.seq = seq ros_image.header.stamp.secs = seconds ros_image.header.stamp.nsecs = nanoseconds ros_image.height = image.shape[0] #rows ros_image.width = image.shape[1] #columns ros_image.step = image.strides[0] ros_image.encoding = "bgr8" ros_image.data = image.tostring() # check http://wiki.ros.org/image_pipeline/FrameConventions # left and right camera_info should have the same frame_id than the left optical frame # we DO NOT follow this convention!!!! ros_image_config.header.frame_id = frame_id ros_image_config.header.stamp = ros_image.header.stamp ros_image_config.header.seq = seq bag.write(img_topic, ros_image, ros_image.header.stamp) # write image in bag bag.write(img_config_topic, ros_image_config, ros_image_config.header.stamp) # write calibration for the image in bag
def capture(self, req):
rospy.loginfo('Capturing panorama')
if not self.single_capture():
rospy.logerr('Could not trigger Richo Theta, retrying in 1s')
rospy.sleep(1)
while not self.single_capture():
rospy.logerr(
'Could not trigger Richo Theta, resetting the usb driver. Is the camera switched on and set to still image capture mode?'
)
reset_usb_driver()
self.init_camera()
rospy.sleep(2)
# Construct image
file_data = gp.check_result(
gp.gp_file_get_data_and_size(self.camera_file))
img = Img.open(io.BytesIO(file_data))
rospy.loginfo('Panorama captured!')
image = Image(height=img.height,
width=img.width,
encoding="rgb8",
is_bigendian=False,
step=img.width * 3,
data=img.tobytes())
image.header.stamp = self.stamp
image.header.frame_id = 'map' #TODO maybe add something sensible here
self.pub_image.publish(image)
rospy.loginfo('Panorama published!')
# Avoid running out of space. Update: No need: Device holds approx 4,800 photos
#try:
# self.camera.file_delete(self.file_path.folder, self.file_path.name)
#except:
# rospy.logwarn('Delete photo on the Ricoh Theta failed. Camera may eventually run out of storage.')
return TriggerResponse(success=True,
message=self.file_path.folder + '/' +
self.file_path.name)
def makeROS(self, data):
# Given a list of the form [{message type (str)}, {message data (dict)}] received via TCP,
# parse and create a ROS message.
print "Data received (" + str(sys.getsizeof(data[1])) + "): " + data[0]
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = RVIZ_FRAME
if data[0] == '/hardware_joint_states':
pass
elif data[0] == '/ihmc_ros/valkyrie/output/joint_states':
msg = JointState()
msg.header = header
msg.name = data[1]['name']
msg.position = data[1]['position']
self.joints_publisher.publish(msg)
elif data[0] == '/tf':
msg = TFMessage()
msg.transforms = data[1]['transforms']
self.tf_publisher.publish(msg)
elif data[0] == '/multisense/camera/points2':
self.points2_publisher.publish(pc2.create_cloud_xyz32(header,data[1]))
elif data[0] == '/multisense/camera/left/image_rect_color':
msg = Image()
msg.header = header
msg.height = data[1]['height']
msg.width = data[1]['width']
msg.encoding = data[1]['encoding']
msg.is_bigendian = data[1]['is_bigendian']
msg.step = data[1]['step']
msg.data = data[1]['data']
self.image_publisher.publish(msg)
bridge = CvBridge()
left_image = cv2.flip(cv2.flip( bridge.imgmsg_to_cv2(msg, "bgr8") ,0),1)
cv2.imshow("left camera",left_image)
cv2.waitKey(3)
else:
print ("WARNING! Unrecognized data packet received. Ignoring data")
