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
Branches
Tags
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"
image_directory = '/data/git/jastadd/ros3rag/ros3rag.placeB/images/'
static_image_route = '/static/'
# MQTT_SERVER = '192.168.0.122'
MQTT_SERVER = 'localhost'
MQTT_SERVER = '192.168.0.122'
# MQTT_SERVER = 'localhost'
mqttc = mqtt.Client()
# mqtt client connected?
......@@ -54,12 +54,12 @@ commands = {
# '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-arm1-moving': ('place-b/demo', 'arm1/moving'),
'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-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-g1': ('place-b/demo', 'big-blue/g1'),
# '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-arm2-moving': ('place-b/demo', 'arm2/moving'),
# '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-g1': ('place-b/demo', 'big-blue/g1'),
'send-coordinator-model': ('coordinator/model', '1'),
'send-coordinator-model-details': ('coordinator/model', 'details'),
......@@ -89,6 +89,10 @@ complex_commands = {
'send-place-b-reachability-2-update':
('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 = {
'/ceti_cell_placeworld/scene/update': (cgv_connector_pb2.Scene(), utils.format_scene),
......@@ -110,12 +114,13 @@ topics_enabled_once_seen = [
"coordinating/rag-b/status",
"place-b/model/svg/path",
"place-b/arm1/position",
"place-b/arm2/position"
"place-b/arm2/position",
"place-b/demo"
]
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"}
textarea_style_normal = {'width': '100%', 'height': '200px', 'resize': 'vertical'}
......@@ -208,12 +213,28 @@ app.layout = html.Div([
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('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-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-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-g1', id='send-place-b-demo-big-blue-g1', 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('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('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.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='row'),
# dcc.Markdown("---"),
......@@ -357,7 +378,9 @@ def send_json_for_object_to_topic(json_content: str, obj, topic: str):
@app.callback(
Output('hidden-div-scene-b', 'children'),
[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(*_):
ctx = dash.callback_context
......@@ -367,6 +390,15 @@ def send_complex(*_):
else:
button_id = ctx.triggered[0]['prop_id'].split('.')[0]
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]
json_content = ctx.states[state_id + ".value"]
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
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):
filter_options.append({'label': topic, 'value': topic})
if topic in topics_enabled_once_seen:
topics_to_filter.append(topic)
topic_match = True
if topic_match:
filtered_value += msg + "\n"
else:
return dash.no_update
log_cmd = '''
......@@ -530,7 +563,7 @@ def serve_image(image_path):
def on_mqtt_connect(_client, _userdata, _flags, _rc, _properties=None):
# 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()
mqttc.subscribe(topic='#')
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):
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:
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 = scene
return result
......@@ -63,9 +64,9 @@ def format_command(command: cgv_connector_pb2.Command):
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}>"
return f"<ConfigChange for {configChange.idRobotNewOwner} owns {configChange.idCollaborationZone}>"
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>"
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment