From 219ff63bf896a4139b6683540b2ab764d7160d83 Mon Sep 17 00:00:00 2001
From: Anton Tetov <anton@tetov.se>
Date: Wed, 13 Oct 2021 08:59:43 +0200
Subject: [PATCH] test nonblocking
---
abb_egm_client/egm_client.py | 180 +++++++++++++++------------------
examples/print_egm_feedback.py | 19 ++--
2 files changed, 88 insertions(+), 111 deletions(-)
diff --git a/abb_egm_client/egm_client.py b/abb_egm_client/egm_client.py
index aba3597..d24c293 100644
--- a/abb_egm_client/egm_client.py
+++ b/abb_egm_client/egm_client.py
@@ -1,123 +1,107 @@
import socket
-import select
-from errno import EINTR
+import logging
-try:
- from egm_pb2 import EgmRobot
-except ModuleNotFoundError:
- from .egm_pb2 import EgmRobot
+from abb_egm_client.atomic_counter import AtomicCounter
+from abb_egm_client.egm_pb2 import EgmRobot, EgmSensor, EgmHeader
-class EGMClient:
- def __init__(self, port=6510):
- self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
- self.socket.bind("", port)
- def receive_from_robot(self, timeout=0):
+log = logging.getLogger(__name__)
- try:
- result = select.select([self.socket], [], [self.socket], timeout=timeout)
- except select.error as err:
- if err.args[0] == EINTR:
- return False, None
- else:
- raise
-
- if len(res[0]) == 0 and len(res[2]) == 0:
- return False, None
- # try:
- buf, addr = self.socket.recvfrom(65536)
- # except:
- # self.egm_addr=None
- # return False, None
-
- # self.egm_addr=addr
-
- msg = egm_pb2.EgmRobot()
- msg.ParseFromString(buf)
-
- Robot_pos = None
- joint_angles = None
- rapid_running = False
- motors_on = False
- global Robot_Position_Flag
-
- if robot_message.HasField("feedBack"):
- if Robot_Position_Flag == False:
- Robot_pos = robot_message.feedBack.cartesian.pos
- Robot_Position_Flag = True
- Joints = robot_message.feedBack.joints.joints
- print(robot_message.feedBack.joints.joints)
- if robot_message.HasField("rapidExecState"):
- rapid_running = (
- robot_message.rapidExecState.state
- == robot_message.rapidExecState.RAPID_RUNNING
- )
- if robot_message.HasField("motorState"):
- motors_on = (
- robot_message.motorState.state == robot_message.motorState.MOTORS_ON
- )
- return True, Robot_pos, Joints
+class EGMClientException(Exception):
+ """Base class for other exceptions"""
+
+ pass
- def send_to_robot_joints(self, joint_angles):
- if not self.egm_addr:
- return False
+class EGMClientNotInitializedException(EGMClientException):
+ """When user tries to access attributes set in self.recieve_from_robot()"""
- self.send_sequence_number += 1
+ pass
- sensorMessage = egm_pb2.EgmSensor()
- header = sensorMessage.header
- header.mtype = egm_pb2.EgmHeader.MessageType.Value("MSGTYPE_CORRECTION")
- header.seqno = self.send_sequence_number
- self.send_sequence_number += 1
+class EGMClient:
+ def __init__(self, port=6510):
+ self.socket = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM)
+ self.robot_controller_address = None
+ self.send_counter = AtomicCounter()
+
+ self.socket.bind(("", port))
+
+ self.socket.setblocking(False)
+
+ # def _ensure_socket_ready(self, timeout=1):
+ # # try:
+ # # except select.error as err:
+ # # if err.args[0] == EINTR:
+ # # return False
+ # # else:
+ # # raise
+ # result = select.select([self.socket], [], [self.socket], timeout)
+
+ # print(result)
+
+ # # if len(result[0]) == 0 and len(result[2]) == 0:
+ # # raise Exception("error in ensure_socket_ready")
+
+ def _get_last_packet(self):
+ last_recieved = (None, None)
+
+ while True:
+ try:
+ data, addr = self.socket.recvfrom(1024)
+ if data:
+ last_recieved = (data, addr)
+ except socket.error as err:
+ # EWOULDBLOCK: recvfrom would block since socket buffer is empty
+ if err.args[0] == socket.EWOULDBLOCK:
+ return last_recieved # return last data recieved
+ else:
+ raise err # if there is another error, raise exception
+
+ def receive_from_robot(self, timeout=0):
+
+ data, addr = self.socket.recvfrom(1024)
- planned = sensorMessage.planned
+ if not self.robot_controller_address:
+ self.robot_controller_address = addr
- if joint_angles is not None:
- joint_angles2 = list(np.rad2deg(cartesian))
- print(joint_angles2)
- print(planned.joints.joints)
- buf2 = sensorMessage.SerializeToString()
+ msg = EgmRobot()
+ msg.ParseFromString(data)
- try:
- self.socket.sendto(buf2, self.egm_addr)
- except:
- return False
+ return msg
- return True
+ def send_msg(self, msg):
+ if not self.robot_controller_address:
+ raise EGMClientNotInitializedException(
+ "You need to start communication with controller."
+ )
- def send_to_robot_cartesian(self, Position, Rob_pos):
+ self.socket.sendto(msg.SerializeToString(), self.robot_controller_address)
- if not self.egm_addr:
- return False
+ def _create_sensor_msg(self, type="MSGTYPE_CORRECTION"):
+ msg = EgmSensor()
+ msg.header.mtype = EgmHeader.MessageType.Value(type)
+ msg.header.seqno = self.send_counter.inc()
- self.send_sequence_number += 1
+ return msg
- sensorMessage = egm_pb2.EgmSensor()
+ def send_planned_configuration(self, configuration):
+ msg = self._create_sensor_msg_type_correction()
- header = sensorMessage.header
- header.mtype = egm_pb2.EgmHeader.MessageType.Value("MSGTYPE_CORRECTION")
- header.seqno = self.send_sequence_number
- self.send_sequence_number += 1
+ msg.planned.joints.joints.extend(configuration)
- planned = sensorMessage.planned
+ self.send_msg(msg)
- if Position is not None:
- planned.cartesian.pos.x = Rob_pos.x + Position[2]
- planned.cartesian.pos.y = Rob_pos.y + Position[0]
- planned.cartesian.pos.z = Rob_pos.z + Position[1]
- planned.cartesian.euler.x = -90
- planned.cartesian.euler.y = 180
- planned.cartesian.euler.z = 0
- print(planned.cartesian.pos)
- buf2 = sensorMessage.SerializeToString()
+ def send_planned_frame(self, x, y, z, rx, ry, rz):
+ msg = self._create_sensor_msg_type_correction()
- try:
- self.socket.sendto(buf2, self.egm_addr)
- except:
- return False
+ msg.planned.cartesian.pos.x = x
+ msg.planned.cartesian.pos.y = y
+ msg.planned.cartesian.pos.z = z
+ msg.planned.cartesian.euler.x = rx
+ msg.planned.cartesian.euler.y = ry
+ msg.planned.cartesian.euler.z = rz
- return True
+ self.send_msg(msg)
diff --git a/examples/print_egm_feedback.py b/examples/print_egm_feedback.py
index 67e621a..1ba02cd 100644
--- a/examples/print_egm_feedback.py
+++ b/examples/print_egm_feedback.py
@@ -1,8 +1,5 @@
#!/usr/bin/env python
import logging
-import sys
-
-log = logging.getLogger()
try:
from abb_egm_client import EGMClient
@@ -11,32 +8,28 @@ except ImportError:
UDP_PORT = 6510
+log = logging.getLogger("egm_client")
-def print_egm_feedback(rate: int):
+def print_egm_feedback() -> None:
"""Print EGM feedback.
Parameters
----------
rate
Frequency of prints in hertz.
-
"""
egm_client = EGMClient(port=UDP_PORT)
- counter = 0
while True:
try:
- ok, feedback = egm_client.receive_from_robot()
- if ok and counter % rate:
- print(f"Seq: {counter}\tMsg: {feedback}")
- else:
- log.error("False from EGMClient.recieve_from_robot")
+ msg = egm_client.receive_from_robot()
+ print(f"Header:\n{msg.header.seqno}")
+ # print(f"Feedback:\n{msg.feedBack}")
except Exception as exc:
log.error(f"Exception raised {exc}")
log.info("Retrying")
- counter = 0
if __name__ == "__main__":
- print_egm_feedback(sys.argv[0])
+ print_egm_feedback()
--
GitLab