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
Branches
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
// this file contains the messages that are exchanged between the cgv framework and the st ROS interface
// connector.proto
// this file contains the messages that are exchanged between the ROS connector and other systems
syntax = "proto3";
......@@ -10,22 +10,22 @@ message Object {
// Position is object-center related
message Position {
float x = 1; // in m
float y = 2; // in m
float z = 3; // height in m
double x = 1; // in m
double y = 2; // in m
double z = 3; // height in m
}
// 3D description of the object
message Size {
float length = 1; // in m
float width = 2; // in m
float height = 3; // in m
double length = 1; // in m
double width = 2; // in m
double height = 3; // in m
}
message Orientation {
float x = 1; // normalized quaternion
float y = 2;
float z = 3;
float w = 4;
double x = 1; // normalized quaternion
double y = 2;
double z = 3;
double w = 4;
}
message Color {
float r = 1; // 0..1
......@@ -38,6 +38,9 @@ message Object {
BIN = 2;
ARM = 3;
DROP_OFF_LOCATION = 4;
HUMAN = 5;
ROBOT = 6;
COLLABORATION_ZONE = 7;
}
string id = 1;
......@@ -46,26 +49,46 @@ message Object {
Size size = 4;
Orientation orientation = 5;
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 {
repeated Object objects = 1;
}
// the selection is done by the CGV framework and sent to ROS
// FIXME can be removed
message Selection {
string id = 1; // the id corresponds to an id of an Object in a Scene
}
// vvv from rs vvv.
// Merged message to contain both pick and place in one
message MergedSelection {
string idRobot = 1; // the id corresponds to and id of the robot Object that should execute this operation
string idPick = 2; // the id corresponds to an id of the Object in a Scene to be picked
string idPlace = 3; // the id corresponds to an id of the Object in a Scene where the picked object shall be placed
message Command {
oneof msg {
PickAndPlace pickAndPlace = 1;
ConfigChange configChange = 2;
Evacuate evacuate = 3;
}
}
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
// FIXME can be removed
message Reachability {
message ObjectReachability {
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
import queue
import threading
import time
import base64
import flask
import dash
from dash import dcc
......@@ -16,8 +18,14 @@ import cgv_connector_pb2
import utils
external_stylesheets = ['https://codepen.io/chriddyp/pen/bWLwgP.css']
MQTT_SERVER = 'localhost'
fixed_svg_filename = "ros3rag/ros3rag.placeB/images/2022-05-04T10-19-30-9155.svg"
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()
# mqtt client connected?
......@@ -41,22 +49,30 @@ commands = {
'send-place-b-rewind': ('place-b/rewind', '1'),
'send-place-b-start': ('coordinating/rag-b/command', 'start'),
'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-red': ('place-a/demo/move/objectRed1/red', '1'),
'send-place-b-demo-objRed-red': ('place-b/demo/move/objectRed1/red', '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-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-details': ('coordinator/model', 'details'),
'send-coordinator-exit': ('coordinator/exit', '1'),
'send-place-a-robot-ctrl-up': ('ros-place-a/status', 'up'),
'send-place-b-robot-ctrl-up': ('ros-place-b/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-a-rag-up': ('rag-a/status', 'up'),
'send-place-b-rag-up': ('rag-b/status', 'up'),
'send-dummy-up': ('random/status', 'up'),
'send-place-a-robot-ctrl-ready': ('ros-place-a/status', 'ready'),
'send-place-b-robot-ctrl-ready': ('ros-place-b/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-a-rag-ready': ('rag-a/status', 'ready'),
'send-place-b-rag-ready': ('rag-b/status', 'ready'),
'send-dummy-ready': ('random/status', 'ready'),
......@@ -75,9 +91,13 @@ complex_commands = {
}
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-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/arm2': (cgv_connector_pb2.Reachability(), utils.format_reachability),
'place-b/reachability/arm3': (cgv_connector_pb2.Reachability(), utils.format_reachability),
......@@ -85,6 +105,7 @@ conversion_topics = {
bytes_topics = [
]
svg_image_topic = 'place-b/model/svg/path'
button_style_normal = {"marginRight": "15px"}
button_style_exit = {**button_style_normal, "backgroundColor": "red", "color": "white"}
......@@ -92,6 +113,8 @@ textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical
app = dash.Dash(__name__, external_stylesheets=external_stylesheets)
app.layout = html.Div([
dcc.Tabs([
dcc.Tab(label="Buttons and Log", children=[
html.Div([ # First Row
html.Div([ # Column for place a
dcc.Textarea(
......@@ -144,15 +167,15 @@ app.layout = html.Div([
html.Button('Exit', id='send-coordinator-exit', style=button_style_exit),
# ], className='row'),
# 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 B Up', id='send-place-b-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('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('Dummy Up', id='send-dummy-up', style=button_style_normal),
# ], className='row'),
# 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 B Ready', id='send-place-b-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('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('Dummy Ready', id='send-dummy-ready', style=button_style_normal),
......@@ -165,8 +188,8 @@ app.layout = html.Div([
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('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 -> Blue', id='send-place-a-demo-objRed-blue', 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),
], className="four columns"),
html.Div([ # Column for commands of place b
html.H3("Commands Place B"),
......@@ -175,7 +198,14 @@ app.layout = html.Div([
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('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='row'),
# dcc.Markdown("---"),
......@@ -215,11 +245,11 @@ app.layout = html.Div([
dcc.Markdown("---"),
html.Div([
html.P("Topic"),
dcc.Input(id='manual-mqtt-topic'),
html.P("Message"),
dcc.Input(id='manual-mqtt-message'),
dcc.Input(id='manual-mqtt-topic', style={"margin-left": "5px"}),
html.P("Message", style={"margin-left": "15px"}),
dcc.Input(id='manual-mqtt-message', style={"margin-left": "5px"}),
# 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')),
# -- Invisible elements --
......@@ -231,7 +261,19 @@ app.layout = html.Div([
),
visdcc.Run_js(id='javascriptLog', run=""),
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(
......@@ -339,15 +381,17 @@ def check_connection(_n_intervals):
Output('mqtt-log', 'value'),
Output('javascriptLog', 'run'),
Output('topics-to-filter', 'options'),
Output('model-b-svg-img', 'src'),
Input('every-1-second', 'n_intervals'),
Input('clear-mqtt-log', 'n_clicks'),
Input('topics-to-filter', 'options'),
Input('topics-to-filter', 'value'),
Input('model-b-svg-img', 'src'),
State('filtered-mqtt-log', 'value'),
State('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):
"""
Periodically update mqtt log
......@@ -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) filter_options: displayed 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) filtered_value: current content of filtered mqtt log
:param (State) should_scroll: checkbox value whether to scroll to the end after update
:return: new content of mqtt log
"""
new_svg_img_src = previous_svg_img_src
ctx = dash.callback_context
if not ctx.triggered:
return dash.no_update
trigger_id = ctx.triggered[0]['prop_id'].split('.')[0]
if trigger_id == 'clear-mqtt-log':
return "", "", "", filter_options
return "", "", "", filter_options, new_svg_img_src
if trigger_id == 'topics-to-filter':
filtered_value = ""
......@@ -392,6 +438,9 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f
filtered_value = ""
for msg in local_messages:
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:
filtered_value += msg + "\n"
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
var textarea = document.getElementById('mqtt-log');
textarea.scrollTop = textarea.scrollHeight;
''' 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(
......@@ -448,6 +497,26 @@ def send_manual(n_clicks, topic, message):
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):
# Callback for mqtt client when connected
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):
def format_scene(scene: cgv_connector_pb2.Scene):
result = ""
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
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
def format_command(command: cgv_connector_pb2.MergedSelection):
return f"<cmd by {command.idRobot} of {command.idPick} to {command.idPlace}>"
def format_command(command: cgv_connector_pb2.Command):
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):
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment