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 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
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 main():
service_name = 'SkeletonsDetector.Detection'
re_topic = re.compile(r'CameraGateway.(\w+).Frame')
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')
detection_span = None
with tracer.span(name='unpack'):
im = msg.unpack(Image)
im_np = get_np_image(im)
with tracer.span(name='detection') as _span:
skeletons = sd.detect(im_np)
detection_span = _span
with tracer.span(name='pack_and_publish_detections'):
sks_msg = Message()
sks_msg.topic = re_topic.sub(r'SkeletonsDetector.\1.Detection',
msg.topic)
sks_msg.inject_tracing(span)
sks_msg.pack(skeletons)
channel.publish(sks_msg)
with tracer.span(name='render_pack_publish'):
im_rendered = draw_skeletons(im_np, skeletons)
rendered_msg = Message()
rendered_msg.topic = re_topic.sub(r'SkeletonsDetector.\1.Rendered',
msg.topic)
rendered_msg.pack(get_pb_image(im_rendered))
channel.publish(rendered_msg)
span.add_attribute('Detections', len(skeletons.objects))
tracer.end_span()
log.info('detections = {:2d}, dropped_messages = {:2d}',
len(skeletons.objects), dropped)
log.info('took_ms = {{ detection: {:5.2f}, service: {:5.2f}}}',
span_duration_ms(detection_span), span_duration_ms(span))
def main():
service_name = "FaceDetector.Detection"
log = Logger(name=service_name)
op = load_options()
face_detector = FaceDetector(op.model)
channel = StreamChannel(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)
subscription.subscribe(topic='CameraGateway.*.Frame')
while True:
msg, dropped = channel.consume_last(return_dropped=True)
tracer = Tracer(exporter, span_context=msg.extract_tracing())
span = tracer.start_span(name='detection_and_render')
detection_span = None
with tracer.span(name='unpack'):
im = msg.unpack(Image)
im_np = to_np(im)
with tracer.span(name='detection') as _span:
camera_id = get_topic_id(msg.topic)
faces = face_detector.detect(im_np)
detection_span = _span
with tracer.span(name='pack_and_publish_detections'):
faces_msg = Message()
faces_msg.topic = 'FaceDetector.{}.Detection'.format(camera_id)
faces_msg.inject_tracing(span)
faces_msg.pack(faces)
channel.publish(faces_msg)
with tracer.span(name='render_pack_publish'):
img_rendered = draw_detection(im_np, faces)
rendered_msg = Message()
rendered_msg.topic = 'FaceDetector.{}.Rendered'.format(camera_id)
rendered_msg.pack(to_image(img_rendered))
channel.publish(rendered_msg)
span.add_attribute('Detections', len(faces.objects))
tracer.end_span()
info = {
'detections': len(faces.objects),
'dropped_messages': dropped,
'took_ms': {
'detection': round(span_duration_ms(detection_span), 2),
'service': round(span_duration_ms(span), 2)
}
}
log.info('{}', str(info).replace("'", '"'))
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
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
useNavigate = True
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:
deadline += period
msgs = []
while True:
try:
msgs.append(channel.consume_until(deadline=deadline))
except:
break
span_context = msgs[-1].extract_tracing() if len(msgs) > 0 else None
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)
plt.draw()
keyPressed = {"letter":None}
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)
else:
log.error("Invalid device option")
sys.exit(-1)
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)
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()
sks_msg.topic = re_topic.sub(r'Skeletons.\1.Detection', msg.topic)
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("'", '"'))
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
config.speed.angular = 0.0
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)
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 input_thread(key):
raw_input()
key.append(None)
#### Main Program #######################
# Using keypad to control the robot with some
# feedback on the screen
# Create a channel to connect to the broker
channel = Channel("amqp://10.10.2.20:30000")
# Create a subscription
subscription = Subscription(channel)
topic = "Skeletons.Localization"
subscription.subscribe(topic)
worldFrame = 1000
tfFetcher = TransformationFetcher("amqp://10.10.2.20:30000")
x = []
y = []
threshold = 0.5
jointList = [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]
#stdscr = curses.initscr()
#curses.cbreak()
#stdscr.keypad(1)
#stdscr.addstr(0,10,"Hit 'q' to quit")
#stdscr.refresh()
#### 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"
with open(mapFile, 'w') as f:
try:
while True:
message = channel.consume()
# Check if the message is of the type expected
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()
# 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
robotOdometry = np.identity(4)
robotWorldPosition = np.identity(4)
arUcoRobotBack = np.identity(4)
n = 1
k = 1
fileName = "frameArUcoRobot.dat"
testPoint = np.matrix([[0], [0], [0], [1]])
try:
while True:
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]))
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])
# Keep executing until 'q' is pressed
from __future__ import print_function
from is_wire.core import Channel, Subscription, Message
# This program shows how to consume a message published
# in some topic using the Intelligent Space framework
# Connect to the broker
channel = Channel("amqp://10.10.2.23:30000")
# Subscribe to the channel
subscription = Subscription(channel)
# Define topic name
Topic= "Aula01"
# Subscribe to the topic
subscription.subscribe(Topic)
while True:
# Keep listening to the channel
message = channel.consume()
# Print message whenever receive one
print ("Received message:")
print (message.body)
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
msgs = []
while True:
try:
msgs.append(channel.consume_until(deadline=deadline))
except:
break
span_context = msgs[-1].extract_tracing() if len(msgs) > 0 else None
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")
print(lastOdometry)
print("######################################")
