diff --git a/src/prm_control/control_communicator/include/ControlCommunicator.h b/src/prm_control/control_communicator/include/ControlCommunicator.h index c7f8b00..048d788 100644 --- a/src/prm_control/control_communicator/include/ControlCommunicator.h +++ b/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); diff --git a/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt b/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt index c669949..139873a 100644 --- a/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt +++ b/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 \ No newline at end of file +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 \ No newline at end of file diff --git a/src/prm_control/control_communicator/src/ControlCommunicator.cpp b/src/prm_control/control_communicator/src/ControlCommunicator.cpp index abf2479..b48248e 100644 --- a/src/prm_control/control_communicator/src/ControlCommunicator.cpp +++ b/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); diff --git a/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp b/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp index 78736fc..9ed7644 100755 --- a/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp +++ b/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 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 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 msg) diff --git a/src/prm_control/control_communicator/src/PitchLookupModel.cpp b/src/prm_control/control_communicator/src/PitchLookupModel.cpp index d1d9839..f68610b 100644 --- a/src/prm_control/control_communicator/src/PitchLookupModel.cpp +++ b/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() || diff --git a/src/prm_control/control_communicator/test/test_ControlCommunicator.cpp b/src/prm_control/control_communicator/test/test_ControlCommunicator.cpp index 51047b6..ae0946e 100644 --- a/src/prm_control/control_communicator/test/test_ControlCommunicator.cpp +++ b/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); diff --git a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h b/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h index 0edbc36..ef54172 100644 --- a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h +++ b/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 diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index 58caaa9..971d6eb 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/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) {