Skip to content

copilot

Copilot #

Main copilot class.

Source code in cogip/tools/copilot_pami/copilot.py 
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
class Copilot:
    """
    Main copilot class.
    """

    _loop: asyncio.AbstractEventLoop = None  # Event loop to use for all coroutines

    def __init__(self, server_url: str, id: int, serial_port: Path, serial_baud: int):
        """
        Class constructor.

        Arguments:
            server_url: server URL
            id: robot id
            serial_port: serial port connected to STM32 device
            serial_baud: baud rate
        """
        self.server_url = server_url
        self.id = id
        self.retry_connection = True
        self.shell_menu: models.ShellMenu | None = None
        self.pb_pids: dict[PB_PidEnum, PB_Pid] = {}

        self.sio = socketio.AsyncClient(logger=False)
        self.sio_events = SioEvents(self)
        self.sio.register_namespace(self.sio_events)

        pb_message_handlers = {
            reset_uuid: self.handle_reset,
            menu_uuid: self.handle_message_menu,
            pose_order_uuid: self.handle_message_pose,
            state_uuid: self.handle_message_state,
            pose_reached_uuid: self.handle_pose_reached,
            actuator_state_uuid: self.handle_actuator_state,
            pid_uuid: self.handle_pid,
        }

        self._pbcom = PBCom(serial_port, serial_baud, pb_message_handlers)

    async def run(self):
        """
        Start copilot.
        """
        self._loop = asyncio.get_running_loop()

        self.retry_connection = True
        await self.try_connect()

        await self._pbcom.send_serial_message(copilot_connected_uuid, None)

        await self._pbcom.run()

    async def try_connect(self):
        """
        Poll to wait for the first connection.
        Disconnections/reconnections are handle directly by the client.
        """
        while self.retry_connection:
            try:
                await self.sio.connect(self.server_url, namespaces=["/copilot"])
            except socketio.exceptions.ConnectionError:
                time.sleep(2)
                continue
            break

    @property
    def pbcom(self) -> PBCom:
        return self._pbcom

    async def handle_reset(self) -> None:
        """
        Handle reset message. This means that the robot has just booted.

        Send a reset message to all connected clients.
        """
        await self._pbcom.send_serial_message(copilot_connected_uuid, None)
        await self.sio_events.emit("reset")

    @pb_exception_handler
    async def handle_message_menu(self, message: bytes | None = None) -> None:
        """
        Send shell menu received from the robot to connected monitors.
        """
        pb_menu = PB_Menu()

        if message:
            await self._loop.run_in_executor(None, pb_menu.ParseFromString, message)

        menu = MessageToDict(pb_menu)
        self.shell_menu = models.ShellMenu.model_validate(menu)
        if self.sio.connected:
            await self.sio_events.emit("menu", self.shell_menu.model_dump(exclude_defaults=True, exclude_unset=True))

    @pb_exception_handler
    async def handle_message_pose(self, message: bytes | None = None) -> None:
        """
        Send robot pose received from the robot to connected monitors and detector.
        """
        pb_pose = PB_Pose()

        if message:
            await self._loop.run_in_executor(None, pb_pose.ParseFromString, message)

        pose = MessageToDict(
            pb_pose,
            including_default_value_fields=True,
            preserving_proto_field_name=True,
            use_integers_for_enums=True,
        )
        if self.sio.connected:
            await self.sio_events.emit("pose", pose)

    @pb_exception_handler
    async def handle_message_state(self, message: bytes | None = None) -> None:
        """
        Send robot state received from the robot to connected monitors.
        """
        pb_state = PB_State()

        if message:
            await self._loop.run_in_executor(None, pb_state.ParseFromString, message)

        state = MessageToDict(
            pb_state,
            including_default_value_fields=True,
            preserving_proto_field_name=True,
            use_integers_for_enums=True,
        )
        if self.sio.connected:
            await self.sio_events.emit("state", state)

    @pb_exception_handler
    async def handle_actuator_state(self, message: bytes | None = None) -> None:
        """
        Send actuator state received from the robot.
        """
        pb_actuator_state = PB_ActuatorState()

        if message:
            await self._loop.run_in_executor(None, pb_actuator_state.ParseFromString, message)

        kind = pb_actuator_state.WhichOneof("type")
        actuator_state = MessageToDict(
            getattr(pb_actuator_state, kind),
            including_default_value_fields=True,
            preserving_proto_field_name=True,
            use_integers_for_enums=True,
        )
        actuator_state["kind"] = ActuatorsKindEnum[kind]
        if self.sio.connected:
            await self.sio_events.emit("actuator_state", actuator_state)

    @pb_exception_handler
    async def handle_pid(self, message: bytes | None = None) -> None:
        """
        Send pids state received from the robot to connected dashboards.
        """
        pb_pid = PB_Pid()
        if message:
            await self._loop.run_in_executor(None, pb_pid.ParseFromString, message)

        self.pb_pids[pb_pid.id] = pb_pid
        pid = Pid(
            id=pb_pid.id,
            kp=pb_pid.kp,
            ki=pb_pid.ki,
            kd=pb_pid.kd,
            integral_term_limit=pb_pid.integral_term_limit,
        )

        # Get JSON Schema
        pid_schema = pid.model_json_schema()
        # Add namespace in JSON Schema
        pid_schema["namespace"] = "/copilot"
        # Add current values in JSON Schema
        pid_schema["title"] = pid.id.name
        for prop, value in pid.model_dump().items():
            if prop == "id":
                continue
            pid_schema["properties"][prop]["value"] = value
            pid_schema["properties"][f"{pid.id}-{prop}"] = pid_schema["properties"][prop]
            del pid_schema["properties"][prop]
        # Send config
        await self.sio_events.emit("config", pid_schema)

    async def handle_pose_reached(self) -> None:
        """
        Handle pose reached message.

        Forward info to the planner.
        """
        if self.sio.connected:
            await self.sio_events.emit("pose_reached")

__init__(server_url, id, serial_port, serial_baud) #

Class constructor.

Parameters:

Name Type Description Default
server_url str

server URL

 required
id int

robot id

 required
serial_port Path

serial port connected to STM32 device

 required
serial_baud int

baud rate

 required
Source code in cogip/tools/copilot_pami/copilot.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
def __init__(self, server_url: str, id: int, serial_port: Path, serial_baud: int):
    """
    Class constructor.

    Arguments:
        server_url: server URL
        id: robot id
        serial_port: serial port connected to STM32 device
        serial_baud: baud rate
    """
    self.server_url = server_url
    self.id = id
    self.retry_connection = True
    self.shell_menu: models.ShellMenu | None = None
    self.pb_pids: dict[PB_PidEnum, PB_Pid] = {}

    self.sio = socketio.AsyncClient(logger=False)
    self.sio_events = SioEvents(self)
    self.sio.register_namespace(self.sio_events)

    pb_message_handlers = {
        reset_uuid: self.handle_reset,
        menu_uuid: self.handle_message_menu,
        pose_order_uuid: self.handle_message_pose,
        state_uuid: self.handle_message_state,
        pose_reached_uuid: self.handle_pose_reached,
        actuator_state_uuid: self.handle_actuator_state,
        pid_uuid: self.handle_pid,
    }

    self._pbcom = PBCom(serial_port, serial_baud, pb_message_handlers)

handle_actuator_state(message=None) async #

Send actuator state received from the robot.

Source code in cogip/tools/copilot_pami/copilot.py 
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
@pb_exception_handler
async def handle_actuator_state(self, message: bytes | None = None) -> None:
    """
    Send actuator state received from the robot.
    """
    pb_actuator_state = PB_ActuatorState()

    if message:
        await self._loop.run_in_executor(None, pb_actuator_state.ParseFromString, message)

    kind = pb_actuator_state.WhichOneof("type")
    actuator_state = MessageToDict(
        getattr(pb_actuator_state, kind),
        including_default_value_fields=True,
        preserving_proto_field_name=True,
        use_integers_for_enums=True,
    )
    actuator_state["kind"] = ActuatorsKindEnum[kind]
    if self.sio.connected:
        await self.sio_events.emit("actuator_state", actuator_state)

handle_message_menu(message=None) async #

Send shell menu received from the robot to connected monitors.

Source code in cogip/tools/copilot_pami/copilot.py 
115
116
117
118
119
120
121
122
123
124
125
126
127
128
@pb_exception_handler
async def handle_message_menu(self, message: bytes | None = None) -> None:
    """
    Send shell menu received from the robot to connected monitors.
    """
    pb_menu = PB_Menu()

    if message:
        await self._loop.run_in_executor(None, pb_menu.ParseFromString, message)

    menu = MessageToDict(pb_menu)
    self.shell_menu = models.ShellMenu.model_validate(menu)
    if self.sio.connected:
        await self.sio_events.emit("menu", self.shell_menu.model_dump(exclude_defaults=True, exclude_unset=True))

handle_message_pose(message=None) async #

Send robot pose received from the robot to connected monitors and detector.

Source code in cogip/tools/copilot_pami/copilot.py 
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
@pb_exception_handler
async def handle_message_pose(self, message: bytes | None = None) -> None:
    """
    Send robot pose received from the robot to connected monitors and detector.
    """
    pb_pose = PB_Pose()

    if message:
        await self._loop.run_in_executor(None, pb_pose.ParseFromString, message)

    pose = MessageToDict(
        pb_pose,
        including_default_value_fields=True,
        preserving_proto_field_name=True,
        use_integers_for_enums=True,
    )
    if self.sio.connected:
        await self.sio_events.emit("pose", pose)

handle_message_state(message=None) async #

Send robot state received from the robot to connected monitors.

Source code in cogip/tools/copilot_pami/copilot.py 
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
@pb_exception_handler
async def handle_message_state(self, message: bytes | None = None) -> None:
    """
    Send robot state received from the robot to connected monitors.
    """
    pb_state = PB_State()

    if message:
        await self._loop.run_in_executor(None, pb_state.ParseFromString, message)

    state = MessageToDict(
        pb_state,
        including_default_value_fields=True,
        preserving_proto_field_name=True,
        use_integers_for_enums=True,
    )
    if self.sio.connected:
        await self.sio_events.emit("state", state)

handle_pid(message=None) async #

Send pids state received from the robot to connected dashboards.

Source code in cogip/tools/copilot_pami/copilot.py 
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
@pb_exception_handler
async def handle_pid(self, message: bytes | None = None) -> None:
    """
    Send pids state received from the robot to connected dashboards.
    """
    pb_pid = PB_Pid()
    if message:
        await self._loop.run_in_executor(None, pb_pid.ParseFromString, message)

    self.pb_pids[pb_pid.id] = pb_pid
    pid = Pid(
        id=pb_pid.id,
        kp=pb_pid.kp,
        ki=pb_pid.ki,
        kd=pb_pid.kd,
        integral_term_limit=pb_pid.integral_term_limit,
    )

    # Get JSON Schema
    pid_schema = pid.model_json_schema()
    # Add namespace in JSON Schema
    pid_schema["namespace"] = "/copilot"
    # Add current values in JSON Schema
    pid_schema["title"] = pid.id.name
    for prop, value in pid.model_dump().items():
        if prop == "id":
            continue
        pid_schema["properties"][prop]["value"] = value
        pid_schema["properties"][f"{pid.id}-{prop}"] = pid_schema["properties"][prop]
        del pid_schema["properties"][prop]
    # Send config
    await self.sio_events.emit("config", pid_schema)

handle_pose_reached() async #

Handle pose reached message.

Forward info to the planner.

Source code in cogip/tools/copilot_pami/copilot.py 
222
223
224
225
226
227
228
229
async def handle_pose_reached(self) -> None:
    """
    Handle pose reached message.

    Forward info to the planner.
    """
    if self.sio.connected:
        await self.sio_events.emit("pose_reached")

handle_reset() async #

Handle reset message. This means that the robot has just booted.

Send a reset message to all connected clients.

Source code in cogip/tools/copilot_pami/copilot.py 
106
107
108
109
110
111
112
113
async def handle_reset(self) -> None:
    """
    Handle reset message. This means that the robot has just booted.

    Send a reset message to all connected clients.
    """
    await self._pbcom.send_serial_message(copilot_connected_uuid, None)
    await self.sio_events.emit("reset")

run() async #

Start copilot.

Source code in cogip/tools/copilot_pami/copilot.py 
76
77
78
79
80
81
82
83
84
85
86
87
async def run(self):
    """
    Start copilot.
    """
    self._loop = asyncio.get_running_loop()

    self.retry_connection = True
    await self.try_connect()

    await self._pbcom.send_serial_message(copilot_connected_uuid, None)

    await self._pbcom.run()

try_connect() async #

Poll to wait for the first connection. Disconnections/reconnections are handle directly by the client.

Source code in cogip/tools/copilot_pami/copilot.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
async def try_connect(self):
    """
    Poll to wait for the first connection.
    Disconnections/reconnections are handle directly by the client.
    """
    while self.retry_connection:
        try:
            await self.sio.connect(self.server_url, namespaces=["/copilot"])
        except socketio.exceptions.ConnectionError:
            time.sleep(2)
            continue
        break