def test_channel(): channel = Channel(uri=URI, exchange=EXCHANGE) subscription = Subscription(channel) subscription.subscribe("MyTopic.Sub.Sub") struct = Struct() struct.fields["value"].number_value = 666.0 sent = Message(struct) sent.reply_to = subscription sent.created_at = int(1000 * now()) / 1000.0 sent.timeout = 1.0 sent.topic = "MyTopic.Sub.Sub" channel.publish(message=sent) received = channel.consume(timeout=1.0) assert sent.reply_to == received.reply_to assert sent.subscription_id == received.subscription_id assert sent.content_type == received.content_type assert sent.body == received.body assert sent.status == received.status assert sent.topic == received.topic assert sent.correlation_id == received.correlation_id assert sent.timeout == received.timeout assert sent.metadata == received.metadata assert sent.created_at == received.created_at assert str(sent) == str(received) struct2 = received.unpack(Struct) assert str(struct) == str(struct2) assert struct == struct2 channel.close()
def test_serve(): channel = Channel(uri=URI, exchange=EXCHANGE) service = ServiceProvider(channel) topic = "MyService" service.delegate(topic, my_service, Struct, Int64Value) subscription = Subscription(channel) sent = Message(content="body".encode('latin')) channel.publish(sent, topic=subscription.name) recv = channel.consume(timeout=1.0) assert recv.subscription_id == subscription.id # Trying to serve a message from another subscription should fail assert service.should_serve(recv) is False with pytest.raises(RuntimeError): service.serve(recv) sent.topic = topic channel.publish(message=sent) recv = channel.consume(timeout=1.0) assert service.should_serve(recv) is True service.serve(recv) channel.close()
class BrokerEvents(object): def __init__(self, broker_uri, management_uri, log_level): self.log = Logger(name="BrokerEvents", level=log_level) self.log.debug("broker_uri='{}'", broker_uri) self.channel = Channel(broker_uri) self.subscription = Subscription(self.channel) self.subscription.subscribe(topic="binding.*") self.log.debug("management_uri='{}'", management_uri) self.consumers = self.query_consumers_http(management_uri) def run(self): while True: msg = self.channel.consume() self.log.debug("topic='{}' metadata={}", msg.topic, msg.metadata) if msg.metadata["destination_kind"] != "queue" or \ msg.metadata["source_name"] != "is": continue event = msg.topic.split('.')[-1] topic = msg.metadata["routing_key"] queue = msg.metadata["destination_name"] if event == "created": self.consumers.info[topic].consumers.append(queue) elif event == "deleted": self.consumers.info[topic].consumers.remove(queue) if len(self.consumers.info[topic].consumers) == 0: del self.consumers.info[topic] self.log.info("event='{}' topic='{}' queue='{}'", event, topic, queue) self.channel.publish( Message(content=self.consumers), topic="BrokerEvents.Consumers", ) @staticmethod def query_consumers_http(management_uri): reply = requests.get(management_uri + "/api/bindings") if reply.status_code != 200: why = "Failed to query management API, code={}".format( reply.status_code) raise RuntimeError(why) bindings = reply.json() bindings = [ b for b in bindings if b["destination_type"] == "queue" and b["source"] == "is" ] consumers = ConsumerList() for b in bindings: consumers.info[b["routing_key"]].consumers.append(b["destination"]) return consumers
def test_multi_subscription(): channel = Channel(uri=URI, exchange=EXCHANGE) message = Message() subscription1 = Subscription(channel) subscription2 = Subscription(channel) channel.publish(message, topic=subscription1.name) recv = channel.consume(timeout=1.0) assert recv.subscription_id == subscription1.name channel.publish(message, topic=subscription2.name) recv = channel.consume(timeout=1.0) assert recv.subscription_id == subscription2.name channel.close()
def test_body(size): channel = Channel(uri=URI, exchange=EXCHANGE) subscription = Subscription(channel) subscription.subscribe("MyTopic.Sub.Sub") sent = Message() sent.reply_to = subscription sent.topic = "MyTopic.Sub.Sub" sent.body = bytes(bytearray(range(256)) * int(size)) channel.publish(message=sent) received = channel.consume(timeout=1.0) assert repr(sent.body) == repr(received.body) assert sent.body == received.body channel.close()
def test_propagation(): topic = "span_test" channel = Channel(uri=URI, exchange=EXCHANGE) subscription = Subscription(channel) subscription.subscribe(topic) tracer = Tracer() with tracer.span(name="span_name") as span: span_id = span.span_id message = Message() message.body = "body".encode('latin1') message.inject_tracing(span) channel.publish(topic=topic, message=message) message2 = channel.consume(timeout=1.0) span_context = message2.extract_tracing() assert span_context.span_id == span_id assert span_context.trace_id == tracer.tracer.span_context.trace_id
def run(self, id, broker_uri): service_name = "CameraGateway.{}".format(id) channel = Channel(broker_uri) server = ServiceProvider(channel) logging = LogInterceptor() server.add_interceptor(logging) server.delegate(topic=service_name + ".GetConfig", request_type=FieldSelector, reply_type=CameraConfig, function=self.get_config) server.delegate(topic=service_name + ".SetConfig", request_type=CameraConfig, reply_type=Empty, function=self.set_config) self.driver.start_capture() self.logger.info("Listening for requests") while True: image = self.driver.grab_image() channel.publish(Message(content=image), topic=service_name + ".Frame") pose = self.driver.get_pose() frameTransList = FrameTransformations() frameTransList.tfs.extend([pose]) channel.publish(Message(content=frameTransList), topic=service_name + ".FrameTransformations") try: message = channel.consume(timeout=0) if server.should_serve(message): server.serve(message) except socket.timeout: pass
def test_empty_topic(): channel = Channel(uri=URI, exchange=EXCHANGE) message = Message(content="body".encode('latin')) with pytest.raises(RuntimeError): channel.publish(message) with pytest.raises(RuntimeError): channel.publish(message, topic="") subscription = Subscription(channel) channel.publish(message, topic=subscription.name) recv = channel.consume(timeout=1.0) assert recv.body == message.body message.topic = subscription.name channel.publish(message) recv = channel.consume(timeout=1.0) assert recv.body == message.body channel.close()
channel = Channel(op_config.broker_uri) ATSPlog.info("event=ChannelInitDone") # Inscreve nos topicos que deseja receber mensagem subscription = Subscription(channel) subscription.subscribe(pose_topic) subscription.subscribe(sonar_topic) ATSPlog.info("SubscriptionsDone") # Envia mensagem de getConfig get_req = Message(reply_to=subscription) # Salva correlation_id do request cor_id = get_req.correlation_id # print("cor_id: ", cor_id) # Broadcast message to anyone interested (subscribed) channel.publish(message=get_req, topic=service_name + ".GetConfig") # Envia msg de setConfig config = RobotConfig() config.speed.linear = 0.5 config.speed.angular = 0.0 set_req = Message(content=config, reply_to=subscription) # Broadcast message to anyone interested (subscribed) channel.publish(topic=service_name + ".SetConfig", message=set_req) print("Andou!") time.sleep(3) print("Parou?")
from is_wire.core import Channel, Subscription, Message from is_msgs.robot_pb2 import RobotConfig import os from sys import argv uri = os.environ[ "BROKER_URI"] if "BROKER_URI" in os.environ else "amqp://10.10.2.20:30000" channel = Channel(uri) subscription = Subscription(channel) config = RobotConfig() config.speed.linear = float(argv[1] or 0.0) config.speed.angular = float(argv[2] if len(argv) is 3 else 0.0) channel.publish(message=Message(content=config, reply_to=subscription), topic="RobotGateway.0.SetConfig") reply = channel.consume(timeout=1.0) print reply.status channel.publish(message=Message(reply_to=subscription), topic="RobotGateway.0.GetConfig") reply = channel.consume(timeout=1.0) print reply.unpack(RobotConfig)
base_folder=options.folder) while True: if state == State.MAKE_REQUESTS: state = State.RECV_REPLIES if len(requests) < MIN_REQUESTS: while len(requests) <= MAX_REQUESTS: base_name, frame_id, frame = frame_fetcher.next() if frame is None: if len(requests) == 0: state = State.EXIT break pb_image = make_pb_image(frame) msg = Message(content=pb_image, reply_to=subscription) msg.timeout = DEADLINE_SEC channel.publish(msg, topic='SkeletonsDetector.Detect') requests[msg.correlation_id] = { 'content': pb_image, 'base_name': base_name, 'frame_id': frame_id, 'requested_at': time.time() } continue elif state == State.RECV_REPLIES: try: msg = channel.consume(timeout=1.0) if msg.status.ok(): annotations = msg.unpack(ObjectAnnotations) cid = msg.correlation_id
posZ = float(goalPepperFrame[2]) if (True): goal = Position() goal.x = posX goal.y = posY goal.z = posZ #print(goal.x,"#",goal.y,"#",goal.z) goalMessage = Message() goalMessage.pack(goal) goalMessage.topic = "RobotGateway.0.NavigateTo" else: goal = Pose() goal.position.x = posX goal.position.y = posY goal.position.z = posZ goal.orientation.yaw = 90 * 3.14 / 180 goal.orientation.pitch = 0 goal.orientation.roll = 0 goalMessage = Message() goalMessage.pack(goal) goalMessage.topic = "RobotGateway.0.MoveTo" channel.publish(goalMessage) if math.sqrt(posX**2 + posY**2) < 0.40: print("Goal achieved") break except KeyboardInterrupt: pass
Loading CameraConfig protobuf from a json file """ with open('camera_config.json', 'r') as f: try: set_config = Parse(f.read(), CameraConfig()) log.info('CameraConfig:\n{}', set_config) except Exception as ex: log.critical('Unable to camera settings. \n{}', ex) """ Another way to create a CameraConfig message """ set_config = CameraConfig( sampling=SamplingSettings(frequency=FloatValue(value=5.0)), image=ImageSettings( resolution=Resolution(width=720, height=540), color_space=ColorSpace(value=ColorSpaces.Value('GRAY')))) msg = Message() msg.reply_to = subscription msg.topic = 'CameraGateway.{}.SetConfig'.format(camera_id) msg.pack(set_config) channel.publish(msg) while True: msg = channel.consume() if msg.status.code == StatusCode.OK: log.info('Configuration applied on camera \'{}\'.', camera_id) else: log.warn('Can\'t apply configuration:\n{}', msg.status) sys.exit(0)
from is_wire.core import Channel, Subscription, Message from is_msgs.robot_pb2 import RobotConfig import json import time options = json.load(open("../etc/conf/options.json")) channel = Channel(options["broker_uri"]) subscription = Subscription(channel) rid = options["robot_parameters"]["id"] for i in range(10): config = RobotConfig() config.speed.linear = -0.2 set_req = Message(content=config, reply_to=subscription) channel.publish(topic="RobotGateway.{}.SetConfig".format(rid), message=set_req) set_rep = channel.consume(timeout=0.05) print("set:", set_rep.status) get_req = Message(reply_to=subscription) channel.publish(topic="RobotGateway.{}.GetConfig".format(rid), message=get_req) get_rep = channel.consume(timeout=0.05) print("get:", get_rep.status, get_rep.unpack(RobotConfig)) time.sleep(0.1) config = RobotConfig() config.speed.linear = 0.0 set_config_req = Message(content=config, reply_to=subscription) channel.publish(topic="RobotGateway.{}.SetConfig".format(rid),
from is_msgs.common_pb2 import FieldSelector import os uri = os.environ[ "BROKER_URI"] if "BROKER_URI" in os.environ else "amqp://10.10.2.20:30000" channel = Channel(uri) subscription = Subscription(channel) config = CameraConfig() config.sampling.frequency.value = 10.0 config.image.resolution.width = 640 config.image.resolution.height = 480 config.image.color_space.value = ColorSpaces.Value("GRAY") # config.camera.exposure.ratio = 0.1 # config.camera.gain.ratio = 0.05 # config.camera.gain.automatic = True channel.publish(Message(content=config, reply_to=subscription), topic="CameraGateway.10.SetConfig") reply = channel.consume(timeout=1.0) print reply selector = FieldSelector(fields=[CameraConfigFields.Value("ALL")]) channel.publish(Message(content=selector, reply_to=subscription), topic="CameraGateway.10.GetConfig") reply = channel.consume(timeout=1.0) print reply.unpack(CameraConfig)
from __future__ import print_function from is_wire.core import Channel, Subscription, Message from np_pb import to_tensor, to_np from is_msgs.common_pb2 import Tensor import numpy as np # This program shows how to publish a message using # the Intelligent Space framework # Connect to the broker channel = Channel("amqp://10.10.2.23:30000") # Create the message that is a matrix message = Message() test = np.array([[1, 2, 3, 4],[ 5, 6, 7, 8],[ 9, 10, 11, 12],[ 13, 14, 15, 16]]) # Convert to Tensor test2 = to_tensor(test) message.pack(test2) # Define topic name Topic= "Aula01" message.topic = Topic # Publish message under the topic channel.publish(message)
def run(self, id, broker_uri): service_name = "RobotGateway.{}".format(id) channel = Channel(broker_uri) server = ServiceProvider(channel) logging = LogInterceptor() server.add_interceptor(logging) server.delegate(topic=service_name + ".GetConfig", request_type=FieldSelector, reply_type=RobotConfig, function=self.get_config) server.delegate(topic=service_name + ".SetConfig", request_type=RobotConfig, reply_type=Empty, function=self.set_config) server.delegate(topic=service_name + ".NavigateTo", request_type=Position, reply_type=Empty, function=self.navigate_to) server.delegate(topic=service_name + ".MoveTo", request_type=Pose, reply_type=Empty, function=self.move_to) server.delegate(topic=service_name + ".PauseAwareness", request_type=Empty, reply_type=Empty, function=self.pause_awareness) server.delegate(topic=service_name + ".ResumeAwareness", request_type=Empty, reply_type=Empty, function=self.resume_awareness) server.delegate(topic=service_name + ".SetAwareness", request_type=Struct, reply_type=Empty, function=self.set_awareness) #server.delegate( # topic=service_name + ".SetAwarenessOff", # request_type=Empty, # reply_type=Empty, # function=self.set_awareness_off) self.logger.info("Listening for requests") while True: pose = self.driver.get_base_pose() frameTransList = FrameTransformations() frameTransList.tfs.extend([pose]) self.logger.debug("Publishing pose") channel.publish(Message(content=frameTransList), topic=service_name + ".FrameTransformations") try: message = channel.consume(timeout=0) if server.should_serve(message): server.serve(message) except socket.timeout: pass
from is_wire.core import Channel, Subscription, Message, Logger from utils import load_options log = Logger(name='ConfigureCameras') options = load_options() c = Channel(options.broker_uri) sb = Subscription(c) cids = {} for camera in options.cameras: log.info("Camera: {}\nConfiguration: {}", camera.id, camera.config) msg = Message() msg.pack(camera.config) msg.reply_to = sb msg.topic = 'CameraGateway.{}.SetConfig'.format(camera.id) c.publish(msg) cids[msg.correlation_id] = {'camera': camera.id, 'ok': False} while True: msg = c.consume() if msg.correlation_id in cids: camera = cids[msg.correlation_id]['camera'] cids[msg.correlation_id]['ok'] = True log.info('Camera: {} Reply: {}', camera, msg.status) if all(map(lambda x: x[1]['ok'], cids.items())): break
import json from is_wire.core import Channel, Subscription, Message from google.protobuf.struct_pb2 import Struct channel = Channel(json.load(open('options.json', 'r'))['broker_uri']) subscription = Subscription(channel) groups = [[0, 1, 2, 3, 4], [0, 2, 4, 6], [5, 6, 7]] config = Struct() config["max_error"] = 20.0 config["min_score"] = 0.5 for group in groups: topic = 'Skeletons.Localization.{}.Config'.format('.'.join(map(str, group))) print(topic) msg = Message(content=config) msg.reply_to = subscription channel.publish(msg, topic=topic) while True: msg = channel.consume() print(msg)
class PING: def __init__(self, *args, **kwargs): self.ICMP_ECHO_REQUEST = 8 self.icmp_seq = 1 self.packet_loss = 0 self.loss_perc = 0 self.last_seq = 0 self.delay_list = [] self.file = open("ping_result.txt", "w") self.connection = Channel(**kwargs) self.status = PingStatus() def checksum(self, source_string): # I'm not too confident that this is right but testing seems to # suggest that it gives the same answers as in_cksum in ping.c. sum = 0 count_to = (len(source_string) / 2) * 2 count = 0 while count < count_to: this_val = ord(source_string[count + 1]) * 256 + ord( source_string[count]) sum = sum + this_val sum = sum & 0xffffffff # Necessary? count = count + 2 if count_to < len(source_string): sum = sum + ord(source_string[len(source_string) - 1]) sum = sum & 0xffffffff # Necessary? sum = (sum >> 16) + (sum & 0xffff) sum = sum + (sum >> 16) answer = ~sum answer = answer & 0xffff # Swap bytes. Bugger me if I know why. answer = answer >> 8 | (answer << 8 & 0xff00) return answer def create_packet(self, id): """Create a new echo request packet based on the given "id".""" # Header is type (8), code (8), checksum (16), id (16), sequence (16) header = struct.pack('bbHHh', self.ICMP_ECHO_REQUEST, 0, 0, id, self.icmp_seq) data = 192 * 'Q' # Calculate the checksum on the data and the dummy header. my_checksum = self.checksum(header + data) # Now that we have the right checksum, we put that in. It's just easier # to make up a new header than to stuff it into the dummy. header = struct.pack('bbHHh', self.ICMP_ECHO_REQUEST, 0, socket.htons(my_checksum), id, self.icmp_seq) return header + data def do_one(self, dest_addr, timeout=1): """ Sends one ping to the given "dest_addr" which can be an ip or hostname. "timeout" can be any integer or float except negatives and zero. Returns either the delay (in seconds) or None on timeout and an invalid address, respectively. """ try: my_socket = socket.socket(socket.AF_INET, socket.SOCK_RAW, ICMP_CODE) except: pass try: host = socket.gethostbyname(dest_addr) except: pass # Maximum for an unsigned short int c object counts to 65535 so # we have to sure that our packet id is not greater than that. packet_id = int((id(timeout) * random.random()) % 65535) packet = self.create_packet(packet_id) while packet: # The icmp protocol does not use a port, but the function # below expects it, so we just give it a dummy port. sent = my_socket.sendto(packet, (dest_addr, 1)) packet = packet[sent:] delay = self.receive_ping(my_socket, packet_id, time.time(), timeout) my_socket.close() return delay def receive_ping(self, my_socket, packet_id, time_sent, timeout): # Receive the ping from the socket. time_left = timeout while True: started_select = time.time() ready = select.select([my_socket], [], [], time_left) how_long_in_select = time.time() - started_select if ready[0] == []: # Timeout return time_received = time.time() rec_packet, addr = my_socket.recvfrom(1024) icmp_header = rec_packet[20:28] type, code, checksum, p_id, sequence = struct.unpack( 'bbHHh', icmp_header) if self.icmp_seq - self.last_seq > 1: loss = self.icmp_seq - self.last_seq - 1 self.packet_loss = self.packet_loss + loss self.last_seq = self.icmp_seq if p_id == packet_id: return time_received - time_sent time_left -= time_received - time_sent if time_left <= 0: return def ping(self, dest_addr, count, timeout): """ Sends one ping to the given "dest_addr" which can be an ip or hostname. "timeout" can be any integer or float except negatives and zero. "count" specifies how many pings will be sent. Displays the result on the screen. """ print('ping {}...'.format(dest_addr)) for self.icmp_seq in range(count): delay = self.do_one(dest_addr, timeout) if delay == None: print('failed. (Timeout within {} seconds.)'.format(timeout)) else: delay = round(delay * 1000.0, 4) print('from {} icmp_seq = {} time = {} ms'.format( dest_addr, (self.icmp_seq + 1), delay)) latency = delay self.status.delay = latency self.connection.publish(topic="Connection.Status", message=Message(content=self.status)) self.file.write('from {} icmp_seq = {} time = {} ms'.format( dest_addr, (self.icmp_seq + 1), delay) + '\n') self.seq_total = self.icmp_seq + 1 self.delay_list.append(latency) time.sleep(1) self.loss_perc = self.percent(self.packet_loss, self.seq_total) self.min_delay = min(self.delay_list) self.max_delay = max(self.delay_list) self.avg_delay = round(sum(self.delay_list) / len(self.delay_list), 4) print('{} packets transmitted, {} received, {}% packet loss'.format( self.seq_total, (self.seq_total - self.packet_loss), self.loss_perc)) print('rtt min/avg/max = {}/{}/{}'.format(self.min_delay, self.avg_delay, self.max_delay)) self.status.max_delay = self.max_delay self.status.min_delay = self.min_delay self.status.avg_delay = self.avg_delay self.status.pkt_tx = self.seq_total self.status.pkt_rx = (self.seq_total - self.packet_loss) self.status.pkt_loss = self.packet_loss self.status.perc_loss = self.loss_perc self.connection.publish(topic="Connection.Status", message=Message(content=self.status)) self.file.write( '\n' + '{} packets transmitted, {} received, {}% packet loss'.format( self.seq_total, (self.seq_total - self.packet_loss), self.loss_perc) + '\n') self.file.write('rtt(ms) min/avg/max = {}/{}/{}'.format( self.min_delay, self.avg_delay, self.max_delay)) self.file.close def percent(self, num1, num2): num1 = float(num1) num2 = float(num2) percentage = '{0:.1f}'.format((num1 / num2 * 100)) return percentage
elif args.type == 'eight': task = lemniscate_of_bernoulli(shape=(args.x, args.y), center=(0, 0), lap_time=25, rate=args.rate) else: task = stop() channel = Channel(options["broker_uri"]) subscription = Subscription(channel) prefix = "RobotController.{}".format(options["parameters"]["robot_id"]) set_task_topic = "{}.SetTask".format(prefix) progress_topic = "{}.Progress".format(prefix) message = Message(content=task, reply_to=subscription) channel.publish(message, topic=set_task_topic) reply = channel.consume(timeout=1.0) if not reply.status.ok(): raise Exception(reply.status.why) print(reply.unpack(RobotTaskReply)) subscription.subscribe(progress_topic) while True: try: message = channel.consume() if message.topic == progress_topic: print(message.unpack(RobotControllerProgress)) except KeyboardInterrupt: channel.publish(Message(content=stop()), topic=set_task_topic) sys.exit()
from is_wire.core import Channel, Subscription, Message, Logger from is_msgs.common_pb2 import Position import os from sys import argv, exit log = Logger(name="client") uri = os.environ[ "BROKER_URI"] if "BROKER_URI" in os.environ else "amqp://10.10.2.20:30000" log.info("uri={}", uri) if len(argv) != 3: log.error("Usage: python navigate.py <X> <Y>") exit(-1) channel = Channel(uri) subscription = Subscription(channel) position = Position() position.x = float(argv[1]) position.y = float(argv[2]) channel.publish(message=Message(content=position, reply_to=subscription), topic="RobotGateway.0.NavigateTo") reply = channel.consume(timeout=1.0) log.info("status={}", reply.status)
def main(): service_name = "CameraGateway" log = Logger(service_name) options = load_options() camera = CameraGateway(fps=options["fps"]) publish_channel = Channel(options['broker_uri']) rpc_channel = Channel(options['broker_uri']) server = ServiceProvider(rpc_channel) logging = LogInterceptor() server.add_interceptor(logging) server.delegate(topic=service_name + ".*.GetConfig", request_type=FieldSelector, reply_type=CameraConfig, function=camera.get_config) server.delegate(topic=service_name + ".*.SetConfig", request_type=CameraConfig, reply_type=Empty, function=camera.set_config) exporter = create_exporter(service_name=service_name, uri=options["zipkin_uri"]) while True: # iterate through videos listed for video in options['videos']: # id of the first sequence of videos person_id = video['person_id'] gesture_id = video['gesture_id'] # getting the path of the 4 videos video_files = { cam_id: os.path.join( options['folder'], 'p{:03d}g{:02d}c{:02d}.mp4'.format(person_id, gesture_id, cam_id)) for cam_id in options["cameras_id"] } for iteration in range(video['iterations']): info = { "person": person_id, "gesture": gesture_id, "iteration": iteration } log.info('{}', str(info).replace("'", '"')) # object that let get images from multiples videos files video_loader = FramesLoader(video_files) # iterate through all samples on video while True: time_initial = time.time() # listen server for messages about change try: message = rpc_channel.consume(timeout=0) if server.should_serve(message): server.serve(message) except socket.timeout: pass frame_id, frames = video_loader.read() for cam in sorted(frames.keys()): tracer = Tracer(exporter) span = tracer.start_span(name='frame') pb_image = to_pb_image(frames[cam]) msg = Message(content=pb_image) msg.inject_tracing(span) topic = 'CameraGateway.{}.Frame'.format(cam) publish_channel.publish(msg, topic=topic) tracer.end_span() took_ms = (time.time() - time_initial) * 1000 dt = (1 / camera.fps) - (took_ms / 1000) if dt > 0: time.sleep(dt) info = { "sample": frame_id, "took_ms": took_ms, "wait_ms": dt * 1000 } log.info('{}', str(info).replace("'", '"')) if frame_id >= (video_loader.num_samples - 1): video_loader.release() del video_loader gc.collect() break if options['loop'] is False: break
pepperPosY) + ' Z: ' + str(pepperPosZ) # Separate the rotation angles rotationMatrix = pepperPose[0:3, 0:3] angles = rotationMatrixToEulerAngles(rotationMatrix) strRotation = 'Yaw: ' + str(angles[2]) + ' Pitch: ' + str( angles[1]) + ' Roll: ' + str(angles[0]) # Print position and orinetation on the screen stdscr.addstr(5, 20, strPosition) stdscr.addstr(8, 20, strRotation) stdscr.addstr(12, 20, "Reading normal odometry") # Convert the transformation matrix to tensor to be sent as a message msgContent = to_tensor(np.asarray(pepperPose)) messageRobotPose.pack(msgContent) channel.publish(messageRobotPose) elif (message.topic == "FrameTransformation.2001.1003"): # unpack the message according to its format frameTransf = message.unpack(FrameTransformation) tensor = frameTransf.tf # get the transformation matrix corresponding to the current pose of the robot corrected when # it sees an ArUco marker robotToWorld = np.matrix(tensor.doubles).reshape( tensor.shape.dims[0].size, tensor.shape.dims[1].size) stdscr.addstr( 14, 20, "Both robot and IS saw the ArUco marker. Correcting odometry.") # waits for a key hit but doesn't block the program