project structure

.gitignore

+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# vscode
+.vscode/settings.json

README.md

 # ABB EGM examples
 
+```bash
+conda env create -f environment.yml
+conda activate abb_egm_examples
+```

abb_egm_client/__init__.py

+from .egm_client import EGMClient  # noqa: F401,F403

abb_egm_client/egm_client.py

+import socket
+import select
+from errno import EINTR
+
+try:
+    from egm_pb2 import EgmRobot
+except ModuleNotFoundError:
+    from .egm_pb2 import EgmRobot
+
+
+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):
+
+        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
+
+    def send_to_robot_joints(self, joint_angles):
+
+        if not self.egm_addr:
+            return False
+
+        self.send_sequence_number += 1
+
+        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
+
+        planned = sensorMessage.planned
+
+        if joint_angles is not None:
+            joint_angles2 = list(np.rad2deg(cartesian))
+            print(joint_angles2)
+            print(planned.joints.joints)
+        buf2 = sensorMessage.SerializeToString()
+
+        try:
+            self.socket.sendto(buf2, self.egm_addr)
+        except:
+            return False
+
+        return True
+
+    def send_to_robot_cartesian(self, Position, Rob_pos):
+
+        if not self.egm_addr:
+            return False
+
+        self.send_sequence_number += 1
+
+        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
+
+        planned = sensorMessage.planned
+
+        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()
+
+        try:
+            self.socket.sendto(buf2, self.egm_addr)
+        except:
+            return False
+
+        return True

environment.yml

+name: abb-egm-examples
+channels:
+  - conda-forge
+dependencies:
+  - numpy
+  - protobuf

examples/print_egm_feedback.py

+#!/usr/bin/env python
+import logging
+import sys
+
+log = logging.getLogger()
+
+try:
+    from abb_egm_client import EGMClient
+except ImportError:
+    raise ImportWarning("abb_egm not found, have you installed the package?")
+
+UDP_PORT = 6510
+
+
+def print_egm_feedback(rate: int):
+    """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")
+        except Exception as exc:
+            log.error(f"Exception raised {exc}")
+            log.info("Retrying")
+            counter = 0
+
+
+if __name__ == "__main__":
+    print_egm_feedback(sys.argv[0])

pyproject.toml

+[build-system]
+requires = [
+  "setuptools >= 40.9.0",
+  "wheel",
+]
+build-backend = "setuptools.build_meta"

setup.cfg

+[metadata]
+name = abb_egm_client
+version = 0.1.0
+
+[options]
+packages = abb_egm_client
+python_requires = >= 3.8
+install_requires =
+    protobuf

setup.py

+import setuptools
+
+setuptools.setup()