Skip to content
Snippets Groups Projects
Commit a79fe71e authored by René Schöne's avatar René Schöne
Browse files

new functions 

- display latest svg image
- updated demo commands
- updated conversion topics
parent 10d918d9
No related branches found
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
cgv_connector.proto
+ 43
− 20
View file @ a79fe71e
// cgv_connector.proto // connector.proto
// this file contains the messages that are exchanged between the cgv framework and the st ROS interface // this file contains the messages that are exchanged between the ROS connector and other systems
syntax = "proto3"; syntax = "proto3";
...@@ -10,22 +10,22 @@ message Object { ...@@ -10,22 +10,22 @@ message Object {
// Position is object-center related // Position is object-center related
message Position { message Position {
float x = 1; // in m double x = 1; // in m
float y = 2; // in m double y = 2; // in m
float z = 3; // height in m double z = 3; // height in m
} }
// 3D description of the object // 3D description of the object
message Size { message Size {
float length = 1; // in m double length = 1; // in m
float width = 2; // in m double width = 2; // in m
float height = 3; // in m double height = 3; // in m
} }
message Orientation { message Orientation {
float x = 1; // normalized quaternion double x = 1; // normalized quaternion
float y = 2; double y = 2;
float z = 3; double z = 3;
float w = 4; double w = 4;
} }
message Color { message Color {
float r = 1; // 0..1 float r = 1; // 0..1
...@@ -38,6 +38,9 @@ message Object { ...@@ -38,6 +38,9 @@ message Object {
BIN = 2; BIN = 2;
ARM = 3; ARM = 3;
DROP_OFF_LOCATION = 4; DROP_OFF_LOCATION = 4;
HUMAN = 5;
ROBOT = 6;
COLLABORATION_ZONE = 7;
} }
string id = 1; string id = 1;
...@@ -46,26 +49,46 @@ message Object { ...@@ -46,26 +49,46 @@ message Object {
Size size = 4; Size size = 4;
Orientation orientation = 5; Orientation orientation = 5;
Color color = 6; Color color = 6;
bool active = 7;
} }
// the scene is stored within the ROS side and sent to the CGV framework // the scene is stored within the ROS side and sent to clients
message Scene { message Scene {
repeated Object objects = 1; repeated Object objects = 1;
} }
// the selection is done by the CGV framework and sent to ROS // the selection is done by the CGV framework and sent to ROS
// FIXME can be removed
message Selection { message Selection {
string id = 1; // the id corresponds to an id of an Object in a Scene string id = 1; // the id corresponds to an id of an Object in a Scene
} }
// vvv from rs vvv. message Command {
// Merged message to contain both pick and place in one oneof msg {
message MergedSelection { PickAndPlace pickAndPlace = 1;
string idRobot = 1; // the id corresponds to and id of the robot Object that should execute this operation ConfigChange configChange = 2;
string idPick = 2; // the id corresponds to an id of the Object in a Scene to be picked Evacuate evacuate = 3;
string idPlace = 3; // the id corresponds to an id of the Object in a Scene where the picked object shall be placed
} }
}
message PickAndPlace {
string idRobot = 1; // id of the robot that should execute this operation
string idPick = 2; // id of the object in the scene to be picked
string idPlace = 3; // id of the location the picked object shall be placed.
}
message ConfigChange {
string idCollaborationZone = 1; // id of collaboration zone to change
string idRobotNewOwner = 2; // id of robot that will become new owner
}
message Evacuate {
string idRobot = 1; // id of robot that need to move out of its currently defined collision objects
string idCollaborationZone = 2; // id of collaboration zone to evacuate
}
// Reachability of objects, as reported by MoveIt // Reachability of objects, as reported by MoveIt
// FIXME can be removed
message Reachability { message Reachability {
message ObjectReachability { message ObjectReachability {
string idObject = 1; // the id of the object to reach string idObject = 1; // the id of the object to reach
......
cgv_connector_pb2.py
+ 231
− 44
View file @ a79fe71e
This diff is collapsed.
main.py
+ 212
− 143
View file @ a79fe71e
...@@ -3,6 +3,8 @@ import datetime ...@@ -3,6 +3,8 @@ import datetime
import queue import queue
import threading import threading
import time import time
import base64
import flask
import dash import dash
from dash import dcc from dash import dcc
...@@ -16,8 +18,14 @@ import cgv_connector_pb2 ...@@ -16,8 +18,14 @@ import cgv_connector_pb2
import utils import utils
external_stylesheets = ['https://codepen.io/chriddyp/pen/bWLwgP.css'] external_stylesheets = ['https://codepen.io/chriddyp/pen/bWLwgP.css']
fixed_svg_filename = "ros3rag/ros3rag.placeB/images/2022-05-04T10-19-30-9155.svg"
MQTT_SERVER = 'localhost' fixed_svg_filename = "2022-05-04T10-19-30-9155.svg"
# fixed_svg_filename = ""
image_directory = '/data/git/jastadd/ros3rag/ros3rag.placeB/images/'
static_image_route = '/static/'
MQTT_SERVER = '192.168.0.122'
# MQTT_SERVER = 'localhost'
mqttc = mqtt.Client() mqttc = mqtt.Client()
# mqtt client connected? # mqtt client connected?
...@@ -41,22 +49,30 @@ commands = { ...@@ -41,22 +49,30 @@ commands = {
'send-place-b-rewind': ('place-b/rewind', '1'), 'send-place-b-rewind': ('place-b/rewind', '1'),
'send-place-b-start': ('coordinating/rag-b/command', 'start'), 'send-place-b-start': ('coordinating/rag-b/command', 'start'),
'send-place-b-exit': ('place-b/exit', '1'), 'send-place-b-exit': ('place-b/exit', '1'),
'send-place-a-demo-objRed-blue': ('place-a/demo/move/objectRed1/blue', '1'), # 'send-place-a-demo-objRed-blue': ('place-a/demo/move/objectRed1/blue', '1'),
'send-place-a-demo-objRed-red': ('place-a/demo/move/objectRed1/red', '1'), # 'send-place-a-demo-objRed-red': ('place-a/demo/move/objectRed1/red', '1'),
'send-place-b-demo-objRed-red': ('place-b/demo/move/objectRed1/red', '1'), # 'send-place-b-demo-objRed-red': ('place-b/demo/move/objectRed1/red', '1'),
'send-place-b-demo-initial_scene': ('demo/initial_scene', '1'),
'send-place-b-demo-arm1-active': ('demo/arm1/active', '1'),
'send-place-b-demo-arm1-inactive': ('demo/arm1/inactive', '1'),
'send-place-b-demo-arm2-active': ('demo/arm2/active', '1'),
'send-place-b-demo-arm2-inactive': ('demo/arm2/inactive', '1'),
'send-place-b-demo-big-blue-cz': ('demo/big-blue/cz', '1'),
'send-place-b-demo-big-blue-g1': ('demo/big-blue/g1', '1'),
'send-coordinator-model': ('coordinator/model', '1'), 'send-coordinator-model': ('coordinator/model', '1'),
'send-coordinator-model-details': ('coordinator/model', 'details'), 'send-coordinator-model-details': ('coordinator/model', 'details'),
'send-coordinator-exit': ('coordinator/exit', '1'), 'send-coordinator-exit': ('coordinator/exit', '1'),
'send-place-a-robot-ctrl-up': ('ros-place-a/status', 'up'), # 'send-place-a-robot-ctrl-up': ('ros-place-a/status', 'up'),
'send-place-b-robot-ctrl-up': ('ros-place-b/status', 'up'), # 'send-place-b-robot-ctrl-up': ('ros-place-b/status', 'up'),
'send-place-a-rag-up': ('rag-a/status', 'up'), 'send-place-a-rag-up': ('rag-a/status', 'up'),
'send-place-b-rag-up': ('rag-b/status', 'up'), 'send-place-b-rag-up': ('rag-b/status', 'up'),
'send-dummy-up': ('random/status', 'up'), 'send-dummy-up': ('random/status', 'up'),
'send-place-a-robot-ctrl-ready': ('ros-place-a/status', 'ready'), # 'send-place-a-robot-ctrl-ready': ('ros-place-a/status', 'ready'),
'send-place-b-robot-ctrl-ready': ('ros-place-b/status', 'ready'), # 'send-place-b-robot-ctrl-ready': ('ros-place-b/status', 'ready'),
'send-place-a-rag-ready': ('rag-a/status', 'ready'), 'send-place-a-rag-ready': ('rag-a/status', 'ready'),
'send-place-b-rag-ready': ('rag-b/status', 'ready'), 'send-place-b-rag-ready': ('rag-b/status', 'ready'),
'send-dummy-ready': ('random/status', 'ready'), 'send-dummy-ready': ('random/status', 'ready'),
...@@ -75,9 +91,13 @@ complex_commands = { ...@@ -75,9 +91,13 @@ complex_commands = {
} }
conversion_topics = { conversion_topics = {
'/ceti_cell_placeworld/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
'/ceti_cell_2_placeworld/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
'/moveit_sorting_controller/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
'place-a/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene), 'place-a/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
'place-b/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene), 'place-b/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
'place-b/command': (cgv_connector_pb2.MergedSelection(), utils.format_command), 'place-b/command': (cgv_connector_pb2.Command(), utils.format_command),
'/ceti_cell_placeworld/command': (cgv_connector_pb2.Command(), utils.format_command),
'place-b/reachability/arm1': (cgv_connector_pb2.Reachability(), utils.format_reachability), 'place-b/reachability/arm1': (cgv_connector_pb2.Reachability(), utils.format_reachability),
'place-b/reachability/arm2': (cgv_connector_pb2.Reachability(), utils.format_reachability), 'place-b/reachability/arm2': (cgv_connector_pb2.Reachability(), utils.format_reachability),
'place-b/reachability/arm3': (cgv_connector_pb2.Reachability(), utils.format_reachability), 'place-b/reachability/arm3': (cgv_connector_pb2.Reachability(), utils.format_reachability),
...@@ -85,6 +105,7 @@ conversion_topics = { ...@@ -85,6 +105,7 @@ conversion_topics = {
bytes_topics = [ bytes_topics = [
] ]
svg_image_topic = 'place-b/model/svg/path'
button_style_normal = {"marginRight": "15px"} button_style_normal = {"marginRight": "15px"}
button_style_exit = {**button_style_normal, "backgroundColor": "red", "color": "white"} button_style_exit = {**button_style_normal, "backgroundColor": "red", "color": "white"}
...@@ -92,6 +113,8 @@ textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical ...@@ -92,6 +113,8 @@ textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical
app = dash.Dash(__name__, external_stylesheets=external_stylesheets) app = dash.Dash(__name__, external_stylesheets=external_stylesheets)
app.layout = html.Div([ app.layout = html.Div([
dcc.Tabs([
dcc.Tab(label="Buttons and Log", children=[
html.Div([ # First Row html.Div([ # First Row
html.Div([ # Column for place a html.Div([ # Column for place a
dcc.Textarea( dcc.Textarea(
...@@ -144,15 +167,15 @@ app.layout = html.Div([ ...@@ -144,15 +167,15 @@ app.layout = html.Div([
html.Button('Exit', id='send-coordinator-exit', style=button_style_exit), html.Button('Exit', id='send-coordinator-exit', style=button_style_exit),
# ], className='row'), # ], className='row'),
# html.Div([ # Row for commands up # html.Div([ # Row for commands up
html.Button('Robot Control A Up', id='send-place-a-robot-ctrl-up', style=button_style_normal), # html.Button('Robot Control A Up', id='send-place-a-robot-ctrl-up', style=button_style_normal),
html.Button('Robot Control B Up', id='send-place-b-robot-ctrl-up', style=button_style_normal), # html.Button('Robot Control B Up', id='send-place-b-robot-ctrl-up', style=button_style_normal),
html.Button('RAG A Up', id='send-place-a-rag-up', style=button_style_normal), html.Button('RAG A Up', id='send-place-a-rag-up', style=button_style_normal),
html.Button('RAG B Up', id='send-place-b-rag-up', style=button_style_normal), html.Button('RAG B Up', id='send-place-b-rag-up', style=button_style_normal),
html.Button('Dummy Up', id='send-dummy-up', style=button_style_normal), html.Button('Dummy Up', id='send-dummy-up', style=button_style_normal),
# ], className='row'), # ], className='row'),
# html.Div([ # Row for commands ready # html.Div([ # Row for commands ready
html.Button('Robot Control A Ready', id='send-place-a-robot-ctrl-ready', style=button_style_normal), # html.Button('Robot Control A Ready', id='send-place-a-robot-ctrl-ready', style=button_style_normal),
html.Button('Robot Control B Ready', id='send-place-b-robot-ctrl-ready', style=button_style_normal), # html.Button('Robot Control B Ready', id='send-place-b-robot-ctrl-ready', style=button_style_normal),
html.Button('RAG A Ready', id='send-place-a-rag-ready', style=button_style_normal), html.Button('RAG A Ready', id='send-place-a-rag-ready', style=button_style_normal),
html.Button('RAG B Ready', id='send-place-b-rag-ready', style=button_style_normal), html.Button('RAG B Ready', id='send-place-b-rag-ready', style=button_style_normal),
html.Button('Dummy Ready', id='send-dummy-ready', style=button_style_normal), html.Button('Dummy Ready', id='send-dummy-ready', style=button_style_normal),
...@@ -165,8 +188,8 @@ app.layout = html.Div([ ...@@ -165,8 +188,8 @@ app.layout = html.Div([
html.Button('Rewind', id='send-place-a-rewind', style=button_style_normal), html.Button('Rewind', id='send-place-a-rewind', style=button_style_normal),
html.Button('Start', id='send-place-a-start', style=button_style_normal), html.Button('Start', id='send-place-a-start', style=button_style_normal),
html.Button('Exit', id='send-place-a-exit', style=button_style_exit), html.Button('Exit', id='send-place-a-exit', style=button_style_exit),
html.Button('obj-Red -> Red', id='send-place-a-demo-objRed-red', style=button_style_normal), # html.Button('obj-Red -> Red', id='send-place-a-demo-objRed-red', style=button_style_normal),
html.Button('obj-Red -> Blue', id='send-place-a-demo-objRed-blue', style=button_style_normal), # html.Button('obj-Red -> Blue', id='send-place-a-demo-objRed-blue', style=button_style_normal),
], className="four columns"), ], className="four columns"),
html.Div([ # Column for commands of place b html.Div([ # Column for commands of place b
html.H3("Commands Place B"), html.H3("Commands Place B"),
...@@ -175,7 +198,14 @@ app.layout = html.Div([ ...@@ -175,7 +198,14 @@ app.layout = html.Div([
html.Button('Rewind', id='send-place-b-rewind', style=button_style_normal), html.Button('Rewind', id='send-place-b-rewind', style=button_style_normal),
html.Button('Start', id='send-place-b-start', style=button_style_normal), html.Button('Start', id='send-place-b-start', style=button_style_normal),
html.Button('Exit', id='send-place-b-exit', style=button_style_exit), html.Button('Exit', id='send-place-b-exit', style=button_style_exit),
html.Button('obj-Red -> Red', id='send-place-b-demo-objRed-red', style=button_style_normal), # html.Button('obj-Red -> Red', id='send-place-b-demo-objRed-red', style=button_style_normal),
html.Button('initial_scene', id='send-place-b-demo-initial_scene', style=button_style_normal),
html.Button('arm1-active', id='send-place-b-demo-arm1-active', style=button_style_normal),
html.Button('arm1-inactive', id='send-place-b-demo-arm1-inactive', style=button_style_normal),
html.Button('arm2-active', id='send-place-b-demo-arm2-active', style=button_style_normal),
html.Button('arm2-inactive', id='send-place-b-demo-arm2-inactive', style=button_style_normal),
html.Button('big-blue-cz', id='send-place-b-demo-big-blue-cz', style=button_style_normal),
html.Button('big-blue-g1', id='send-place-b-demo-big-blue-g1', style=button_style_normal),
], className="four columns"), ], className="four columns"),
], className='row'), ], className='row'),
# dcc.Markdown("---"), # dcc.Markdown("---"),
...@@ -215,11 +245,11 @@ app.layout = html.Div([ ...@@ -215,11 +245,11 @@ app.layout = html.Div([
dcc.Markdown("---"), dcc.Markdown("---"),
html.Div([ html.Div([
html.P("Topic"), html.P("Topic"),
dcc.Input(id='manual-mqtt-topic'), dcc.Input(id='manual-mqtt-topic', style={"margin-left": "5px"}),
html.P("Message"), html.P("Message", style={"margin-left": "15px"}),
dcc.Input(id='manual-mqtt-message'), dcc.Input(id='manual-mqtt-message', style={"margin-left": "5px"}),
# dcc.Textarea(id='manual-mqtt-message', style={'width': '50%', 'font-family': 'Consolas, monospace'}), # dcc.Textarea(id='manual-mqtt-message', style={'width': '50%', 'font-family': 'Consolas, monospace'}),
html.Button('Send', id='manual-mqtt-send') html.Button('Send', id='manual-mqtt-send', style={"margin-left": "15px"})
], className='row', style=dict(display='flex')), ], className='row', style=dict(display='flex')),
# -- Invisible elements -- # -- Invisible elements --
...@@ -231,7 +261,19 @@ app.layout = html.Div([ ...@@ -231,7 +261,19 @@ app.layout = html.Div([
), ),
visdcc.Run_js(id='javascriptLog', run=""), visdcc.Run_js(id='javascriptLog', run=""),
html.Div(id='hidden-div', style={'display': 'none'}) html.Div(id='hidden-div', style={'display': 'none'})
]) ]), # Tab "Buttons and Log"
dcc.Tab(label="SVG Model B", children=[
html.Div([
html.P("Name:"),
html.P(id='model-b-svg-name', style={"margin-left": "15px"}),
], className='row', style=dict(display='flex')),
html.Div([
html.Img(id='model-b-svg-img', src="/static/{}".format(fixed_svg_filename))
], style=dict(position='fixed', overflow='scroll', width='100%')),
# html.Img(src="data:image/svg;base64,{}".format(base64.b64encode(open(fixed_svg_filename, 'rb').read()).decode()))
]) # Tab "SVG Model B"
]) # Tabs
]) # Div
@app.callback( @app.callback(
...@@ -339,15 +381,17 @@ def check_connection(_n_intervals): ...@@ -339,15 +381,17 @@ def check_connection(_n_intervals):
Output('mqtt-log', 'value'), Output('mqtt-log', 'value'),
Output('javascriptLog', 'run'), Output('javascriptLog', 'run'),
Output('topics-to-filter', 'options'), Output('topics-to-filter', 'options'),
Output('model-b-svg-img', 'src'),
Input('every-1-second', 'n_intervals'), Input('every-1-second', 'n_intervals'),
Input('clear-mqtt-log', 'n_clicks'), Input('clear-mqtt-log', 'n_clicks'),
Input('topics-to-filter', 'options'), Input('topics-to-filter', 'options'),
Input('topics-to-filter', 'value'), Input('topics-to-filter', 'value'),
Input('model-b-svg-img', 'src'),
State('filtered-mqtt-log', 'value'), State('filtered-mqtt-log', 'value'),
State('mqtt-log', 'value'), State('mqtt-log', 'value'),
State('should-scroll-mqtt-log', 'value') State('should-scroll-mqtt-log', 'value')
) )
def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_filter, def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_filter, previous_svg_img_src,
filtered_value, value, should_scroll): filtered_value, value, should_scroll):
""" """
Periodically update mqtt log Periodically update mqtt log
...@@ -355,18 +399,20 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f ...@@ -355,18 +399,20 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f
:param (Input) clear_n_clicks: clear.n_clicks :param (Input) clear_n_clicks: clear.n_clicks
:param (Input) filter_options: displayed topics to show in filtered log :param (Input) filter_options: displayed topics to show in filtered log
:param (Input) topics_to_filter: topics to show in filtered log :param (Input) topics_to_filter: topics to show in filtered log
:param (Input) previous_svg_img_src: previous 'src' of the svg showing latest model of site B
:param (State) value: current content of mqtt log :param (State) value: current content of mqtt log
:param (State) filtered_value: current content of filtered mqtt log :param (State) filtered_value: current content of filtered mqtt log
:param (State) should_scroll: checkbox value whether to scroll to the end after update :param (State) should_scroll: checkbox value whether to scroll to the end after update
:return: new content of mqtt log :return: new content of mqtt log
""" """
new_svg_img_src = previous_svg_img_src
ctx = dash.callback_context ctx = dash.callback_context
if not ctx.triggered: if not ctx.triggered:
return dash.no_update return dash.no_update
trigger_id = ctx.triggered[0]['prop_id'].split('.')[0] trigger_id = ctx.triggered[0]['prop_id'].split('.')[0]
if trigger_id == 'clear-mqtt-log': if trigger_id == 'clear-mqtt-log':
return "", "", "", filter_options return "", "", "", filter_options, new_svg_img_src
if trigger_id == 'topics-to-filter': if trigger_id == 'topics-to-filter':
filtered_value = "" filtered_value = ""
...@@ -392,6 +438,9 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f ...@@ -392,6 +438,9 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f
filtered_value = "" filtered_value = ""
for msg in local_messages: for msg in local_messages:
topic_match, topic = utils.topic_match(topics_to_filter, msg) topic_match, topic = utils.topic_match(topics_to_filter, msg)
if topic == svg_image_topic:
new_svg_img_src = '/static/' + utils.parse_log_msg(msg)[2]
print('found new svg: ' + new_svg_img_src)
if topic_match: if topic_match:
filtered_value += msg + "\n" filtered_value += msg + "\n"
if topic not in (option['label'] for option in filter_options): if topic not in (option['label'] for option in filter_options):
...@@ -404,7 +453,7 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f ...@@ -404,7 +453,7 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f
var textarea = document.getElementById('mqtt-log'); var textarea = document.getElementById('mqtt-log');
textarea.scrollTop = textarea.scrollHeight; textarea.scrollTop = textarea.scrollHeight;
''' if should_scroll else "" ''' if should_scroll else ""
return filtered_value, value, log_cmd, filter_options return filtered_value, value, log_cmd, filter_options, new_svg_img_src
@app.callback( @app.callback(
...@@ -448,6 +497,26 @@ def send_manual(n_clicks, topic, message): ...@@ -448,6 +497,26 @@ def send_manual(n_clicks, topic, message):
return dash.no_update return dash.no_update
@app.callback(
Output('model-b-svg-name', 'children'),
Input('model-b-svg-img', 'src'),
)
def update_svg_name(name):
return name
# Add a static image route that serves images from desktop
# Be *very* careful here - you don't want to serve arbitrary files
# from your computer or server
@app.server.route('{}<image_path>.svg'.format(static_image_route))
def serve_image(image_path):
image_name = '{}.svg'.format(image_path)
print('{}/{}'.format(image_directory, image_name))
# if image_name not in list_of_images:
# raise Exception('"{}" is excluded from the allowed static files'.format(image_path))
return flask.send_from_directory(image_directory, image_name)
def on_mqtt_connect(_client, _userdata, _flags, _rc, _properties=None): def on_mqtt_connect(_client, _userdata, _flags, _rc, _properties=None):
# Callback for mqtt client when connected # Callback for mqtt client when connected
print('\nConnected at ' + datetime.datetime.now().isoformat()) print('\nConnected at ' + datetime.datetime.now().isoformat())
......
utils.py
+ 14
− 4
View file @ a79fe71e
...@@ -49,14 +49,24 @@ def topic_match(topics_to_filter, msg, last_match=True): ...@@ -49,14 +49,24 @@ def topic_match(topics_to_filter, msg, last_match=True):
def format_scene(scene: cgv_connector_pb2.Scene): def format_scene(scene: cgv_connector_pb2.Scene):
result = "" result = ""
for obj in scene.objects: for obj in scene.objects:
if obj.type == cgv_connector_pb2.Object.Type.BOX: if obj.type != cgv_connector_pb2.Object.Type.DROP_OFF_LOCATION and obj.type != cgv_connector_pb2.Object.Type.UNKNOWN:
pos = obj.pos pos = obj.pos
result += f"\n<obj {obj.id:15} at ({pos.x:6.2} {pos.y:6.2} {pos.z:6.2})>" result += f"\n<{cgv_connector_pb2.Object.Type.Name(obj.type):20} {obj.id:15} at ({pos.x:6.2} {pos.y:6.2} {pos.z:6.2}) {obj.active}>"
# result += str(obj)
# result = scene
return result return result
def format_command(command: cgv_connector_pb2.MergedSelection): def format_command(command: cgv_connector_pb2.Command):
return f"<cmd by {command.idRobot} of {command.idPick} to {command.idPlace}>" if command.HasField("pickAndPlace"):
pickAndPlace = command.pickAndPlace
return f"<PickAndPlace by {pickAndPlace.idRobot} of {pickAndPlace.idPick} to {pickAndPlace.idPlace}>"
if command.HasField("configChange"):
configChange = command.configChange
return f"<ConfigChange by {configChange.idRobotNewOwner} for {configChange.idCollaborationZone}>"
if command.HasField("evacuate"):
return f"<Evacuate by {command.evacuate.idRobot}>"
return "<unknown command>"
def _get_reach_objects(r): def _get_reach_objects(r):
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment