refactor: remove SPADE dependencies
Did not look at tests yet, this is a very non-final commit. ref: N25B-300
This commit is contained in:
@@ -1,8 +1,8 @@
|
||||
import asyncio
|
||||
import json
|
||||
|
||||
import zmq.asyncio
|
||||
from spade.behaviour import CyclicBehaviour
|
||||
import zmq
|
||||
import zmq.asyncio as azmq
|
||||
from zmq.asyncio import Context
|
||||
|
||||
from control_backend.agents import BaseAgent
|
||||
@@ -12,109 +12,38 @@ from ..actuation.robot_speech_agent import RobotSpeechAgent
|
||||
|
||||
|
||||
class RICommunicationAgent(BaseAgent):
|
||||
req_socket: zmq.Socket
|
||||
_address = ""
|
||||
_bind = True
|
||||
connected = False
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
jid: str,
|
||||
password: str,
|
||||
port: int = settings.agent_settings.default_spade_port,
|
||||
verify_security: bool = False,
|
||||
name: str,
|
||||
address=settings.zmq_settings.ri_command_address,
|
||||
bind=False,
|
||||
):
|
||||
super().__init__(jid, password, port, verify_security)
|
||||
super().__init__(name)
|
||||
self._address = address
|
||||
self._bind = bind
|
||||
self._req_socket: zmq.asyncio.Socket | None = None
|
||||
self.pub_socket: zmq.asyncio.Socket | None = None
|
||||
self._req_socket: azmq.Socket | None = None
|
||||
self.pub_socket: azmq.Socket | None = None
|
||||
self.connected = False
|
||||
|
||||
class ListenBehaviour(CyclicBehaviour):
|
||||
async def run(self):
|
||||
"""
|
||||
Run the listening (ping) loop indefinetely.
|
||||
"""
|
||||
assert self.agent is not None
|
||||
async def setup(self):
|
||||
"""
|
||||
Try to set up the communication agent, we have `behaviour_settings.comm_setup_max_retries`
|
||||
retries in case we don't have a response yet.
|
||||
"""
|
||||
self.logger.info("Setting up %s", self.name)
|
||||
|
||||
if not self.agent.connected:
|
||||
await asyncio.sleep(settings.behaviour_settings.sleep_s)
|
||||
return
|
||||
# Bind request socket
|
||||
await self._setup_sockets()
|
||||
|
||||
# We need to listen and sent pings.
|
||||
message = {"endpoint": "ping", "data": {"id": "e.g. some reference id"}}
|
||||
seconds_to_wait_total = settings.behaviour_settings.sleep_s
|
||||
try:
|
||||
await asyncio.wait_for(
|
||||
self.agent._req_socket.send_json(message), timeout=seconds_to_wait_total / 2
|
||||
)
|
||||
except TimeoutError:
|
||||
self.agent.logger.debug(
|
||||
"Waited too long to send message - "
|
||||
"we probably dont have any receivers... but let's check!"
|
||||
)
|
||||
if await self._negotiate_connection():
|
||||
self.connected = True
|
||||
await self.add_background_task(self._listen_loop())
|
||||
else:
|
||||
self.logger.warning("Failed to negotiate connection during setup.")
|
||||
|
||||
# Wait up to {seconds_to_wait_total/2} seconds for a reply
|
||||
try:
|
||||
message = await asyncio.wait_for(
|
||||
self.agent._req_socket.recv_json(), timeout=seconds_to_wait_total / 2
|
||||
)
|
||||
self.logger.info("Finished setting up %s", self.name)
|
||||
|
||||
# We didnt get a reply
|
||||
except TimeoutError:
|
||||
self.agent.logger.info(
|
||||
f"No ping retrieved in {seconds_to_wait_total} seconds, "
|
||||
"sending UI disconnection event and attempting to restart."
|
||||
)
|
||||
|
||||
# Make sure we dont retry receiving messages untill we're setup.
|
||||
self.agent.connected = False
|
||||
self.agent.remove_behaviour(self)
|
||||
|
||||
# Tell UI we're disconnected.
|
||||
topic = b"ping"
|
||||
data = json.dumps(False).encode()
|
||||
if self.agent.pub_socket is None:
|
||||
self.agent.logger.warning(
|
||||
"Communication agent pub socket not correctly initialized."
|
||||
)
|
||||
else:
|
||||
try:
|
||||
await asyncio.wait_for(
|
||||
self.agent.pub_socket.send_multipart([topic, data]), 5
|
||||
)
|
||||
except TimeoutError:
|
||||
self.agent.logger.warning(
|
||||
f"Initial connection ping for router timed out in {self.agent.name}."
|
||||
)
|
||||
|
||||
# Try to reboot.
|
||||
self.agent.logger.debug("Restarting communication agent.")
|
||||
await self.agent.setup()
|
||||
|
||||
self.agent.logger.debug(f'Received message "{message}" from RI.')
|
||||
if "endpoint" not in message:
|
||||
self.agent.logger.warning(
|
||||
"No received endpoint in message, expected ping endpoint."
|
||||
)
|
||||
return
|
||||
|
||||
# See what endpoint we received
|
||||
match message["endpoint"]:
|
||||
case "ping":
|
||||
topic = b"ping"
|
||||
data = json.dumps(True).encode()
|
||||
if self.agent.pub_socket is not None:
|
||||
await self.agent.pub_socket.send_multipart([topic, data])
|
||||
await asyncio.sleep(settings.behaviour_settings.sleep_s)
|
||||
case _:
|
||||
self.agent.logger.debug(
|
||||
"Received message with topic different than ping, while ping expected."
|
||||
)
|
||||
|
||||
async def setup_sockets(self, force=False):
|
||||
async def _setup_sockets(self, force=False):
|
||||
"""
|
||||
Sets up request socket for communication agent.
|
||||
"""
|
||||
@@ -130,21 +59,13 @@ class RICommunicationAgent(BaseAgent):
|
||||
self.pub_socket = Context.instance().socket(zmq.PUB)
|
||||
self.pub_socket.connect(settings.zmq_settings.internal_pub_address)
|
||||
|
||||
async def setup(self, max_retries: int = settings.behaviour_settings.comm_setup_max_retries):
|
||||
"""
|
||||
Try to set up the communication agent, we have `behaviour_settings.comm_setup_max_retries`
|
||||
retries in case we don't have a response yet.
|
||||
"""
|
||||
self.logger.info("Setting up %s", self.jid)
|
||||
|
||||
# Bind request socket
|
||||
await self.setup_sockets()
|
||||
|
||||
async def _negotiate_connection(
|
||||
self, max_retries: int = settings.behaviour_settings.comm_setup_max_retries
|
||||
):
|
||||
retries = 0
|
||||
# Let's try a certain amount of times before failing connection
|
||||
while retries < max_retries:
|
||||
# Make sure the socket is properly setup.
|
||||
if self._req_socket is None:
|
||||
retries += 1
|
||||
continue
|
||||
|
||||
# Send our message and receive one back
|
||||
@@ -156,7 +77,6 @@ class RICommunicationAgent(BaseAgent):
|
||||
received_message = await asyncio.wait_for(
|
||||
self._req_socket.recv_json(), timeout=retry_frequency
|
||||
)
|
||||
|
||||
except TimeoutError:
|
||||
self.logger.warning(
|
||||
"No connection established in %d seconds (attempt %d/%d)",
|
||||
@@ -166,7 +86,6 @@ class RICommunicationAgent(BaseAgent):
|
||||
)
|
||||
retries += 1
|
||||
continue
|
||||
|
||||
except Exception as e:
|
||||
self.logger.warning("Unexpected error during negotiation: %s", e)
|
||||
retries += 1
|
||||
@@ -187,64 +106,129 @@ class RICommunicationAgent(BaseAgent):
|
||||
|
||||
# At this point, we have a valid response
|
||||
try:
|
||||
for port_data in received_message["data"]:
|
||||
id = port_data["id"]
|
||||
port = port_data["port"]
|
||||
bind = port_data["bind"]
|
||||
|
||||
if not bind:
|
||||
addr = f"tcp://localhost:{port}"
|
||||
else:
|
||||
addr = f"tcp://*:{port}"
|
||||
|
||||
match id:
|
||||
case "main":
|
||||
if addr != self._address:
|
||||
if not bind:
|
||||
self._req_socket.connect(addr)
|
||||
else:
|
||||
self._req_socket.bind(addr)
|
||||
case "actuation":
|
||||
ri_commands_agent = RobotSpeechAgent(
|
||||
settings.agent_settings.robot_speech_name
|
||||
+ "@"
|
||||
+ settings.agent_settings.host,
|
||||
settings.agent_settings.robot_speech_name,
|
||||
address=addr,
|
||||
bind=bind,
|
||||
)
|
||||
await ri_commands_agent.start()
|
||||
case _:
|
||||
self.logger.warning("Unhandled negotiation id: %s", id)
|
||||
|
||||
await self._handle_negotiation_response(received_message)
|
||||
# Let UI know that we're connected
|
||||
topic = b"ping"
|
||||
data = json.dumps(True).encode()
|
||||
if self.pub_socket:
|
||||
await self.pub_socket.send_multipart([topic, data])
|
||||
return True
|
||||
except Exception as e:
|
||||
self.logger.warning("Error unpacking negotiation data: %s", e)
|
||||
retries += 1
|
||||
await asyncio.sleep(1)
|
||||
continue
|
||||
|
||||
# setup succeeded
|
||||
break
|
||||
return False
|
||||
|
||||
else:
|
||||
self.logger.warning("Failed to set up %s after %d retries", self.name, max_retries)
|
||||
return
|
||||
async def _handle_negotiation_response(self, received_message):
|
||||
for port_data in received_message["data"]:
|
||||
id = port_data["id"]
|
||||
port = port_data["port"]
|
||||
bind = port_data["bind"]
|
||||
|
||||
# Set up ping behaviour
|
||||
listen_behaviour = self.ListenBehaviour()
|
||||
self.add_behaviour(listen_behaviour)
|
||||
if not bind:
|
||||
addr = f"tcp://localhost:{port}"
|
||||
else:
|
||||
addr = f"tcp://*:{port}"
|
||||
|
||||
# Let UI know that we're connected
|
||||
match id:
|
||||
case "main":
|
||||
if addr != self._address:
|
||||
assert self._req_socket is not None
|
||||
if not bind:
|
||||
self._req_socket.connect(addr)
|
||||
else:
|
||||
self._req_socket.bind(addr)
|
||||
case "actuation":
|
||||
ri_commands_agent = RobotSpeechAgent(
|
||||
settings.agent_settings.robot_speech_name,
|
||||
address=addr,
|
||||
bind=bind,
|
||||
)
|
||||
await ri_commands_agent.start()
|
||||
case _:
|
||||
self.logger.warning("Unhandled negotiation id: %s", id)
|
||||
|
||||
async def stop(self):
|
||||
if self._req_socket:
|
||||
self._req_socket.close()
|
||||
if self.pub_socket:
|
||||
self.pub_socket.close()
|
||||
await super().stop()
|
||||
|
||||
async def _listen_loop(self):
|
||||
"""
|
||||
Run the listening (ping) loop indefinitely.
|
||||
"""
|
||||
while self._running:
|
||||
if not self.connected:
|
||||
await asyncio.sleep(settings.behaviour_settings.sleep_s)
|
||||
continue
|
||||
|
||||
# We need to listen and send pings.
|
||||
message = {"endpoint": "ping", "data": {"id": "e.g. some reference id"}}
|
||||
seconds_to_wait_total = settings.behaviour_settings.sleep_s
|
||||
try:
|
||||
assert self._req_socket is not None
|
||||
await asyncio.wait_for(
|
||||
self._req_socket.send_json(message), timeout=seconds_to_wait_total / 2
|
||||
)
|
||||
except TimeoutError:
|
||||
self.logger.debug(
|
||||
"Waited too long to send message - "
|
||||
"we probably dont have any receivers... but let's check!"
|
||||
)
|
||||
|
||||
# Wait up to {seconds_to_wait_total/2} seconds for a reply
|
||||
try:
|
||||
assert self._req_socket is not None
|
||||
message = await asyncio.wait_for(
|
||||
self._req_socket.recv_json(), timeout=seconds_to_wait_total / 2
|
||||
)
|
||||
|
||||
self.logger.debug(f'Received message "{message}" from RI.')
|
||||
if "endpoint" not in message:
|
||||
self.logger.warning("No received endpoint in message, expected ping endpoint.")
|
||||
continue
|
||||
|
||||
# See what endpoint we received
|
||||
match message["endpoint"]:
|
||||
case "ping":
|
||||
topic = b"ping"
|
||||
data = json.dumps(True).encode()
|
||||
if self.pub_socket is not None:
|
||||
await self.pub_socket.send_multipart([topic, data])
|
||||
await asyncio.sleep(settings.behaviour_settings.sleep_s)
|
||||
case _:
|
||||
self.logger.debug(
|
||||
"Received message with topic different than ping, while ping expected."
|
||||
)
|
||||
# We didnt get a reply
|
||||
except TimeoutError:
|
||||
self.logger.info(
|
||||
f"No ping retrieved in {seconds_to_wait_total} seconds, "
|
||||
"sending UI disconnection event and attempting to restart."
|
||||
)
|
||||
await self._handle_disconnection()
|
||||
continue
|
||||
except Exception:
|
||||
self.logger.error("Error while waiting for ping message.", exc_info=True)
|
||||
raise
|
||||
|
||||
async def _handle_disconnection(self):
|
||||
self.connected = False
|
||||
|
||||
# Tell UI we're disconnected.
|
||||
topic = b"ping"
|
||||
data = json.dumps(True).encode()
|
||||
if self.pub_socket is None:
|
||||
self.logger.warning("Communication agent pub socket not correctly initialized.")
|
||||
else:
|
||||
data = json.dumps(False).encode()
|
||||
if self.pub_socket:
|
||||
try:
|
||||
await asyncio.wait_for(self.pub_socket.send_multipart([topic, data]), 5)
|
||||
except TimeoutError:
|
||||
self.logger.warning("Initial connection ping for router timed out in com_ri_agent.")
|
||||
self.logger.warning("Connection ping for router timed out.")
|
||||
|
||||
# Make sure to start listening now that we're connected.
|
||||
self.connected = True
|
||||
self.logger.info("Finished setting up %s", self.jid)
|
||||
# Try to reboot/renegotiate
|
||||
self.logger.debug("Restarting communication negotiation.")
|
||||
if await self._negotiate_connection(max_retries=1):
|
||||
self.connected = True
|
||||
|
||||
Reference in New Issue
Block a user