Yaw estim

run

@@ -32,7 +32,7 @@ clean() {
 build() {
     print_green "[*] Building project with Release configuration and optimization flags."
 
-    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3 -DPYTHON_EXECUTABLE=/usr/bin/python3.8"
+    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3"
 
     # Add the DEBUG flag if '--debug' or '-d' was provided
     if [[ "$debug" == "true" ]]; then

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)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+
+include_directories(/usr/include)
+include_directories(include)
+
+add_executable(ControlCommunicatorNode 
+	src/ControlCommunicatorNode.cpp
+)
+
+
+# include_directories(controlhandler_node
+#     PUBLIC
+#     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#     $<INSTALL_INTERFACE:include>
+# )
+
+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}
+)
+
+ament_package()

src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp

@@ -0,0 +1,104 @@
+
+#ifndef _CONTROL_COMMUNICATOR_NODE_H
+#define _CONTROL_COMMUNICATOR_NODE_H
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <Eigen/Dense>
+#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.h"
+
+#include "vision_msgs/msg/yaw_pitch.hpp"
+#include "vision_msgs/msg/predicted_armor.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "messages.h"
+
+// using namespace std::chrono_literals;
+// using namespace std::placeholders;
+// namespace rmoss_msgs = rmoss_interfaces::msg;
+
+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;
+
+	const char *port;
+	int port_fd = -1;
+	bool is_connected = false;
+
+	uint32_t auto_aim_frame_id = 0;
+	uint32_t nav_frame_id = 0;
+	uint32_t heart_beat_frame_id = 0;
+
+	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.h

@@ -0,0 +1,64 @@
+#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)
+	uint8_t 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; // blank
+	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;
+	};
+	// uint8_t crc8;
+} 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
+	// uint8_t is_blue;   //  0 = red, 1 = blue
+} __attribute__((packed)) PackageIn;
+
+#endif // _MESSAGES_H