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")