diff --git a/src/prm_control/control_communicator/CMakeLists.txt b/src/prm_control/control_communicator/CMakeLists.txt index d72d48e..f339894 100755 --- a/src/prm_control/control_communicator/CMakeLists.txt +++ b/src/prm_control/control_communicator/CMakeLists.txt @@ -34,6 +34,7 @@ include_directories(include) add_executable(ControlCommunicatorNode src/ControlCommunicatorNode.cpp + src/PitchLookupModel.cpp ) diff --git a/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp b/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp index bebcd72..78ff48c 100755 --- a/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp +++ b/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 tf_broadcaster; std::shared_ptr tf_static_broadcaster; std::unique_ptr 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); diff --git a/src/prm_control/control_communicator/include/PitchLookupModel.hpp b/src/prm_control/control_communicator/include/PitchLookupModel.hpp new file mode 100644 index 0000000..88b8f76 --- /dev/null +++ b/src/prm_control/control_communicator/include/PitchLookupModel.hpp @@ -0,0 +1,30 @@ +#ifndef PITCH_LOOKUP_MODEL_HPP +#define PITCH_LOOKUP_MODEL_HPP + +#include +#include +#include +#include +#include + +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> pitch_lookup_table; + + float map(float value, float old_min, float old_max, float new_min, float new_max); +}; + +#endif // PITCH_LOOKUP_MODEL_HPP \ No newline at end of file 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 new file mode 100644 index 0000000..5485fe9 --- /dev/null +++ b/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt @@ -0,0 +1,2092 @@ +51 41 +1000 -1000 -44.55384 +1000 -900 -41.54070 +1000 -800 -38.21295 +1000 -700 -34.54482 +1000 -600 -30.51621 +1000 -500 -26.11715 +1000 -400 -21.35316 +1000 -300 -16.25064 +1000 -200 -10.86097 +1000 -100 -5.26127 +1000 0 0.44968 +1000 100 6.16063 +1000 200 11.76033 +1000 300 17.15000 +1000 400 22.25253 +1000 500 27.01653 +1000 600 31.41560 +1000 700 35.44422 +1000 800 39.11238 +1000 900 42.44015 +1000 1000 45.45330 +1000 1100 48.17998 +1000 1200 50.64847 +1000 1300 52.88581 +1000 1400 54.91710 +1000 1500 56.76508 +1000 1600 58.45014 +1000 1700 59.99035 +1000 1800 61.40166 +1000 1900 62.69810 +1000 2000 63.89197 +1000 2100 64.99405 +1000 2200 66.01382 +1000 2300 66.95959 +1000 2400 67.83867 +1000 2500 68.65751 +1000 2600 69.42179 +1000 2700 70.13656 +1000 2800 70.80625 +1000 2900 71.43486 +1000 3000 72.02591 +1100 -1000 -41.78290 +1100 -900 -38.79824 +1100 -800 -35.53582 +1100 -700 -31.97926 +1100 -600 -28.11814 +1100 -500 -23.95125 +1100 -400 -19.49002 +1100 -300 -14.76164 +1100 -200 -9.81098 +1100 -100 -4.70017 +1100 0 0.49465 +1100 100 5.68947 +1100 200 10.80028 +1100 300 15.75095 +1100 400 20.47933 +1100 500 24.94058 +1100 600 29.10748 +1100 700 32.96861 +1100 800 36.52520 +1100 900 39.78763 +1100 1000 42.77232 +1100 1100 45.49903 +1100 1200 47.98899 +1100 1300 50.26348 +1100 1400 52.34302 +1100 1500 54.24682 +1100 1600 55.99255 +1100 1700 57.59624 +1100 1800 59.07232 +1100 1900 60.43372 +1100 2000 61.69192 +1100 2100 62.85716 +1100 2200 63.93850 +1100 2300 64.94400 +1100 2400 65.88083 +1100 2500 66.75532 +1100 2600 67.57313 +1100 2700 68.33931 +1100 2800 69.05835 +1100 2900 69.73426 +1100 3000 70.37063 +1200 -1000 -39.27015 +1200 -900 -36.33407 +1200 -800 -33.15382 +1200 -700 -29.71977 +1200 -600 -26.02797 +1200 -500 -22.08236 +1200 -400 -17.89703 +1200 -300 -13.49790 +1200 -200 -8.92355 +1200 -100 -4.22445 +1200 0 0.53962 +1200 100 5.30369 +1200 200 10.00280 +1200 300 14.57715 +1200 400 18.97629 +1200 500 23.16164 +1200 600 27.10726 +1200 700 30.79908 +1200 800 34.23315 +1200 900 37.41342 +1200 1000 40.34953 +1200 1100 43.05485 +1200 1200 45.54484 +1200 1300 47.83589 +1200 1400 49.94443 +1200 1500 51.88636 +1200 1600 53.67672 +1200 1700 55.32947 +1200 1800 56.85745 +1200 1900 58.27232 +1200 2000 59.58466 +1200 2100 60.80399 +1200 2200 61.93887 +1200 2300 62.99697 +1200 2400 63.98519 +1200 2500 64.90970 +1200 2600 65.77602 +1200 2700 66.58914 +1200 2800 67.35350 +1200 2900 68.07312 +1200 3000 68.75161 +1300 -1000 -36.98855 +1300 -900 -34.11466 +1300 -800 -31.02656 +1300 -700 -27.71936 +1300 -600 -24.19330 +1300 -500 -20.45521 +1300 -400 -16.51997 +1300 -300 -12.41140 +1300 -200 -8.16249 +1300 -100 -3.81457 +1300 0 0.58460 +1300 100 4.98376 +1300 200 9.33169 +1300 300 13.58061 +1300 400 17.68919 +1300 500 21.62444 +1300 600 25.36254 +1300 700 28.88862 +1300 800 32.19584 +1300 900 35.28397 +1300 1000 38.15788 +1300 1100 40.82613 +1300 1200 43.29964 +1300 1300 45.59072 +1300 1400 47.71230 +1300 1500 49.67731 +1300 1600 51.49831 +1300 1700 53.18730 +1300 1800 54.75549 +1300 1900 56.21329 +1300 2000 57.57026 +1300 2100 58.83513 +1300 2200 60.01588 +1300 2300 61.11971 +1300 2400 62.15317 +1300 2500 63.12216 +1300 2600 64.03204 +1300 2700 64.88764 +1300 2800 65.69333 +1300 2900 66.45306 +1300 3000 67.17042 +1400 -1000 -34.91300 +1400 -900 -32.11007 +1400 -800 -29.11924 +1400 -700 -25.93892 +1400 -600 -22.57198 +1400 -500 -19.02672 +1400 -400 -15.31780 +1400 -300 -11.46667 +1400 -200 -7.50152 +1400 -100 -3.45654 +1400 0 0.62957 +1400 100 4.71569 +1400 200 8.76067 +1400 300 12.72583 +1400 400 16.57697 +1400 500 20.28590 +1400 600 23.83118 +1400 700 27.19814 +1400 800 30.37848 +1400 900 33.36934 +1400 1000 36.17230 +1400 1100 38.79236 +1400 1200 41.23695 +1400 1300 43.51507 +1400 1400 45.63668 +1400 1500 47.61214 +1400 1600 49.45180 +1400 1700 51.16579 +1400 1800 52.76379 +1400 1900 54.25495 +1400 2000 55.64781 +1400 2100 56.95029 +1400 2200 58.16969 +1400 2300 59.31273 +1400 2400 60.38552 +1400 2500 61.39366 +1400 2600 62.34227 +1400 2700 63.23599 +1400 2800 64.07906 +1400 2900 64.87532 +1400 3000 65.62830 +1500 -1000 -33.02076 +1500 -900 -30.29393 +1500 -800 -27.40215 +1500 -700 -24.34603 +1500 -600 -21.13003 +1500 -500 -17.76304 +1500 -400 -14.25899 +1500 -300 -10.63697 +1500 -200 -6.92116 +1500 -100 -3.14006 +1500 0 0.67455 +1500 100 4.48916 +1500 200 8.27026 +1500 300 11.98609 +1500 400 15.60811 +1500 500 19.11218 +1500 600 22.47919 +1500 700 25.69521 +1500 800 28.75135 +1500 900 31.64317 +1500 1000 34.37003 +1500 1100 36.93435 +1500 1200 39.34087 +1500 1300 41.59599 +1500 1400 43.70723 +1500 1500 45.68272 +1500 1600 47.53089 +1500 1700 49.26018 +1500 1800 50.87883 +1500 1900 52.39480 +1500 2000 53.81563 +1500 2100 55.14842 +1500 2200 56.39979 +1500 2300 57.57589 +1500 2400 58.68243 +1500 2500 59.72463 +1500 2600 60.70732 +1500 2700 61.63494 +1500 2800 62.51153 +1500 2900 63.34083 +1500 3000 64.12623 +1600 -1000 -31.29144 +1600 -900 -28.64326 +1600 -800 -25.85001 +1600 -700 -22.91378 +1600 -600 -19.83989 +1600 -500 -16.63731 +1600 -400 -13.31897 +1600 -300 -9.90182 +1600 -200 -6.40662 +1600 -100 -2.85737 +1600 0 0.71953 +1600 100 4.29643 +1600 200 7.84568 +1600 300 11.34090 +1600 400 14.75806 +1600 500 18.07641 +1600 600 21.27901 +1600 700 24.35292 +1600 800 27.28918 +1600 900 30.08246 +1600 1000 32.73068 +1600 1100 35.23440 +1600 1200 37.59637 +1600 1300 39.82092 +1600 1400 41.91357 +1600 1500 43.88063 +1600 1600 45.72884 +1600 1700 47.46514 +1600 1800 49.09649 +1600 1900 50.62973 +1600 2000 52.07143 +1600 2100 53.42789 +1600 2200 54.70508 +1600 2300 55.90857 +1600 2400 57.04360 +1600 2500 58.11504 +1600 2600 59.12739 +1600 2700 60.08485 +1600 2800 60.99125 +1600 2900 61.85017 +1600 3000 62.66489 +1700 -1000 -29.70697 +1700 -900 -27.13811 +1700 -800 -24.44138 +1700 -700 -21.61980 +1700 -600 -18.67911 +1700 -500 -15.62802 +1700 -400 -12.47841 +1700 -300 -9.24527 +1700 -200 -5.94653 +1700 -100 -2.60255 +1700 0 0.76451 +1700 100 4.13157 +1700 200 7.47556 +1700 300 10.77431 +1700 400 14.00746 +1700 500 17.15709 +1700 600 20.20820 +1700 700 23.14891 +1700 800 25.97052 +1700 900 28.66728 +1700 1000 31.23618 +1700 1100 33.67650 +1700 1200 35.98947 +1700 1300 38.17786 +1700 1400 40.24559 +1700 1500 42.19743 +1700 1600 44.03869 +1700 1700 45.77503 +1700 1800 47.41224 +1700 1900 48.95614 +1700 2000 50.41241 +1700 2100 51.78659 +1700 2200 53.08399 +1700 2300 54.30965 +1700 2400 55.46832 +1700 2500 56.56448 +1700 2600 57.60232 +1700 2700 58.58576 +1700 2800 59.51841 +1700 2900 60.40368 +1700 3000 61.24469 +1800 -1000 -28.25140 +1800 -900 -25.76122 +1800 -800 -23.15804 +1800 -700 -20.44543 +1800 -600 -17.62925 +1800 -500 -14.71779 +1800 -400 -11.72185 +1800 -300 -8.65474 +1800 -200 -5.53197 +1800 -100 -2.37098 +1800 0 0.80949 +1800 100 3.98996 +1800 200 7.15096 +1800 300 10.27374 +1800 400 13.34087 +1800 500 16.33682 +1800 600 19.24831 +1800 700 22.06451 +1800 800 24.77715 +1800 900 27.38037 +1800 1000 29.87058 +1800 1100 32.24620 +1800 1200 34.50736 +1800 1300 36.65561 +1800 1400 38.69361 +1800 1500 40.62486 +1800 1600 42.45350 +1800 1700 44.18406 +1800 1800 45.82131 +1800 1900 47.37014 +1800 2000 48.83545 +1800 2100 50.22204 +1800 2200 51.53462 +1800 2300 52.77766 +1800 2400 53.95550 +1800 2500 55.07220 +1800 2600 56.13162 +1800 2700 57.13740 +1800 2800 58.09294 +1800 2900 59.00142 +1800 3000 59.86581 +1900 -1000 -26.91069 +1900 -900 -24.49768 +1900 -800 -21.98450 +1900 -700 -19.37505 +1900 -600 -16.67510 +1900 -500 -13.89243 +1900 -400 -11.03686 +1900 -300 -8.12016 +1900 -200 -5.15587 +1900 -100 -2.15898 +1900 0 0.85448 +1900 100 3.86794 +1900 200 6.86483 +1900 300 9.82913 +1900 400 12.74585 +1900 500 15.60143 +1900 600 18.38412 +1900 700 21.08410 +1900 800 23.69359 +1900 900 26.20680 +1900 1000 28.61986 +1900 1100 30.93060 +1900 1200 33.13836 +1900 1300 35.24375 +1900 1400 37.24846 +1900 1500 39.15498 +1900 1600 40.96643 +1900 1700 42.68640 +1900 1800 44.31879 +1900 1900 45.86766 +1900 2000 47.33717 +1900 2100 48.73150 +1900 2200 50.05473 +1900 2300 51.31087 +1900 2400 52.50378 +1900 2500 53.63715 +1900 2600 54.71453 +1900 2700 55.73926 +1900 2800 56.71449 +1900 2900 57.64321 +1900 3000 58.52822 +2000 -1000 -25.67256 +2000 -900 -23.33457 +2000 -800 -20.90755 +2000 -700 -18.39549 +2000 -600 -15.80399 +2000 -500 -13.14030 +2000 -400 -10.41329 +2000 -300 -7.63342 +2000 -200 -4.81254 +2000 -100 -1.96365 +2000 0 0.89946 +2000 100 3.76258 +2000 200 6.61148 +2000 300 9.43237 +2000 400 12.21225 +2000 500 14.93928 +2000 600 17.60300 +2000 700 20.19452 +2000 800 22.70661 +2000 900 25.13368 +2000 1000 27.47171 +2000 1100 29.71819 +2000 1200 31.87189 +2000 1300 33.93273 +2000 1400 35.90162 +2000 1500 37.78024 +2000 1600 39.57090 +2000 1700 41.27637 +2000 1800 42.89980 +2000 1900 44.44453 +2000 2000 45.91409 +2000 2100 47.31202 +2000 2200 48.64192 +2000 2300 49.90728 +2000 2400 51.11156 +2000 2500 52.25809 +2000 2600 53.35007 +2000 2700 54.39058 +2000 2800 55.38253 +2000 2900 56.32869 +2000 3000 57.23169 +2100 -1000 -24.52622 +2100 -900 -22.26074 +2100 -800 -19.91588 +2100 -700 -17.49565 +2100 -600 -15.00537 +2100 -500 -12.45174 +2100 -400 -9.84280 +2100 -300 -7.18787 +2100 -200 -4.49736 +2100 -100 -1.78260 +2100 0 0.94445 +2100 100 3.67151 +2100 200 6.38628 +2100 300 9.07680 +2100 400 11.73175 +2100 500 14.34070 +2100 600 16.89436 +2100 700 19.38467 +2100 800 21.80494 +2100 900 24.14983 +2100 1000 26.41536 +2100 1100 28.59875 +2100 1200 30.69843 +2100 1300 32.71380 +2100 1400 34.64517 +2100 1500 36.49356 +2100 1600 38.26061 +2100 1700 39.94844 +2100 1800 41.55952 +2100 1900 43.09661 +2100 2000 44.56263 +2100 2100 45.96060 +2100 2200 47.29362 +2100 2300 48.56476 +2100 2400 49.77708 +2100 2500 50.93354 +2100 2600 52.03707 +2100 2700 53.09044 +2100 2800 54.09633 +2100 2900 55.05732 +2100 3000 55.97584 +2200 -1000 -23.46218 +2200 -900 -21.26649 +2200 -800 -18.99982 +2200 -700 -16.66608 +2200 -600 -14.27031 +2200 -500 -11.81869 +2200 -400 -9.31850 +2200 -300 -6.77805 +2200 -200 -4.20654 +2200 -100 -1.61390 +2200 0 0.98944 +2200 100 3.59279 +2200 200 6.18544 +2200 300 8.75696 +2200 400 11.29743 +2200 500 13.79764 +2200 600 16.24928 +2200 700 18.64508 +2200 800 20.97886 +2200 900 23.24558 +2200 1000 25.44131 +2200 1100 27.56322 +2200 1200 29.60943 +2200 1300 31.57901 +2200 1400 33.47179 +2200 1500 35.28829 +2200 1600 37.02960 +2200 1700 38.69729 +2200 1800 40.29328 +2200 1900 41.81978 +2200 2000 43.27922 +2200 2100 44.67414 +2200 2200 46.00719 +2200 2300 47.28106 +2200 2400 48.49842 +2200 2500 49.66194 +2200 2600 50.77420 +2200 2700 51.83775 +2200 2800 52.85503 +2200 2900 53.82841 +2200 3000 54.76013 +2300 -1000 -22.47215 +2300 -900 -20.34341 +2300 -800 -18.15101 +2300 -700 -15.89872 +2300 -600 -13.59128 +2300 -500 -11.23438 +2300 -400 -8.83461 +2300 -300 -6.39940 +2300 -200 -3.93693 +2300 -100 -1.45593 +2300 0 1.03444 +2300 100 3.52481 +2300 200 6.00581 +2300 300 8.46830 +2300 400 10.90352 +2300 500 13.30332 +2300 600 15.66025 +2300 700 17.96772 +2300 800 20.22004 +2300 900 22.41249 +2300 1000 24.54128 +2300 1100 26.60352 +2300 1200 28.59721 +2300 1300 30.52113 +2300 1400 32.37479 +2300 1500 34.15829 +2300 1600 35.87229 +2300 1700 37.51790 +2300 1800 39.09656 +2300 1900 40.61005 +2300 2000 42.06034 +2300 2100 43.44956 +2300 2200 44.77997 +2300 2300 46.05387 +2300 2400 47.27362 +2300 2500 48.44157 +2300 2600 49.56004 +2300 2700 50.63132 +2300 2800 51.65763 +2300 2900 52.64113 +2300 3000 53.58390 +2400 -1000 -21.54880 +2400 -900 -19.48415 +2400 -800 -17.36223 +2400 -700 -15.18665 +2400 -600 -12.96186 +2400 -500 -10.69307 +2400 -400 -8.38627 +2400 -300 -6.04812 +2400 -200 -3.68590 +2400 -100 -1.30736 +2400 0 1.07943 +2400 100 3.46623 +2400 200 5.84478 +2400 300 8.20701 +2400 400 10.54518 +2400 500 12.85201 +2400 600 15.12082 +2400 700 17.34565 +2400 800 19.52127 +2400 900 21.64324 +2400 1000 23.70793 +2400 1100 25.71251 +2400 1200 27.65488 +2400 1300 29.53364 +2400 1400 31.34803 +2400 1500 33.09787 +2400 1600 34.78345 +2400 1700 36.40549 +2400 1800 37.96507 +2400 1900 39.46356 +2400 2000 40.90255 +2400 2100 42.28381 +2400 2200 43.60924 +2400 2300 44.88083 +2400 2400 46.10063 +2400 2500 47.27068 +2400 2600 48.39307 +2400 2700 49.46985 +2400 2800 50.50302 +2400 2900 51.49456 +2400 3000 52.44638 +2500 -1000 -20.68569 +2500 -900 -18.68230 +2500 -800 -16.62723 +2500 -700 -14.52394 +2500 -600 -12.37656 +2500 -500 -10.18989 +2500 -400 -7.96936 +2500 -300 -5.72098 +2500 -200 -3.45125 +2500 -100 -1.16706 +2500 0 1.12443 +2500 100 3.41593 +2500 200 5.70013 +2500 300 7.96987 +2500 400 10.21827 +2500 500 12.43883 +2500 600 14.62553 +2500 700 16.77294 +2500 800 18.87628 +2500 900 20.93139 +2500 1000 22.93483 +2500 1100 24.88384 +2500 1200 26.77626 +2500 1300 28.61061 +2500 1400 30.38593 +2500 1500 32.10179 +2500 1600 33.75820 +2500 1700 35.35559 +2500 1800 36.89471 +2500 1900 38.37660 +2500 2000 39.80252 +2500 2100 41.17391 +2500 2200 42.49238 +2500 2300 43.75961 +2500 2400 44.97737 +2500 2500 46.14746 +2500 2600 47.27173 +2500 2700 48.35199 +2500 2800 49.39006 +2500 2900 50.38772 +2500 3000 51.34673 +2600 -1000 -19.87714 +2600 -900 -17.93223 +2600 -800 -15.94057 +2600 -700 -13.90543 +2600 -600 -11.83065 +2600 -500 -9.72066 +2600 -400 -7.58039 +2600 -300 -5.41526 +2600 -200 -3.23111 +2600 -100 -1.03408 +2600 0 1.16943 +2600 100 3.37295 +2600 200 5.56999 +2600 300 7.75415 +2600 400 9.91930 +2600 500 12.05960 +2600 600 14.16963 +2600 700 16.24444 +2600 800 18.27962 +2600 900 20.27133 +2600 1000 22.21630 +2600 1100 24.11184 +2600 1200 25.95583 +2600 1300 27.74670 +2600 1400 29.48337 +2600 1500 31.16521 +2600 1600 32.79205 +2600 1700 34.36403 +2600 1800 35.88164 +2600 1900 37.34565 +2600 2000 38.75703 +2600 2100 40.11697 +2600 2200 41.42677 +2600 2300 42.68787 +2600 2400 43.90178 +2600 2500 45.07009 +2600 2600 46.19439 +2600 2700 47.27632 +2600 2800 48.31749 +2600 2900 49.31954 +2600 3000 50.28404 +2700 -1000 -19.11811 +2700 -900 -17.22899 +2700 -800 -15.29747 +2700 -700 -13.32663 +2700 -600 -11.32005 +2700 -500 -9.28178 +2700 -400 -7.21633 +2700 -300 -5.12861 +2700 -200 -3.02386 +2700 -100 -0.90761 +2700 0 1.21444 +2700 100 3.33649 +2700 200 5.45275 +2700 300 7.55751 +2700 400 9.64526 +2700 500 11.71073 +2700 600 13.74904 +2700 700 15.75566 +2700 800 17.72654 +2700 900 19.65811 +2700 1000 21.54729 +2700 1100 23.39148 +2700 1200 25.18862 +2700 1300 26.93708 +2700 1400 28.63570 +2700 1500 30.28373 +2700 1600 31.88080 +2700 1700 33.42687 +2700 1800 34.92221 +2700 1900 36.36735 +2700 2000 37.76303 +2700 2100 39.11018 +2700 2200 40.40988 +2700 2300 41.66333 +2700 2400 42.87182 +2700 2500 44.03672 +2700 2600 45.15943 +2700 2700 46.24139 +2700 2800 47.28406 +2700 2900 48.28889 +2700 3000 49.25732 +2800 -1000 -18.40414 +2800 -900 -16.56824 +2800 -800 -14.69378 +2800 -700 -12.78366 +2800 -600 -10.84120 +2800 -500 -8.87014 +2800 -400 -6.87460 +2800 -300 -4.85902 +2800 -200 -2.82815 +2800 -100 -0.78695 +2800 0 1.25945 +2800 100 3.30585 +2800 200 5.34705 +2800 300 7.37794 +2800 400 9.39354 +2800 500 11.38911 +2800 600 13.36021 +2800 700 15.30271 +2800 800 17.21287 +2800 900 19.08738 +2800 1000 20.92334 +2800 1100 22.71828 +2800 1200 24.47016 +2800 1300 26.17737 +2800 1400 27.83869 +2800 1500 29.45326 +2800 1600 31.02060 +2800 1700 32.54049 +2800 1800 34.01303 +2800 1900 35.43856 +2800 2000 36.81759 +2800 2100 38.15087 +2800 2200 39.43926 +2800 2300 40.68376 +2800 2400 41.88547 +2800 2500 43.04554 +2800 2600 44.16522 +2800 2700 45.24577 +2800 2800 46.28848 +2800 2900 47.29466 +2800 3000 48.26560 +2900 -1000 -17.73126 +2900 -900 -15.94611 +2900 -800 -14.12581 +2900 -700 -12.27308 +2900 -600 -10.39101 +2900 -500 -8.48304 +2900 -400 -6.55294 +2900 -300 -4.60475 +2900 -200 -2.64278 +2900 -100 -0.67150 +2900 0 1.30446 +2900 100 3.28042 +2900 200 5.25170 +2900 300 7.21369 +2900 400 9.16190 +2900 500 11.09204 +2900 600 13.00005 +2900 700 14.88216 +2900 800 16.73494 +2900 900 18.55529 +2900 1000 20.34049 +2900 1100 22.08820 +2900 1200 23.79645 +2900 1300 25.46362 +2900 1400 27.08848 +2900 1500 28.67010 +2900 1600 30.20789 +2900 1700 31.70152 +2900 1800 33.15092 +2900 1900 34.55627 +2900 2000 35.91795 +2900 2100 37.23648 +2900 2200 38.51256 +2900 2300 39.74701 +2900 2400 40.94075 +2900 2500 42.09478 +2900 2600 43.21016 +2900 2700 44.28800 +2900 2800 45.32944 +2900 2900 46.33566 +2900 3000 47.30781 +3000 -1000 -17.09593 +3000 -900 -15.35920 +3000 -800 -13.59034 +3000 -700 -11.79190 +3000 -600 -9.96677 +3000 -500 -8.11812 +3000 -400 -6.24939 +3000 -300 -4.36429 +3000 -200 -2.46672 +3000 -100 -0.56074 +3000 0 1.34947 +3000 100 3.25969 +3000 200 5.16568 +3000 300 7.06327 +3000 400 8.94839 +3000 500 10.81715 +3000 600 12.66584 +3000 700 14.49101 +3000 800 16.28949 +3000 900 18.05841 +3000 1000 19.79521 +3000 1100 21.49766 +3000 1200 23.16387 +3000 1300 24.79226 +3000 1400 26.38157 +3000 1500 27.93084 +3000 1600 29.43939 +3000 1700 30.90681 +3000 1800 32.33290 +3000 1900 33.71772 +3000 2000 35.06147 +3000 2100 36.36456 +3000 2200 37.62751 +3000 2300 38.85100 +3000 2400 40.03577 +3000 2500 41.18268 +3000 2600 42.29265 +3000 2700 43.36664 +3000 2800 44.40565 +3000 2900 45.41073 +3000 3000 46.38292 +3100 -1000 -16.49501 +3100 -900 -14.80446 +3100 -800 -13.08448 +3100 -700 -11.33747 +3100 -600 -9.56610 +3100 -500 -7.77330 +3100 -400 -5.96225 +3100 -300 -4.13633 +3100 -200 -2.29908 +3100 -100 -0.45422 +3100 0 1.39449 +3100 100 3.24320 +3100 200 5.08808 +3100 300 6.92534 +3100 400 8.75129 +3100 500 10.56237 +3100 600 12.35520 +3100 700 14.12661 +3100 800 15.87368 +3100 900 17.59371 +3100 1000 19.28434 +3100 1100 20.94343 +3100 1200 22.56917 +3100 1300 24.16003 +3100 1400 25.71475 +3100 1500 27.23234 +3100 1600 28.71208 +3100 1700 30.15346 +3100 1800 31.55621 +3100 1900 32.92025 +3100 2000 34.24570 +3100 2100 35.53280 +3100 2200 36.78196 +3100 2300 37.99371 +3100 2400 39.16867 +3100 2500 40.30755 +3100 2600 41.41113 +3100 2700 42.48026 +3100 2800 43.51580 +3100 2900 44.51869 +3100 3000 45.48986 +3200 -1000 -15.92567 +3200 -900 -14.27918 +3200 -800 -12.60568 +3200 -700 -10.90742 +3200 -600 -9.18688 +3200 -500 -7.44677 +3200 -400 -5.69001 +3200 -300 -3.91970 +3200 -200 -2.13908 +3200 -100 -0.35153 +3200 0 1.43951 +3200 100 3.23056 +3200 200 5.01812 +3200 300 6.79875 +3200 400 8.56909 +3200 500 10.32588 +3200 600 12.06603 +3200 700 13.78662 +3200 800 15.48493 +3200 900 17.15849 +3200 1000 18.80504 +3200 1100 20.42260 +3200 1200 22.00941 +3200 1300 23.56399 +3200 1400 25.08511 +3200 1500 26.57175 +3200 1600 28.02316 +3200 1700 29.43877 +3200 1800 30.81825 +3200 1900 32.16142 +3200 2000 33.46829 +3200 2100 34.73900 +3200 2200 35.97385 +3200 2300 37.17324 +3200 2400 38.33767 +3200 2500 39.46773 +3200 2600 40.56408 +3200 2700 41.62746 +3200 2800 42.65863 +3200 2900 43.65839 +3200 3000 44.62758 +3300 -1000 -15.38537 +3300 -900 -13.78095 +3300 -800 -12.15169 +3300 -700 -10.49969 +3300 -600 -8.82726 +3300 -500 -7.13691 +3300 -400 -5.43134 +3300 -300 -3.71338 +3300 -200 -1.98602 +3300 -100 -0.25234 +3300 0 1.48453 +3300 100 3.22141 +3300 200 4.95511 +3300 300 6.68249 +3300 400 8.40046 +3300 500 10.10607 +3300 600 11.79646 +3300 700 13.46894 +3300 800 15.12100 +3300 900 16.75032 +3300 1000 18.35480 +3300 1100 19.93256 +3300 1200 21.48193 +3300 1300 23.00147 +3300 1400 24.48998 +3300 1500 25.94644 +3300 1600 27.37007 +3300 1700 28.76027 +3300 1800 30.11664 +3300 1900 31.43893 +3300 2000 32.72707 +3300 2100 33.98110 +3300 2200 35.20123 +3300 2300 36.38775 +3300 2400 37.54106 +3300 2500 38.66163 +3300 2600 39.75004 +3300 2700 40.80689 +3300 2800 41.83286 +3300 2900 42.82866 +3300 3000 43.79502 +3400 -1000 -14.87183 +3400 -900 -13.30760 +3400 -800 -11.72047 +3400 -700 -10.11241 +3400 -600 -8.48557 +3400 -500 -6.84230 +3400 -400 -5.18506 +3400 -300 -3.51648 +3400 -200 -1.83930 +3400 -100 -0.15632 +3400 0 1.52956 +3400 100 3.21545 +3400 200 4.89844 +3400 300 6.57564 +3400 400 8.24425 +3400 500 9.90152 +3400 600 11.54483 +3400 700 13.17172 +3400 800 14.77984 +3400 900 16.36703 +3400 1000 17.93133 +3400 1100 19.47096 +3400 1200 20.98432 +3400 1300 22.47004 +3400 1400 23.92693 +3400 1500 25.35400 +3400 1600 26.75045 +3400 1700 28.11564 +3400 1800 29.44913 +3400 1900 30.75063 +3400 2000 32.01997 +3400 2100 33.25714 +3400 2200 34.46224 +3400 2300 35.63549 +3400 2400 36.77719 +3400 2500 37.88772 +3400 2600 38.96756 +3400 2700 40.01723 +3400 2800 41.03729 +3400 2900 42.02838 +3400 3000 42.99114 +3500 -1000 -14.38300 +3500 -900 -12.85718 +3500 -800 -11.31020 +3500 -700 -9.74392 +3500 -600 -8.16035 +3500 -500 -6.56166 +3500 -400 -4.95013 +3500 -300 -3.32820 +3500 -200 -1.69836 +3500 -100 -0.06322 +3500 0 1.57459 +3500 100 3.21241 +3500 200 4.84757 +3500 300 6.47742 +3500 400 8.09938 +3500 500 9.71094 +3500 600 11.30968 +3500 700 12.89330 +3500 800 14.45964 +3500 900 16.00668 +3500 1000 17.53258 +3500 1100 19.03566 +3500 1200 20.51440 +3500 1300 21.96748 +3500 1400 23.39375 +3500 1500 24.79222 +3500 1600 26.16211 +3500 1700 27.50275 +3500 1800 28.81367 +3500 1900 30.09451 +3500 2000 31.34507 +3500 2100 32.56527 +3500 2200 33.75514 +3500 2300 34.91480 +3500 2400 36.04450 +3500 2500 37.14453 +3500 2600 38.21527 +3500 2700 39.25717 +3500 2800 40.27071 +3500 2900 41.25644 +3500 3000 42.21492 +3600 -1000 -13.91703 +3600 -900 -12.42793 +3600 -800 -10.91925 +3600 -700 -9.39274 +3600 -600 -7.85027 +3600 -500 -6.29386 +3600 -400 -4.72563 +3600 -300 -3.14782 +3600 -200 -1.56274 +3600 -100 0.02722 +3600 0 1.61963 +3600 100 3.21205 +3600 200 4.80202 +3600 300 6.38711 +3600 400 7.96495 +3600 500 9.53322 +3600 600 11.08967 +3600 700 12.63219 +3600 800 14.15877 +3600 900 15.66751 +3600 1000 17.15669 +3600 1100 18.62472 +3600 1200 20.07017 +3600 1300 21.49176 +3600 1400 22.88839 +3600 1500 24.25908 +3600 1600 25.60305 +3600 1700 26.91962 +3600 1800 28.20830 +3600 1900 29.46870 +3600 2000 30.70056 +3600 2100 31.90376 +3600 2200 33.07825 +3600 2300 34.22411 +3600 2400 35.34150 +3600 2500 36.43065 +3600 2600 37.49185 +3600 2700 38.52549 +3600 2800 39.53197 +3600 2900 40.51176 +3600 3000 41.46536 +3700 -1000 -13.47224 +3700 -900 -12.01827 +3700 -800 -10.54615 +3700 -700 -9.05753 +3700 -600 -7.55415 +3700 -500 -6.03788 +3700 -400 -4.51071 +3700 -300 -2.97471 +3700 -200 -1.43200 +3700 -100 0.11520 +3700 0 1.66467 +3700 100 3.21414 +3700 200 4.76136 +3700 300 6.30408 +3700 400 7.84012 +3700 500 9.36733 +3700 600 10.88363 +3700 700 12.38707 +3700 800 13.87576 +3700 900 15.34794 +3700 1000 16.80199 +3700 1100 18.23641 +3700 1200 19.64984 +3700 1300 21.04105 +3700 1400 22.40899 +3700 1500 23.75270 +3700 1600 25.07140 +3700 1700 26.36442 +3700 1800 27.63124 +3700 1900 28.87145 +3700 2000 30.08475 +3700 2100 31.27097 +3700 2200 32.43002 +3700 2300 33.56192 +3700 2400 34.66676 +3700 2500 35.74472 +3700 2600 36.79602 +3700 2700 37.82097 +3700 2800 38.81992 +3700 2900 39.79327 +3700 3000 40.74146 +3800 -1000 -13.04710 +3800 -900 -11.62676 +3800 -800 -10.18958 +3800 -700 -8.73708 +3800 -600 -7.27091 +3800 -500 -5.79282 +3800 -400 -4.30464 +3800 -300 -2.80829 +3800 -200 -1.30576 +3800 -100 0.20093 +3800 0 1.70971 +3800 100 3.21850 +3800 200 4.72520 +3800 300 6.22776 +3800 400 7.72414 +3800 500 9.21235 +3800 600 10.69049 +3800 700 12.15672 +3800 800 13.60928 +3800 900 15.04653 +3800 1000 16.46695 +3800 1100 17.86912 +3800 1200 19.25174 +3800 1300 20.61366 +3800 1400 21.95384 +3800 1500 23.27137 +3800 1600 24.56546 +3800 1700 25.83546 +3800 1800 27.08082 +3800 1900 28.30113 +3800 2000 29.49605 +3800 2100 30.66537 +3800 2200 31.80897 +3800 2300 32.92681 +3800 2400 34.01894 +3800 2500 35.08546 +3800 2600 36.12655 +3800 2700 37.14247 +3800 2800 38.13350 +3800 2900 39.09998 +3800 3000 40.04227 +3900 -1000 -12.64023 +3900 -900 -11.25211 +3900 -800 -9.84833 +3900 -700 -8.43031 +3900 -600 -6.99961 +3900 -500 -5.55785 +3900 -400 -4.10674 +3900 -300 -2.64807 +3900 -200 -1.18367 +3900 -100 0.28458 +3900 0 1.75476 +3900 100 3.22495 +3900 200 4.69321 +3900 300 6.15763 +3900 400 7.61634 +3900 500 9.06748 +3900 600 10.50929 +3900 700 11.94005 +3900 800 13.35813 +3900 900 14.76199 +3900 1000 16.15019 +3900 1100 17.52140 +3900 1200 18.87439 +3900 1300 20.20804 +3900 1400 21.52137 +3900 1500 22.81350 +3900 1600 24.08364 +3900 1700 25.33116 +3900 1800 26.55550 +3900 1900 27.75621 +3900 2000 28.93297 +3900 2100 30.08552 +3900 2200 31.21371 +3900 2300 32.31744 +3900 2400 33.39673 +3900 2500 34.45163 +3900 2600 35.48229 +3900 2700 36.48888 +3900 2800 37.47165 +3900 2900 38.43087 +3900 3000 39.36687 +4000 -1000 -12.25038 +4000 -900 -10.89314 +4000 -800 -9.52131 +4000 -700 -8.13624 +4000 -600 -6.73937 +4000 -500 -5.33223 +4000 -400 -3.91641 +4000 -300 -2.49357 +4000 -200 -1.06542 +4000 -100 0.36630 +4000 0 1.79981 +4000 100 3.23333 +4000 200 4.66506 +4000 300 6.09324 +4000 400 7.51611 +4000 500 8.93197 +4000 600 10.33916 +4000 700 11.73609 +4000 800 13.12123 +4000 900 14.49313 +4000 1000 15.85045 +4000 1100 17.19193 +4000 1200 18.51639 +4000 1300 19.82278 +4000 1400 21.11015 +4000 1500 22.37763 +4000 1600 23.62448 +4000 1700 24.85006 +4000 1800 26.05381 +4000 1900 27.23529 +4000 2000 28.39413 +4000 2100 29.53007 +4000 2200 30.64291 +4000 2300 31.73254 +4000 2400 32.79892 +4000 2500 33.84208 +4000 2600 34.86211 +4000 2700 35.85914 +4000 2800 36.83335 +4000 2900 37.78500 +4000 3000 38.71435 +4100 -1000 -11.87639 +4100 -900 -10.54878 +4100 -800 -9.20755 +4100 -700 -7.85397 +4100 -600 -6.48941 +4100 -500 -5.11529 +4100 -400 -3.73310 +4100 -300 -2.34438 +4100 -200 -0.95073 +4100 -100 0.44624 +4100 0 1.84487 +4100 100 3.24351 +4100 200 4.64049 +4100 300 6.03417 +4100 400 7.42292 +4100 500 8.80515 +4100 600 10.17932 +4100 700 11.54394 +4100 800 12.89758 +4100 900 14.23889 +4100 1000 15.56659 +4100 1100 16.87949 +4100 1200 18.17649 +4100 1300 19.45657 +4100 1400 20.71883 +4100 1500 21.96242 +4100 1600 23.18663 +4100 1700 24.39081 +4100 1800 25.57443 +4100 1900 26.73702 +4100 2000 27.87822 +4100 2100 28.99773 +4100 2200 30.09534 +4100 2300 31.17091 +4100 2400 32.22436 +4100 2500 33.25569 +4100 2600 34.26494 +4100 2700 35.25221 +4100 2800 36.21765 +4100 2900 37.16145 +4100 3000 38.08384 +4200 -1000 -11.51722 +4200 -900 -10.21803 +4200 -800 -8.90612 +4200 -700 -7.58269 +4200 -600 -6.24902 +4200 -500 -4.90642 +4200 -400 -3.55632 +4200 -300 -2.20013 +4200 -200 -0.83935 +4200 -100 0.52452 +4200 0 1.88993 +4200 100 3.25535 +4200 200 4.61923 +4200 300 5.98004 +4200 400 7.33626 +4200 500 8.68641 +4200 600 10.02905 +4200 700 11.36279 +4200 800 12.68629 +4200 900 13.99828 +4200 1000 15.29755 +4200 1100 16.58299 +4200 1200 17.85353 +4200 1300 19.10823 +4200 1400 20.34619 +4200 1500 21.56662 +4200 1600 22.76882 +4200 1700 23.95216 +4200 1800 25.11610 +4200 1900 26.26018 +4200 2000 27.38402 +4200 2100 28.48731 +4200 2200 29.56983 +4200 2300 30.63141 +4200 2400 31.67195 +4200 2500 32.69139 +4200 2600 33.68977 +4200 2700 34.66714 +4200 2800 35.62361 +4200 2900 36.55933 +4200 3000 37.47449 +4300 -1000 -11.17190 +4300 -900 -9.90001 +4300 -800 -8.61622 +4300 -700 -7.32166 +4300 -600 -6.01753 +4300 -500 -4.70507 +4300 -400 -3.38560 +4300 -300 -2.06046 +4300 -200 -0.73104 +4300 -100 0.60126 +4300 0 1.93500 +4300 100 3.26874 +4300 200 4.60106 +4300 300 5.93051 +4300 400 7.25568 +4300 500 8.57519 +4300 600 9.88770 +4300 700 11.19190 +4300 800 12.48653 +4300 900 13.77040 +4300 1000 15.04237 +4300 1100 16.30139 +4300 1200 17.54645 +4300 1300 18.77663 +4300 1400 19.99110 +4300 1500 21.18908 +4300 1600 22.36989 +4300 1700 23.53293 +4300 1800 24.67764 +4300 1900 25.80359 +4300 2000 26.91038 +4300 2100 27.99769 +4300 2200 29.06529 +4300 2300 30.11297 +4300 2400 31.14063 +4300 2500 32.14819 +4300 2600 33.13563 +4300 2700 34.10298 +4300 2800 35.05033 +4300 2900 35.97778 +4300 3000 36.88550 +4400 -1000 -10.83955 +4400 -900 -9.59390 +4400 -800 -8.33710 +4400 -700 -7.07021 +4400 -600 -5.79437 +4400 -500 -4.51074 +4400 -400 -3.22056 +4400 -300 -1.92508 +4400 -200 -0.62560 +4400 -100 0.67656 +4400 0 1.98007 +4400 100 3.28358 +4400 200 4.58576 +4400 300 5.88526 +4400 400 7.18078 +4400 500 8.47100 +4400 600 9.75468 +4400 700 11.03059 +4400 800 12.29755 +4400 900 13.55444 +4400 1000 14.80018 +4400 1100 16.03377 +4400 1200 17.25426 +4400 1300 18.46078 +4400 1400 19.65252 +4400 1500 20.82874 +4400 1600 21.98877 +4400 1700 23.13203 +4400 1800 24.25799 +4400 1900 25.36618 +4400 2000 26.45623 +4400 2100 27.52781 +4400 2200 28.58067 +4400 2300 29.61459 +4400 2400 30.62944 +4400 2500 31.62512 +4400 2600 32.60159 +4400 2700 33.55885 +4400 2800 34.49696 +4400 2900 35.41599 +4400 3000 36.31607 +4500 -1000 -10.51937 +4500 -900 -9.29894 +4500 -800 -8.06805 +4500 -700 -6.82771 +4500 -600 -5.57898 +4500 -500 -4.32296 +4500 -400 -3.06080 +4500 -300 -1.79369 +4500 -200 -0.52284 +4500 -100 0.75052 +4500 0 2.02515 +4500 100 3.29977 +4500 200 4.57315 +4500 300 5.84403 +4500 400 7.11117 +4500 500 8.37338 +4500 600 9.62945 +4500 700 10.87825 +4500 800 12.11867 +4500 900 13.34964 +4500 1000 14.57016 +4500 1100 15.77927 +4500 1200 16.97607 +4500 1300 18.15973 +4500 1400 19.32948 +4500 1500 20.48460 +4500 1600 21.62446 +4500 1700 22.74846 +4500 1800 23.85611 +4500 1900 24.94694 +4500 2000 26.02058 +4500 2100 27.07669 +4500 2200 28.11500 +4500 2300 29.13530 +4500 2400 30.13743 +4500 2500 31.12128 +4500 2600 32.08677 +4500 2700 33.03390 +4500 2800 33.96268 +4500 2900 34.87317 +4500 3000 35.76546 +4600 -1000 -10.21060 +4600 -900 -9.01445 +4600 -800 -7.80846 +4600 -700 -6.59361 +4600 -600 -5.37087 +4600 -500 -4.14131 +4600 -400 -2.90600 +4600 -300 -1.66604 +4600 -200 -0.42258 +4600 -100 0.82323 +4600 0 2.07023 +4600 100 3.31722 +4600 200 4.56305 +4600 300 5.80654 +4600 400 7.04653 +4600 500 8.28189 +4600 600 9.51151 +4600 700 10.73431 +4600 800 11.94925 +4600 900 13.15532 +4600 1000 14.35157 +4600 1100 15.53710 +4600 1200 16.71105 +4600 1300 17.87262 +4600 1400 19.02109 +4600 1500 20.15575 +4600 1600 21.27601 +4600 1700 22.38128 +4600 1800 23.47106 +4600 1900 24.54493 +4600 2000 25.60248 +4600 2100 26.64338 +4600 2200 27.66737 +4600 2300 28.67421 +4600 2400 29.66373 +4600 2500 30.63580 +4600 2600 31.59034 +4600 2700 32.52731 +4600 2800 33.44671 +4600 2900 34.34856 +4600 3000 35.23294 +4700 -1000 -9.91257 +4700 -900 -8.73978 +4700 -800 -7.55775 +4700 -700 -6.36737 +4700 -600 -5.16960 +4700 -500 -3.96541 +4700 -400 -2.75583 +4700 -300 -1.54189 +4700 -200 -0.32466 +4700 -100 0.89477 +4700 0 2.11531 +4700 100 3.33586 +4700 200 4.55530 +4700 300 5.77256 +4700 400 6.98654 +4700 500 8.19617 +4700 600 9.40041 +4700 700 10.59825 +4700 800 11.78871 +4700 900 12.97083 +4700 1000 14.14372 +4700 1100 15.30652 +4700 1200 16.45842 +4700 1300 17.59865 +4700 1400 18.72651 +4700 1500 19.84135 +4700 1600 20.94255 +4700 1700 22.02959 +4700 1800 23.10197 +4700 1900 24.15924 +4700 2000 25.20103 +4700 2100 26.22701 +4700 2200 27.23690 +4700 2300 28.23045 +4700 2400 29.20749 +4700 2500 30.16787 +4700 2600 31.11150 +4700 2700 32.03832 +4700 2800 32.94830 +4700 2900 33.84145 +4700 3000 34.71782 +4800 -1000 -9.62464 +4800 -900 -8.47436 +4800 -800 -7.31537 +4800 -700 -6.14853 +4800 -600 -4.97473 +4800 -500 -3.79490 +4800 -400 -2.61000 +4800 -300 -1.42101 +4800 -200 -0.22893 +4800 -100 0.96522 +4800 0 2.16040 +4800 100 3.35560 +4800 200 4.54976 +4800 300 5.74187 +4800 400 6.93090 +4800 500 8.11585 +4800 600 9.29573 +4800 700 10.46960 +4800 800 11.63652 +4800 900 12.79560 +4800 1000 13.94599 +4800 1100 15.08687 +4800 1200 16.21747 +4800 1300 17.33706 +4800 1400 18.44498 +4800 1500 19.54058 +4800 1600 20.62329 +4800 1700 21.69259 +4800 1800 22.74799 +4800 1900 23.78906 +4800 2000 24.81543 +4800 2100 25.82676 +4800 2200 26.82277 +4800 2300 27.80322 +4800 2400 28.76793 +4800 2500 29.71672 +4800 2600 30.64950 +4800 2700 31.56618 +4800 2800 32.46672 +4800 2900 33.35113 +4800 3000 34.21941 +4900 -1000 -9.34623 +4900 -900 -8.21763 +4900 -800 -7.08083 +4900 -700 -5.93663 +4900 -600 -4.78589 +4900 -500 -3.62946 +4900 -400 -2.46827 +4900 -300 -1.30322 +4900 -200 -0.13527 +4900 -100 1.03463 +4900 0 2.20550 +4900 100 3.37638 +4900 200 4.54629 +4900 300 5.71427 +4900 400 6.87935 +4900 500 8.04060 +4900 600 9.19709 +4900 700 10.34790 +4900 800 11.49218 +4900 900 12.62908 +4900 1000 13.75778 +4900 1100 14.87751 +4900 1200 15.98754 +4900 1300 17.08718 +4900 1400 18.17578 +4900 1500 19.25273 +4900 1600 20.31748 +4900 1700 21.36951 +4900 1800 22.40835 +4900 1900 23.43360 +4900 2000 24.44488 +4900 2100 25.44184 +4900 2200 26.42423 +4900 2300 27.39177 +4900 2400 28.34429 +4900 2500 29.28161 +4900 2600 30.20360 +4900 2700 31.11019 +4900 2800 32.00131 +4900 2900 32.87695 +4900 3000 33.73709 +5000 -1000 -9.07679 +5000 -900 -7.96911 +5000 -800 -6.85369 +5000 -700 -5.73129 +5000 -600 -4.60271 +5000 -500 -3.46879 +5000 -400 -2.33037 +5000 -300 -1.18832 +5000 -200 -0.04354 +5000 -100 1.10307 +5000 0 2.25060 +5000 100 3.39814 +5000 200 4.54477 +5000 300 5.68958 +5000 400 6.83166 +5000 500 7.97013 +5000 600 9.10412 +5000 700 10.23277 +5000 800 11.35525 +5000 900 12.47077 +5000 1000 13.57856 +5000 1100 14.67788 +5000 1200 15.76803 +5000 1300 16.84836 +5000 1400 17.91824 +5000 1500 18.97710 +5000 1600 20.02439 +5000 1700 21.05962 +5000 1800 22.08234 +5000 1900 23.09214 +5000 2000 24.08865 +5000 2100 25.07154 +5000 2200 26.04053 +5000 2300 26.99537 +5000 2400 27.93587 +5000 2500 28.86183 +5000 2600 29.77314 +5000 2700 30.66969 +5000 2800 31.55141 +5000 2900 32.41826 +5000 3000 33.27024 +5100 -1000 -8.81583 +5100 -900 -7.72833 +5100 -800 -6.63351 +5100 -700 -5.53211 +5100 -600 -4.42488 +5100 -500 -3.31261 +5100 -400 -2.19609 +5100 -300 -1.07615 +5100 -200 0.04636 +5100 -100 1.17060 +5100 0 2.29571 +5100 100 3.42083 +5100 200 4.54508 +5100 300 5.66763 +5100 400 6.78760 +5100 500 7.90417 +5100 600 9.01651 +5100 700 10.12381 +5100 800 11.22530 +5100 900 12.32021 +5100 1000 13.40782 +5100 1100 14.48742 +5100 1200 15.55836 +5100 1300 16.62000 +5100 1400 17.67175 +5100 1500 18.71305 +5100 1600 19.74339 +5100 1700 20.76227 +5100 1800 21.76927 +5100 1900 22.76399 +5100 2000 23.74606 +5100 2100 24.71515 +5100 2200 25.67100 +5100 2300 26.61335 +5100 2400 27.54198 +5100 2500 28.45673 +5100 2600 29.35745 +5100 2700 30.24403 +5100 2800 31.11639 +5100 2900 31.97448 +5100 3000 32.81826 +5200 -1000 -8.56287 +5200 -900 -7.49485 +5200 -800 -6.41992 +5200 -700 -5.33876 +5200 -600 -4.25209 +5200 -500 -3.16066 +5200 -400 -2.06522 +5200 -300 -0.96655 +5200 -200 0.13455 +5200 -100 1.23728 +5200 0 2.34082 +5200 100 3.44438 +5200 200 4.54712 +5200 300 5.64825 +5200 400 6.74696 +5200 500 7.84245 +5200 600 8.93395 +5200 700 10.02069 +5200 800 11.10194 +5200 900 12.17697 +5200 1000 13.24510 +5200 1100 14.30566 +5200 1200 15.35801 +5200 1300 16.40156 +5200 1400 17.43573 +5200 1500 18.45999 +5200 1600 19.47385 +5200 1700 20.47683 +5200 1800 21.46851 +5200 1900 22.44850 +5200 2000 23.41645 +5200 2100 24.37204 +5200 2200 25.31499 +5200 2300 26.24505 +5200 2400 27.16201 +5200 2500 28.06568 +5200 2600 28.95592 +5200 2700 29.83261 +5200 2800 30.69566 +5200 2900 31.54500 +5200 3000 32.38060 +5300 -1000 -8.31749 +5300 -900 -7.26829 +5300 -800 -6.21254 +5300 -700 -5.15090 +5300 -600 -4.08405 +5300 -500 -3.01270 +5300 -400 -1.93756 +5300 -300 -0.85937 +5300 -200 0.22111 +5300 -100 1.30314 +5300 0 2.38594 +5300 100 3.46875 +5300 200 4.55080 +5300 300 5.63131 +5300 400 6.70954 +5300 500 7.78473 +5300 600 8.85615 +5300 700 9.92308 +5300 800 10.98481 +5300 900 12.04065 +5300 1000 13.08997 +5300 1100 14.13211 +5300 1200 15.16648 +5300 1300 16.19251 +5300 1400 17.20964 +5300 1500 18.21736 +5300 1600 19.21519 +5300 1700 20.20270 +5300 1800 21.17945 +5300 1900 22.14507 +5300 2000 23.09922 +5300 2100 24.04159 +5300 2200 24.97189 +5300 2300 25.88987 +5300 2400 26.79533 +5300 2500 27.68808 +5300 2600 28.56796 +5300 2700 29.43485 +5300 2800 30.28865 +5300 2900 31.12928 +5300 3000 31.95671 +5400 -1000 -8.07928 +5400 -900 -7.04826 +5400 -800 -6.01105 +5400 -700 -4.96825 +5400 -600 -3.92052 +5400 -500 -2.86852 +5400 -400 -1.81295 +5400 -300 -0.75449 +5400 -200 0.30615 +5400 -100 1.36824 +5400 0 2.43107 +5400 100 3.49390 +5400 200 4.55601 +5400 300 5.61668 +5400 400 6.67518 +5400 500 7.73081 +5400 600 8.78287 +5400 700 9.83068 +5400 800 10.87357 +5400 900 11.91089 +5400 1000 12.94202 +5400 1100 13.96636 +5400 1200 14.98332 +5400 1300 15.99237 +5400 1400 16.99297 +5400 1500 17.98463 +5400 1600 18.96690 +5400 1700 19.93933 +5400 1800 20.90153 +5400 1900 21.85314 +5400 2000 22.79380 +5400 2100 23.72322 +5400 2200 24.64111 +5400 2300 25.54724 +5400 2400 26.44139 +5400 2500 27.32336 +5400 2600 28.19301 +5400 2700 29.05020 +5400 2800 29.89482 +5400 2900 30.72680 +5400 3000 31.54607 +5500 -1000 -7.84787 +5500 -900 -6.83444 +5500 -800 -5.81512 +5500 -700 -4.79052 +5500 -600 -3.76124 +5500 -500 -2.72792 +5500 -400 -1.69120 +5500 -300 -0.65176 +5500 -200 0.38974 +5500 -100 1.43262 +5500 0 2.47620 +5500 100 3.51978 +5500 200 4.56268 +5500 300 5.60422 +5500 400 6.64370 +5500 500 7.68047 +5500 600 8.71386 +5500 700 9.74322 +5500 800 10.76791 +5500 900 11.78734 +5500 1000 12.80089 +5500 1100 13.80799 +5500 1200 14.80810 +5500 1300 15.80069 +5500 1400 16.78525 +5500 1500 17.76132 +5500 1600 18.72845 +5500 1700 19.68622 +5500 1800 20.63424 +5500 1900 21.57216 +5500 2000 22.49964 +5500 2100 23.41639 +5500 2200 24.32213 +5500 2300 25.21661 +5500 2400 26.09963 +5500 2500 26.97100 +5500 2600 27.83054 +5500 2700 28.67814 +5500 2800 29.51366 +5500 2900 30.33703 +5500 3000 31.14819 +5600 -1000 -7.62291 +5600 -900 -6.62649 +5600 -800 -5.62448 +5600 -700 -4.61745 +5600 -600 -3.60599 +5600 -500 -2.59069 +5600 -400 -1.57218 +5600 -300 -0.55109 +5600 -200 0.47196 +5600 -100 1.49632 +5600 0 2.52134 +5600 100 3.54636 +5600 200 4.57073 +5600 300 5.59382 +5600 400 6.61496 +5600 500 7.63352 +5600 600 8.64889 +5600 700 9.66043 +5600 800 10.66755 +5600 900 11.66967 +5600 1000 12.66621 +5600 1100 13.65664 +5600 1200 14.64042 +5600 1300 15.61705 +5600 1400 16.58605 +5600 1500 17.54697 +5600 1600 18.49938 +5600 1700 19.44287 +5600 1800 20.37707 +5600 1900 21.30164 +5600 2000 22.21624 +5600 2100 23.12058 +5600 2200 24.01441 +5600 2300 24.89747 +5600 2400 25.76955 +5600 2500 26.63046 +5600 2600 27.48004 +5600 2700 28.31815 +5600 2800 29.14467 +5600 2900 29.95951 +5600 3000 30.76259 +5700 -1000 -7.40408 +5700 -900 -6.42411 +5700 -800 -5.43884 +5700 -700 -4.44880 +5700 -600 -3.45456 +5700 -500 -2.45668 +5700 -400 -1.45574 +5700 -300 -0.45235 +5700 -200 0.55289 +5700 -100 1.55937 +5700 0 2.56648 +5700 100 3.57359 +5700 200 4.58010 +5700 300 5.58537 +5700 400 6.58881 +5700 500 7.58980 +5700 600 8.58775 +5700 700 9.58208 +5700 800 10.57221 +5700 900 11.55759 +5700 1000 12.53768 +5700 1100 13.51195 +5700 1200 14.47990 +5700 1300 15.44106 +5700 1400 16.39495 +5700 1500 17.34115 +5700 1600 18.27924 +5700 1700 19.20883 +5700 1800 20.12956 +5700 1900 21.04109 +5700 2000 21.94311 +5700 2100 22.83532 +5700 2200 23.71747 +5700 2300 24.58932 +5700 2400 25.45066 +5700 2500 26.30128 +5700 2600 27.14104 +5700 2700 27.96978 +5700 2800 28.78738 +5700 2900 29.59376 +5700 3000 30.38881 +5800 -1000 -7.19106 +5800 -900 -6.22703 +5800 -800 -5.25796 +5800 -700 -4.28436 +5800 -600 -3.30676 +5800 -500 -2.32570 +5800 -400 -1.34175 +5800 -300 -0.35546 +5800 -200 0.63259 +5800 -100 1.62181 +5800 0 2.61163 +5800 100 3.60145 +5800 200 4.59070 +5800 300 5.57878 +5800 400 6.56511 +5800 500 7.54912 +5800 600 8.53025 +5800 700 9.50794 +5800 800 10.48164 +5800 900 11.45082 +5800 1000 12.41497 +5800 1100 13.37359 +5800 1200 14.32620 +5800 1300 15.27234 +5800 1400 16.21157 +5800 1500 17.14346 +5800 1600 18.06762 +5800 1700 18.98367 +5800 1800 19.89127 +5800 1900 20.79008 +5800 2000 21.67980 +5800 2100 22.56014 +5800 2200 23.43086 +5800 2300 24.29171 +5800 2400 25.14248 +5800 2500 25.98299 +5800 2600 26.81306 +5800 2700 27.63256 +5800 2800 28.44135 +5800 2900 29.23934 +5800 3000 30.02643 +5900 -1000 -6.98358 +5900 -900 -6.03499 +5900 -800 -5.08160 +5900 -700 -4.12390 +5900 -600 -3.16240 +5900 -500 -2.19761 +5900 -400 -1.23007 +5900 -300 -0.26031 +5900 -200 0.71112 +5900 -100 1.68367 +5900 0 2.65679 +5900 100 3.62991 +5900 200 4.60248 +5900 300 5.57394 +5900 400 6.54375 +5900 500 7.51135 +5900 600 8.47621 +5900 700 9.43780 +5900 800 10.39560 +5900 900 11.34910 +5900 1000 12.29782 +5900 1100 13.24127 +5900 1200 14.17899 +5900 1300 15.11056 +5900 1400 16.03553 +5900 1500 16.95351 +5900 1600 17.86412 +5900 1700 18.76699 +5900 1800 19.66178 +5900 1900 20.54818 +5900 2000 21.42588 +5900 2100 22.29461 +5900 2200 23.15413 +5900 2300 24.00419 +5900 2400 24.84459 +5900 2500 25.67514 +5900 2600 26.49568 +5900 2700 27.30605 +5900 2800 28.10614 +5900 2900 28.89582 +5900 3000 29.67502 +6000 -1000 -6.78137 +6000 -900 -5.84774 +6000 -800 -4.90954 +6000 -700 -3.96724 +6000 -600 -3.02132 +6000 -500 -2.07227 +6000 -400 -1.12060 +6000 -300 -0.16682 +6000 -200 0.78855 +6000 -100 1.74498 +6000 0 2.70195 +6000 100 3.65893 +6000 200 4.61538 +6000 300 5.57078 +6000 400 6.52461 +6000 500 7.47634 +6000 600 8.42546 +6000 700 9.37147 +6000 800 10.31387 +6000 900 11.25218 +6000 1000 12.18594 +6000 1100 13.11468 +6000 1200 14.03797 +6000 1300 14.95538 +6000 1400 15.86650 +6000 1500 16.77096 +6000 1600 17.66838 +6000 1700 18.55840 +6000 1800 19.44071 +6000 1900 20.31499 +6000 2000 21.18095 +6000 2100 22.03832 +6000 2200 22.88687 +6000 2300 23.72635 +6000 2400 24.55657 +6000 2500 25.37734 +6000 2600 26.18848 +6000 2700 26.98986 +6000 2800 27.78134 +6000 2900 28.56282 +6000 3000 29.33419 \ No newline at end of file diff --git a/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp b/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp index 364d616..c909a2d 100755 --- a/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp +++ b/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp @@ -6,7 +6,7 @@ #include // Error integer and strerror() function #include // Contains POSIX terminal control definitions #include // write(), read(), close() - +#include #include "time_debug.h" #define PI 3.141592653589793238462643383 // It was 4 AM when I wrote this. @@ -17,6 +17,10 @@ using namespace std::placeholders; ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("control_communicator_node") { + this->lookup_table_path = this->declare_parameter("aim.lookup_table_path", "./src/prm_control/include/lookup_tables/pitch_lookup_table.txt"); + + this->pitch_lookup_model = PitchLookupModel(this->lookup_table_path); + aim_stop_null_frame_count = this->declare_parameter("aim.stop_null_frame_count", 3); aim_bullet_speed = this->declare_parameter("aim.bullet_speed", 16.0f); @@ -52,6 +56,7 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr this->match_status_publisher = this->create_publisher("match_start", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this)); + this->read_lookup_table_time = this->create_wall_timer(5000ms, std::bind(&PitchLookupModel::load_file, &this->pitch_lookup_model)); RCLCPP_INFO(this->get_logger(), "Control Communicator Node Started."); } @@ -189,6 +194,11 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptrpitch_lookup_model.get_pitch(dst, msg->y); + RCLCPP_INFO(this->get_logger(), "Old Pitch, Pitch: %.3f, %.3f", old_pitch, pitch); + PackageOut package; this->auto_aim_frame_id++; package.frame_id = 0xAA; diff --git a/src/prm_control/control_communicator/src/PitchLookupModel.cpp b/src/prm_control/control_communicator/src/PitchLookupModel.cpp new file mode 100644 index 0000000..5b31316 --- /dev/null +++ b/src/prm_control/control_communicator/src/PitchLookupModel.cpp @@ -0,0 +1,163 @@ +#include "PitchLookupModel.hpp" + +PitchLookupModel::PitchLookupModel() {} + +PitchLookupModel::PitchLookupModel(std::string filename) { + this->filename = filename; + this->load_file(); +} + +/** + * @brief Loads lookup table from file + * + * The first line of the file contains two space-separated integers: M, N, + * where M represents the number of z coordinates (distance from the camera) + * and N represents the number of y coordinates (height above the ground). + * The next M x N lines contain three space-separated numbers: + * z coordinate, y coordinate, and pitch. Length measurements are in + * millimeters, and angle measurements are in degrees. + */ +void PitchLookupModel::load_file() { + FILE* fp = fopen(filename.c_str(), "r"); + if (fp == nullptr) { + return; + } + + int y_count = 0; + int z_count = 0; + fscanf(fp, "%d %d\n", &z_count, &y_count); + + this->pitch_lookup_table = std::vector>( + z_count, std::vector(y_count, 0)); + + for (int row = 0; row < z_count; row++) { + for (int col = 0; col < y_count; col++) { + int z = 0, y = 0; + float pitch = 0; + int read_count = fscanf(fp, "%d %d %f\n", &z, &y, &pitch); + + if (read_count != 3) { + fclose(fp); + fp = NULL; + return; + } + + if (z < this->lower_z) this->lower_z = z; + if (z > this->upper_z) this->upper_z = z; + if (y < this->lower_y) this->lower_y = y; + if (y > this->upper_y) this->upper_y = y; + + this->pitch_lookup_table[row][col] = pitch; + } + } + + fclose(fp); + fp = NULL; +} + +/** + * @brief Writes the lookup table to a file + * + * @param filename Name of the file to write the lookup table to + */ + +void PitchLookupModel::write_file() { + FILE* fp = fopen(filename.c_str(), "w"); + if (fp == nullptr) { + return; + } + + int z_count = this->pitch_lookup_table.size(); + int y_count = this->pitch_lookup_table[0].size(); + + fprintf(fp, "%d %d\n", z_count, y_count); + + for (int row = 0; row < z_count; row++) { + for (int col = 0; col < y_count; col++) { + fprintf(fp, "%d %d %f\n", row, col, this->pitch_lookup_table[row][col]); + } + } + + fclose(fp); + fp = NULL; +} + +/** + * @brief Lookup the corresponding pitch with distance and height + * + * @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) { + // Calculate the step size of the lookup table + 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 + 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; + + // 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; + + // 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(distance), + static_cast(lower_step_z), + static_cast(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(distance), + static_cast(lower_step_z), + static_cast(upper_step_z), + std::fmin(pitch_low_high, pitch_high_high), + std::fmax(pitch_low_high, pitch_high_high)); + + // Combine the calculated new bounds to calculate the new pitch + return this->map( + static_cast(height), + static_cast(lower_step_y), + static_cast(upper_step_y), + std::fmin(pitch_y_low, pitch_y_high), + std::fmax(pitch_y_low, pitch_y_high)); +} + +/** + * @brief Maps a value from a range to another range + * + * @param value Value to be mapped + * @param old_min The minimum of the old range + * @param old_may The maximum of the old range + * @param new_min The minimum of the new range + * @param new_may The maximum of the new range + * + * @return Mapped value + */ +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; + } + + if (old_min == old_may) { + return (new_min == new_may) ? new_min : -1; + } + + return new_min + (value - old_min) * (new_may - new_min) / (old_may - old_min); +} \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index ddf1eac..fc214da 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -167,12 +167,11 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha Eigen::Matrix r_mat = r_roll * r_pitch * r_yaw; - Eigen::Matrix transform_mat { - {r_mat(0, 0), r_mat(0, 1), r_mat(0, 2), cam_barrel_x}, - {r_mat(1, 0), r_mat(1, 1), r_mat(1, 2), cam_barrel_y}, - {r_mat(2, 0), r_mat(2, 1), r_mat(2, 2), cam_barrel_z}, - {0, 0, 0, 1} - }; + Eigen::Matrix transform_mat; + transform_mat << r_mat(0, 0), r_mat(0, 1), r_mat(0, 2), cam_barrel_x, + r_mat(1, 0), r_mat(1, 1), r_mat(1, 2), cam_barrel_y, + r_mat(2, 0), r_mat(2, 1), r_mat(2, 2), cam_barrel_z, + 0, 0, 0, 1; // Multiply cam -> target vector by transformation matrix to get barrel -> target vector Eigen::Vector4d cam_to_target = {tvec.at(0), tvec.at(1), tvec.at(2), 1};