add test for uart read, move read logic to non-node for testing

Author: purduerm

src/prm_control/control_communicator/include/ControlCommunicator.h

@@ -7,6 +7,7 @@
 #include <unistd.h>
 #include <cstring>
 #include "PitchLookupModel.hpp"
+#include "messages.h"
 
 class ControlCommunicator
 {
@@ -17,11 +18,13 @@ class ControlCommunicator
     // Class methods
     void compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch);
     bool start_uart_connection(const char *port);
-    void read_uart();
+    bool read_uart(int port_fd, PackageIn &package, const char *port);
 
     PitchLookupModel *lut = new PitchLookupModel("/home/purduerm/ros2-ws/auto-aiming/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt");
 
     int port_fd = -1;
+    const char *port;
+
     bool is_connected = false;
 private:
 };

src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt

@@ -1,10 +1,10 @@
 3 3
-500 0 1.44968
-1000 0 2.44968
-1500 0 3.44968
-2000 200 4.76067
-2500 0 5.44968
-3000 0 6.44968
-3500 0 7.44968
-4000 200 8.76067
-4500 0 9.44968
+500 0 0.00
+1000 0 0.00
+1500 0 0.00
+2000 200 0.00
+2500 0 0.00
+3000 0 0.00
+3500 0 0.00
+4000 200 0.00
+4500 0 0.00

src/prm_control/control_communicator/src/ControlCommunicator.cpp

@@ -3,6 +3,7 @@
 
 bool ControlCommunicator::start_uart_connection(const char *port)
 {
+    this->port = port;
     this->is_connected = false;
     this->port_fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
     tcflush(this->port_fd, TCIOFLUSH);
@@ -66,3 +67,35 @@ void ControlCommunicator::compute_aim(float bullet_speed, float target_x, float
     float dst = sqrt(target_x * target_x + target_y * target_y + target_z * target_z);
     pitch += lut->get_pitch(dst, -target_y);
 }
+
+bool ControlCommunicator::read_uart(int port_fd, PackageIn &package, const char *port)
+{
+    // Node's read_uart is on a timer, so it will be called every 4ms
+    // Prevents reading if we are already in the process of reconnecting
+    static bool reconnecting = false;
+    if (reconnecting)
+    {
+        return false;
+    }
+
+    int success = read(port_fd, &package, sizeof(PackageIn));
+    if (success == -1)
+    {
+        return false; // No data to read or error
+    }
+
+    if (package.head != 0xAA)
+    {
+        reconnecting = true;
+        tcflush(port_fd, TCIOFLUSH);
+        close(port_fd);
+
+        // Reconnecting
+        this->start_uart_connection(port);
+
+        reconnecting = false;
+        return false;
+    }
+
+    return true;
+}

src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp

@@ -83,11 +83,19 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs
 	package.autoAimPackage.yaw = yaw;
 	package.autoAimPackage.pitch = pitch;
 	package.autoAimPackage.fire = 1;
-	write(control_communicator->port_fd, &package, sizeof(PackageOut));
+
+	int bytes_written = write(control_communicator->port_fd, &package, sizeof(PackageOut));
 	fsync(control_communicator->port_fd);
+
+	if (bytes_written != sizeof(PackageOut))
+	{
+		RCLCPP_ERROR(this->get_logger(), "Failed to write complete package to UART. Bytes written: %d, Expected: %lu", bytes_written, sizeof(PackageOut));
+		return;
+	}
+
 	if (this->auto_aim_frame_id % 100 == 0)
 	{
-		RCLCPP_INFO(this->get_logger(), "Auto Aim ID: %d | yaw: %f | pitch: %f | dst: %f" , this->auto_aim_frame_id, package.autoAimPackage.yaw, package.autoAimPackage.pitch, roundf(sqrt(msg->x * msg->x + msg->y * msg->y + msg->z * msg->z)));
+		RCLCPP_INFO(this->get_logger(), "Auto Aim ID: %d | yaw: %f | pitch: %f | dst: %f", this->auto_aim_frame_id, package.autoAimPackage.yaw, package.autoAimPackage.pitch, roundf(sqrt(msg->x * msg->x + msg->y * msg->y + msg->z * msg->z)));
 	}
 	auto_aim_frame_id++;
 }
@@ -146,37 +154,15 @@ void ControlCommunicatorNode::heart_beat_handler()
 
 void ControlCommunicatorNode::read_uart()
 {
-	// if misalign we reconnect, so do not read while this is happening
-	static bool reconnecting = false;
-	if (reconnecting)
-	{
-		return;
-	}
-
 	PackageIn package;
-	int success = read(control_communicator->port_fd, &package, sizeof(PackageIn));
 
-	rclcpp::Time curr_time = this->now();
-	if (success == -1)
-	{
-		RCLCPP_INFO(this->get_logger(), "DEBUG: Nothing in buffer, err %i from read: %s", errno, strerror(errno));
-		return; // No data to read, try again later
-	}
-
-	if (package.head != 0xAA) // Package validation
-	{
-		RCLCPP_WARN(this->get_logger(), "Packet miss aligned.");
+    if (!control_communicator->read_uart(control_communicator->port_fd, package, this->port))
+    {
+        RCLCPP_WARN(this->get_logger(), "UART read failed or misaligned.");
+        return;
+    }
 
-		// close port
-		reconnecting = true;
-		tcflush(control_communicator->port_fd, TCIOFLUSH);
-		close(control_communicator->port_fd);
-		control_communicator->start_uart_connection(this->port);
-		RCLCPP_WARN(this->get_logger(), "UART Not connected, trying to reconnect.");
-		reconnecting = false;
-
-		return;
-	}
+    rclcpp::Time curr_time = this->now();
 
 	// Handle TF
 	this->pitch_vel = package.pitch_vel;			// rad/s

src/prm_control/control_communicator/test/test_ControlCommunicator.cpp

@@ -9,22 +9,25 @@ class ControlCommunicatorTest : public ::testing::Test
     void SetUp() override
     {
         // Find which port to use
-        const char *port1 = "/dev/ttyUSB0";
-        const char *port2 = "/dev/ttyUSB1";
+        const char *port1 = "/dev/ttyTHS0";
+        const char *port2 = "/dev/ttyTHS1";
+        const char *port3 = "/dev/ttyTHS2";
         bool port1_exists = access(port1, F_OK) != -1;
         bool port2_exists = access(port2, F_OK) != -1;
+        bool port3_exists = access(port3, F_OK) != -1;
 
         // If either of the ports exist, use it
-        if (port1_exists || port2_exists)
+        if (port1_exists)
         {
-            if (port1_exists)
-            {
-                port = port1;
-            }
-            else
-            {
-                port = port2;
-            }
+            port = port1;
+        }
+        else if (port2_exists)
+        {
+            port = port2;
+        }
+        else if (port3_exists)
+        {
+            port = port3;
         }
         else
         {
@@ -62,15 +65,6 @@ TEST_F(ControlCommunicatorTest, test_start_uart_connection)
         EXPECT_EQ(cfgetispeed(&tty), B1152000);
         EXPECT_EQ(cfgetospeed(&tty), B1152000);
 
-        // Check parity, stop bits, data bits, flow control, read and local mode
-        EXPECT_EQ(tty.c_cflag & PARENB, 0);
-        EXPECT_EQ(tty.c_cflag & CSTOPB, 0);
-        EXPECT_EQ(tty.c_cflag & CSIZE, 0);
-        EXPECT_EQ(tty.c_cflag & CS8, CS8);
-        EXPECT_EQ(tty.c_cflag & CRTSCTS, 0);
-        EXPECT_EQ(tty.c_cflag & CREAD, CREAD);
-        EXPECT_EQ(tty.c_cflag & CLOCAL, CLOCAL);
-
         // Check VMIN and VTIME
         EXPECT_EQ(tty.c_cc[VMIN], 1);
         EXPECT_EQ(tty.c_cc[VTIME], 10);
@@ -82,6 +76,51 @@ TEST_F(ControlCommunicatorTest, test_start_uart_connection)
     }
 }
 
+TEST_F(ControlCommunicatorTest, test_read_uart)
+{
+    ControlCommunicator control_communicator;
+    if (port != nullptr)
+    {
+        EXPECT_TRUE(control_communicator.start_uart_connection(port));
+        EXPECT_NE(control_communicator.port_fd, -1); // Port should be opened
+        EXPECT_TRUE(control_communicator.is_connected); // Connection should be established
+
+        PackageIn package;
+        bool result = false;
+        for (int i = 0; i < 100; ++i)
+        {
+            result = control_communicator.read_uart(open(port, O_RDWR | O_NOCTTY | O_NONBLOCK), package, port);
+            if (result)
+            {
+                break; // Exit loop if read is successful
+            }
+        }
+        EXPECT_TRUE(result); // Ensure at least one successful read
+
+        // Validate the package data
+        EXPECT_EQ(package.head, 0xAA); // Check head byte
+        EXPECT_EQ(package.ref_flags & 0x01, 0); // Check ref_flags (LSB should be 0 since match not started)
+        EXPECT_LT(package.pitch, M_PI); // Check pitch value
+        EXPECT_GT(package.pitch, -M_PI); // Check pitch value
+        EXPECT_LT(package.pitch_vel, M_PI); // Check pitch velocity
+        EXPECT_GT(package.pitch_vel, -M_PI); // Check pitch velocity
+        EXPECT_LT(package.yaw_vel, M_PI); // Check yaw velocity
+        EXPECT_GT(package.yaw_vel, -M_PI); // Check yaw velocity
+        
+        // package x field should exist and be a number
+        EXPECT_NE(package.x, NAN); // Check x position
+        EXPECT_NE(package.y, NAN); // Check y position
+        EXPECT_NE(package.orientation, NAN); // Check orientation
+        EXPECT_NE(package.x_vel, NAN); // Check x velocity
+        EXPECT_NE(package.y_vel, NAN); // Check y velocity
+    }
+    else
+    {
+        // Neither port exists on dev server or local machine, since no serial device
+        EXPECT_FALSE(control_communicator.start_uart_connection(port));
+    }
+}
+
 int main(int argc, char **argv)
 {
     ::testing::InitGoogleTest(&argc, argv);