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/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()

src/prm_control/control_communicator/include/ControlCommunicator.hpp

@@ -0,0 +1,36 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <Eigen/Dense>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <math.h>
+
+#include <errno.h> // 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<std::string, PackageIn> read_package();
+private:
+    const char *port;
+    int port_fd = -1;
+    bool is_connected = false;
+};

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 <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <tuple>
+#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 "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<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 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();
+    void read_uart();
+};
+
+#endif // CONTROL_COMMUNICATOR_NODE_H

src/prm_control/control_communicator/include/PitchLookupModel.hpp

@@ -0,0 +1,28 @@
+#ifndef PITCH_LOOKUP_MODEL_HPP
+#define PITCH_LOOKUP_MODEL_HPP
+
+#include <climits>
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <string>
+
+class PitchLookupModel {
+public:
+    PitchLookupModel();
+    explicit PitchLookupModel(std::string filename);
+
+    void load_file(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<std::vector<float>> pitch_lookup_table;
+
+    float map(float value, float old_min, float old_max, float new_min, float new_max);
+};
+
+#endif // PITCH_LOOKUP_MODEL_HPP

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/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="tkodonne@purdue.edu">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>