diff --git a/src/prm_control/control_communicator/CMakeLists.txt b/src/prm_control/control_communicator/CMakeLists.txt new file mode 100644 index 0000000..53c19b7 --- /dev/null +++ b/src/prm_control/control_communicator/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.5) +project(control_communicator) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(nav_msgs REQUIRED) + + +include_directories(/usr/include) +include_directories(include) + +add_executable(ControlCommunicatorNode + src/ControlCommunicatorNode.cpp + src/ControlCommunicator.cpp + src/PitchLookupModel.cpp +) + + +ament_target_dependencies(ControlCommunicatorNode + rclcpp + std_msgs + geometry_msgs + vision_msgs + tf2_ros + tf2 + nav_msgs +) + +target_link_libraries(ControlCommunicatorNode Eigen3::Eigen) + +install(TARGETS + ControlCommunicatorNode + DESTINATION lib/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/prm_control/control_communicator/include/ControlCommunicator.hpp b/src/prm_control/control_communicator/include/ControlCommunicator.hpp new file mode 100644 index 0000000..984f78e --- /dev/null +++ b/src/prm_control/control_communicator/include/ControlCommunicator.hpp @@ -0,0 +1,36 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // Error integer and strerror() function + +#include "messages.hpp" + +class ControlCommunicator +{ +public: + ControlCommunicator(); + ControlCommunicator(const char *port); + ~ControlCommunicator(); + + bool connected(); + int get_port_fd(); + void set_port(const char *port); + std::string start_uart(); + int send_heart_beat_packet(); + int send_auto_aim_packet(float yaw, float pitch, bool fire); + int send_nav_packet(float x_vel, float y_vel, float yaw_rad, uint8_t state); + bool read_alignment(); + std::tuple read_package(); +private: + const char *port; + int port_fd = -1; + bool is_connected = false; +}; \ No newline at end of file diff --git a/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp b/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp new file mode 100644 index 0000000..ed39f7c --- /dev/null +++ b/src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp @@ -0,0 +1,94 @@ + +#ifndef _CONTROL_COMMUNICATOR_NODE_H +#define _CONTROL_COMMUNICATOR_NODE_H + +#define PI 3.141592653589793238462643383 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // Error integer and strerror() function + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float64.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +#include "PitchLookupModel.hpp" + +#include "vision_msgs/msg/yaw_pitch.hpp" +#include "vision_msgs/msg/predicted_armor.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "messages.hpp" + +#include "ControlCommunicator.hpp" + +class ControlCommunicatorNode : public rclcpp::Node +{ +public: + ControlCommunicatorNode(const char *port); + ~ControlCommunicatorNode(); + +private: + float yaw_vel = 0; // rad/s (+ccw, -cw) + float pitch_vel = 0; // rad/s + float pitch = 0; // rad (+up, -down)? + bool is_red = 0; + bool is_match_running = 0; + + uint32_t recive_frame_id = 0; + uint32_t auto_aim_frame_id = 0; + uint32_t nav_frame_id = 0; + uint32_t heart_beat_frame_id = 0; + + int8_t curr_pois = 0; + bool right = true; + + std::string lookup_table_path; + + ControlCommunicator control_communicator; + PitchLookupModel pitch_lookup_model; + + std::unique_ptr tf_broadcaster; + std::shared_ptr tf_static_broadcaster; + std::unique_ptr tf_buffer; + std::shared_ptr tf_listener; + + rclcpp::Subscription::SharedPtr auto_aim_subscriber; + rclcpp::Subscription::SharedPtr nav_subscriber; + + rclcpp::Publisher::SharedPtr odometry_publisher; + std::shared_ptr> target_robot_color_publisher = NULL; + std::shared_ptr> match_status_publisher = NULL; + + rclcpp::TimerBase::SharedPtr uart_read_timer; + rclcpp::TimerBase::SharedPtr heart_beat_timer; + + void publish_static_tf(float, float, float, float, float, float, const char *, const char *); + void auto_aim_handler(const std::shared_ptr msg); + void nav_handler(const std::shared_ptr msg); + void heart_beat_handler(); + void read_uart(); +}; + +#endif // CONTROL_COMMUNICATOR_NODE_H 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..d82df66 --- /dev/null +++ b/src/prm_control/control_communicator/include/PitchLookupModel.hpp @@ -0,0 +1,28 @@ +#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(std::string filename); + float get_pitch(int distance, int height); + +private: + int lower_x = INT_MAX; + int lower_z = INT_MAX; + int upper_x = INT_MIN; + int upper_z = INT_MIN; + 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 diff --git a/src/prm_control/control_communicator/include/messages.hpp b/src/prm_control/control_communicator/include/messages.hpp new file mode 100644 index 0000000..415772b --- /dev/null +++ b/src/prm_control/control_communicator/include/messages.hpp @@ -0,0 +1,62 @@ +#ifndef _MESSAGES_H +#define _MESSAGES_H + +#include + +#define FRAME_TYPE_AUTO_AIM 0 +#define FRAME_TYPE_NAV 1 +#define FRAME_TYPE_HEART_BEAT 2 +#define FRAME_TYPE_OTHER 3 + +typedef struct _AutoAimPackage +{ + float yaw; // yaw (deg) + float pitch; // pitch (deg) + bool fire; // 0 = no fire, 1 = fire +} AutoAimPackage; + +typedef struct _NavPackage +{ + float x_vel; // m/s + float y_vel; // m/s + float yaw_rad; // rad/s + uint8_t state; // 0 = stationary, 1 = moving, 2 = spin +} NavPackage; + +typedef struct _HeartBeatPackage +{ + uint8_t _a; + uint8_t _b; + uint8_t _c; + uint8_t _d; +} HeartBeatPackage; + +typedef struct _PackageOut +{ + uint8_t frame_id; + uint8_t frame_type; + union + { + AutoAimPackage autoAimPackage; + NavPackage navPackage; + HeartBeatPackage heartBeatPackage; + }; +} PackageOut; + +typedef struct __attribute__((__packed__)) _PackageIn +{ + uint8_t head; + uint8_t ref_flags; + float pitch; // rad + float pitch_vel; // rad/s + float yaw_vel; // rad/s (ccw: +, cw: -) + + float x; // m + float y; // m + float orientation; // rad (ccw: +, cw: -) + + float x_vel; // m/s + float y_vel; // m/s +} __attribute__((packed)) PackageIn; + +#endif // _MESSAGES_H diff --git a/src/prm_control/control_communicator/package.xml b/src/prm_control/control_communicator/package.xml new file mode 100644 index 0000000..b1a69bd --- /dev/null +++ b/src/prm_control/control_communicator/package.xml @@ -0,0 +1,23 @@ + + + + control_communicator + 0.0.0 + A package communicator to comunnicate with the STM32 board + Tom O'Donnell + Apache-2.0 + + ament_cmake + std_msgs + vision_msgs + geometry_msgs + nav_msgs + tf2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/prm_control/control_communicator/pitch_lookup_table.txt b/src/prm_control/control_communicator/pitch_lookup_table.txt new file mode 100644 index 0000000..fff4285 --- /dev/null +++ b/src/prm_control/control_communicator/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 diff --git a/src/prm_control/control_communicator/src/ControlCommunicator.cpp b/src/prm_control/control_communicator/src/ControlCommunicator.cpp new file mode 100644 index 0000000..d717d9e --- /dev/null +++ b/src/prm_control/control_communicator/src/ControlCommunicator.cpp @@ -0,0 +1,191 @@ +#include "ControlCommunicator.hpp" + +ControlCommunicator::ControlCommunicator() { +} + +ControlCommunicator::ControlCommunicator(const char *port) { + this->set_port(port); +} + +ControlCommunicator::~ControlCommunicator() +{ + close(this->port_fd); +} + +bool ControlCommunicator::connected() { + return this->is_connected; +} +int ControlCommunicator::get_port_fd() { + return this->port_fd; +} + +void ControlCommunicator::set_port(const char *port) { + this->port = port; + this->start_uart(); +} + +/** + * @brief Starts the UART interface with the STM32 board + * + * @return Error message for starting UART (Empty if successful) + */ +std::string ControlCommunicator::start_uart() { + this->is_connected = false; + this->port_fd = open(this->port, O_RDWR); + + // Check for errors + if (this->port_fd < 0) + { + std::stringstream ss; + ss << "Failed to open: " << this->port << ", " << strerror(errno); + return ss.str(); + } + + struct termios tty; + + // Set UART TTY to 8n1 + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; + + tty.c_cflag &= ~CRTSCTS; // No RTS/CTS flow control + tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines + tty.c_lflag &= ~ICANON; // Disable canonical mode + + // Disable echo, erasure and newline echo + tty.c_lflag &= ~ECHO; + tty.c_lflag &= ~ECHOE; + tty.c_lflag &= ~ECHONL; + + // Disable interpretation of INTR, QUIT and SUSP + tty.c_lflag &= ~ISIG; + + // Disable special handling, interpretation, S/W flow control, \n conv. + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); + tty.c_oflag &= ~OPOST; + tty.c_oflag &= ~ONLCR; + + tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds) + tty.c_cc[VMIN] = sizeof(PackageIn); // Block for sizeof(PackageOut) bits + + // Set the baud rate + cfsetispeed(&tty, B1152000); + + // Save tty settings, also checking for error + if (tcsetattr(this->port_fd, TCSANOW, &tty) != 0) + { + std::stringstream ss; + ss << "Error " << std::to_string(errno) << " from tcsetattr: " << strerror(errno); + return ss.str(); + } + this->is_connected = true; + + return std::string(); +} + +/** + * @brief Sends a heart beat packet to the STM32 board + * + * @return whether the write operation is successful + */ +int ControlCommunicator::send_heart_beat_packet() { + PackageOut package; + package.frame_id = 0xAA; + package.frame_type = FRAME_TYPE_HEART_BEAT; + package.heartBeatPackage._a = 0xAA; + package.heartBeatPackage._b = 0xAA; + package.heartBeatPackage._c = 0xAA; + package.heartBeatPackage._d = 0xAA; + int success = write(this->port_fd, &package, sizeof(PackageOut)); + fsync(this->port_fd); + + if (success == -1) { + this->is_connected = false; + } + + return success; +} + +/** + * @brief Sends an auto aim packet to the STM32 board + * + * @return whether the write operation is successful + */ +int ControlCommunicator::send_auto_aim_packet(float yaw, float pitch, bool fire) { + PackageOut package; + package.frame_id = 0xAA; + package.frame_type = FRAME_TYPE_AUTO_AIM; + package.autoAimPackage.yaw = yaw; + package.autoAimPackage.pitch = pitch; + package.autoAimPackage.fire = fire; + int success = write(this->port_fd, &package, sizeof(PackageOut)); + fsync(this->port_fd); + + return success; +} + +/** + * @brief Sends a nav packet to the STM32 board + * + * @return whether the write operation is successful + */ +int ControlCommunicator::send_nav_packet(float x_vel, float y_vel, float yaw_rad, uint8_t state) { + PackageOut package; + package.frame_id = 0xAA; + package.frame_type = FRAME_TYPE_NAV; + package.navPackage.x_vel = x_vel; + package.navPackage.y_vel = y_vel; + package.navPackage.yaw_rad = yaw_rad; + package.navPackage.state = state; + int success = write(this->port_fd, &package, sizeof(PackageOut)); + fsync(this->port_fd); + + return success; +} + +/** + * @brief Attempts to align packet reading + * + * @return true if the packet is successfully aligned + * @return false if the packet fails to align + */ +bool ControlCommunicator::read_alignment() { + uint8_t i = 0; + uint8_t buffer[32]; + do + { + int success = read(this->port_fd, &(buffer[0]), sizeof(buffer[0])); + if (success) i++; + } while (buffer[0] != 0xAA || i > sizeof(PackageIn) * 2); + read(this->port_fd, &buffer, sizeof(PackageIn) - 1); + + return i <= sizeof(PackageIn) * 2; +} + +/** + * @brief Reads in a packet from the STM32 board + * + * @return A tuple containing the error message (empty if successful) and the + * package struct read in + */ +std::tuple ControlCommunicator::read_package() { + PackageIn package; + int success = read(this->port_fd, &package, sizeof(PackageIn)); + + if (success == -1) { + std::stringstream ss; + ss << "Error " << errno << " from read: " << strerror(errno); + return std::tuple(ss.str(), package); + } + + // Package validation + if (package.head != 0xAA) { + return this->read_alignment() + ? std::tuple("Read alignment success.", package) + : std::tuple("Read alignment failed.", package); + } + + return std::tuple(std::string(), package); +} \ 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 new file mode 100644 index 0000000..5bb816c --- /dev/null +++ b/src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp @@ -0,0 +1,252 @@ +#include "ControlCommunicatorNode.hpp" + +using namespace std::chrono_literals; +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/control_communicator/pitch_lookup_table.txt"); + + this->pitch_lookup_model = PitchLookupModel(this->lookup_table_path); + this->control_communicator = ControlCommunicator(port); + + if (this->control_communicator.read_alignment()) + { + RCLCPP_INFO(this->get_logger(), "Inital Read alignment success."); + } + else + { + RCLCPP_WARN(this->get_logger(), "Inital Read alignment failed."); + } + + RCLCPP_INFO(this->get_logger(), "should have printed."); + + this->tf_broadcaster = std::make_unique(*this); + this->tf_static_broadcaster = std::make_shared(this); + this->tf_buffer = std::make_unique(this->get_clock()); + this->tf_listener = std::make_shared(*tf_buffer); + this->heart_beat_timer = this->create_wall_timer(1000ms, std::bind(&ControlCommunicatorNode::heart_beat_handler, this)); + + this->auto_aim_subscriber = this->create_subscription( + "predicted_armor", + 1, + std::bind(&ControlCommunicatorNode::auto_aim_handler, this, _1) + ); + + // should not be twiststamped + this->nav_subscriber = this->create_subscription( + "cmd_vel", + 1, + std::bind(&ControlCommunicatorNode::nav_handler, this, _1) + ); + + this->odometry_publisher = this->create_publisher("odom", 1); + this->target_robot_color_publisher = this->create_publisher("color_set", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); + 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)); + + publish_static_tf(158.7f / 1000.f, 0.f / 1000.f, 47.5 / 1000.f, 0, 0, 0, "pitch_link", "camera_link"); + publish_static_tf(0, 0, 478.f / 1000.f, 0, 0, 0, "base_link", "yaw_link"); + publish_static_tf(0, 0, 0, 0, 0, 0, "base_footprint", "base_link"); + publish_static_tf(0, 0, 0.3, 0, 0, 0, "base_link", "laser"); + + RCLCPP_INFO(this->get_logger(), "Control Communicator Node Started."); +} + +ControlCommunicatorNode::~ControlCommunicatorNode() +{ +} + +void ControlCommunicatorNode::publish_static_tf(float x, float y, float z, float roll, float pitch, float yaw, const char *frame_id, const char *child_frame_id) +{ + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = frame_id; + t.child_frame_id = child_frame_id; + + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + tf_static_broadcaster->sendTransform(t); +} + +void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr msg) +{ + if (!this->control_communicator.connected() || this->control_communicator.get_port_fd() < 0) + { + RCLCPP_WARN(this->get_logger(), "UART Not connected, ignoring aim message %d.", this->auto_aim_frame_id++); + return; + } + + float pitch = this->pitch_lookup_model.get_pitch(msg->x / 1000, msg->z / 1000); + float yaw = -atan(msg->y / msg->x) * 180 / PI; + + this->auto_aim_frame_id++; + this->control_communicator.send_auto_aim_packet(yaw, pitch, msg->fire); + if (this->auto_aim_frame_id % 1000 == 0) + { + RCLCPP_INFO(this->get_logger(), "Yaw, Pitch: %.3f, %.3f, FIRE=%.3f", yaw, pitch, msg->fire); + } + RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent."); +} + +void ControlCommunicatorNode::nav_handler(const std::shared_ptr msg) +{ + if (!this->control_communicator.connected() || this->control_communicator.get_port_fd() < 0) + { + RCLCPP_WARN(this->get_logger(), "UART Not connected, ignoring nav message %d.", this->nav_frame_id++); + return; + } + + this->control_communicator.send_nav_packet(msg->linear.x, msg->linear.y, msg->angular.z, 1); + RCLCPP_INFO(this->get_logger(), "x_vel = %f, y_vel = %f, yaw = %f", msg->linear.x, msg->linear.y, msg->angular.z); +} + +void ControlCommunicatorNode::heart_beat_handler() +{ + if (!this->control_communicator.connected() || this->control_communicator.get_port_fd() < 0) + { + RCLCPP_WARN(this->get_logger(), "UART Not connected, trying to reconnect."); + std::string error_message = this->control_communicator.start_uart(); + if (error_message.length() > 0) { + RCLCPP_ERROR(this->get_logger(), error_message); + } + } + + int success = this->control_communicator.send_heart_beat_packet(); + if (success == -1) + { + RCLCPP_ERROR(this->get_logger(), "Erro r %i from write: %s", errno, strerror(errno)); + std::string error_message = this->control_communicator.start_uart(); + if (error_message.length() > 0) { + RCLCPP_ERROR(this->get_logger(), error_message); + } + } + if (this->heart_beat_frame_id % 10 == 0) + { + RCLCPP_INFO(this->get_logger(), "Heart Beat %d", this->heart_beat_frame_id); + } +} + +void ControlCommunicatorNode::read_uart() +{ + rclcpp::Time curr_time = this->now(); + std::tuple response = this->control_communicator.read_package(); + std::string error_message = std::get<0>(response); + PackageIn package = std::get<1>(response); + + if (error_message.length() > 0) { + RCLCPP_INFO(this->get_logger(), error_message); + return; + } + + // Handle TF + this->pitch_vel = package.pitch_vel; // rad/s + this->pitch = package.pitch; // rad + this->yaw_vel = package.yaw_vel; // rad/s + this->is_red = package.ref_flags & 2; // second lowest denotes if enemy is red + this->is_match_running = package.ref_flags & 1; // LSB denotes if match is started + + // publishing color and match status + std_msgs::msg::String target_robot_color; + target_robot_color.data = this->is_red ? "red" : "blue"; + // target_robot_color_publisher->publish(target_robot_color); + std_msgs::msg::Bool match_status; + match_status.data = this->is_match_running; + // match_status_publisher->publish(match_status); + + geometry_msgs::msg::TransformStamped pitch_tf; + pitch_tf.header.stamp = curr_time; + pitch_tf.header.frame_id = "yaw_link"; + pitch_tf.child_frame_id = "pitch_link"; + pitch_tf.transform.translation.x = 0; + pitch_tf.transform.translation.y = 0; + pitch_tf.transform.translation.z = 0; + + tf2::Quaternion pitch_q; + pitch_q.setRPY(0, this->pitch, 0); + pitch_tf.transform.rotation.x = pitch_q.x(); + pitch_tf.transform.rotation.y = pitch_q.y(); + pitch_tf.transform.rotation.z = pitch_q.z(); + pitch_tf.transform.rotation.w = pitch_q.w(); + + tf_broadcaster->sendTransform(pitch_tf); + + // Handle Odom + geometry_msgs::msg::TransformStamped odom_tf; + nav_msgs::msg::Odometry odom; + + odom_tf.header.stamp = curr_time; + odom_tf.header.frame_id = "odom"; + odom_tf.child_frame_id = "base_footprint"; + odom_tf.transform.translation.x = package.x; + odom_tf.transform.translation.y = package.y; + odom_tf.transform.translation.z = 0; + + odom.header.stamp = curr_time; + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_footprint"; + odom.pose.pose.position.x = package.x; + odom.pose.pose.position.y = package.y; + odom.pose.pose.position.z = 0; + + tf2::Quaternion odom_q; + odom_q.setRPY(0, 0, package.orientation); + + odom_tf.transform.rotation.x = odom_q.x(); + odom_tf.transform.rotation.y = odom_q.y(); + odom_tf.transform.rotation.z = odom_q.z(); + odom_tf.transform.rotation.w = odom_q.w(); + + odom.pose.pose.orientation.x = odom_q.x(); + odom.pose.pose.orientation.y = odom_q.y(); + odom.pose.pose.orientation.z = odom_q.z(); + odom.pose.pose.orientation.w = odom_q.w(); + + odom.pose.covariance[0] = 0.01; + odom.pose.covariance[7] = 0.01; + odom.pose.covariance[14] = 1e13; + odom.pose.covariance[21] = 1e13; + odom.pose.covariance[28] = 1e13; + odom.pose.covariance[35] = 0.01; + + odom.twist.twist.linear.x = package.x_vel; + odom.twist.twist.linear.y = package.y_vel; + odom.twist.twist.linear.z = 0; + odom.twist.twist.angular.x = 0; + odom.twist.twist.angular.y = 0; + odom.twist.twist.angular.z = this->yaw_vel; + + odom.twist.covariance[0] = 0.01; + odom.twist.covariance[7] = 0.01; + odom.twist.covariance[14] = 1e13; + odom.twist.covariance[21] = 1e13; + odom.twist.covariance[28] = 1e13; + odom.twist.covariance[35] = 0.01; + + tf_broadcaster->sendTransform(odom_tf); + this->odometry_publisher->publish(odom); + recive_frame_id++; + + return; +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("/dev/ttyTHS0"); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} 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..fdb579e --- /dev/null +++ b/src/prm_control/control_communicator/src/PitchLookupModel.cpp @@ -0,0 +1,133 @@ +#include "PitchLookupModel.hpp" + +PitchLookupModel::PitchLookupModel() {} + +PitchLookupModel::PitchLookupModel(std::string filename) { + this->load_file(filename); +} + +/** + * @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 x coordinates (distance from the camera) + * and N represents the number of z coordinates (height above the ground). + * The next M x N lines contain three space-separated numbers: + * x coordinate, z coordinate, and pitch. Length measurements are in + * millimeters, and angle measurements are in degrees. + */ +void PitchLookupModel::load_file(std::string filename) { + FILE* fp = fopen(filename.c_str(), "r"); + if (fp == nullptr) { + return; + } + + int x_count = 0; + int z_count = 0; + fscanf(fp, "%d %d\n", &x_count, &z_count); + + this->pitch_lookup_table = std::vector>( + x_count, std::vector(z_count, 0)); + + for (int row = 0; row < x_count; row++) { + for (int col = 0; col < z_count; col++) { + int x = 0, z = 0; + float pitch = 0; + int read_count = fscanf(fp, "%d %d %f\n", &x, &z, &pitch); + + if (read_count != 3) { + fclose(fp); + return; + } + + if (x < this->lower_x) this->lower_x = x; + if (x > this->upper_x) this->upper_x = x; + if (z < this->lower_z) this->lower_z = z; + if (z > this->upper_z) this->upper_z = z; + + this->pitch_lookup_table[row][col] = pitch; + } + } + + fclose(fp); +} + +/** + * @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_x - this->lower_x) / + (this->pitch_lookup_table.size() - 1); + + // Calculate the indices of the tightest bound containing distance and height + int lower_step_x_idx = (distance - this->lower_x) / step_size; + int upper_step_x_idx = (distance - this->lower_x + step_size - 1) / step_size; + int lower_step_z_idx = (height - this->lower_z) / step_size; + int upper_step_z_idx = (height - this->lower_z + step_size - 1) / step_size; + + // Calculate the actual values of the tightest bounds + int lower_step_x = this->lower_x + step_size * lower_step_x_idx; + int upper_step_x = this->lower_x + step_size * upper_step_x_idx; + 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; + + // Calculate the pitch of the four points with tightest bounds + float pitch_low_low = this->pitch_lookup_table[lower_step_x_idx][lower_step_z_idx]; + float pitch_low_high = this->pitch_lookup_table[lower_step_x_idx][upper_step_z_idx]; + float pitch_high_low = this->pitch_lookup_table[upper_step_x_idx][lower_step_z_idx]; + float pitch_high_high = this->pitch_lookup_table[upper_step_x_idx][upper_step_z_idx]; + + // Calculate pitch of the corresponding x position by fixing z to the lower bound + float pitch_z_low = this->map( + static_cast(distance), + static_cast(lower_step_x), + static_cast(upper_step_x), + std::fmin(pitch_low_low, pitch_high_low), + std::fmax(pitch_low_low, pitch_high_low)); + + // Calculate pitch of the corresponding x position by fixing z to the upper bound + float pitch_z_high = this->map( + static_cast(distance), + static_cast(lower_step_x), + static_cast(upper_step_x), + 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_z), + static_cast(upper_step_z), + std::fmin(pitch_z_low, pitch_z_high), + std::fmax(pitch_z_low, pitch_z_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_max The maximum of the old range + * @param new_min The minimum of the new range + * @param new_max The maximum of the new range + * + * @return Mapped value + */ +float PitchLookupModel::map(float value, float old_min, float old_max, + float new_min, float new_max) { + if (old_min > old_max || new_min > new_max) { + return -1; + } + + if (old_min == old_max) { + return (new_min == new_max) ? new_min : -1; + } + + return new_min + (value - old_min) * (new_max - new_min) / (old_max - old_min); +} diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index a7857e3..c41cf58 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -7,7 +7,7 @@ def generate_launch_description(): webcam_publisher = get_package_share_path('webcam_publisher') - video_path = "/home/tom/Videos/far_back_spin_and_move.avi" # example, can change to your liking + video_path = "/home/user-accounts/public/spintop/spinning_in_place.avi" return LaunchDescription([ Node( package='webcam_publisher', @@ -22,5 +22,13 @@ def generate_launch_description(): Node( package='opencv_armor_detector', executable='OpenCVArmorDetectorNode', + ), + Node( + package='pnp_solver', + executable='PNPSolverNode', + ), + Node( + package='control_communicator', + executable='ControlCommunicatorNode', ) ]) diff --git a/src/prm_vision/opencv_armor_detector/CMakeLists.txt b/src/prm_vision/opencv_armor_detector/CMakeLists.txt index 1969e3b..62f028f 100644 --- a/src/prm_vision/opencv_armor_detector/CMakeLists.txt +++ b/src/prm_vision/opencv_armor_detector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(opencv_armor_detector CXX CUDA) +project(opencv_armor_detector) # Dependencies find_package(ament_cmake REQUIRED) diff --git a/src/prm_vision/pnp_solver/CMakeLists.txt b/src/prm_vision/pnp_solver/CMakeLists.txt new file mode 100644 index 0000000..f6fadfc --- /dev/null +++ b/src/prm_vision/pnp_solver/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.5) +project(pnp_solver) + + +include_directories(/usr/include) +include_directories(include) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(OpenCV 4.6.0 REQUIRED) +find_package(Eigen3 REQUIRED) + +add_library(KalmanFilter STATIC src/KalmanFilter.cpp) +target_link_libraries(KalmanFilter Eigen3::Eigen) + +add_library(ValidityFilter STATIC src/ValidityFilter.cpp) + +add_library(PNPSolver STATIC src/PNPSolver.cpp) +ament_target_dependencies(PNPSolver + OpenCV +) + +add_executable(PNPSolverNode + src/PNPSolverNode.cpp +) +target_link_libraries(PNPSolverNode PNPSolver KalmanFilter ValidityFilter) +ament_target_dependencies(PNPSolverNode + rclcpp + vision_msgs + geometry_msgs + tf2 + tf2_ros + OpenCV +) + +install( + TARGETS PNPSolverNode + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_dependencies(rclcpp rclcpp vision_msgs geometry_msgs tf2) + +ament_package() diff --git a/src/prm_vision/pnp_solver/include/KalmanFilter.hpp b/src/prm_vision/pnp_solver/include/KalmanFilter.hpp new file mode 100644 index 0000000..6417518 --- /dev/null +++ b/src/prm_vision/pnp_solver/include/KalmanFilter.hpp @@ -0,0 +1,51 @@ +#ifndef _KALMAN_FILTER_HPP +#define _KALMAN_FILTER_HPP + +#include +#include + +class KalmanFilter +{ +public: + KalmanFilter(float, float, float, float, float, float); + + KalmanFilter(); + + ~KalmanFilter(); + + void update(float, float, float, float, float[]); + + void reset(); + +private: + + + float q_0; + float p_0; + float r; + float x_0; // in mm + float y_0; // in mm + float z_0; // in mm + + Eigen::Matrix A; + Eigen::Matrix H; + Eigen::Matrix Q; + Eigen::Matrix P; + Eigen::Matrix R; + Eigen::Matrix X; + + Eigen::Matrix X_t_measure; + + Eigen::Matrix X_hat_t; + Eigen::Matrix P_hat_t; + + Eigen::Matrix y_t; + Eigen::Matrix z_t; + + Eigen::Matrix s_t; + Eigen::Matrix K; + + +}; + +#endif // _KALMAN_FILTER_HPP \ No newline at end of file diff --git a/src/prm_vision/pnp_solver/include/PNPSolver.hpp b/src/prm_vision/pnp_solver/include/PNPSolver.hpp new file mode 100644 index 0000000..2cc401c --- /dev/null +++ b/src/prm_vision/pnp_solver/include/PNPSolver.hpp @@ -0,0 +1,37 @@ +#ifndef _PNP_SOLVER_HPP +#define _PNP_SOLVER_HPP + +#include +#include +#include + +#include +#include + +#define LIGHTBAR_HALF_HEIGHT 54.f / 2.f +#define SMALL_ARMOR_HALF_WIDTH 134.f / 2.f +#define LARGE_ARMOR_HALF_WIDTH 225.f / 2.f + +typedef struct Coordinates { + std::vector tvec; + std::vector rvec; +} Coordinates; + +class PNPSolver +{ +public: + PNPSolver(); + ~PNPSolver(); + + Coordinates getArmorCoordinates(std::vector points, int points_size, bool large_armor); +private: + std::vector small_armor_object_points; + std::vector large_armor_object_points; + cv::Mat distortion_coefficient; + cv::Mat camera_matrix; + + cv::Mat rvec; + cv::Mat tvec; +}; + +#endif // _PNP_SOLVER_HPP diff --git a/src/prm_vision/pnp_solver/include/PNPSolverNode.hpp b/src/prm_vision/pnp_solver/include/PNPSolverNode.hpp new file mode 100644 index 0000000..a01d08f --- /dev/null +++ b/src/prm_vision/pnp_solver/include/PNPSolverNode.hpp @@ -0,0 +1,66 @@ +#include +#include + +#include +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "vision_msgs/msg/key_points.hpp" +#include "vision_msgs/msg/predicted_armor.hpp" +#include "std_msgs/msg/string.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +#include "PNPSolver.hpp" +#include "KalmanFilter.hpp" +#include "ValidityFilter.hpp" + +#define PI 3.141592653589793238462643383 + +#define X_OFFSET 0 // Right of sensor +#define Y_OFFSET 0 // Down of sensor +#define Z_OFFSET 0 // Out of sensor + +using namespace std::chrono_literals; + +class PNPSolverNode : public rclcpp::Node +{ +public: + PNPSolverNode(const rclcpp::NodeOptions &); + ~PNPSolverNode(); + +private: + rclcpp::Subscription::SharedPtr key_points_subscriber; + + std::unique_ptr tf_broadcaster; + std::shared_ptr> predicted_armor_publisher; + std::shared_ptr> auto_aim_tracking_status_publisher = NULL; + + uint32_t seq_; + + PNPSolver pnp_solver_; + KalmanFilter kalman_filter_; + ValidityFilter validity_filter_; + + rclcpp::Time prev_time_; + + bool publish_tf_; + float dst_[6]; + + int locked_in_frames = 0; + int num_frames_to_fire_after = 10; // corresponds to approximately 2/3 of a second + double last_fire_time = 0; + + rclcpp::TimerBase::SharedPtr firing_timer; + void keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr); + void validateCoordinates(vision_msgs::msg::PredictedArmor* predicted_armor_msg, std_msgs::msg::Header header, std::vector tvec); + void publishPredictedArmor(vision_msgs::msg::PredictedArmor* predicted_armor_msg, std_msgs::msg::Header header, std::vector tvec, std::vector rvec); + void publishTF(std_msgs::msg::Header header, std::vector tvec); + void publishZeroPredictedArmor(std_msgs::msg::Header header); + void check_last_firing_time(); +}; diff --git a/src/prm_vision/pnp_solver/include/ValidityFilter.hpp b/src/prm_vision/pnp_solver/include/ValidityFilter.hpp new file mode 100644 index 0000000..4bc0d37 --- /dev/null +++ b/src/prm_vision/pnp_solver/include/ValidityFilter.hpp @@ -0,0 +1,55 @@ +#ifndef _RESULT_FILTER_HPP_ +#define _RESULT_FILTER_HPP_ + +#include +#include +#include +#include + +enum ValidityFilterState +{ + TRACKING, + IDLING, + STOPPING, +}; + + +class ValidityFilter +{ +public: + ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len); + ValidityFilter(); + ~ValidityFilter(); + + bool validation(float, float, float, double); + int get_lock_in_counter(); + + ValidityFilterState state = STOPPING; + +private: + + + int lock_in_after = 3; // lock in after n frames + int lock_in_counter = 0; + + float max_distance = 10000.f; // mm + float min_distance = 10.f; // mm + + double max_dt = 2000; // ms + + int prev_len = 5; // check the back n frams for max shift distance vilolation + + float prev_coordinates[20][3]; + int prev_idx = 0; + + float max_shift_distance = 150.f; // mm + + void update_prev(float, float, float); + int position_validity(float, float, float); + bool distance_validity(float, float, float); + void increment_lock_in_counter(); + void decrement_lock_in_counter(); + void reset_lock_in_counter(); +}; + +#endif // _RESULT_FILTER_HPP_ \ No newline at end of file diff --git a/src/prm_vision/pnp_solver/package.xml b/src/prm_vision/pnp_solver/package.xml new file mode 100644 index 0000000..b1571ac --- /dev/null +++ b/src/prm_vision/pnp_solver/package.xml @@ -0,0 +1,23 @@ + + + pnp_solver + 0.0.1 + Module written for Purdue Robomaster to calculate 3D armor position of RoboMaster Robots. + + Tom O'Donnell + Apache-2.0 + + ament_cmake + sensor_msgs + image_transport + rclcpp + cv_bridge + vision_msgs + + + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/prm_vision/pnp_solver/src/KalmanFilter.cpp b/src/prm_vision/pnp_solver/src/KalmanFilter.cpp new file mode 100644 index 0000000..b0a4c6f --- /dev/null +++ b/src/prm_vision/pnp_solver/src/KalmanFilter.cpp @@ -0,0 +1,70 @@ +#include "KalmanFilter.hpp" + +KalmanFilter::KalmanFilter(float q_0, float p_0, float r, float x_0, float y_0, float z_0) : q_0(q_0), p_0(p_0), r(r), x_0(x_0), y_0(y_0), z_0(z_0) +{ + this->A = Eigen::Matrix::Identity(); + + this->H = Eigen::Matrix::Zero(); + this->H(0, 0) = 1; + this->H(1, 1) = 1; + this->H(2, 2) = 1; + + this->Q = Eigen::Matrix::Identity() * this->q_0; + this->R = Eigen::Matrix::Identity() * this->r; + this->X_t_measure = Eigen::Matrix::Zero(); + + this->reset(); +} + +// q0, p0, r = process (kinematic) noise, state covariance, measurement noise +KalmanFilter::KalmanFilter() : KalmanFilter(10, 10000, 20, 0, 0, 0) {} + +KalmanFilter::~KalmanFilter() +{ + return; +} + +void KalmanFilter::reset() +{ + + this->X = Eigen::Matrix::Zero(); + this->X(0, 0) = this->x_0; + this->X(1, 0) = this->y_0; + this->X(2, 0) = this->z_0; + this->X(3, 0) = 0.f; + this->X(4, 0) = 0.f; + this->X(5, 0) = 0.f; + + this->P = Eigen::Matrix::Identity() * this->p_0; +} + +void KalmanFilter::update(float pos_x, float pos_y, float pos_z, float dt, float dst[6]) +{ + + this->X_t_measure(0, 0) = pos_x; + this->X_t_measure(1, 0) = pos_y; + this->X_t_measure(2, 0) = pos_z; + + this->A(0, 3) = dt; + this->A(1, 4) = dt; + this->A(2, 5) = dt; + + this->X_hat_t = this->A * this->X; + this->P_hat_t = this->A * this->P * this->A.transpose() + this->Q; + + this->z_t = Eigen::Matrix::Identity() * this->X_t_measure; + this->y_t = this->z_t - this->H * this->X_hat_t; + + this->s_t = this->H * this->P_hat_t * this->H.transpose() + this->R; + this->K = this->P_hat_t * this->H.transpose() * this->s_t.inverse(); + + this->P = (Eigen::Matrix::Identity() - this->K * this->H) * this->P_hat_t; + this->X = this->X_hat_t + this->K * this->y_t; + + dst[0] = this->X(0, 0); + dst[1] = this->X(1, 0); + dst[2] = this->X(2, 0); + dst[3] = this->X(3, 0); + dst[4] = this->X(4, 0); + dst[5] = this->X(5, 0); +} diff --git a/src/prm_vision/pnp_solver/src/PNPSolver.cpp b/src/prm_vision/pnp_solver/src/PNPSolver.cpp new file mode 100644 index 0000000..f70b63b --- /dev/null +++ b/src/prm_vision/pnp_solver/src/PNPSolver.cpp @@ -0,0 +1,75 @@ +#include "PNPSolver.hpp" + +PNPSolver::PNPSolver() +{ + // +X: Out, +Y: Right, +Z: Up + small_armor_object_points.emplace_back(cv::Point3f(0, -SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT)); // Top Left + small_armor_object_points.emplace_back(cv::Point3f(0, -SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT)); // Bot Left + small_armor_object_points.emplace_back(cv::Point3f(0, SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT)); // Top Right + small_armor_object_points.emplace_back(cv::Point3f(0, SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT)); // Bot Right + + large_armor_object_points.emplace_back(cv::Point3f(0, -LARGE_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT)); // Top Left + large_armor_object_points.emplace_back(cv::Point3f(0, -LARGE_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT)); // Bot Left + large_armor_object_points.emplace_back(cv::Point3f(0, LARGE_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT)); // Top Righy + large_armor_object_points.emplace_back(cv::Point3f(0, LARGE_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT)); // Bot Right + + camera_matrix = (cv::Mat_(3, 3) << + 1019.108731f, 0.0f, 601.884969f, + 0.0f, 1016.784980f, 521.004587f, + 0.0f, 0.0f, 1.0f + ); + + distortion_coefficient = (cv::Mat_(4, 1) << + -0.108767f, + -0.072085f, + -0.000847f, + 0.0f + ); + + rvec = cv::Mat(3, 1, CV_32F); + tvec = cv::Mat(3, 1, CV_32F); +} + +PNPSolver::~PNPSolver() +{ +} + +/** + * @brief Perform PNP solving with image points + * + * @param points image points + * @param points_size number of items in the image points vector + * @param large_armor whether the armor is a large armor + * @return tvec and rvec of the pnp solution + */ +Coordinates PNPSolver::getArmorCoordinates(std::vector points, int points_size, bool large_armor) +{ + if (points_size < 8 || points[0] == 0) + { + return (Coordinates){}; + } + std::vector image_points; + image_points.push_back(cv::Point2f(points[0], points[1])); + image_points.push_back(cv::Point2f(points[2], points[3])); + image_points.push_back(cv::Point2f(points[4], points[5])); + image_points.push_back(cv::Point2f(points[6], points[7])); + bool result = cv::solvePnP(small_armor_object_points, image_points, camera_matrix, distortion_coefficient, rvec, tvec, false, cv::SOLVEPNP_IPPE); + + if (!result) + { + return (Coordinates){}; + } + + float Y = -tvec.at(0, 0); + float Z = -tvec.at(1, 0); + float X = tvec.at(2, 0); + tvec.at(0, 0) = X; + tvec.at(1, 0) = Y; + tvec.at(2, 0) = Z; + + Coordinates output; + output.tvec = {X, Y, Z}; + output.rvec = {rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0)}; + + return output; +} diff --git a/src/prm_vision/pnp_solver/src/PNPSolverNode.cpp b/src/prm_vision/pnp_solver/src/PNPSolverNode.cpp new file mode 100644 index 0000000..6af8914 --- /dev/null +++ b/src/prm_vision/pnp_solver/src/PNPSolverNode.cpp @@ -0,0 +1,293 @@ +#include "PNPSolverNode.hpp" + +PNPSolverNode::PNPSolverNode(const rclcpp::NodeOptions &options) : Node("pnp_solver", options) +{ + publish_tf_ = this->declare_parameter("publish_tf", false); + publish_tf_ = true; + RCLCPP_INFO(this->get_logger(), "Publish TF: %s", publish_tf_ ? "true" : "false"); + + pnp_solver_ = PNPSolver(); + kalman_filter_ = KalmanFilter(); + validity_filter_ = ValidityFilter(); + + seq_ = 0; + + prev_time_ = this->now(); + + if (publish_tf_) + { + tf_broadcaster = std::make_unique(*this); + } + + // STOP FIRING if no target detected for 2 seconds + this->firing_timer = this->create_wall_timer(500ms, std::bind(&PNPSolverNode::check_last_firing_time, this)); + + // Initialize subscriber and publisher for predicted armor, key points, and auto aim tracking status + predicted_armor_publisher = this->create_publisher("predicted_armor", 10); + auto_aim_tracking_status_publisher = this->create_publisher("auto_aim_tracking_status", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); + key_points_subscriber = this->create_subscription( + "key_points", 10, std::bind(&PNPSolverNode::keyPointsCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "PNP Solver Created"); +} + +PNPSolverNode::~PNPSolverNode() +{ + RCLCPP_INFO_ONCE(this->get_logger(), "Destorying PNP Solver Node"); + return; +} + +/** + * @brief Validates the translational coordinates, determines the status of auto aiming, and resets the + * Kalman filter accordingly + * + * @param predicted_armor_msg The reference of the predicted armor message object + * @param header The header to be published + * @param tvec The translatoinal coordinates + */ +void PNPSolverNode::validateCoordinates(vision_msgs::msg::PredictedArmor *predicted_armor_msg, std_msgs::msg::Header header, std::vector tvec) +{ + std_msgs::msg::String auto_aim_tracking_status_msg; + rclcpp::Time curr_time_(header.stamp.sec, header.stamp.nanosec, RCL_ROS_TIME); + double dt = (curr_time_.seconds() - prev_time_.seconds()); + + bool reset_kalman = validity_filter_.validation(tvec[0], tvec[1], tvec[2], dt); + if (reset_kalman) + { + auto_aim_tracking_status_msg.data = "RESET KALMAN"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + kalman_filter_.reset(); + RCLCPP_INFO(this->get_logger(), "Validity Filter Reset Kalman Filter"); + prev_time_ = curr_time_; + + predicted_armor_msg->x_vel = 0; + predicted_armor_msg->y_vel = 0; + predicted_armor_msg->z_vel = 0; + } + else + { + predicted_armor_msg->x_vel = dst_[0]; + predicted_armor_msg->y_vel = dst_[1]; + predicted_armor_msg->z_vel = dst_[2]; + } + + // Stopping motor + if (validity_filter_.state == STOPPING) + { + auto_aim_tracking_status_msg.data = "STOPPING"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + publishZeroPredictedArmor(header); + locked_in_frames = 0; + return; + } + + // Idling, dont send anything + else if (validity_filter_.state == IDLING) + { + auto_aim_tracking_status_msg.data = "IDLING"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + locked_in_frames = 0; + return; + } + + // Tracking, update kalman filter + else if (validity_filter_.state == TRACKING) + { + // if tracking and we have locked in for enough frames, fire at the target + if (locked_in_frames > num_frames_to_fire_after) + { + auto_aim_tracking_status_msg.data = "FIRE"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + last_fire_time = this->now().seconds(); + } + else + { + auto_aim_tracking_status_msg.data = "TRACKING"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + } + + kalman_filter_.update(tvec[0], tvec[1], tvec[2], dt, dst_); + prev_time_ = curr_time_; + locked_in_frames++; + } +} + +/** + * @brief Publishes the predicted armor as a ROS message + * + * @param predicted_armor_msg The reference of the predicted armor message object + * @param header The header to be published + * @param tvec The translational coordinates + * @param rvec The rotational coordinates + */ +void PNPSolverNode::publishPredictedArmor(vision_msgs::msg::PredictedArmor *predicted_armor_msg, std_msgs::msg::Header header, std::vector tvec, std::vector rvec) +{ + predicted_armor_msg->header = header; + double distance = sqrt(tvec[0] * tvec[0] + tvec[1] * tvec[1] + tvec[2] * tvec[2]); + predicted_armor_msg->x = tvec[0]; + predicted_armor_msg->y = tvec[1] + 50; + predicted_armor_msg->z = tvec[2] - 60; // 50mm coordinate transform in Z // + (0.1*distance + -127.75); + + // fire at the target + predicted_armor_msg->fire = locked_in_frames > num_frames_to_fire_after; + + // Rotation XYZ + // NOTE: Eventually this should be passed into the kalman filter and predicted_rvec should be published instead + predicted_armor_msg->rvec_x = rvec[0]; + predicted_armor_msg->rvec_y = rvec[1]; + predicted_armor_msg->rvec_z = rvec[2]; + predicted_armor_publisher->publish(*predicted_armor_msg); +} + +/** + * @brief Publishes the transformation of detected, filtered, and predicted armor, as a ROS message + * + * @param header The header to be published + * @param tvec The translational coordinates + */ +void PNPSolverNode::publishTF(std_msgs::msg::Header header, std::vector tvec) +{ + // Publish TF raw results + geometry_msgs::msg::TransformStamped detected_armor_tf; + detected_armor_tf.header.stamp = header.stamp; + detected_armor_tf.header.frame_id = "camera_link"; + detected_armor_tf.child_frame_id = "detected_armor"; + detected_armor_tf.transform.translation.x = tvec[0] / 1000; + detected_armor_tf.transform.translation.y = tvec[1] / 1000; + detected_armor_tf.transform.translation.z = tvec[2] / 1000; + detected_armor_tf.transform.rotation.x = 1; + detected_armor_tf.transform.rotation.y = 0; + detected_armor_tf.transform.rotation.z = 0; + detected_armor_tf.transform.rotation.w = 0; + tf_broadcaster->sendTransform(detected_armor_tf); + + // Publish TF filtered results + geometry_msgs::msg::TransformStamped filtered_armor_tf; + filtered_armor_tf.header.stamp = header.stamp; + filtered_armor_tf.header.frame_id = "camera_link"; + filtered_armor_tf.child_frame_id = "filtered_armor"; + filtered_armor_tf.transform.translation.x = dst_[0] / 1000; + filtered_armor_tf.transform.translation.y = dst_[1] / 1000; + filtered_armor_tf.transform.translation.z = dst_[2] / 1000; + filtered_armor_tf.transform.rotation.x = 1; + filtered_armor_tf.transform.rotation.y = 0; + filtered_armor_tf.transform.rotation.z = 0; + filtered_armor_tf.transform.rotation.w = 0; + tf_broadcaster->sendTransform(filtered_armor_tf); + + // Publish TF predictedion after 0.3s + geometry_msgs::msg::TransformStamped predicted_armor_tf; + predicted_armor_tf.header.stamp = header.stamp; + predicted_armor_tf.header.frame_id = "camera_link"; + predicted_armor_tf.child_frame_id = "predicted_armor"; + predicted_armor_tf.transform.translation.x = (dst_[0] + dst_[3] * 0.5) / 1000; + predicted_armor_tf.transform.translation.y = (dst_[1] + dst_[4] * 0.5) / 1000; + predicted_armor_tf.transform.translation.z = (dst_[2] + dst_[5] * 0.5) / 1000; + predicted_armor_tf.transform.rotation.x = 1; + predicted_armor_tf.transform.rotation.y = 0; + predicted_armor_tf.transform.rotation.z = 0; + predicted_armor_tf.transform.rotation.w = 0; + tf_broadcaster->sendTransform(predicted_armor_tf); +} + +/** + * @brief Callback function when receiving armor key points from armor detector node + * + * The callback performs PNP solving, kalman filtering, and determine when to fire. + * + * @param key_points_msg Key point messagge + */ +void PNPSolverNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr key_points_msg) +{ + std::vector points; + int points_size = key_points_msg->points.size(); + bool large_armor = key_points_msg->large_armor; + + for (int i = 0; i < 8; i++) + { + points.push_back(key_points_msg->points[i]); + } + + Coordinates coordinates = pnp_solver_.getArmorCoordinates(points, points_size, large_armor); + + if (coordinates.tvec.size() == 0) + { + publishZeroPredictedArmor(key_points_msg->header); + return; + } + + vision_msgs::msg::PredictedArmor predicted_armor_msg; + this->validateCoordinates(&predicted_armor_msg, key_points_msg->header, coordinates.tvec); + this->publishPredictedArmor(&predicted_armor_msg, key_points_msg->header, coordinates.tvec, coordinates.rvec); + + if (publish_tf_) + { + this->publishTF(key_points_msg->header, coordinates.tvec); + } + + RCLCPP_INFO_ONCE(this->get_logger(), "First TF published"); + + if (seq_ % 1000 == 0) + { + RCLCPP_INFO(this->get_logger(), "seq: %d", seq_); + } + seq_++; +} + +/** + * @brief Publishes a message to stop firing if one second have passed the last fire time + * + */ +void PNPSolverNode::check_last_firing_time() +{ + if (this->now().seconds() - last_fire_time > 1 && locked_in_frames > num_frames_to_fire_after) + { + RCLCPP_INFO(this->get_logger(), "Stopping firing"); + std_msgs::msg::String auto_aim_tracking_status_msg; + auto_aim_tracking_status_msg.data = "STOPPING_FIRING"; + auto_aim_tracking_status_publisher->publish(auto_aim_tracking_status_msg); + locked_in_frames = 0; + publishZeroPredictedArmor(std_msgs::msg::Header()); + } +} + +/** + * @brief Publishes a predicted armor message with all transformation initialized to 0 + * + * @param header The header to be published + */ +void PNPSolverNode::publishZeroPredictedArmor(std_msgs::msg::Header header) +{ + vision_msgs::msg::PredictedArmor predicted_armor_msg; + predicted_armor_msg.header = header; + predicted_armor_msg.x = 0; + predicted_armor_msg.y = 0; + predicted_armor_msg.z = 0; + predicted_armor_msg.rvec_x = 0; + predicted_armor_msg.rvec_y = 0; + predicted_armor_msg.rvec_z = 0; + predicted_armor_msg.x_vel = 0; + predicted_armor_msg.y_vel = 0; + predicted_armor_msg.z_vel = 0; + predicted_armor_msg.fire = locked_in_frames > num_frames_to_fire_after; // allows firing to still occur until the target is lost for 2 seconds and locked_in_frames is reset + + predicted_armor_publisher->publish(predicted_armor_msg); +} + +int main(int argc, char *argv[]) +{ + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + + const rclcpp::NodeOptions options; + auto pnp_solver_node = std::make_shared(options); + + exec.add_node(pnp_solver_node); + exec.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/prm_vision/pnp_solver/src/ValidityFilter.cpp b/src/prm_vision/pnp_solver/src/ValidityFilter.cpp new file mode 100644 index 0000000..d43e526 --- /dev/null +++ b/src/prm_vision/pnp_solver/src/ValidityFilter.cpp @@ -0,0 +1,172 @@ +#include "ValidityFilter.hpp" + +ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len) +{ + this->lock_in_after = lock_in_after; + this->max_distance = max_distance; + this->min_distance = min_distance; + this->max_shift_distance = max_shift_distance; + this->prev_len = prev_len < 20 ? prev_len : 20; +} + +ValidityFilter::ValidityFilter() +{ +} + +ValidityFilter::~ValidityFilter() +{ +} + +/** + * @brief Get the lock in counter + * + * @returns number of valid detections + */ +int ValidityFilter::get_lock_in_counter() +{ + return lock_in_counter; +} + +/** + * @brief Updates the coordinates of previous detections + * + * @param x x coordinate + * @param y y coordinate + * @param z z coordinate + */ +void ValidityFilter::update_prev(float x, float y, float z) +{ + this->prev_coordinates[prev_idx][0] = x; + this->prev_coordinates[prev_idx][1] = y; + this->prev_coordinates[prev_idx][2] = z; + prev_idx = (prev_idx + 1) % prev_len; +} + +/** + * @brief Check if the distance of the detection is valid + * + * @param x x coordinate + * @param y y coordinate + * @param z z coordinate + */ +bool ValidityFilter::distance_validity(float x, float y, float z) +{ + float dst = std::sqrt(x * x + y * y + z * z); + return dst <= max_distance && dst >= min_distance; +} + +/** + * @brief Check if the position of the detection is valid + * + * The function compare previous detections with the current detection, + * and count the previous detections that is significantly far from + * the current detection. + * + * @param x x coordinate + * @param y y coordinate + * @param z z coordinate + */ +int ValidityFilter::position_validity(float x, float y, float z) +{ + int num_valid = 0; + + for (int i = 0; i < prev_len; i++) + { + float dx = x - this->prev_coordinates[i][0]; + float dy = y - this->prev_coordinates[i][1]; + float dz = z - this->prev_coordinates[i][2]; + if (std::sqrt(dx * dx + dy * dy + dz * dz) < max_shift_distance) + { + num_valid++; + } + } + return num_valid; +} + +void ValidityFilter::increment_lock_in_counter() +{ + lock_in_counter++; + if (lock_in_counter > lock_in_after) + { + state = TRACKING; + lock_in_counter = lock_in_after; + } else { + state = IDLING; + } +} + +void ValidityFilter::decrement_lock_in_counter() +{ + lock_in_counter--; + if (lock_in_counter != 0) + { + state = IDLING; + } + else + { + state = STOPPING; + lock_in_counter = 0; + } +} + +void ValidityFilter::reset_lock_in_counter() +{ + lock_in_counter = 0; + state = STOPPING; +} + +/** + * @brief Check if whether to reset the kalman filter + * + * @param x x coordinate + * @param y y coordinate + * @param z z coordinate + * @param dt delta time + * + * @returns whether to reset the kalman filter + */ +bool ValidityFilter::validation(float x, float y, float z, double dt) +{ + + // Invalid distance + if (!distance_validity(x, y, z)) + { + decrement_lock_in_counter(); + return state == STOPPING; + } + + // New detection based on time + if (dt > max_dt) + { + reset_lock_in_counter(); + increment_lock_in_counter(); + update_prev(x, y, z); + return true; + } + + int num_valid = position_validity(x, y, z); + + // Invalid detection + if (num_valid == 0) + { + decrement_lock_in_counter(); + update_prev(x, y, z); + return state == STOPPING; + } + + // Valid detection + else + { + increment_lock_in_counter(); + update_prev(x, y, z); + return false; + } + + // New detection based on overwriting prev positions + if (num_valid > prev_len * 0.3) + { + increment_lock_in_counter(); + update_prev(x, y, z); + return true; + } +} \ No newline at end of file