improve pitch lookup table for aiming accuracy

src/prm_control/control_communicator/include/ControlCommunicator.h

@@ -16,7 +16,8 @@ class ControlCommunicator
     ~ControlCommunicator() {}
 
     // Class methods
-    void compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch);
+    int aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch, bool &impossible);
+    void compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch, bool &impossible);
     bool start_uart_connection(const char *port);
     bool read_uart(int port_fd, PackageIn &package, const char *port);
 

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

@@ -1,10 +1,25 @@
-3 3
-1000 -1000 23.00
-1000 0 20.50
-1000 1000 18.00
-2000 -1000 17.00
-2000 0 12.50
-2000 1000 10.00
-3000 -1000 11.25
-3000 0 8.00
-3000 1000 4.00
+12 2
+500 -200 6
+500 300 6
+1000 -200 6
+1000 300 6
+1500 -200 7
+1500 300 7
+2000 -200 5.0
+2000 300 5.0
+2500 -200 5.0
+2500 300 5.0
+3000 -200 3
+3000 300 3
+3500 -200 -1
+3500 300 -1
+4000 -200 -2
+4000 300 -2
+4500 -200 0
+4500 300 0
+5000 -200 0
+5000 300 0
+5500 -200 0
+5500 300 0
+6000 -200 0
+6000 300 0

src/prm_control/control_communicator/src/ControlCommunicator.cpp

@@ -44,7 +44,23 @@ bool ControlCommunicator::start_uart_connection(const char *port)
     return true;
 }
 
-void ControlCommunicator::compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch)
+int ControlCommunicator::aim(float aim_bullet_speed, float x, float y, float z, float &yaw, float &pitch, bool &impossible)
+{
+	this->compute_aim(aim_bullet_speed, x, y, z, yaw, pitch, impossible);
+
+	PackageOut package;
+	package.frame_id = 0xAA;
+	package.frame_type = FRAME_TYPE_AUTO_AIM;
+	package.autoAimPackage.yaw = yaw;
+	package.autoAimPackage.pitch = pitch;
+	package.autoAimPackage.fire = 1;
+
+	int bytes_written = write(this->port_fd, &package, sizeof(PackageOut));
+	fsync(this->port_fd);
+    return bytes_written;
+}
+
+void ControlCommunicator::compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch, bool &impossible)
 {
     // if X and Y and Z are 0
     if (target_x == 0 && target_y == 0 && target_z == 0)
@@ -55,7 +71,6 @@ void ControlCommunicator::compute_aim(float bullet_speed, float target_x, float
     }
 
     // projectile model based on quartic solver
-    bool impossible = false;
     double p;
     double y;
     pitch_yaw_gravity_model_movingtarget_const_v({ target_z, target_x, -target_y }, {0, 0, 0}, {0, 0, 9810}, 0.0, &p, &y, &impossible);

src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp

@@ -68,29 +68,28 @@ void ControlCommunicatorNode::publish_static_tf(float x, float y, float z, float
 
 void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg)
 {	
+	// Timing to monitor for degradation in performance
+	static rclcpp::Time last_time = this->now();
+	static int degraded_perf_count = 0;
+	rclcpp::Time curr_time = this->now();
+
 	if (!control_communicator->is_connected || control_communicator->port_fd < 0)
 	{
 		RCLCPP_WARN(this->get_logger(), "UART Not connected, ignoring aim message.");
 		return;
 	}
 
-	float yaw, pitch;
-	control_communicator->compute_aim(aim_bullet_speed, msg->x, msg->y, msg->z, yaw, pitch);
-
-	PackageOut package;
-	package.frame_id = 0xAA;
-	package.frame_type = FRAME_TYPE_AUTO_AIM;
-	package.autoAimPackage.yaw = yaw;
-	package.autoAimPackage.pitch = pitch;
-	package.autoAimPackage.fire = 1;
-
-	int bytes_written = write(control_communicator->port_fd, &package, sizeof(PackageOut));
-	fsync(control_communicator->port_fd);
+	// Compute yaw/pitch and send over UART
+	float yaw;
+	float pitch;
+	bool impossible;
+	int bytes_written = control_communicator->aim(aim_bullet_speed, msg->x, msg->y, msg->z, yaw, pitch, impossible);
+	bool AIMING = (msg->x != 0 || msg->y != 0 || msg->z != 0);
 
 	if (this->auto_aim_frame_id % 100 == 0 && this->auto_aim_frame_id != 0)
 	{
 		float dst = sqrt(pow(msg->x, 2) + pow(msg->y, 2) + pow(msg->z, 2));
-		RCLCPP_INFO(this->get_logger(), "Yaw: %.2f | Pitch: %.2f | dst: %.2f | (x, y, z): (%.2f, %.2f, %.2f)", yaw, pitch, dst, msg->x, msg->y, msg->z);
+		RCLCPP_INFO(this->get_logger(), "Yaw: %.1f | Pitch: %.1f | dst: %.1f | (x,y,z): (%.1f, %.1f, %.1f)", yaw, pitch, dst, msg->x, msg->y, msg->z);
 	}
 
 	if (bytes_written != sizeof(PackageOut))
@@ -100,6 +99,29 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs
 	}
 
 	auto_aim_frame_id++;
+
+	// check performance for degradation in sending frequency
+	rclcpp::Duration elapsed_time = curr_time - last_time;
+	last_time = curr_time;
+	float TARGET_FREQUENCY = 90.0; // Hz
+	float TARGET_PERIOD = 1 / TARGET_FREQUENCY; // seconds
+
+	if (degraded_perf_count > 50 && AIMING)
+	{
+		RCLCPP_WARN(this->get_logger(), "DEGRADED PERFORMANCE DETECTED - AUTO-AIM SEND FREQUENCY: [%0.1f Hz]", 1 / elapsed_time.seconds());
+		degraded_perf_count = 0;
+	}
+	else if (this->auto_aim_frame_id % 1000 == 0)
+	{
+		RCLCPP_INFO(this->get_logger(), "Auto-Aim send frequency: [%0.1f Hz]", 1 / elapsed_time.seconds());
+	}
+	if (elapsed_time.seconds() > TARGET_PERIOD && AIMING)
+	{
+		degraded_perf_count++;	
+	}
+	else {
+		degraded_perf_count = 0;
+	}
 }
 
 void ControlCommunicatorNode::nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg)

src/prm_control/control_communicator/src/PitchLookupModel.cpp

@@ -101,20 +101,22 @@ void PitchLookupModel::write_file() {
  */
 float PitchLookupModel::get_offset(int distance, int height) {
     // Calculate the step size of the lookup table
-    int step_size = (this->upper_z - this->lower_z) /
+    int z_step_size = (this->upper_z - this->lower_z) /
                     (this->pitch_lookup_table.size() - 1);
+    int y_step_size = (this->upper_y - this->lower_y) /
+                    (this->pitch_lookup_table[0].size() - 1);
 
     // Calculate the indices of the tightest bound containing distance and height
-    int lower_step_z_idx = (distance - this->lower_z) / step_size;
-    int upper_step_z_idx = (distance - this->lower_z + step_size - 1) / step_size;
-    int lower_step_y_idx = (height - this->lower_y) / step_size;
-    int upper_step_y_idx = (height - this->lower_y + step_size - 1) / step_size;
+    int lower_step_z_idx = (distance - this->lower_z) / z_step_size;
+    int upper_step_z_idx = (distance - this->lower_z + z_step_size - 1) / z_step_size;
+    int lower_step_y_idx = (height - this->lower_y) / y_step_size;
+    int upper_step_y_idx = (height - this->lower_y + y_step_size - 1) / y_step_size;
 
     // Calculate the actual values of the tightest bounds
-    int lower_step_z = this->lower_z + step_size * lower_step_z_idx;
-    int upper_step_z = this->lower_z + step_size * upper_step_z_idx;
-    int lower_step_y = this->lower_y + step_size * lower_step_y_idx;
-    int upper_step_y = this->lower_y + step_size * upper_step_y_idx;
+    int lower_step_z = this->lower_z + z_step_size * lower_step_z_idx;
+    int upper_step_z = this->lower_z + z_step_size * upper_step_z_idx;
+    int lower_step_y = this->lower_y + y_step_size * lower_step_y_idx;
+    int upper_step_y = this->lower_y + y_step_size * upper_step_y_idx;
 
     if (lower_step_z_idx < 0 || lower_step_z_idx >= this->pitch_lookup_table.size() ||
         upper_step_z_idx < 0 || upper_step_z_idx >= this->pitch_lookup_table.size() ||

src/prm_control/control_communicator/test/test_ControlCommunicator.cpp

@@ -7,6 +7,7 @@ class ControlCommunicatorTest : public ::testing::Test
 protected:
     const char *port = nullptr;
     int port_fd = -1;
+    bool PORT_EXISTS = false;
 
     void SetUp() override
     {
@@ -17,6 +18,7 @@ class ControlCommunicatorTest : public ::testing::Test
             if (access(p, F_OK) != -1 && open(p, O_RDWR | O_NOCTTY | O_NONBLOCK) != -1)
             {
                 port = p;
+                PORT_EXISTS = true;
                 break;
             }
         }
@@ -30,9 +32,10 @@ class ControlCommunicatorTest : public ::testing::Test
 TEST_F(ControlCommunicatorTest, test_start_uart_connection)
 {
     ControlCommunicator control_communicator;
-    if (port != nullptr)
+    if (PORT_EXISTS)
     {
         EXPECT_TRUE(control_communicator.start_uart_connection(port));
+        EXPECT_NE(control_communicator.port_fd, -1);    // Port should be opened
 
         /**
          * Check UART configuration
@@ -62,10 +65,10 @@ TEST_F(ControlCommunicatorTest, test_start_uart_connection)
     }
 }
 
-TEST_F(ControlCommunicatorTest, test_read_uart)
+TEST_F(ControlCommunicatorTest, test_read_uart_values)
 {
     ControlCommunicator control_communicator;
-    if (port != nullptr)
+    if (PORT_EXISTS)
     {
         EXPECT_TRUE(control_communicator.start_uart_connection(port));
         EXPECT_NE(control_communicator.port_fd, -1);    // Port should be opened
@@ -107,6 +110,59 @@ TEST_F(ControlCommunicatorTest, test_read_uart)
     }
 }
 
+TEST_F(ControlCommunicatorTest, test_compute_aim)
+{
+    ControlCommunicator control_communicator;
+    float target_x = 40.0;     // mm 
+    float target_y = 180.0;       // mm
+    float target_z = 1500.0;    // mm
+    float bullet_speed = 16.0;  // m/s
+    float yaw, pitch;
+    bool impossible;
+
+    control_communicator.compute_aim(bullet_speed, target_x, target_y, target_z, yaw, pitch, impossible);
+    EXPECT_EQ(impossible, false);   // Shot should be possible
+    EXPECT_NEAR(yaw, 0.0, 2.0);     // Yaw = 0.0 since no X and Y offset
+    EXPECT_NEAR(pitch, 12.0, 1.50);  // Check pitch angle
+}
+
+TEST_F(ControlCommunicatorTest, test_aim)
+{
+    ControlCommunicator control_communicator;
+    if (PORT_EXISTS)
+    {
+        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
+
+        float target_x;
+        float target_y;
+        float target_z;
+        float bullet_speed;
+        float yaw, pitch;
+        bool impossible;
+
+        // Now generate random values and test sending to board
+        int NUM_ITERS = 100;
+        for (int i = 0; i < NUM_ITERS; ++i)
+        {
+            target_x = 10.0 + rand() % 2000; // mm
+            target_y = 10.0 + rand() % 2000; // mm
+            target_z = 10.0 + rand() % 2000; // mm
+            bullet_speed = rand() % 20 + 1; // m/s
+
+            int bytes_written = control_communicator.aim(bullet_speed, target_x, target_y, target_z, yaw, pitch, impossible);
+            EXPECT_EQ(bytes_written, sizeof(PackageOut)); // Ensure complete package is written
+        }
+    }
+    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);

src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h

@@ -11,11 +11,11 @@
 #define LIGHT_BAR_ASPECT_RATIO_LOWER_LIMIT 1.0
 #define LIGHT_BAR_WIDTH_LOWER_LIMIT 2.0
 #define LIGHT_BAR_HEIGHT_LOWER_LIMIT 5.0
-#define ARMOR_ANGLE_DIFF_LIMIT 10.0
+#define ARMOR_ANGLE_DIFF_LIMIT 5.0
 #define ARMOR_LIGHT_BAR_ASPECT_RATIO_RATIO_LIMIT 5.0
 #define ARMOR_Y_DIFF_LIMIT 0.5
 #define ARMOR_HEIGHT_RATIO_LIMIT 1.5
-#define ARMOR_ASPECT_RATIO_LIMIT 4.75
+#define ARMOR_ASPECT_RATIO_LIMIT 7
 
 #define WIDTH 1280
 #define HEIGHT 1024

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -285,13 +285,6 @@ bool OpenCVArmorDetector::isArmor(cv::RotatedRect &left_rect, cv::RotatedRect &r
         return false;
     }
 
-    // Distance Check
-    float distance = cv::norm(left_rect.center - right_rect.center);
-    if (distance < left_rect.size.height || distance > 5 * left_rect.size.height)
-    {
-        return false;
-    }
-
     // X Position distance Check
     if (std::abs(left_rect.center.x - right_rect.center.x) < left_rect.size.width)
     {