Split ControlCommunicatorNode Into xxxNode.cpp and xxx/cpp files (#9)

* add controlcommunicator cpp structure

* add gtest and change file name

* fix mat initialization (idk lol)

* basic structure

* work on structure

* fix control_comm to build

* add building code. set up tests using mocking to replace syscalls

* basic uart connection test

* port param tests

* Got target color subscriber working in OpenCVArmorDetector

* Irving said that we have to invert

* update msg to get enemy color

* fix vars

* fix tests

* fix tests

* add debug and script to run on boot

* fix port_fd and add some debug

* debug

* fix uart connect

* print uart read every 500 auto_aim sends

* fix startup script

* fix enemy targeting color

* fix NaN

* revert aim model

* fix uart if fails to align, and fix coord transform offsets I think

* fix large armor threshold and coord transform

* change to projectile_angle_convel from trig, add aim on boot, add lookup table

* fixes for LUT

* fix file pathing to allow lokup table to work, add 2 sec timer to refresh LUT for live tuning

* make pitch += LUT, make values not huge

* correct yaw/pitch computation

---------

Co-authored-by: Purdue RoboMaster <me@purduerm.org>
Co-authored-by: purduerm <rmc@purdue.edu>

Author: 3 people

.gitignore

@@ -1,2 +1,3 @@
 .vscode/
 .idea/
+log/

aim_on_boot.sh

@@ -0,0 +1,73 @@
+#!/bin/bash
+
+# Automatically get the current user's username
+SERVICE_NAME="ros2_aiming_service"
+SERVICE_FILE="/etc/systemd/system/$SERVICE_NAME.service"
+
+# Create the auto_aim_logs directory if it doesn't exist
+LOG_DIR="/home/purduerm/auto_aim_logs"
+mkdir -p "$LOG_DIR"
+
+# Generate the log file name with current timestamp
+LOG_FILE="$LOG_DIR/$(date +'%Y-%m-%d_%H-%M-%S')_auto_aim_log.txt"
+
+# Command to run the service
+COMMANDS_TO_RUN="source /opt/ros/humble/setup.bash && source /home/purduerm/ros2-ws/install/setup.bash && cd /home/purduerm/ros2-ws && ./auto-aiming/run run mv2control.py > $LOG_FILE 2>&1"
+
+# Function to create the service
+create_service() {
+    echo "Creating the systemd service..."
+
+    # Create the systemd service unit file
+    cat <<EOL | sudo tee $SERVICE_FILE > /dev/null
+[Unit]
+Description=ROS 2 Auto Aiming Service
+After=network.target
+
+[Service]
+ExecStart=/bin/bash -c "$COMMANDS_TO_RUN"
+WorkingDirectory=/home/purduerm/ros2-ws
+User=purduerm
+Group=purduerm
+Restart=always
+Environment=ROS_DISTRO=humble
+Environment=HOME=/home/purduerm
+
+[Install]
+WantedBy=multi-user.target
+EOL
+
+    # Reload systemd to recognize the new service
+    sudo systemctl daemon-reload
+}
+
+# Function to enable the service to run on boot
+enable_service() {
+    echo "Enabling the service to start on boot..."
+    sudo systemctl enable $SERVICE_NAME.service
+    sudo systemctl start $SERVICE_NAME.service
+    echo "Service started and enabled."
+}
+
+# Function to disable and remove the service
+disable_service() {
+    if [ -f "$SERVICE_FILE" ]; then
+        echo "Disabling and removing the service..."
+        sudo systemctl stop $SERVICE_NAME.service
+        sudo systemctl disable $SERVICE_NAME.service
+        sudo rm $SERVICE_FILE
+        sudo systemctl daemon-reload
+        echo "Service stopped, disabled, and removed."
+    else
+        echo "Service does not exist. Nothing to disable."
+    fi
+}
+
+# Parse arguments
+if [[ "$1" == "--disable" ]]; then
+    disable_service
+else
+    # Create and enable the service by default
+    create_service
+    enable_service
+fi

src/prm_control/control_communicator/CMakeLists.txt

@@ -36,12 +36,10 @@ add_executable(ControlCommunicatorNode
 	src/ControlCommunicatorNode.cpp
 )
 
-
-# include_directories(controlhandler_node
-#     PUBLIC
-#     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#     $<INSTALL_INTERFACE:include>
-# )
+add_library(ControlCommunicator STATIC 
+  src/ControlCommunicator.cpp
+  src/PitchLookupModel.cpp
+)
 
 ament_target_dependencies(ControlCommunicatorNode 
 	rclcpp
@@ -53,7 +51,13 @@ ament_target_dependencies(ControlCommunicatorNode
   nav_msgs
 )
 
-target_link_libraries(ControlCommunicatorNode Eigen3::Eigen)
+# GTest setup
+find_package(ament_cmake_gtest REQUIRED)
+ament_add_gtest(test_ControlCommunicator test/test_ControlCommunicator.cpp)
+target_include_directories(test_ControlCommunicator PRIVATE include)
+target_link_libraries(test_ControlCommunicator ControlCommunicator)
+target_link_libraries(ControlCommunicatorNode Eigen3::Eigen ControlCommunicator)
+ament_target_dependencies(test_ControlCommunicator rclcpp std_msgs geometry_msgs vision_msgs tf2_ros tf2 nav_msgs)
 
 install(TARGETS
 ControlCommunicatorNode

src/prm_control/control_communicator/include/ControlCommunicator.h

@@ -0,0 +1,29 @@
+#ifndef CONTROL_COMMUNICATOR_HPP
+#define CONTROL_COMMUNICATOR_HPP
+
+#include <cmath>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <cstring>
+#include "PitchLookupModel.hpp"
+
+class ControlCommunicator
+{
+public:
+    ControlCommunicator() {}
+    ~ControlCommunicator() {}
+
+    // Class methods
+    void compute_aim(float bullet_speed, float target_x, float target_y, float target_z, float &yaw, float &pitch);
+    bool start_uart_connection(const char *port);
+    void read_uart();
+
+    PitchLookupModel *lut = new PitchLookupModel("/home/purduerm/ros2-ws/auto-aiming/src/prm_control/control_communicator/include/lookup_tables/pitch_lookup_table.txt");
+
+    int port_fd = -1;
+    bool is_connected = false;
+private:
+};
+
+#endif // CONTROL_COMMUNICATOR_HPP

src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp

@@ -1,102 +1,75 @@
+#ifndef CONTROL_COMMUNICATOR_NODE_HPP
+#define CONTROL_COMMUNICATOR_NODE_HPP
 
-#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 "rclcpp/rclcpp.hpp"
 
-#include <errno.h> // Error integer and strerror() function
+#include "vision_msgs/msg/predicted_armor.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "vision_msgs/msg/yaw_pitch.hpp"
+#include "messages.h"
 
-#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/static_transform_broadcaster.h"
+#include "tf2/transform_datatypes.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"
+#include <memory>
+#include <functional>
+#include <string>
+#include <chrono>
 
-// using namespace std::chrono_literals;
-// using namespace std::placeholders;
-// namespace rmoss_msgs = rmoss_interfaces::msg;
+#include "ControlCommunicator.h"
 
 class ControlCommunicatorNode : public rclcpp::Node
 {
 public:
 	ControlCommunicatorNode(const char *port);
-
 	~ControlCommunicatorNode();
 
+	ControlCommunicator *control_communicator = new ControlCommunicator();
+
+	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 read_uart();
+
 private:
+	void publish_static_tf(float x, float y, float z, float roll, float pitch, float yaw, const char *frame_id, const char *child_frame_id);
+	void heart_beat_handler();
+
+	uint32_t auto_aim_frame_id = 0;
+	uint32_t nav_frame_id = 0;
+	uint32_t heart_beat_frame_id = 0;
+
+	// Read UART results
 	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_enemy_red = 0;
 	bool is_match_running = 0;
 	bool valid_read = false;
-
-	uint32_t recive_frame_id = 0;
+	std::string old_target_robot_color; 
 
 	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;
 
+	bool is_connected;
+	float aim_bullet_speed;
 	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::TimerBase::SharedPtr heart_beat_timer;
 	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::Publisher<std_msgs::msg::String>::SharedPtr target_robot_color_publisher;
+	rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr match_status_publisher;
 	rclcpp::TimerBase::SharedPtr uart_read_timer;
-	rclcpp::TimerBase::SharedPtr heart_beat_timer;
-
-	bool 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();
 
-	void read_uart();
+	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;
 };
-
-#endif // CONTROL_COMMUNICATOR_NODE_H
+#endif // CONTROL_COMMUNICATOR_NODE_HPP

src/prm_control/control_communicator/include/PitchLookupModel.hpp

@@ -0,0 +1,33 @@
+#ifndef PITCH_LOOKUP_MODEL_HPP
+#define PITCH_LOOKUP_MODEL_HPP
+
+#include <climits>
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <cassert>
+#include <string>
+#include <thread>
+
+class PitchLookupModel {
+public:
+    PitchLookupModel();
+    explicit PitchLookupModel(std::string filename);
+
+    int load_file();
+    void write_file();
+    float get_pitch(int distance, int height);
+
+private:
+    int lower_y = INT_MAX;
+    int lower_z = INT_MAX;
+    int upper_y = INT_MIN;
+    int upper_z = INT_MIN;
+    std::string filename;
+    std::vector<std::vector<float>> pitch_lookup_table;
+    std::thread timer;
+
+    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/lookup_tables/pitch_lookup_table.txt

@@ -0,0 +1,10 @@
+3 3
+500 0 1.44968
+1000 0 2.44968
+1500 0 3.44968
+2000 200 4.76067
+2500 0 5.44968
+3000 0 6.44968
+3500 0 7.44968
+4000 200 8.76067
+4500 0 9.44968

src/prm_control/control_communicator/include/projectile_angle_convel.h

@@ -1,10 +1,12 @@
+#ifndef PROJECTILE_ANGLE_CONVEL_H
+#define PROJECTILE_ANGLE_CONVEL_H
 
 #include <iostream>
 #include <complex>
 #include <algorithm>
 #include <cmath>
 
-#define MUZZLE_VELOCITY 30.00
+#define MUZZLE_VELOCITY 25000
 #define SHOT_IMPOSSIBLE -100
 #define PI 3.14159
 #define RAD_2_DEG 180/PI
@@ -187,3 +189,4 @@ void pitch_yaw_gravity_model_movingtarget_const_v(vec3 Pos, vec3 Vel, vec3 G, do
     *pitch = *pitch * RAD_2_DEG;
     *yaw = *yaw * RAD_2_DEG;
 }
+#endif // PROJECTILE_ANGLE_CONVEL_H