dream

1
import threading
2
import rospy
3

4
from std_msgs.msg import String
5
from flask import Flask, request
6
from flask import jsonify
7

8
import logging
9
import time
10
from os import getenv
11

12
import sentry_sdk
13

14

15
sentry_sdk.init(getenv("SENTRY_DSN"))
16

17
logging.basicConfig(format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", level=logging.INFO)
18
logger = logging.getLogger(__name__)
19

20
app = Flask(__name__)
21

22
talker = rospy.Publisher("talker", String, queue_size=1)
23

24
threading.Thread(target=lambda: rospy.init_node("listener", disable_signals=True)).start()
25

26
VALID_COMMANDS = ["test_command"]
27
COMMAND_QUEUE = []
28
EXECUTING_COMMAND = None
29

30

31
@app.route("/ping", methods=["POST"])
32
def ping():
33
    return "pong"
34

35

36
@app.route("/set_commands", methods=["POST"])  # this endpoint should not be accessed from within dream
37
def respond_set_commands():
38
    global VALID_COMMANDS
39

40
    st_time = time.perf_counter()
41
    VALID_COMMANDS = list(map(lambda i: i.lower(), request.json.get("commands", [])))
42
    if not VALID_COMMANDS:
43
        logger.info("embodied-server user did not send valid commands list")
44
    logger.info(f"embodied-server `VALID_COMMANDS` set: {VALID_COMMANDS}")
45

46
    total_time = time.perf_counter() - st_time
47

48
    logger.info(f"embodied-server `is_command_valid` exec time: {total_time:.3f}s")
49

50
    return {"result": bool(VALID_COMMANDS)}
51

52

53
@app.route("/is_command_valid", methods=["POST"])
54
def respond_is_command_valid():
55
    st_time = time.perf_counter()
56

57
    command = request.json.get("command", None)
58
    results = {"result": any(item in command for item in VALID_COMMANDS)}
59
    logger.info(f"embodied-server `is_command_valid` results: {results}")
60

61
    total_time = time.perf_counter() - st_time
62

63
    logger.info(f"embodied-server `is_command_valid` exec time: {total_time:.3f}s")
64

65
    return jsonify(results)
66

67

68
@app.route("/perform_command", methods=["POST"])
69
def respond_perform_command():
70
    st_time = time.perf_counter()
71

72
    command = request.json.get("command", None)
73
    cmd_valid = command in VALID_COMMANDS
74
    logger.info(f"ros-flask-server received command: {command}, valid? -{cmd_valid}")
75
    if cmd_valid:
76
        logger.info("Sending command to ROS...")
77
        try:
78
            talker.publish(command)
79
            logger.info("Successfully returned from ROS!")
80
            COMMAND_QUEUE.append(command)
81
        except Exception as e:
82
            logger.info(f"Error inside ROS: {e}")
83
    results = {"result": cmd_valid}
84
    logger.info(f"embodied-server `perform_command` {command} appended to queue?: {results}")
85

86
    total_time = time.perf_counter() - st_time
87

88
    logger.info(f"embodied-server `perform_command` exec time: {total_time:.3f}s")
89

90
    return jsonify(results)
91

92

93
@app.route("/receive_command", methods=["POST"])  # this endpoint should not be accessed from within dream
94
def respond_receive_command():
95
    global EXECUTING_COMMAND
96

97
    st_time = time.perf_counter()
98

99
    command = COMMAND_QUEUE.pop(0) if COMMAND_QUEUE else None
100
    results = {"command": command}
101
    logger.info(f"embodied-server `receive_command` results: {results}")
102

103
    total_time = time.perf_counter() - st_time
104

105
    logger.info(f"embodied-server `receive_command` exec time: {total_time:.3f}s")
106

107
    return jsonify(results)
108

109

110
@app.route("/is_command_performed", methods=["POST"])
111
def respond_is_command_performed():
112
    st_time = time.perf_counter()
113

114
    results = {"result": EXECUTING_COMMAND}
115
    logger.info(f"embodied-server `is_command_performed` results: {results}")
116

117
    total_time = time.perf_counter() - st_time
118

119
    logger.info(f"embodied-server `is_command_performed` exec time: {total_time:.3f}s")
120

121
    return jsonify(results)
122

123

124
@app.route("/command_is_performed", methods=["POST"])  # this endpoint should not be accessed from within dream
125
def respond_command_is_performed():
126
    global EXECUTING_COMMAND
127

128
    st_time = time.perf_counter()
129

130
    results = {"result": True}
131
    logger.info(f"embodied-server `command_is_performed` results: {results}")
132
    EXECUTING_COMMAND = None
133

134
    total_time = time.perf_counter() - st_time
135

136
    logger.info(f"embodied-server `command_is_performed` exec time: {total_time:.3f}s")
137

138
    return jsonify(results)
139

140

141
if __name__ == "__main__":
142
    app.run(host="0.0.0.0", port=6000, debug=True)
143