def makeTurn(linear, angular, channel):
# Create message with command for
# # linear and angular velocities
message = Message()
robotConfig = RobotConfig()
robotConfig.speed.linear = linear
robotConfig.speed.angular = angular
message.pack(robotConfig)
message.topic = "RobotGateway.0.SetConfig"
# Public message
channel.publish(message)
t_a = datetime.now()
t_b = t_a
t_diff = relativedelta(t_b, t_a)
while t_diff.seconds < 4:
t_b = datetime.now()
t_diff = relativedelta(t_b, t_a)
robotConfig.speed.linear = 0
robotConfig.speed.angular = 0
message.pack(robotConfig)
message.topic = "RobotGateway.0.SetConfig"
# Public message
channel.publish(message)
t_a = datetime.now()
t_b = t_a
t_diff = relativedelta(t_b, t_a)
while t_diff.seconds < 2:
t_b = datetime.now()
t_diff = relativedelta(t_b, t_a)
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():
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 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 request(self, content, topic, timeout_ms, metadata=None):
if not self.can_request():
raise Exception(
"Can't request more than {}. Use 'RequestManager.can_request' "
"method to check if you can do requests.")
tracer = Tracer(
exporter=self._zipkin_exporter) if self._do_tracing else None
span = tracer.start_span(name='request') if self._do_tracing else None
msg = Message(content=content)
msg.topic = topic
msg.reply_to = self._subscription
msg.timeout = timeout_ms / 1000.0
self._log.debug("[Sending] metadata={}, cid={}", metadata,
msg.correlation_id)
if self._do_tracing:
for key, value in (metadata or {}).items():
span.add_attribute(key, value)
tracer.end_span()
msg.inject_tracing(span)
self._publish(msg, metadata)
if len(self._requests) >= self._max_requests:
self._can_request = False
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 awarenessOn(channel):
message = Message()
condition = Struct()
condition["enabled"] = True
message.pack(condition)
message.topic = "RobotGateway.0.SetAwareness"
channel.publish(message)
def test_create_reply_empty_request():
request = Message()
request.topic = "topic"
reply = request.create_reply()
assert reply.topic == request.reply_to
assert reply.correlation_id == request.correlation_id
assert reply.content_type == request.content_type
def makeMessage(linear, angular):
message = Message()
robotConfig = RobotConfig()
robotConfig.speed.linear = linear
robotConfig.speed.angular = angular
message.pack(robotConfig)
message.topic = "RobotGateway.0.SetConfig"
# Public message
channel.publish(message)
def commandNavigateTo(posX, posY, channel):
goal = Position()
goal.x = posX
goal.y = posY
goal.z = 0
goalMessage = Message()
goalMessage.pack(goal)
goalMessage.topic = "RobotGateway.0.NavigateTo"
channel.publish(goalMessage)
def test_create_reply():
request = Message()
request.topic = "topic"
request.reply_to = "reply_to"
request.content_type = ContentType.JSON
reply = request.create_reply()
assert reply.topic == request.reply_to
assert reply.correlation_id == request.correlation_id
assert reply.content_type == request.content_type
def makeMessage(linear, angular):
# Create message with command for
# linear and angular velocities
message = Message()
robotConfig = RobotConfig()
robotConfig.speed.linear = linear
robotConfig.speed.angular = angular
message.pack(robotConfig)
message.topic = "RobotGateway.0.SetConfig"
# Public message
channel.publish(message)
def awarenessOff(channel):
'''
message = Message()
condition = Struct()
condition["enabled"] = False
message.pack(condition)
message.topic = "RobotGateway.0.SetAwareness"
channel.publish(message)
'''
message = Message()
message.topic = "RobotGateway.0.PauseAwareness"
channel.publish(message)
def commandMoveTo(posX, posY, channel):
goal = Pose()
goal.position.x = posX
goal.position.y = posY
goal.position.z = 0
goal.orientation.yaw = math.atan2(goal.position.y, goal.position.x)
goal.orientation.pitch = 0
goal.orientation.roll = 0
goalMessage = Message()
goalMessage.pack(goal)
goalMessage.topic = "RobotGateway.0.MoveTo"
channel.publish(goalMessage)
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_amqp_conversion():
sent = Message()
sent.created_at = int(now() * 1000) / 1000.0
sent.reply_to = "reply_to"
sent.subscription_id = "subscription_id"
sent.content_type = ContentType.JSON
sent.body = '{"field":"value"}'.encode('latin1')
sent.topic = "MyTopic"
sent.status = Status(
code=StatusCode.FAILED_PRECONDITION,
why="Bad Args...",
)
sent.metadata = {
'x-b3-sampled': '1',
'x-b3-traceid': 'f047c6f208eb36ab',
'x-b3-flags': '0',
'x-b3-spanid': 'ef81a2f9c261473d',
'x-b3-parentspanid': '0000000000000000'
}
body = sent.body
amqp_message = amqp.Message(channel=None,
body=body,
**WireV1.to_amqp_properties(sent))
amqp_message.delivery_info = {
"routing_key": sent.topic,
"consumer_tag": sent.subscription_id,
}
received = WireV1.from_amqp_message(amqp_message)
print(sent.__str__(), received.__str__())
assert str(sent) == str(received)
assert sent.created_at == received.created_at
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
def consume_ready(self, timeout=1.0):
received_msgs = []
# wait for new message
try:
stated_at = now()
while True:
_timeout = max(0.0, stated_at + timeout - now())
msg = self._channel.consume(timeout=_timeout)
if msg.status.ok() and msg.has_correlation_id():
cid = msg.correlation_id
if cid in self._requests:
received_msgs.append(
(msg, self._requests[cid]["metadata"]))
del self._requests[cid]
except socket.timeout:
pass
# check for timeouted requests
for cid in self._requests.keys():
timeouted_msg = self._requests[cid]["msg"]
if timeouted_msg.deadline_exceeded():
msg = Message()
msg.body = timeouted_msg.body
msg.topic = timeouted_msg.topic
msg.reply_to = self._subscription
msg.timeout = timeouted_msg.timeout
metadata = self._requests[cid]["metadata"]
del self._requests[cid]
self._log.debug("[Retring] metadata={}, cid={}", metadata,
msg.correlation_id)
self._publish(msg, metadata)
if not self._can_request and len(self._requests) <= self._min_requests:
self._can_request = True
return received_msgs
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()
toPepperFrame = inv(pepperPose)
# transform the goal from the world frame to the robot frame
goalPepperFrame = toPepperFrame * goalWorldFrame
posX = float(goalPepperFrame[0])
posY = float(goalPepperFrame[1])
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")
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)
stdscr.keypad(1)
stdscr.addstr(0, 10, "Hit 'q' to quit")
stdscr.refresh()
key = ''
linear, angular = 0, 0
# Initialize transformation matrices used for storing odometry and correction
pepperPose = np.identity(4)
robotToWorld = np.identity(4)
# Create message used to send the odometry to the
# program that plots the odometry using matplotlib
messageRobotPose = Message()
messageRobotPose.topic = "NewRobotPose"
while True:
# listen the channel
message = channel.consume()
# Check if the message received is the robot's odometry - FrameTransformation type
if (message.topic == "FrameTransformation.2000.2001"):
# 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)
pepperPose = np.matmul(robotToWorld, lastOdometry)
# Separate the position
'--uri', '-u', type=str, required=False, help='broker_uri', default='amqp://localhost:5672')
args = parser.parse_args()
device = args.device
fps = args.fps
broker_uri = args.uri
cap = VideoCapture(device)
if cap.isOpened():
log.info('Connected to device: /dev/video{}'.format(device))
else:
log.error('Coudn\'t find device: /dev/video{}'.format(device))
exit()
log.info('Camera FPS set to {}'.format(fps))
cap.set(CAP_PROP_FPS, fps)
channel = StreamChannel(broker_uri)
log.info('Connected to broker {}', broker_uri)
log.info('Starting to capture')
while True:
ret, frame = cap.read()
msg = Message()
msg.topic = 'CameraGateway.{}.Frame'.format(args.id)
msg.pack(get_pb_image(frame))
channel.publish(msg)
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)
if len(sys.argv) > 1:
broker_uri = sys.argv[1]
topic_id = sys.argv[2]
channel = Channel(broker_uri)
exporter = ZipkinExporter(
service_name='CameraGateway.{}'.format(topic_id),
host_name='localhost',
port=9411,
transport=BackgroundThreadTransport(max_batch_size=100),
)
image = cv2.imread('../image.png')
for k in range(10):
tracer = Tracer(exporter)
with tracer.span(name='image') as span:
cimage = cv2.imencode(ext='.jpeg',
img=image,
params=[cv2.IMWRITE_JPEG_QUALITY, 80])
data = cimage[1].tobytes()
im = Image(data=data)
msg = Message()
msg.topic = 'CameraGateway.{}.Frame'.format(topic_id)
msg.inject_tracing(span)
msg.metadata.update({'image_id': k})
msg.pack(im)
channel.publish(msg)
log.info('Message {} published', k)
time.sleep(0.250)
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
tracer = Tracer(exporter=exporter, span_context=span_context)
with tracer.span(name='Render') as span:
sks_list = list(map(lambda x: x.unpack(ObjectAnnotations), msgs))
sks_hm.update_heatmap(sks_list)
im_pb = sks_hm.get_pb_image()
msg = Message()
msg.topic = service_name
msg.pack(im_pb)
msg.inject_tracing(span)
channel.publish(msg)
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)
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'Skeletons.\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}][{:5.2f}ms][Dropped {}]',
len(skeletons.objects),
span_duration_ms(span),
