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

some more commands

parent 22ead654
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
main.py
+ 56
− 23
View file @ cb6055e7
...@@ -24,8 +24,8 @@ fixed_svg_filename = "2022-05-04T10-19-30-9155.svg" ...@@ -24,8 +24,8 @@ fixed_svg_filename = "2022-05-04T10-19-30-9155.svg"
image_directory = '/data/git/jastadd/ros3rag/ros3rag.placeB/images/' image_directory = '/data/git/jastadd/ros3rag/ros3rag.placeB/images/'
static_image_route = '/static/' static_image_route = '/static/'
# MQTT_SERVER = '192.168.0.122' MQTT_SERVER = '192.168.0.122'
MQTT_SERVER = 'localhost' # MQTT_SERVER = 'localhost'
mqttc = mqtt.Client() mqttc = mqtt.Client()
# mqtt client connected? # mqtt client connected?
...@@ -54,12 +54,12 @@ commands = { ...@@ -54,12 +54,12 @@ commands = {
# 'send-place-b-demo-objRed-red': ('place-b/demo', 'objectRed1/red'), # 'send-place-b-demo-objRed-red': ('place-b/demo', 'objectRed1/red'),
'send-place-b-demo-initial_scene': ('place-b/demo', 'initial_scene'), 'send-place-b-demo-initial_scene': ('place-b/demo', 'initial_scene'),
'send-place-b-demo-arm1-moving': ('place-b/demo', 'arm1/moving'), # 'send-place-b-demo-arm1-moving': ('place-b/demo', 'arm1/moving'),
'send-place-b-demo-arm1-idle': ('place-b/demo', 'arm1/idle'), # 'send-place-b-demo-arm1-idle': ('place-b/demo', 'arm1/idle'),
'send-place-b-demo-arm2-moving': ('place-b/demo', 'arm2/moving'), # 'send-place-b-demo-arm2-moving': ('place-b/demo', 'arm2/moving'),
'send-place-b-demo-arm2-idle': ('place-b/demo', 'arm2/idle'), # 'send-place-b-demo-arm2-idle': ('place-b/demo', 'arm2/idle'),
'send-place-b-demo-big-blue-cz': ('place-b/demo', 'big-blue/cz'), # 'send-place-b-demo-big-blue-cz': ('place-b/demo', 'big-blue/cz'),
'send-place-b-demo-big-blue-g1': ('place-b/demo', 'big-blue/g1'), # 'send-place-b-demo-big-blue-g1': ('place-b/demo', 'big-blue/g1'),
'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'),
...@@ -89,6 +89,10 @@ complex_commands = { ...@@ -89,6 +89,10 @@ complex_commands = {
'send-place-b-reachability-2-update': 'send-place-b-reachability-2-update':
('place-b/reachability/arm2', 'place-b-reachability-2-json', cgv_connector_pb2.Reachability()), ('place-b/reachability/arm2', 'place-b-reachability-2-json', cgv_connector_pb2.Reachability()),
} }
other_complex_commands = {
'send-arm-state': ('send-arm-state-robot', 'send-arm-state-state'),
'send-obj-pos': ('send-obj-pos-obj', 'send-obj-pos-pos')
}
conversion_topics = { conversion_topics = {
'/ceti_cell_placeworld/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene), '/ceti_cell_placeworld/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
...@@ -110,12 +114,13 @@ topics_enabled_once_seen = [ ...@@ -110,12 +114,13 @@ topics_enabled_once_seen = [
"coordinating/rag-b/status", "coordinating/rag-b/status",
"place-b/model/svg/path", "place-b/model/svg/path",
"place-b/arm1/position", "place-b/arm1/position",
"place-b/arm2/position" "place-b/arm2/position",
"place-b/demo"
] ]
svg_image_topic = 'place-b/model/svg/path' svg_image_topic = 'place-b/model/svg/path'
button_style_normal = {"marginRight": "15px"} button_style_normal = {"marginRight": "15px", "padding": "2px", "height": "2ßpx", "line-height": "0", "marginTop": "5px"}
button_style_exit = {**button_style_normal, "backgroundColor": "red", "color": "white"} button_style_exit = {**button_style_normal, "backgroundColor": "red", "color": "white"}
textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical'} textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical'}
...@@ -208,12 +213,28 @@ app.layout = html.Div([ ...@@ -208,12 +213,28 @@ app.layout = html.Div([
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('initial_scene', id='send-place-b-demo-initial_scene', style=button_style_normal),
html.Button('arm1-moving', id='send-place-b-demo-arm1-moving', style=button_style_normal), # html.Button('arm1-moving', id='send-place-b-demo-arm1-moving', style=button_style_normal),
html.Button('arm1-idle', id='send-place-b-demo-arm1-idle', style=button_style_normal), # html.Button('arm1-idle', id='send-place-b-demo-arm1-idle', style=button_style_normal),
html.Button('arm2-moving', id='send-place-b-demo-arm2-moving', style=button_style_normal), # html.Button('arm2-moving', id='send-place-b-demo-arm2-moving', style=button_style_normal),
html.Button('arm2-idle', id='send-place-b-demo-arm2-idle', style=button_style_normal), # html.Button('arm2-idle', id='send-place-b-demo-arm2-idle', 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-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), # html.Button('big-blue-g1', id='send-place-b-demo-big-blue-g1', style=button_style_normal),
html.Div([
dcc.RadioItems(['arm1', 'arm2'], 'arm1', id='send-arm-state-robot', inline=True, style={"marginRight": "10px", "marginTop": "5px"}),
dcc.RadioItems([{'label': 'Idle', 'value': 'STATE_IDLE'},
{'label': 'Picking', 'value': 'STATE_PICKING'},
{'label': 'Placing', 'value': 'STATE_PLACING'},
{'label': 'Moving', 'value': 'STATE_MOVING'}]
, 'STATE_MOVING', id='send-arm-state-state', inline=True, style={"marginRight": "10px", "marginTop": "5px"}),
html.Button('Send', id='send-arm-state', style=button_style_normal)
], className='row', style={"display":"flex", "border": "1px black solid", "border-radius": "5px", "marginTop": "5px"}),
html.Div([
dcc.RadioItems(['bigBlue', 'bigGreen'], 'bigBlue',
id='send-obj-pos-obj', inline=True, style={"marginRight": "10px", "marginTop": "5px"}),
dcc.RadioItems(['cz1', 'G1', 'G2', 'I1', 'I2'], 'cz1',
id='send-obj-pos-pos', inline=True, style={"marginRight": "10px", "marginTop": "5px"}),
html.Button('Send', id='send-obj-pos', style=button_style_normal)
], className='row', style={"display":"flex", "border": "1px black solid", "border-radius": "5px", "marginTop": "5px"})
], className="four columns"), ], className="four columns"),
], className='row'), ], className='row'),
# dcc.Markdown("---"), # dcc.Markdown("---"),
...@@ -357,7 +378,9 @@ def send_json_for_object_to_topic(json_content: str, obj, topic: str): ...@@ -357,7 +378,9 @@ def send_json_for_object_to_topic(json_content: str, obj, topic: str):
@app.callback( @app.callback(
Output('hidden-div-scene-b', 'children'), Output('hidden-div-scene-b', 'children'),
[Input(button_id, 'n_clicks') for button_id in complex_commands], [Input(button_id, 'n_clicks') for button_id in complex_commands],
[State(value[1], 'value') for value in complex_commands.values()] [Input(button_id, 'n_clicks') for button_id in other_complex_commands],
[State(value[1], 'value') for value in complex_commands.values()],
[State(state, 'value') for states in other_complex_commands.values() for state in states]
) )
def send_complex(*_): def send_complex(*_):
ctx = dash.callback_context ctx = dash.callback_context
...@@ -367,6 +390,15 @@ def send_complex(*_): ...@@ -367,6 +390,15 @@ def send_complex(*_):
else: else:
button_id = ctx.triggered[0]['prop_id'].split('.')[0] button_id = ctx.triggered[0]['prop_id'].split('.')[0]
if button_id: if button_id:
if button_id == 'send-arm-state':
robot = ctx.states["send-arm-state-robot.value"]
state = ctx.states["send-arm-state-state.value"]
mqttc.publish(topic='place-b/demo', payload=robot + '/' + state[6:].lower())
elif button_id == 'send-obj-pos':
obj = ctx.states["send-obj-pos-obj.value"]
pos = ctx.states["send-obj-pos-pos.value"]
mqttc.publish(topic='place-b/demo', payload=obj + '/' + pos)
else:
topic, state_id, protobuf_obj = complex_commands[button_id] topic, state_id, protobuf_obj = complex_commands[button_id]
json_content = ctx.states[state_id + ".value"] json_content = ctx.states[state_id + ".value"]
send_json_for_object_to_topic(json_content, protobuf_obj, topic) send_json_for_object_to_topic(json_content, protobuf_obj, topic)
...@@ -450,12 +482,13 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f ...@@ -450,12 +482,13 @@ def append_to_mqtt_log(_n_intervals, clear_n_clicks, filter_options, topics_to_f
if topic == svg_image_topic: if topic == svg_image_topic:
new_svg_img_src = '/static/' + utils.parse_log_msg(msg)[2] new_svg_img_src = '/static/' + utils.parse_log_msg(msg)[2]
print('found new svg: ' + new_svg_img_src) 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): if topic not in (option['label'] for option in filter_options):
filter_options.append({'label': topic, 'value': topic}) filter_options.append({'label': topic, 'value': topic})
if topic in topics_enabled_once_seen: if topic in topics_enabled_once_seen:
topics_to_filter.append(topic) topics_to_filter.append(topic)
topic_match = True
if topic_match:
filtered_value += msg + "\n"
else: else:
return dash.no_update return dash.no_update
log_cmd = ''' log_cmd = '''
...@@ -530,7 +563,7 @@ def serve_image(image_path): ...@@ -530,7 +563,7 @@ def serve_image(image_path):
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(f'\nConnected to {MQTT_SERVER} at {datetime.datetime.now().isoformat()}')
ready_event.set() ready_event.set()
mqttc.subscribe(topic='#') mqttc.subscribe(topic='#')
threading.Thread(target=publish_test_message).start() threading.Thread(target=publish_test_message).start()
......
utils.py
+ 4
− 3
View file @ cb6055e7
...@@ -51,7 +51,8 @@ def format_scene(scene: cgv_connector_pb2.Scene): ...@@ -51,7 +51,8 @@ def format_scene(scene: cgv_connector_pb2.Scene):
for obj in scene.objects: for obj in scene.objects:
if obj.type != cgv_connector_pb2.Object.Type.DROP_OFF_LOCATION and obj.type != cgv_connector_pb2.Object.Type.UNKNOWN: 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<{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.state}>" owner_str = f"owner: {obj.owner}" if obj.owner else "no owner"
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}) {cgv_connector_pb2.Object.State.Name(obj.state)} ({obj.state}) {owner_str}>"
# result += str(obj) # result += str(obj)
# result = scene # result = scene
return result return result
...@@ -63,9 +64,9 @@ def format_command(command: cgv_connector_pb2.Command): ...@@ -63,9 +64,9 @@ def format_command(command: cgv_connector_pb2.Command):
return f"<PickAndPlace by {pickAndPlace.idRobot} of {pickAndPlace.idPick} to {pickAndPlace.idPlace}>" return f"<PickAndPlace by {pickAndPlace.idRobot} of {pickAndPlace.idPick} to {pickAndPlace.idPlace}>"
if command.HasField("configChange"): if command.HasField("configChange"):
configChange = command.configChange configChange = command.configChange
return f"<ConfigChange by {configChange.idRobotNewOwner} for {configChange.idCollaborationZone}>" return f"<ConfigChange for {configChange.idRobotNewOwner} owns {configChange.idCollaborationZone}>"
if command.HasField("evacuate"): if command.HasField("evacuate"):
return f"<Evacuate by {command.evacuate.idRobot}>" return f"<Evacuate of {command.evacuate.idRobot} from {command.evacuate.idCollaborationZone}>"
return "<unknown command>" return "<unknown command>"
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment