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