Added pnp solver and control communicator

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/projectile_angle_convel.cpp
+  src/utils.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()

src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp

@@ -0,0 +1,101 @@
+
+#ifndef _CONTROL_COMMUNICATOR_NODE_H
+#define _CONTROL_COMMUNICATOR_NODE_H
+
+#define PI 3.141592653589793238462643383
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <Eigen/Dense>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <math.h>
+
+#include <errno.h> // 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 <geometry_msgs/msg/twist_stamped.hpp>
+
+#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 "projectile_angle_convel.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 "utils.hpp"
+#include "time_debug.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;
+    bool valid_read = false;
+
+    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;
+
+    const char *port;
+    int port_fd = -1;
+    bool is_connected = false;
+
+    int aim_stop_null_frame_count;
+    int aim_null_frame_count = 0;
+
+    float aim_bullet_speed; // mm/s
+
+    int8_t curr_pois = 0;
+    bool right = true;
+
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener;
+
+    rclcpp::Subscription<vision_msgs::msg::PredictedArmor>::SharedPtr auto_aim_subscriber;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr nav_subscriber;
+
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_publisher;
+    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>> target_robot_color_publisher = NULL;
+    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> match_status_publisher = NULL;
+
+    rclcpp::TimerBase::SharedPtr uart_read_timer;
+    rclcpp::TimerBase::SharedPtr heart_beat_timer;
+
+    void start_uart(const char *port);
+    void publish_static_tf(float, float, float, float, float, float, const char *, const char *);
+    void auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg);
+    void nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
+    void heart_beat_handler();
+    bool read_alignment();
+    void read_uart();
+};
+
+#endif // CONTROL_COMMUNICATOR_NODE_H

src/prm_control/control_communicator/include/messages.hpp

@@ -0,0 +1,62 @@
+#ifndef _MESSAGES_H
+#define _MESSAGES_H
+
+#include <stdint.h>
+
+#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

src/prm_control/control_communicator/include/projectile_angle_convel.hpp

@@ -0,0 +1,35 @@
+#define MUZZLE_VELOCITY 30.00
+#define SHOT_IMPOSSIBLE -100
+#define RAD_2_DEG 180 / PI
+#define OoM -10
+
+#include <iostream>
+#include <complex>
+#include <algorithm>
+#include <cmath>
+
+static std::complex<double> complex_sqrt(const std::complex<double> & z);
+static std::complex<double> complex_cbrt(const std::complex<double> & z);
+void solve_quartic(const std::complex<double> coefficients[5], std::complex<double> roots[4]);
+
+struct vec3
+{
+    double x = 0, y = 0, z = 0;
+    std::string id = "N/A";
+
+    vec3();
+    vec3(double a, double b, double c);
+
+    vec3 operator + (vec3 const &a);
+    vec3 operator - (vec3 const &a);
+    double operator * (vec3 const &a);
+    vec3 operator * (double c);
+    vec3 operator / (double c);
+
+    double norm();
+    void print();
+};
+
+bool compare_complex(std::complex<double> a, std::complex<double> b);
+double smallest_positive_real_soln(std::complex<double> roots[4]);
+void pitch_yaw_gravity_model_movingtarget_const_v(vec3 Pos, vec3 Vel, vec3 G, double time_delay, double* pitch, double* yaw, bool* impossible);

src/prm_control/control_communicator/include/time_debug.hpp

@@ -0,0 +1,9 @@
+#ifndef _TIME_DEBUG_H
+#define _TIME_DEBUG_H
+
+#define START_TIME auto start = std::chrono::steady_clock::now();
+#define END_TIME auto end = std::chrono::steady_clock::now();
+#define ROS_LOG_DURATION RCLCPP_INFO(this->get_logger(), "duration: %dns", std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count());
+#define PRINT_DURATION std::cout << "duration: " << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() << "ns" << std::endl;
+
+#endif // _TIME_DEBUG_H

src/prm_control/control_communicator/include/utils.hpp

@@ -0,0 +1,10 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+
+#define G -9.80665 / 1000 // (mm/ms^2)
+
+uint8_t crc8(uint8_t *data, size_t len);
+int8_t float2int8(double f);
+float quadratic(float a, float b, float c);
+int gravity_pitch_offset(float v0, int min_p, int max_p, float x, float dz, float xv);

src/prm_control/control_communicator/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>control_communicator</name>
+  <version>0.0.0</version>
+  <description>A package communicator to comunnicate with the STM32 board</description>
+  <maintainer email="[email protected]">Tom O'Donnell</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <depend>vision_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>