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 __init__(self, channel, max_requests, min_requests=None, log_level=Logger.INFO, zipkin_exporter=None): if min_requests is None: min_requests = max_requests if min_requests < 0: min_requests = 0 if min_requests > max_requests: raise Exception("'min_requests' must be lower than 'max_requests'") self._channel = channel self._subscription = Subscription(self._channel) self._do_tracing = zipkin_exporter is not None self._zipkin_exporter = zipkin_exporter self._log = Logger(name='RequestManager') self._log.set_level(level=log_level) self._min_requests = min_requests self._max_requests = max_requests self._can_request = True self._requests = {}
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 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
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_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 test_rpc(): channel = Channel(uri=URI, exchange=EXCHANGE) service = ServiceProvider(channel) service.add_interceptor(LogInterceptor()) service.delegate("MyService", my_service, Struct, Int64Value) subscription = Subscription(channel) def request_serve_consume(number): struct = Struct() struct.fields["value"].number_value = number message = Message(struct) message.reply_to = subscription message.pack(struct) channel.publish(topic="MyService", message=message) request = channel.consume(timeout=1.0) assert request.body == message.body service.serve(request) assert request.unpack(Struct) == struct return channel.consume(timeout=1.0) reply = request_serve_consume(90.0) int64 = reply.unpack(Int64Value) assert int64.value == 90 assert reply.status.ok() is True reply = request_serve_consume(666.0) assert reply.status.ok() is False assert reply.status.code == StatusCode.FAILED_PRECONDITION reply = request_serve_consume(10.0) assert reply.status.ok() is False assert reply.status.code == StatusCode.INTERNAL_ERROR channel.close()
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 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("'", '"'))
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)
from is_wire.core import Channel, Message, Subscription from is_msgs.common_pb2 import Position, Pose from is_msgs.camera_pb2 import FrameTransformation import numpy as np # from numpy.linalg import inv import math # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.20:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to get the relation from the initial reference frame where the robot was # turned on and its current reference frame (dead reckoning odometry) ##################subscription.subscribe("FrameTransformation.1000.2000") subscription.subscribe("FrameTransformation.2000.2001") subscription.subscribe("FrameTransformation.2001.1003") # Goal in the World Frame # Change the goals value to make the robot move to different coordinates. goalX = 0 goalY = 0 #goalWorldFrame = math.reshape(math.matrix([goalX, goalY, 0, 1]), [4, 1]) goalWorldFrame = np.matrix([goalX, goalY, 0, 1]).T # Initialize transformation matrices used for storing odometry and correction pepperPose = np.identity(4) robotToWorld = np.identity(4) # Change this variable to test between NavigateTo and MoveTo
parser.add_argument('-y', type=float, default=0) parser.add_argument('--rate', type=float, default=10) args = parser.parse_args() if args.type == 'goto': task = final_position(target=(args.x, args.y), rate=args.rate) 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:
skeletons = transform_object_annotations(annotations, tf, worldFrame) return skeletons else: return annotations #### Main Program ####################### # Colleting all the 3D positions where skeletons were detected # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.23:30000") # Create a subscription subscription = Subscription(channel) # Subscribe in all the topics that can return the skeletons localization in the different areas # In this case the areas are 0, 1 and 2 topics = [] for i in range(0, 3): topics.append("SkeletonsGrouper." + str(i) + ".Localization") subscription.subscribe(topics[i]) worldFrame = 1000 tfFetcher = TransformationFetcher("amqp://10.10.2.20:30000") x = [] y = [] mapFile = "mapTest.dat"
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
dt = dp.parse(span.end_time) - dp.parse(span.start_time) return dt.total_seconds() * 1000.0 class MyChannel(Channel): def consume_until(self, deadline): timeout = max([deadline - now(), 0.0]) return super().consume(timeout=timeout) service_name = 'SkeletonsHeatmap' log = Logger(name=service_name) ops = load_options() channel = MyChannel(ops.broker_uri) subscription = Subscription(channel, name='{}.Render'.format(service_name)) exporter = ZipkinExporter( service_name=service_name, host_name=ops.zipkin_host, port=ops.zipkin_port, transport=BackgroundThreadTransport(max_batch_size=20), ) for group_id in ops.group_ids: subscription.subscribe('SkeletonsGrouper.{}.Localization'.format(group_id)) sks_hm = SkeletonsHeatmap(ops) period = ops.period_ms / 1000.0 deadline = now() while True:
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)
'Path to SUBJECT_ID={} LABEL={} ({}) already exists.\n \ Would you like to proceed? All data will be deleted! [y/n]', subject_id, label_id, labels[label_id]) key = input() if key == 'y': shutil.rmtree(sequence_folder) elif key == 'n': sys.exit(0) else: log.critical('Invalid command \'{}\', exiting.', key) sys.exit(-1) os.makedirs(sequence_folder) channel = Channel(op.broker_uri) subscription = Subscription(channel) if device == "webcam": # subscribe to one camera subscription.subscribe('CameraGateway.{}.Frame'.format(cam.id)) elif device == "cameras": # subscribe to multiple cameras for c in cam: subscription.subscribe('CameraGateway.{}.Frame'.format(c.id)) # get display image size size = (2 * cam[0].config["image"]["resolution"]["height"], 2 * cam[0].config["image"]["resolution"]["width"], 3) # create empty image do display full_image = np.zeros(size, dtype=np.uint8)
# Carrega parametros do robo e endereco do broker do arquivo Json op_config = load_options() ATSPlog = Logger(name='unpack') service_name = "RobotGateway.{}".format(op_config.robot_parameters.id) sonar_topic = "RobotGateway.{}.SonarScan".format(op_config.robot_parameters.id) pose_topic = "RobotGateway.{}.Pose".format(op_config.robot_parameters.id) # instania channel passando endereco do broker # estabelece conexao com o broker 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
ax.set_xlabel('X axis') ax.set_ylabel('Y axis') ax.set_zlabel('Z axis') def get_keyPressed(event): keyPressed["letter"] = event.key ##### Main Program ## Plot world frame and robot frame in the same graphic ####################### # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.20:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to get the relation from the initial reference frame (2001) where the robot was # turned on and its current reference frame (2000). This Frame Transforamtion corressponds # to the dead reckoning odometry subscription.subscribe("NewRobotPose") # Create a 3D figure fig = plt.figure() ax = fig.add_subplot(111, projection='3d') update_axis (ax,0,0) # world reference frame posx,posy,posz,u_vectors,v_vectors,w_vectors = create_world_frame() # plot frame ax.quiver(posx,posy,posz,u_vectors,v_vectors,w_vectors)
from __future__ import print_function from is_wire.core import Channel, Subscription from is_msgs.common_pb2 import Tensor from is_msgs.camera_pb2 import FrameTransformations import numpy as np # Connect to the broker 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:
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)
op = load_options() sd = SkeletonsDetector(op) log = Logger(name=service_name) channel = StreamChannel(op.broker_uri) log.info('Connected to broker {}', op.broker_uri) max_batch_size = max(100, op.zipkin_batch_size) exporter = ZipkinExporter( service_name=service_name, host_name=op.zipkin_host, port=op.zipkin_port, transport=BackgroundThreadTransport(max_batch_size=max_batch_size), ) subscription = Subscription(channel=channel, name=service_name) subscription.subscribe('CameraGateway.*.Frame') while True: msg, dropped = channel.consume(return_dropped=True) tracer = Tracer(exporter, span_context=msg.extract_tracing()) span = tracer.start_span(name='detection_and_render') with tracer.span(name='unpack'): im = msg.unpack(Image) im_np = get_np_image(im) with tracer.span(name='detect'): skeletons = sd.detect(im_np) with tracer.span(name='pack_and_publish_detections'): sks_msg = Message()
from is_wire.core import Channel, Message, Subscription #from is_msgs.common_pb2 import Position, Pose from is_msgs.camera_pb2 import FrameTransformation import numpy as np # from numpy.linalg import inv import math # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.20:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to get the relations between referential frames ################## robotArUco = 8 worldFrame = 1003 # Relation between the initial reference frame where the robot was turned on and its current reference frame (dead reckoning odometry) getRobotOdometry = "FrameTransformation.2000.2001" # Relation between the initial reference frame where the robot was turned on and the world frame getRobotWorldPosition = "FrameTransformation.2001." + str(worldFrame) # Relation between the ArUco on the robot's back and the world frame getArUcoRobotBack = "FrameTransformation." + str(robotArUco + 100) + "." + str(worldFrame) subscription.subscribe(getRobotOdometry) subscription.subscribe(getRobotWorldPosition) subscription.subscribe(getArUcoRobotBack) # Initialize transformation matrices used for storing odometry and correction
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()
from is_wire.core import Channel, Subscription from is_msgs.image_pb2 import Image import numpy as np import cv2 import sys import re # This program allows to see the images from one of the 8 cameras of # the Intelligent Space # User can chage the camera by pressing a number between 0 and 7 # Connect to the broker channel = Channel("amqp://10.10.2.23:30000") # Subscribe to the desired topic(s) subscription = Subscription(channel) # Create the topic name to the camera gateway that can be changed later Topic = ["CameraGateway.0.Frame"] subscription.subscribe(Topic[0]) # ... subscription.subscribe(topic="Other.Topic") # Function to change the camera gateway def changeCamera(camID): # First we must unsubscribe from the latest topic (camera gateway) subscription.unsubscribe(Topic[0]) # Change for the new topic Topic[0] = "CameraGateway." + camID + ".Frame" # Subscribe in the new topic subscription.subscribe(Topic[0])
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]))
from utils import load_options from heatmap import SkeletonsHeatmap from builtins import super class MyChannel(Channel): def consume_until(self, deadline): timeout = max([deadline - now(), 0.0]) return super().consume(timeout=timeout) service_name = 'Skeletons.Heatmap' log = Logger(name=service_name) ops = load_options() channel = MyChannel(ops.broker_uri) subscription = Subscription(channel) exporter = ZipkinExporter( service_name=service_name, host_name=ops.zipkin_host, port=ops.zipkin_port, transport=BackgroundThreadTransport(max_batch_size=20), ) subscription.subscribe('Skeletons.Localization') sks_hm = SkeletonsHeatmap(ops) period = ops.period_ms / 1000.0 deadline = now() while True: deadline += period
from is_msgs.camera_pb2 import FrameTransformation import numpy as np import qi session = qi.Session() try: session.connect("tcp://10.10.0.111:9559") except RuntimeError: print("Can't connect to Naoqi") motion_service = session.service("ALMotion") # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.20:30000") # Create a subscription subscription = Subscription(channel) topicGetRobotOdometry = "FrameTransformation.2000.2001" subscription.subscribe(topicGetRobotOdometry) try: while True: message = channel.consume() if (message.topic == topicGetRobotOdometry): # unpack the message according to its format frameTransf = message.unpack(FrameTransformation) tensor = frameTransf.tf # get the transformation matrix corresponding to the current rotation and position of the robot lastOdometry = np.matrix(tensor.doubles).reshape( tensor.shape.dims[0].size, tensor.shape.dims[1].size) print("######################################") print("Last odometry")