diff --git a/main.py b/main.py index 868fd53015e1c066fdccd0e51c2052dfa9f650ee..3886a6a1edc5c4b3e4854ff766997342a447888f 100644 --- a/main.py +++ b/main.py @@ -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,9 +390,18 @@ def send_complex(*_): else: button_id = ctx.triggered[0]['prop_id'].split('.')[0] if button_id: - 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) + 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) return dash.no_update @@ -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() diff --git a/utils.py b/utils.py index d758ac2f0410916899dcaa7fee6d708a58516e7d..c2348b98a6892967d6b3466119dd515c8bdb0ff6 100644 --- a/utils.py +++ b/utils.py @@ -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>"