fix bugs in PitchLookupModel

Author: purduerm

src/prm_control/control_communicator/include/PitchLookupModel.hpp

@@ -14,9 +14,9 @@ class PitchLookupModel {
     PitchLookupModel();
     explicit PitchLookupModel(std::string filename);
 
-    int load_file();
+    void load_file();
     void write_file();
-    float get_pitch(int distance, int height);
+    float get_offset(int distance, int height);
 
 private:
     int lower_y = INT_MAX;
@@ -28,6 +28,7 @@ class PitchLookupModel {
     std::thread timer;
 
     float map(float value, float old_min, float old_max, float new_min, float new_max);
+    void print_to_file(std::string text);
 };
 
 #endif // PITCH_LOOKUP_MODEL_HPP

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

@@ -1,10 +1,10 @@
 3 3
-500 0 0.00
+1000 -1000 0.00
 1000 0 0.00
-1500 0 0.00
-2000 200 0.00
-2500 0 0.00
+1000 1000 0.00
+2000 -1000 0.00
+2000 0 0.00
+2000 1000 10.00
+3000 -1000 0.00
 3000 0 0.00
-3500 0 0.00
-4000 200 0.00
-4500 0 0.00
+3000 1000 10.00

src/prm_control/control_communicator/src/ControlCommunicator.cpp

@@ -65,7 +65,7 @@ void ControlCommunicator::compute_aim(float bullet_speed, float target_x, float
 
     // lookup table for empirical pitch correction
     float dst = sqrt(target_x * target_x + target_y * target_y + target_z * target_z);
-    pitch += lut->get_pitch(dst, -target_y);
+    pitch += lut->get_offset(dst, target_y);
 
     // check nan and ensure between -180 and 180
     if (std::isnan(pitch) || std::isnan(yaw) ||

src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp

@@ -67,7 +67,7 @@ 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)
-{
+{	
 	if (!control_communicator->is_connected || control_communicator->port_fd < 0)
 	{
 		RCLCPP_WARN(this->get_logger(), "UART Not connected, ignoring aim message.");
@@ -87,16 +87,18 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs
 	int bytes_written = write(control_communicator->port_fd, &package, sizeof(PackageOut));
 	fsync(control_communicator->port_fd);
 
+	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);
+	}
+	
 	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)));
-	}
 	auto_aim_frame_id++;
 }
 
@@ -183,7 +185,7 @@ void ControlCommunicatorNode::read_uart()
 		old_target_robot_color = target_robot_color.data;	
 	}
 
-	if (this->auto_aim_frame_id % 500 == 0 && this->auto_aim_frame_id != 0)
+	if (this->auto_aim_frame_id % 5000 == 0 && this->auto_aim_frame_id != 0)
 	{
 		RCLCPP_INFO(this->get_logger(), "READ UART: x: %f | y: %f | x_vel: %f | y_vel: %f | yaw_vel: %f | pitch_vel: %f | pitch: %f | is_enemy_red: %d | is_match_running: %d", package.x, package.y, package.x_vel, package.y_vel, this->yaw_vel, this->pitch_vel, this->pitch, this->is_enemy_red, this->is_match_running);
 	}

src/prm_control/control_communicator/src/PitchLookupModel.cpp

@@ -4,8 +4,7 @@ PitchLookupModel::PitchLookupModel() {}
 
 PitchLookupModel::PitchLookupModel(std::string filename) {
     this->filename = filename;
-    int succ = this->load_file();
-    assert(succ == 1);
+    this->load_file();
 
     // timer to read file every 2 seconds for updates
     this->timer = std::thread([this]() {
@@ -26,10 +25,10 @@ PitchLookupModel::PitchLookupModel(std::string filename) {
  * z coordinate, y coordinate, and pitch. Length measurements are in
  * millimeters, and angle measurements are in degrees.
  */
-int PitchLookupModel::load_file() {
+void PitchLookupModel::load_file() {
     FILE* fp = fopen(filename.c_str(), "r");
     if (fp == nullptr) {
-        return -1;
+        return;
     }
 
     int y_count = 0;
@@ -48,7 +47,7 @@ int PitchLookupModel::load_file() {
             if (read_count != 3) {
                 fclose(fp);
                 fp = NULL;
-                return -2;
+                return;
             }
 
             if (z < this->lower_z) this->lower_z = z;
@@ -62,7 +61,6 @@ int PitchLookupModel::load_file() {
 
     fclose(fp);
     fp = NULL;
-    return 1;
 }
 
 /**
@@ -92,20 +90,18 @@ void PitchLookupModel::write_file() {
     fp = NULL;
 }
 
+
 /**
- * @brief Lookup the corresponding pitch with distance and height
+ * @brief Lookup the corresponding pitch with distance and height, using bilinear interpolation
  *
  * @param distance Distance away from the camera in millimeters
  * @param height   Height above the ground in millimeters
  *
  * @return Pitch in degrees
  */
-float PitchLookupModel::get_pitch(int distance, int height) {
-    assert(this->pitch_lookup_table.size() > 1);
-    assert(this->pitch_lookup_table[0].size() > 1);
-
+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 step_size = (this->upper_z - this->lower_z) /
                     (this->pitch_lookup_table.size() - 1);
 
     // Calculate the indices of the tightest bound containing distance and height
@@ -120,45 +116,31 @@ float PitchLookupModel::get_pitch(int distance, int height) {
     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;
 
-    // Calculate the pitch of the four points with tightest bounds
     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() ||
         lower_step_y_idx < 0 || lower_step_y_idx >= this->pitch_lookup_table[0].size() ||
         upper_step_y_idx < 0 || upper_step_y_idx >= this->pitch_lookup_table[0].size()) {
         return 0; // No computed pitch offset
     }
 
+    // Calculate the pitch of the four points with tightest bounds
     float pitch_low_low = this->pitch_lookup_table[lower_step_z_idx][lower_step_y_idx];
     float pitch_low_high = this->pitch_lookup_table[lower_step_z_idx][upper_step_y_idx];
     float pitch_high_low = this->pitch_lookup_table[upper_step_z_idx][lower_step_y_idx];
     float pitch_high_high = this->pitch_lookup_table[upper_step_z_idx][upper_step_y_idx];
 
-    // Calculate pitch of the corresponding z position by fixing y to the lower bound
-    float pitch_y_low = this->map(
-        static_cast<float>(distance), 
-        static_cast<float>(lower_step_z), 
-        static_cast<float>(upper_step_z), 
-        std::fmin(pitch_low_low, pitch_high_low), 
-        std::fmax(pitch_low_low, pitch_high_low));
-
-    // Calculate pitch of the corresponding z position by fixing y to the upper bound
-    float pitch_y_high = this->map(
-        static_cast<float>(distance), 
-        static_cast<float>(lower_step_z), 
-        static_cast<float>(upper_step_z), 
-        std::fmin(pitch_low_high, pitch_high_high), 
-        std::fmax(pitch_low_high, pitch_high_high));
-
-    assert(upper_z != lower_z);
-    assert(step_size != 0);
-
-    // Combine the calculated new bounds to calculate the new pitch
-    return this->map(
-        static_cast<float>(height), 
-        static_cast<float>(lower_step_y), 
-        static_cast<float>(upper_step_y), 
-        std::fmin(pitch_y_low, pitch_y_high), 
-        std::fmax(pitch_y_low, pitch_y_high));
+    // Calculate the pitch using bilinear interpolation
+    float pitch_low = (pitch_low_low * (upper_step_y - height) +
+                       pitch_low_high * (height - lower_step_y)) /
+                      (upper_step_y - lower_step_y);
+    float pitch_high = (pitch_high_low * (upper_step_y - height) +
+                        pitch_high_high * (height - lower_step_y)) /
+                       (upper_step_y - lower_step_y);
+    float pitch = (pitch_low * (upper_step_z - distance) +
+                   pitch_high * (distance - lower_step_z)) /
+                  (upper_step_z - lower_step_z);
+
+    return pitch;
 }
 
 /**
@@ -172,8 +154,8 @@ float PitchLookupModel::get_pitch(int distance, int height) {
  *
  * @return Mapped value
  */
-float PitchLookupModel::map(float value, float old_min, float old_may, 
-                            float new_min, float new_may) {
+float PitchLookupModel::map(float value, float old_min, float old_may, float new_min, float new_may) 
+{
     if (old_min > old_may || new_min > new_may) {
         return -1;
     }
@@ -183,4 +165,15 @@ float PitchLookupModel::map(float value, float old_min, float old_may,
     }
 
     return new_min + (value - old_min) * (new_may - new_min) / (old_may - old_min);
-}
+}
+
+void PitchLookupModel::print_to_file(std::string text) {
+    FILE* fp = fopen("/home/purduerm/Documents/output.txt", "a");
+    if (fp == nullptr) {
+        return;
+    }
+
+    fprintf(fp, "%s\n", text.c_str());
+    fclose(fp);
+}
+