class robot_cli: def __init__(self,host,port=7777): rospy.init_node('robot', anonymous=False) self.bridge = CvBridge() self.tcpCliSock = socket(AF_INET, SOCK_STREAM) addr = (host, port) self.tcpCliSock.connect(addr) print "The connection is successful" self.comImage = CompressedImage() self.image=Image() self.time = Clock() self.box_pos = ModelStates() self.isEnd=False def get_comImage(self): """ :return: """ if not self.isEnd: message=b"cim" try: self.tcpCliSock.send(message) flag= self.tcpCliSock.recv(4) length=struct.unpack('i',flag)[0] data="" while len(data)<length: data+=self.tcpCliSock.recv(length) try: self.comImage.deserialize(data) cv_image = None try: cv_image = self.bridge.compressed_imgmsg_to_cv2(self.comImage, "bgr8") except CvBridgeError as e: print(e) return cv_image except genpy.DeserializationError: print rospy.loginfo("deserialize comImage failed!") except Exception,msg: print msg self.isEnd=True
def receive_image(UDP_PORT, timeout_limit, name, msg): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) sock.settimeout(timeout_limit) sockets_list.append(sock) bridge = CvBridge() while True: try: data = sock.recv(65536) if name == 'left' or name == 'right': image_data = CompressedImage() image_data.deserialize(data) image = bridge.compressed_imgmsg_to_cv2(image_data) # np_arr = np.fromstring(data, np.uint8) # image = cv2.imdecode(np_arr,cv2.IMREAD_COLOR) cv2.imwrite('{}.png'.format(name), image) print('{} saved'.format(msg)) except timeout: print('Error: {} Timeout'.format(msg)) sys.exit(1)