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()
def test_negative_timeout(): channel = Channel(uri=URI, exchange=EXCHANGE) with pytest.raises(AssertionError): channel.consume(timeout=-1e-10) with pytest.raises(socket.timeout): channel.consume(timeout=0) channel.close()
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()
class TransformationFetcher: def __init__(self, broker_uri=None): self.channel = Channel() if broker_uri is None else Channel(broker_uri) self.subscription = Subscription(self.channel) self.transformations = {} def get_transformation(self, _from, _to): if _from in self.transformations and _to in self.transformations[_from]: return self.transformations[_from][_to] if self._request_transformation(_from, _to): return self.transformations[_from][_to] return None def _request_transformation(self, _from, _to): topic = 'FrameTransformation.{}.{}'.format(_from, _to) self.subscription.subscribe(topic) try: msg = self.channel.consume(timeout=5.0) self.subscription.unsubscribe(topic) except: self.subscription.unsubscribe(topic) return False transformation = msg.unpack(FrameTransformation) if _from not in self.transformations: self.transformations[_from] = {} self.transformations[_from][_to] = to_np(transformation.tf) return True
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()
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_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()
def main(_argv): # Connect to the broker broker = "ampq://*****:*****@10.10.2.1:30000" channel = Channel(broker) # Subscribe to the desired topic subscription = Subscription(channel) camera_id = "CameraGateway." + FLAGS.camera + ".Frame" subscription.subscribe(topic=camera_id) if FLAGS.tiny: yolo = YoloV3Tiny() else: yolo = YoloV3() yolo.load_weights(FLAGS.weights) logging.info('weights loaded') class_names = [c.strip() for c in open(FLAGS.classes).readlines()] logging.info('classes loaded') msg = channel.consume() img = msg.unpack(Image) img = get_np_image(img) img_to_draw = img #img = tf.image.decode_image(img, channels=3) img = tf.expand_dims(img, 0) img = transform_images(img, FLAGS.size) t1 = time.time() boxes, scores, classes, nums = yolo(img) t2 = time.time() logging.info('time: {}'.format(t2 - t1)) logging.info('detections:') for i in range(nums[0]): logging.info('\t{}, {}, {}'.format(class_names[int(classes[0][i])], np.array(scores[0][i]), np.array(boxes[0][i]))) img_to_draw = draw_outputs(img_to_draw, (boxes, scores, classes, nums), class_names) cv2.imwrite(FLAGS.output, img_to_draw) logging.info('output saved to: {}'.format(FLAGS.output))
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
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)
def navigate(goalX, goalY, robotArUco, worldFrame, mapFile, step, robot_size, N_KNN, MAX_EDGE_LEN, show_path): exporter = ZipkinExporter( service_name="Robot.Controller", host_name="10.10.2.13", port="30200", transport=AsyncTransport, ) # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.13:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to the following topics: # - To get the robot's current position in relation where it was turned on, i.e., dead reckoning odometry # - To get the position of the ArUco marker attached to the robot's back #robotArUco = 8 # ArUco marker ID attached to the robot's back #worldFrame = 1000 # Number of the world reference frame in the HPN Intelligent Space awarenessOff(channel) time.sleep(6) # to wait some time to be sure the awareness if off topicGetRobotOdometry = "FrameTransformation.2000.2001" topicGetArUcoRobotBack = "FrameTransformation." + str( robotArUco + 100) + "." + str(worldFrame) subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) # Initialise transformation matrices used for storing odometry and correction pepperPose = np.identity(4) robotOriginToWorld = np.identity(4) lastOdometry = np.identity(4) # Load the frame transformation between the ArUco marker on the robot's back and the robot's base frame. fileName = "frameArUcoRobot.dat" arUcoToRobot = read_frameTransformation(fileName) robotToArUco = inv(arUcoToRobot) print("Goal: ", goalX, " ", goalY) # Localize the robot before start path planning robotLocalized = False notSeen = 0 count = 0 while not robotLocalized: # Source must be the current position of the robot in the world frame message = channel.consume() if (message.topic == topicGetRobotOdometry): print("reading odometry") # get the transformation matrix corresponding to the current rotation and position of the robot lastOdometry = unpackFrameTransformation(message) notSeen = notSeen + 1 print(notSeen) if (message.topic == topicGetArUcoRobotBack): print("correcting odometry") # get the frame transformation betweeb the ArUco marker on the robot's back and the world arUcoToWorld = unpackFrameTransformation(message) # calculates the robot pose in the world frame pepperPose = arUcoToWorld * robotToArUco # calculate the frame transformation needed to correct robot's odometry while the ArUco marker is not seen by the intelligent space robotOriginToWorld = pepperPose * inv(lastOdometry) sourceX = pepperPose[0, 3] sourceY = pepperPose[1, 3] print("x= ", sourceX, " y= ", sourceY) robotLocalized = True notSeen = 0 if notSeen > 30: notSeen = 0 count = count + 1 # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetRobotOdometry) subscription.unsubscribe(topicGetArUcoRobotBack) if count > 4: print("I can't get localized by the Intelligent Space.") print( "Please take me to a spot where the marker on my back can be seen by one of the cameras." ) sys.exit(0) makeTurn(0, 0.3, channel) subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetRobotOdometry) subscription.unsubscribe(topicGetArUcoRobotBack) #sys.exit(0) # Create obstacles ox, oy = create_virtualObstacles() # Call path planning # rx, ry contains the positions in the path rx, ry = PRM_planning(mapFile, sourceX, sourceY, goalX, goalY, ox, oy, robot_size, step, N_KNN, MAX_EDGE_LEN) # Check if a path was found/home/raquel/ProgrammingIS/learningISBristol// #assert len(rx) != 0, 'Cannot found path' if len(rx) == 0: print('Cannot find path') raise SystemError #sys.exit(0) # Plot map points and obstacles if show_path: map_x, map_y = read_map(mapFile, step) plt.plot(ox, oy, ".k") plt.plot(sourceX, sourceY, "^r") plt.plot(goalX, goalY, "^c") plt.plot(map_x, map_y, ".b") plt.grid(True) plt.axis("equal") plt.plot(rx, ry, "-r") plt.plot(rx, ry, "og") plt.show() # Reverse the order of the path (list) returned by the path-planning algorithm # The original list contains the goal at the beginning and the source at the end. We need the reverse rx = rx[::-1] ry = ry[::-1] print(rx) print(ry) #sys.exit(0) # Subscribe to the previous topics subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) i = 0 k = 0 stuck = 0 dist = 100.0 threshold = 0.2 try: while True: try: # listen the channelprint(frameArUcoRobot) message = channel.consume(timeout=0.5) spanr = message.extract_tracing() tracer = Tracer(exporter, spanr) tracer.start_span(name="Navigate") # Check if the message received is the robot's odometry - FrameTransformation type if (message.topic == topicGetRobotOdometry): # get the transformation matrix corresponding to the current rotation and position of the robot lastOdometry = unpackFrameTransformation(message) pepperPose = robotOriginToWorld * lastOdometry #print(pepperPose) elif (message.topic == topicGetArUcoRobotBack): print("Odometry Corrected") # get the transformation matrix corresponding to the current pose of the robot corrected when # it sees an ArUco marker arUcoToWorld = unpackFrameTransformation(message) pepperPose = arUcoToWorld * robotToArUco robotOriginToWorld = pepperPose * inv(lastOdometry) except socket.timeout: print("Time out") # matrix Inverse toPepperFrame = inv(pepperPose) # transform the goal from the world frame to the robot frame posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) distPrevious = dist dist = math.sqrt(posX**2 + posY**2) #print(dist) # If distance to current goal is less than the threshold, pick the next point in the path to be the next goal if dist < threshold: i = i + 1 stuck = 0 print(dist) print("Path index: ", i, " of ", len(rx)) if i == (len(rx) - 1): threshold = 0.5 # If that was the last point in the path, finish navigation. Goal achieved. if i >= len(rx): print("Goal achieved") break # If not the last point in the path, get the next point and converte it to robot frame print("Next point in the path") posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) # Command robot to move commandMoveTo(posX, posY, channel) #commandNavigateTo(posX,posY,channel) # If distance to current goal is greater than the threshold, check if robot is stopped # To check if robot is stopped, calculate the difference between current distance and previous one elif abs(dist - distPrevious ) < 0.005: # if difference is less than 0.5 cm k = k + 1 # accumulate if k == 20: #see if situation remains for 20 times # Robot is stuck. Let's send a move command again print(dist) k = 0 print( "Ooops.... I got stuck.... I will try to move. Stuck = ", stuck) posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) stuck = stuck + 1 if stuck == 4: goBack(-10, 0, channel) stuck = 0 posX, posY, posZ = changeRefFrame( rx[i], ry[i], 0, toPepperFrame) # Command robot to move commandMoveTo(posX, posY, channel) else: commandMoveTo(posX, posY, channel) tracer.end_span() except KeyboardInterrupt: commandMoveTo(0, 0, channel) #awarenessOff(channel) awarenessOn(channel) time.sleep(6)
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 if cid in requests: base_name = requests[cid]['base_name'] frame_id = requests[cid]['frame_id'] annotations_received[base_name][frame_id] = MessageToDict( annotations, preserving_proto_field_name=True, including_default_value_fields=True) del requests[cid] state = State.CHECK_END_OF_VIDEO_AND_SAVE except socket.timeout: state = State.CHECK_FOR_TIMEOUTED_REQUESTS
if len(sys.argv) > 1: broker_uri = sys.argv[1] channel = Channel(broker_uri) subscription = Subscription(channel) exporter = ZipkinExporter( service_name='SkeletonsDetectorRequester', host_name='localhost', port=9411, transport=BackgroundThreadTransport(max_batch_size=100), ) image = cv2.imread('../image.png') tracer = Tracer(exporter) with tracer.span(name='image') as span: cimage = cv2.imencode(ext='.jpeg', img=image, params=[cv2.IMWRITE_JPEG_QUALITY, 80]) data = cimage[1].tobytes() im = Image(data=data) msg = Message(content=im, reply_to=subscription) msg.inject_tracing(span) channel.publish(message=msg, topic='SkeletonsDetector.Detect') cid = msg.correlation_id while True: msg = channel.consume() if msg.correlation_id == cid: skeletons = msg.unpack(ObjectAnnotations) print(skeletons) sys.exit(0)
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), message=set_config_req)
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
def main(_argv): if FLAGS.tiny: yolo = YoloV3Tiny() else: yolo = YoloV3() yolo.load_weights(FLAGS.weights) logging.info('weights loaded') class_names = [c.strip() for c in open(FLAGS.classes).readlines()] logging.info('classes loaded') times = [] # Connect to the broker broker = "ampq://*****:*****@10.10.2.1:30000" channel = Channel(broker) # Subscribe to the desired topic subscription = Subscription(channel) camera_id = "CameraGateway." + FLAGS.camera + ".Frame" subscription.subscribe(topic=camera_id) #fourcc = cv2.VideoWriter_fourcc(*'MJPG') #fourcc = cv2.VideoWriter_fourcc(*'XVID') fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v') out = cv2.VideoWriter(FLAGS.output, fourcc, 5.0, (1288, 728)) for i in range(FLAGS.nframes): msg = channel.consume() img = msg.unpack(Image) img = get_np_image(img) img_to_draw = img #img = tf.image.decode_image(img, channels=3) img = tf.expand_dims(img, 0) img = transform_images(img, FLAGS.size) t1 = time.time() boxes, scores, classes, nums = yolo.predict(img) t2 = time.time() times.append(t2 - t1) times = times[-20:] for i in range(nums[0]): logging.info('\t{}, {}, {}'.format(class_names[int(classes[0][i])], np.array(scores[0][i]), np.array(boxes[0][i]))) rects = get_rects(img_to_draw, (boxes, scores, classes, nums)) img_to_draw = draw_outputs(img_to_draw, (boxes, scores, classes, nums), class_names) objects = centroidTracker.update(rects) # loop over the tracked objects for (objectID, centroid) in objects.items(): # draw both the ID of the object and the centroid of the # object on the output frame text = "{}".format(objectID) cv2.putText(img_to_draw, text, (centroid[0], centroid[1]), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 240, 0), 4) #cv2.circle(frame, (centroid[0], centroid[1]), 3, (0, 255, 0), -1) out.write(img_to_draw) out.release()
def main(): service_name = 'GestureRecognizer.Recognition' log = Logger(name=service_name) op = load_options() channel = Channel(op.broker_uri) log.info('Connected to broker {}', op.broker_uri) exporter = create_exporter(service_name=service_name, uri=op.zipkin_uri) subscription = Subscription(channel=channel, name=service_name) for group_id in list(op.group_ids): subscription.subscribe( 'SkeletonsGrouper.{}.Localization'.format(group_id)) model = GestureRecognizer("model_gesture1_72.00.pth") log.info('Initialize the model') unc = Gauge('uncertainty_total', "Uncertainty about predict") unc.set(0.0) start_http_server(8000) buffer = list() predict_flag = False mean = lambda x: (sum(x) / len(x)) while True: msg = channel.consume() tracer = Tracer(exporter, span_context=msg.extract_tracing()) span = tracer.start_span(name='detection_and_info') annotations = msg.unpack(ObjectAnnotations) skeleton = select_skeletons(annotations=annotations, min_keypoints=op.skeletons.min_keypoints, x_range=op.skeletons.x_range, y_range=op.skeletons.y_range) if skeleton is None: tracer.end_span() continue skl = Skeleton(skeleton) skl_normalized = skl.normalize() pred, prob, uncertainty = model.predict(skl_normalized) if pred == 0 and predict_flag is False: pass elif pred != 0 and predict_flag is False: initial_time = time.time() predict_flag = True buffer.append(uncertainty) elif pred != 0 and predict_flag is True: buffer.append(uncertainty) elif pred == 0 and predict_flag is True: predict_flag = False exec_time = time.time() - initial_time if exec_time >= op.exec_time: unc.set(mean(buffer)) log.info("execution_ms: {}, buffer_mean: {}", (exec_time * 1000), mean(buffer)) buffer = [] tracer.end_span() info = { 'prediction': pred, 'probability': prob, 'uncertainty': uncertainty, 'took_ms': { 'service': round(span_duration_ms(span), 2) } } log.info('{}', str(info).replace("'", '"'))
def call_via_aruco(arucoID): # parameters for navigation mapFile = "../lesson09_mapping_with_ArUco/map3011.dat" robotArUco = 8 worldFrame = 1000 step = 2 robotRadius = .8 N_KNN = 40 # number of edge from one sampled point MAX_EDGE_LEN = 2.5 # [m] Maximum edge length show_path = False # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.23:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to the following topic: # - To get the position of the ArUco marker used as a calling signal for the robot topicGetArUcoLocation = "FrameTransformation."+str(arucoID+100)+"."+str(worldFrame) subscription.subscribe(topicGetArUcoLocation) # Localise the marker for calling the robot markerLocalized = False notSeen = 0 count = 0 while not markerLocalized: # Source must be the current position of the robot in the world frame message = channel.consume() notSeen = notSeen + 1 if (message.topic == topicGetArUcoLocation): print("Found ArUco") # get the frame transformation betweeb the ArUco marker on the robot's back and the world arUcoToWorld = unpackFrameTransformation (message) goalX = arUcoToWorld[0,3] goalY = arUcoToWorld[1,3] print("x= ",goalX," y= ",goalY) markerLocalized = True notSeen = 0 if notSeen > 30: notSeen = 0 count = count + 1 print("Try to turn the marker or show to other camera.") if count > 4: print("I can't localize the marker in the Intelligent Space.") sys.exit(0) # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetArUcoLocation) # call the robot navigate(goalX,goalY,robotArUco, worldFrame, mapFile,step,robotRadius,N_KNN,MAX_EDGE_LEN,show_path)
channel = Channel("amqp://10.10.2.23:30000") # Subscribe to the desired topic(s) subscription = Subscription(channel) topic02 = "ArUco.7.FrameTransformations" # get relation between camera 7 and the ArUco detected subscription.subscribe(topic02) # Blocks forever waiting for message diretArucoPose = np.matrix('0,0,0,0;0,0,0,0;0,0,0,0;0,0,0,0') matrix = np.matrix('0,0,0,0;0,0,0,0;0,0,0,0;0,0,0,0') # Compare the Frame Transforamtion obtained directly from the Aruco detecion service when an Aruco Marker is seen # with the calculated Frame Transforamtion when considering the camera extrinsic matrix and the Aruco Marker position in the world while True: message = channel.consume() # Get the Frame Transformation that represents the pose of the Aruco mark in the camera 7 reference frame # Then multiply by the extrinsic matrix of camera 7 to obtain the Aruco pose in world reference frame if message.topic == topic02: # Message returns a list of Frame Transformations frameTransList = message.unpack(FrameTransformations) # Check if there is any element (Frame Transformation) in the list if frameTransList.tfs: # Go through the list for frameTrans in frameTransList.tfs: # Check if the Frame transformation has a tensor containing the transformation matrix if frameTrans.HasField("tf"): tensor = frameTrans.tf print(frameTrans) #arucoID = frameTrans - 100
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
# Cria o ServiceProvider passando o channel server = ServiceProvider(channel) logs = LogInterceptor() # Log requests to console server.add_interceptor(logs) # Linka cada tipo de mensagem recebida a um metodo do gateway server.delegate(topic=service_name + ".GetConfig", request_type=Empty, reply_type=RobotConfig, function=gateway.get_configuration) server.delegate(topic=service_name + ".SetConfig", request_type=RobotConfig, reply_type=Empty, function=gateway.set_configuration) ATSPlog.info("event=InitAllDone") while(1): try: # Espera receber uma mensagem no canal dentro de um timeout message = channel.consume(timeout=max(gateway.next_deadline()-time(),0)) # Verifica se essa mensagem esta dentro dos delegates if server.should_serve(message): server.serve(message) # Executa o pedido da mensagem except socket.timeout: pass # Publica os sensores gateway.run() # Exemplo de uso do Logger: # >>> from is_wire.core import Logger # >>> log = Logger(name="Ronan") # >>> log.info("vel={:.2f}", 32.0320909)
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()
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
from __future__ import print_function from is_wire.core import Channel, Subscription from is_msgs.camera_pb2 import FrameTransformation import numpy as np import json import sys if len(sys.argv) < 3: print("USAGE: python consume.py <FROM> <HINTS> <TO>") sys.exit(0) c = Channel(json.load(open("../etc/conf/options.json"))["broker_uri"]) s = Subscription(c) np.set_printoptions(precision=3, suppress=True) topic = "FrameTransformation.%s" % ".".join(sys.argv[1:]) s.subscribe(topic) while True: message = c.consume() transformation = message.unpack(FrameTransformation) tf = transformation.tf if len(tf.shape.dims): T = np.matrix(tf.doubles).reshape(tf.shape.dims[0].size, tf.shape.dims[1].size) print(T[:, 3], np.linalg.norm(T[:3, 3]))