Pitch lookup table

src/prm_control/control_communicator/CMakeLists.txt

@@ -34,6 +34,7 @@ include_directories(include)
 
 add_executable(ControlCommunicatorNode 
 	src/ControlCommunicatorNode.cpp
+  src/PitchLookupModel.cpp
 )
 
 

src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp

@@ -1,4 +1,3 @@
-
 #ifndef _CONTROL_COMMUNICATOR_NODE_H
 #define _CONTROL_COMMUNICATOR_NODE_H
 
@@ -34,6 +33,7 @@
 #include "vision_msgs/msg/predicted_armor.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "messages.h"
+#include "PitchLookupModel.hpp"
 
 // using namespace std::chrono_literals;
 // using namespace std::placeholders;
@@ -72,6 +72,9 @@ class ControlCommunicatorNode : public rclcpp::Node
 	int8_t curr_pois = 0;
 	bool right = true;
 
+	std::string lookup_table_path;
+    PitchLookupModel pitch_lookup_model;
+
 	std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
 	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster;
 	std::unique_ptr<tf2_ros::Buffer> tf_buffer;
@@ -86,6 +89,7 @@ class ControlCommunicatorNode : public rclcpp::Node
 
 
 	rclcpp::TimerBase::SharedPtr uart_read_timer;
+	rclcpp::TimerBase::SharedPtr read_lookup_table_time;
 	rclcpp::TimerBase::SharedPtr heart_beat_timer;
 
 	bool start_uart(const char *port);

src/prm_control/control_communicator/include/PitchLookupModel.hpp

@@ -0,0 +1,30 @@
+#ifndef PITCH_LOOKUP_MODEL_HPP
+#define PITCH_LOOKUP_MODEL_HPP
+
+#include <climits>
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <string>
+
+class PitchLookupModel {
+public:
+    PitchLookupModel();
+    explicit PitchLookupModel(std::string filename);
+
+    void load_file();
+    void write_file();
+    float get_pitch(int distance, int height);
+
+private:
+    int lower_y = INT_MAX;
+    int lower_z = INT_MAX;
+    int upper_y = INT_MIN;
+    int upper_z = INT_MIN;
+    std::string filename;
+    std::vector<std::vector<float>> pitch_lookup_table;
+
+    float map(float value, float old_min, float old_max, float new_min, float new_max);
+};
+
+#endif // PITCH_LOOKUP_MODEL_HPP