From f5e11c126ab7711e888809e5549af9d21f9343e2 Mon Sep 17 00:00:00 2001
From: Thomas Muller <tmuller2017@my.fit.edu>
Date: Mon, 6 Sep 2021 23:37:14 +0000
Subject: [PATCH] Net robot works

Kill me
---
 .../ar3_description/state_publisher_net.py    | 11 +--
 src/net_robot/net_robot/net_robot.py          | 68 ++++++++++++++++++-
 2 files changed, 73 insertions(+), 6 deletions(-)

diff --git a/src/ar3_description/ar3_description/state_publisher_net.py b/src/ar3_description/ar3_description/state_publisher_net.py
index 28b2caf..75e0a02 100644
--- a/src/ar3_description/ar3_description/state_publisher_net.py
+++ b/src/ar3_description/ar3_description/state_publisher_net.py
@@ -6,6 +6,7 @@ from geometry_msgs.msg import Quaternion
 from sensor_msgs.msg import JointState
 from tf2_ros import TransformBroadcaster, TransformStamped
 import socket
+import struct
 
 class StatePublisher(Node):
 
@@ -38,16 +39,18 @@ class StatePublisher(Node):
             cli.settimeout(0)
             cli.setblocking(0)
             print(cli, addr)
+            self.prev_time = time.time()
             while rclpy.ok():
                 try:
-                    msg = cli.recv(4096)
+                    msg = cli.recv(2048)
                     if msg == b'':
+                        cli.close()
                         print('bailing')
                         break
-                    # print(repr(msg))
+                    print(repr(msg))
 
-                    if len(msg) >= 7 and msg[6] == 10:
-                        self.cmd_joints = [msg[0], msg[1], msg[2], msg[3], msg[4], msg[5]]
+                    if len(msg) >= 13 and msg[12] == 10:
+                        self.cmd_joints = struct.unpack('hhhhhh', msg[:-1])
                         self.cmd_joints = [a / 180. * pi for a in self.cmd_joints]
                 except BlockingIOError as e:
                     pass
diff --git a/src/net_robot/net_robot/net_robot.py b/src/net_robot/net_robot/net_robot.py
index 49eb138..5902232 100644
--- a/src/net_robot/net_robot/net_robot.py
+++ b/src/net_robot/net_robot/net_robot.py
@@ -1,6 +1,70 @@
-def main():
-    print('Hi from net_robot.')
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
 
+from std_msgs.msg import Float32
+
+import socket
+import struct
+
+class NetRobot(Node):
+    def __init__(self):
+        super().__init__('state_publisher')
+
+        qos = QoSProfile(depth=10)
+        self.create_subscription(Float32, '~/drives/j1', self._on_j1, qos)
+        self.create_subscription(Float32, '~/drives/j2', self._on_j2, qos)
+        self.create_subscription(Float32, '~/drives/j3', self._on_j3, qos)
+        self.create_subscription(Float32, '~/drives/j4', self._on_j4, qos)
+        self.create_subscription(Float32, '~/drives/j5', self._on_j5, qos)
+        self.create_subscription(Float32, '~/drives/j6', self._on_j6, qos)
+
+        self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        # self._sock.connect(('tmuller.xyz', 9999))
+        self._sock.connect(('127.0.0.1', 9999))
+
+        self._joints = [0.] * 6
+        self._prev_joints = [0.] * 6
+
+    def _send(self):
+        if self._joints != self._prev_joints:
+            self._sock.send(struct.pack('hhhhhh', *[int(j) for j in self._joints]) + b'\n')
+            self._prev_joints = self._joints.copy()
+
+    def _on_j1(self, msg):
+        self._joints[0] = msg.data
+        self._send()
+
+    def _on_j2(self, msg):
+        self._joints[1] = msg.data
+        self._send()
+
+    def _on_j3(self, msg):
+        self._joints[2] = msg.data
+        self._send()
+
+    def _on_j4(self, msg):
+        self._joints[3] = msg.data
+        self._send()
+
+    def _on_j5(self, msg):
+        self._joints[4] = msg.data
+        self._send()
+
+    def _on_j6(self, msg):
+        self._joints[5] = msg.data
+        self._send()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = NetRobot()
+
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
 
 if __name__ == '__main__':
     main()