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 awarenessOn(channel):
message = Message()
condition = Struct()
condition["enabled"] = True
message.pack(condition)
message.topic = "RobotGateway.0.SetAwareness"
channel.publish(message)
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 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 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 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)
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_pack_unpack():
struct = Struct()
struct.fields["key"].number_value = 0.1212121921839893438974837
struct.fields["value"].number_value = 90.0
struct.fields["string"].string_value = "0.1212121921839893438974837"
message = Message()
message.pack(struct)
unpacked = message.unpack(Struct)
assert message.content_type == ContentType.PROTOBUF
assert str(struct) == str(unpacked)
assert struct == unpacked
message.content_type = ContentType.JSON
message.pack(struct)
unpacked = message.unpack(Struct)
assert message.content_type == ContentType.JSON
assert str(struct) == str(unpacked)
assert struct == unpacked
# test pack/unpack for dict object with default serializtion as JSON
dictionary = {"key": 0.1233444444, "hodor": "hold the door", "default": 0}
message = Message(content=dictionary)
assert message.content_type == ContentType.JSON
unpacked = message.unpack()
assert dictionary == unpacked
# test pack/unpack for dict object with protobuf content-type
message = Message()
message.content_type = ContentType.PROTOBUF
message.pack(dictionary)
unpacked = message.unpack()
assert dictionary == unpacked
# matrix Inverse
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:
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, 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
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)
'--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)
strPosition = 'X: ' + str(pepperPosX) + ' Y: ' + str(
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.")
from is_wire.core import Channel, Message, Subscription
from is_msgs.robot_pb2 import RobotConfig
#Create a channel to connet to the broker
channel = Channel("amqp://10.10.2.23:30000")
# Create Message
message = Message()
# Create and set Robot Configuration
robotConfig = RobotConfig()
robotConfig.speed.linear = 40.0
robotConfig.speed.angular = 0.3
# Insert Robot Configuration in the Message
message.pack(robotConfig)
# Define message topic
Topic = "Aula2"
message.topic = Topic
# Publish the message
channel.publish(message)
from is_wire.core import Channel, Subscription, Message, Logger
from is_wire.core.wire.status import StatusCode
from is_msgs.common_pb2 import FieldSelector
from is_msgs.camera_pb2 import CameraConfigFields, CameraConfig
from google.protobuf.json_format import Parse
uri = 'amqp://10.10.2.15:30000'
channel = Channel(uri)
subscription = Subscription(channel)
log = Logger(name='GetConfig')
camera_id = 0
get_config = FieldSelector()
get_config.fields.append(CameraConfigFields.Value('SAMPLING_SETTINGS'))
get_config.fields.append(CameraConfigFields.Value('IMAGE_SETTINGS'))
msg = Message()
msg.topic = 'CameraGateway.{}.GetConfig'.format(camera_id)
msg.reply_to = subscription
msg.pack(get_config)
channel.publish(msg)
while True:
msg = channel.consume()
if msg.status.code == StatusCode.OK:
config = msg.unpack(CameraConfig)
log.info('Configuration received from camera \'{}\'\n{}', camera_id, config)
else:
log.warn('Can\'t apply configuration:\n{}', msg.status)
sys.exit(0)
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)
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)
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),
dropped
)
