Merge pull request #11 from little-red-rover/10-switch-to-tcp

Switch from UDP to TCP

docker/docker-compose.yml

@@ -5,12 +5,9 @@ services:
             context: ..
             dockerfile: docker/ros_workspace.Dockerfile
         tty: true # Prevent immediate exit when running with dev containers
-        network_mode: "host"
         volumes:
             - ../little_red_rover:/little_red_rover_ws/src/little_red_rover
             - ../tools:/tools
         ports:
-            - "9002:9002" # gzweb
-            - "9090:9090" # rosbridge
-            - "8001:8001/udp" # agent -> rover
+            - "8001:8001" # agent -> rover
             - "8765:8765" # foxglove bridge

little_red_rover/little_red_rover/drive_base_peripheral.py

@@ -0,0 +1,40 @@
+import rospy
+
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import Twist
+
+import little_red_rover.pb.messages_pb2 as messages
+from little_red_rover.rover_connection import RoverConnection
+
+
+class DriveBasePeripheral:
+    def __init__(self, connection: RoverConnection):
+        self.joint_state_publisher = rospy.Publisher(
+            "joint_states", JointState, queue_size=10
+        )
+
+        self.joint_state_msg = JointState()
+        self.joint_state_msg.header.frame_id = "robot_body"
+        self.subscription = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
+
+        self.connection = connection
+
+    def handle_packet(self, packet: messages.NetworkPacket):
+        if not packet.HasField("joint_states"):
+            return
+
+        self.joint_state_msg.header.stamp.set(
+            packet.joint_states.time.sec, packet.joint_states.time.nanosec
+        )
+        self.joint_state_msg.name = list(packet.joint_states.name)
+        self.joint_state_msg.effort = packet.joint_states.effort
+        self.joint_state_msg.position = packet.joint_states.position
+        self.joint_state_msg.velocity = packet.joint_states.velocity
+        self.joint_state_publisher.publish(self.joint_state_msg)
+
+    def cmd_vel_callback(self, msg: Twist):
+        packet = messages.NetworkPacket()
+        packet.cmd_vel.v = msg.linear.x
+        packet.cmd_vel.w = msg.angular.z
+
+        self.connection.send(packet.SerializeToString())

little_red_rover/little_red_rover/hal.py

@@ -1,144 +1,54 @@
+from google.protobuf.message import DecodeError
 import rospy
 
-from math import floor, inf, pi
-
-from sensor_msgs.msg import Imu, JointState
-from sensor_msgs.msg import LaserScan
-from geometry_msgs.msg import Twist
-
 import threading
-import socket
 import little_red_rover.pb.messages_pb2 as messages
 
+from little_red_rover.rover_connection import RoverConnection
+from little_red_rover.lidar_peripheral import LidarPeripheral
+from little_red_rover.drive_base_peripheral import DriveBasePeripheral
+from little_red_rover.imu_peripheral import ImuPeripheral
+
 
 class HAL:
     def __init__(self):
-        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
-        self.socket.bind(("0.0.0.0", 8001))
-
-        self.send_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-
-        self.subscription = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
+        self.connection = RoverConnection(("192.168.4.1", 8001))
 
-        self.joint_state_publisher = rospy.Publisher(
-            "joint_states", JointState, queue_size=10
-        )
-        self.scan_publisher = rospy.Publisher("scan", LaserScan, queue_size=10)
-        self.imu_publisher = rospy.Publisher("imu/data_raw", Imu, queue_size=10)
+        self.peripherals = [
+            LidarPeripheral(),
+            DriveBasePeripheral(self.connection),
+            ImuPeripheral(),
+        ]
 
-        self.ranges = [0.0] * 720
-        self.intensities = [0.0] * 720
+        self.decode_error_count = 0
 
-        threading.Thread(target=self.run_loop).start()
+        threading.Thread(target=self.run_loop, daemon=True).start()
 
     def run_loop(self):
-        laser_msg = LaserScan()
-        laser_msg.header.frame_id = "lidar"
-        laser_msg.range_min = 0.1
-        laser_msg.range_max = 8.0
-        laser_msg.angle_min = 0.0
-        laser_msg.angle_max = 2.0 * pi
-        laser_msg.angle_increment = (2 * pi) / (len(self.ranges))
-
-        joint_state_msg = JointState()
-        joint_state_msg.header.frame_id = "robot_body"
-
-        imu_msg = Imu()
-        imu_msg.header.frame_id = "robot_body"
-
         while not rospy.is_shutdown():
-            self.socket.settimeout(1.0)
             try:
-                data = self.socket.recv(1500)
-                packet = messages.UdpPacket()
-                packet.ParseFromString(data)
-            except socket.timeout:
-                print(
-                    "No data recieved from Little Red Rover for the past 1.0 seconds. As you sure you're connected to the rover's wifi hotspot?"
-                )
-                continue
-            except socket.error as e:
-                print(e)
-                continue
+                data = None
+                while data == None:
+                    try:
+                        data = self.connection.recv_packet()
+                    except Exception as e:
+                        print(e)
+
+                packet = messages.NetworkPacket()
+                packet.ParseFromString(bytes(data))
+
+                for peripheral in self.peripherals:
+                    peripheral.handle_packet(packet)
+
+            except DecodeError:
+                self.decode_error_count += 1
+                if self.decode_error_count > 20:
+                    print(f"Failed to decode {self.decode_error_count} packets.")
             except Exception as e:
-                print(e)
-                continue
-
-            if len(packet.laser) != 0:
-                for scan in packet.laser:
-                    self.handle_laser_scan(laser_msg, scan)
-            elif packet.HasField("joint_states"):
-                self.handle_joint_states(joint_state_msg, packet.joint_states)
-            elif packet.HasField("imu"):
-                self.handle_imu(imu_msg, packet.imu)
-
-    def handle_joint_states(self, msg: JointState, packet: messages.JointStates):
-        msg.header.stamp.set(packet.time.sec, packet.time.nanosec)
-        msg.name = list(packet.name)
-        msg.effort = packet.effort
-        msg.position = packet.position
-        msg.velocity = packet.velocity
-        self.joint_state_publisher.publish(msg)
-
-    def handle_laser_scan(self, msg: LaserScan, packet: messages.LaserScan):
-        break_in_packet = False
-        for i in range(len(packet.ranges)):
-            angle = packet.angle_min + (packet.angle_max - packet.angle_min) * (
-                i / (len(packet.ranges) - 1)
-            )
-            index = int(((angle % (2.0 * pi)) / (2.0 * pi)) * 720.0)
-
-            if angle > pi * 2.0 and not break_in_packet:
-                msg.time_increment = packet.time_increment
-                msg.scan_time = packet.scan_time
-                msg.ranges = self.ranges
-                msg.intensities = self.intensities
-
-                self.scan_publisher.publish(msg)
-                break_in_packet = True
-
-                self.ranges = [0.0] * 720
-                self.intensities = [0.0] * 720
-
-                msg.header.stamp.set(packet.time.sec, packet.time.nanosec)
-                # msg.header.stamp = self.get_clock().now().to_msg()
-
-            self.ranges[index] = packet.ranges[i]
-            self.intensities[index] = packet.intensities[i]
-
-            if (
-                self.ranges[index] > 8.0
-                or self.ranges[index] < 0.1
-                or self.intensities[index] == 0
-            ):
-                self.ranges[index] = inf
-                self.intensities[index] = 0.0
-
-    def handle_imu(self, msg: Imu, packet: messages.IMU):
-        msg.header.stamp.set(packet.time.sec, packet.time.nanosec)
-        msg.orientation_covariance = [-1.0] + [0.0] * 8
-        msg.linear_acceleration.x = packet.accel_x
-        msg.linear_acceleration.y = packet.accel_y
-        msg.linear_acceleration.z = packet.accel_z
-        # TODO
-        msg.linear_acceleration_covariance = [0.0] * 9
-        msg.angular_velocity.x = packet.gyro_x
-        msg.angular_velocity.y = packet.gyro_y
-        msg.angular_velocity.z = packet.gyro_z
-        # TODO
-        msg.angular_velocity_covariance = [0.0] * 9
-        self.imu_publisher.publish(msg)
-
-    def cmd_vel_callback(self, msg: Twist):
-        packet = messages.UdpPacket()
-        packet.cmd_vel.v = msg.linear.x
-        packet.cmd_vel.w = msg.angular.z
-
-        self.send_socket.sendto(packet.SerializeToString(), ("192.168.4.1", 8001))
-
-
-def main(args=None):
+                print(f"HAL: Error - {e}")
+
+
+def main(_=None):
     rospy.init_node("hal", anonymous=True)
     _ = HAL()
 

little_red_rover/little_red_rover/imu_peripheral.py

@@ -0,0 +1,32 @@
+import rospy
+from sensor_msgs.msg import Imu
+import little_red_rover.pb.messages_pb2 as messages
+
+
+class ImuPeripheral:
+    def __init__(self):
+        self.imu_publisher = rospy.Publisher("imu/data_raw", Imu, queue_size=10)
+        self.imu_msg = Imu()
+        self.imu_msg.header.frame_id = "robot_body"
+
+    def handle_packet(self, packet: messages.NetworkPacket):
+        if not packet.HasField("imu"):
+            return
+        self.imu_msg.header.stamp.set(packet.imu.time.sec, packet.imu.time.nanosec)
+
+        # disable orientation
+        self.imu_msg.orientation_covariance = [-1.0] + [0.0] * 8
+
+        # accel
+        self.imu_msg.linear_acceleration.x = packet.imu.accel_x
+        self.imu_msg.linear_acceleration.y = packet.imu.accel_y
+        self.imu_msg.linear_acceleration.z = packet.imu.accel_z
+        self.imu_msg.linear_acceleration_covariance = [0.0] * 9  # TODO
+
+        # gyro
+        self.imu_msg.angular_velocity.x = packet.imu.gyro_x
+        self.imu_msg.angular_velocity.y = packet.imu.gyro_y
+        self.imu_msg.angular_velocity.z = packet.imu.gyro_z
+        self.imu_msg.angular_velocity_covariance = [0.0] * 9  # TODO
+
+        self.imu_publisher.publish(self.imu_msg)

little_red_rover/little_red_rover/lidar_peripheral.py

@@ -0,0 +1,62 @@
+import rospy
+from sensor_msgs.msg import LaserScan
+import little_red_rover.pb.messages_pb2 as messages
+
+from math import inf, pi
+
+
+class LidarPeripheral:
+    def __init__(self):
+        self.ranges = [0.0] * 720
+        self.intensities = [0.0] * 720
+
+        self.scan_publisher = rospy.Publisher("scan", LaserScan, queue_size=10)
+
+        self.laser_msg = LaserScan()
+        self.laser_msg.header.frame_id = "lidar"
+        self.laser_msg.range_min = 0.1
+        self.laser_msg.range_max = 8.0
+        self.laser_msg.angle_min = 0.0
+        self.laser_msg.angle_max = 2.0 * pi
+        self.laser_msg.angle_increment = (2 * pi) / (len(self.ranges))
+
+    def handle_packet(self, packet: messages.NetworkPacket):
+        if len(packet.laser) == 0:
+            return
+        for scan in packet.laser:
+            self.handle_laser_scan(self.laser_msg, scan)
+        pass
+
+    def handle_laser_scan(self, msg: LaserScan, packet: messages.LaserScan):
+        break_in_packet = False
+        for i in range(len(packet.ranges)):
+            angle = packet.angle_min + (packet.angle_max - packet.angle_min) * (
+                i / (len(packet.ranges) - 1)
+            )
+            index = int(((angle % (2.0 * pi)) / (2.0 * pi)) * 720.0)
+
+            if angle > pi * 2.0 and not break_in_packet:
+                msg.time_increment = packet.time_increment
+                msg.scan_time = packet.scan_time
+                msg.ranges = self.ranges
+                msg.intensities = self.intensities
+
+                self.scan_publisher.publish(msg)
+                break_in_packet = True
+
+                self.ranges = [0.0] * 720
+                self.intensities = [0.0] * 720
+
+                msg.header.stamp.set(packet.time.sec, packet.time.nanosec)
+                # msg.header.stamp = self.get_clock().now().to_msg()
+
+            self.ranges[index] = packet.ranges[i]
+            self.intensities[index] = packet.intensities[i]
+
+            if (
+                self.ranges[index] > 8.0
+                or self.ranges[index] < 0.1
+                or self.intensities[index] == 0
+            ):
+                self.ranges[index] = inf
+                self.intensities[index] = 0.0

little_red_rover/little_red_rover/pb/messages.proto

@@ -42,7 +42,7 @@ message IMU {
   float accel_z = 7;
 }
 
-message UdpPacket {
+message NetworkPacket {
   repeated LaserScan laser = 1;
   optional JointStates joint_states = 2;
   optional TwistCmd cmd_vel = 3;

little_red_rover/little_red_rover/pb/messages_pb2.pyi

@@ -79,7 +79,7 @@ class IMU(_message.Message):
     accel_z: float
     def __init__(self, time: _Optional[_Union[TimeStamp, _Mapping]] = ..., gyro_x: _Optional[float] = ..., gyro_y: _Optional[float] = ..., gyro_z: _Optional[float] = ..., accel_x: _Optional[float] = ..., accel_y: _Optional[float] = ..., accel_z: _Optional[float] = ...) -> None: ...
 
-class UdpPacket(_message.Message):
+class NetworkPacket(_message.Message):
     __slots__ = ("laser", "joint_states", "cmd_vel", "imu")
     LASER_FIELD_NUMBER: _ClassVar[int]
     JOINT_STATES_FIELD_NUMBER: _ClassVar[int]