Uart read nonblock, and coordinate transform

.gitignore

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

src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp

@@ -88,16 +88,14 @@ class ControlCommunicatorNode : public rclcpp::Node
 	rclcpp::TimerBase::SharedPtr uart_read_timer;
 	rclcpp::TimerBase::SharedPtr heart_beat_timer;
 
-	void start_uart(const char *port);
+	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();
 
-	bool read_alignment();
-
 	void read_uart();
 };
 

src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp

@@ -25,15 +25,6 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr
 
 	this->start_uart(port);
 
-	if (this->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<tf2_ros::TransformBroadcaster>(*this);
@@ -60,7 +51,7 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr
 	this->target_robot_color_publisher = this->create_publisher<std_msgs::msg::String>("color_set", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
 	this->match_status_publisher = this->create_publisher<std_msgs::msg::Bool>("match_start", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
 
-	// this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));
+	this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));
 
 	RCLCPP_INFO(this->get_logger(), "Control Communicator Node Started.");
 }
@@ -92,60 +83,69 @@ void ControlCommunicatorNode::publish_static_tf(float x, float y, float z, float
 	tf_static_broadcaster->sendTransform(t);
 }
 
-void ControlCommunicatorNode::start_uart(const char *port)
+bool ControlCommunicatorNode::start_uart(const char *port)
 {
+	// Open the serial port in read/write mode, no controlling terminal, and no delay
+    int baud_rate = B1152000;
 	this->is_connected = false;
-	this->port_fd = open(port, O_RDWR);
-
-	// Check for errors
-	if (this->port_fd < 0)
-	{
-		RCLCPP_ERROR(this->get_logger(), "Failed to open: %s, %s", port, strerror(errno));
-		return;
-	}
-
-	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(PackageOut); // Block for sizeof(PackageOut) bits
-
-	// Set the baud rate
-	cfsetispeed(&tty, B1152000);
-
-	// Save tty settings, also checking for error
+	this->port_fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+    if (this->port_fd == -1)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Error %i from open: %s", errno, strerror(errno));
+        return false;
+    }
+    else
+    {
+        tcflush(this->port_fd, TCIOFLUSH); // Flush both input and output buffers
+    }
+
+    // Configure serial port settings
+    struct termios tty;
+    if (tcgetattr(this->port_fd, &tty) != 0)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Error %i from tcgetattr: %s", errno, strerror(errno));
+        close(this->port_fd);
+        return false;
+    }
+
+    // Set baud rate
+    if (cfsetispeed(&tty, baud_rate) != 0 || cfsetospeed(&tty, baud_rate) != 0)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Error setting baud rate: %s", strerror(errno));
+        close(this->port_fd);
+        return false;
+    }
+
+    // Set 8N1 (8 data bits, no parity, 1 stop bit)
+    tty.c_cflag &= ~PARENB; // Disable parity
+    tty.c_cflag &= ~CSTOPB; // Use one stop bit
+    tty.c_cflag &= ~CSIZE;  // Clear current data size setting
+    tty.c_cflag |= CS8;     // 8 data bits
+
+    // Disable hardware flow control
+    tty.c_cflag &= ~CRTSCTS;
+
+    // Enable reading and ignore modem control lines
+    tty.c_cflag |= CREAD | CLOCAL;
+
+    // Set raw mode (disable canonical mode, echo, signals, etc.)
+    cfmakeraw(&tty);
+
+    // Set blocking mode with a 1-second read timeout
+    tty.c_cc[VMIN] = 1;   // Minimum 1 character to read
+    tty.c_cc[VTIME] = 10; // Timeout in deciseconds (1 second)
+
+    // Apply the configuration to the serial port
 	if (tcsetattr(this->port_fd, TCSANOW, &tty) != 0)
 	{
 		RCLCPP_ERROR(this->get_logger(), "Error %i from tcsetattr: %s", errno, strerror(errno));
-		return;
+		return false;
 	}
 	this->is_connected = true;
 	RCLCPP_INFO(this->get_logger(), "UART Connected");
 
-	return;
+    return true;
 }
 
 void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg)
@@ -198,11 +198,10 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs
 	package.autoAimPackage.fire = 1;
 	write(this->port_fd, &package, sizeof(PackageOut));
 	fsync(this->port_fd);
-	if (auto_aim_frame_id % 1000 == 0)
+	if (this->auto_aim_frame_id % 1000 == 0)
 	{
 		RCLCPP_INFO(this->get_logger(), "Yaw, Pitch: %.3f, %.3f, FIRE=%i", package.autoAimPackage.yaw, package.autoAimPackage.pitch, package.autoAimPackage.fire);
-	}
-	// RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent, pkg is %i bytes", sizeof(PackageOut));
+	}	// RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent, pkg is %i bytes", sizeof(PackageOut));
 }
 
 void ControlCommunicatorNode::nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg)
@@ -259,38 +258,20 @@ void ControlCommunicatorNode::heart_beat_handler()
 	}
 }
 
-bool ControlCommunicatorNode::read_alignment()
-{
-	RCLCPP_INFO(this->get_logger(), "Attemp to alignment.");
-	uint8_t i = 0;
-	uint8_t buffer[33];
-	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;
-}
 
 void ControlCommunicatorNode::read_uart()
 {
-	RCLCPP_INFO(this->get_logger(), "reading uart");
 	PackageIn package;
 	int success = read(this->port_fd, &package, sizeof(PackageIn));
 
 	rclcpp::Time curr_time = this->now();
 	if (success == -1)
 	{
-		RCLCPP_ERROR(this->get_logger(), "Error %i from read: %s", errno, strerror(errno));
-		return;
+		RCLCPP_INFO(this->get_logger(), "DEBUG: Nothing in buffer, err %i from read: %s", errno, strerror(errno));
+		return; // No data to read, try again later
 	}
 
-	// // print each field of package
+	// print each field of package
 	// RCLCPP_INFO(this->get_logger(), "\n");
 	// RCLCPP_INFO(this->get_logger(), "head: %x", package.head);
 	// RCLCPP_INFO(this->get_logger(), "pitch: %f", package.pitch);
@@ -306,14 +287,6 @@ void ControlCommunicatorNode::read_uart()
 	if (package.head != 0xAA) // Package validation
 	{
 		RCLCPP_WARN(this->get_logger(), "Packet miss aligned.");
-		if (this->read_alignment())
-		{
-			RCLCPP_INFO(this->get_logger(), "Read alignment success.");
-		}
-		else
-		{
-			RCLCPP_WARN(this->get_logger(), "Read alignment failed.");
-		}
 		return;
 	}
 
@@ -414,7 +387,7 @@ void ControlCommunicatorNode::read_uart()
 int main(int argc, char **argv)
 {
 	rclcpp::init(argc, argv);
-	auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS0");
+	auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS1");
 
 	rclcpp::spin(node);
 	rclcpp::shutdown();

src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp

@@ -18,6 +18,9 @@
 // Pose Estimator classes
 #include "PoseEstimator.h"
 
+// Standard cpp math
+#include <cmath>
+
 class PoseEstimatorNode : public rclcpp::Node
 {
 public:
@@ -29,11 +32,19 @@ class PoseEstimatorNode : public rclcpp::Node
 
 private:
     double _last_yaw_estimate = 0.0;
-
+    
     // Class methods
     void publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status);
     void drawTopDownViewGivenRotation(double yaw, double X, double Y, double Z);
 
+    // dynamic parameters
+    double cam_barrel_roll;
+    double cam_barrel_pitch;
+    double cam_barrel_yaw;
+    double cam_barrel_x; 
+    double cam_barrel_y;
+    double cam_barrel_z;
+
     // Callbacks and publishers/subscribers
     rclcpp::Subscription<vision_msgs::msg::KeyPoints>::SharedPtr key_points_subscriber;
     std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::PredictedArmor>> predicted_armor_publisher;
@@ -42,4 +53,4 @@ class PoseEstimatorNode : public rclcpp::Node
     rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter> &parameters);
 };
 
-#endif // POSE_ESTIMATOR_NODE_HPP
+#endif // POSE_ESTIMATOR_NODE_HPP

src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp

@@ -9,7 +9,14 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node(
     predicted_armor_publisher = this->create_publisher<vision_msgs::msg::PredictedArmor>("predicted_armor", 10);
 
     // Dynamic parameters
-    pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 150));
+    // Get ros2 parameters
+    cam_barrel_roll = this -> declare_parameter("cam_barrel_roll", 0.0);
+    cam_barrel_pitch = this -> declare_parameter("cam_barrel_pitch", 0.0);
+    cam_barrel_yaw = this -> declare_parameter("cam_barrel_yaw", 0.0);
+    cam_barrel_x = this -> declare_parameter("cam_barrel_x", -88.0);
+    cam_barrel_y = this -> declare_parameter("cam_barrel_y", -73.0);
+    cam_barrel_z = this -> declare_parameter("cam_barrel_z", 80.0);
+    pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 15));
     pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3));
     validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3));
     validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000));
@@ -71,6 +78,36 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback(
             pose_estimator->setAllowedMissedFramesBeforeNoFire(param.as_int());
             RCLCPP_INFO(this->get_logger(), "Parameter '_allowed_missed_frames_before_no_fire' updated to: %d", param.as_int());
         }
+        else if (param.get_name() == "cam_barrel_roll" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_roll = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_roll' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "cam_barrel_pitch" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_pitch = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_pitch' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "cam_barrel_yaw" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_yaw = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_yaw' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "cam_barrel_x" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_x = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_x' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "cam_barrel_y" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_y = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_y' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "cam_barrel_z" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            cam_barrel_z = param.as_double();
+            RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_z' updated to: %f", param.as_double());
+        }
         else
         {
             result.successful = false;
@@ -106,6 +143,45 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
     pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec);
     _last_yaw_estimate = pose_estimator->estimateYaw(_last_yaw_estimate, image_points, tvec);
     bool valid_pose_estimate = pose_estimator->isValid(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2), new_auto_aim_status, reset_kalman);
+
+    // TODO:: Ensure that the tvec is order x, y, z
+    // TODO:: Verify units and set parameters
+
+    // Transform camera coordinates to barrel coordinates
+
+    // Set up transformation matrices
+    Eigen::Matrix<double, 3, 3> r_roll;
+    r_roll << 1, 0, 0,
+          0, cos(cam_barrel_roll), -sin(cam_barrel_roll),
+          0, sin(cam_barrel_roll), cos(cam_barrel_roll);
+
+    Eigen::Matrix<double, 3, 3> r_pitch;
+    r_pitch << cos(cam_barrel_pitch), 0, -sin(cam_barrel_pitch),
+           0, 1, 0,
+           sin(cam_barrel_pitch), 0, cos(cam_barrel_pitch);
+
+    Eigen::Matrix<double, 3, 3> r_yaw;
+    r_yaw << cos(cam_barrel_yaw), sin(cam_barrel_yaw), 0,
+         -sin(cam_barrel_yaw), cos(cam_barrel_yaw), 0,
+         0, 0, 1;
+
+    Eigen::Matrix<double, 3, 3> r_mat = r_roll * r_pitch * r_yaw;
+
+    Eigen::Matrix<double, 4, 4> transform_mat {
+      {r_mat(0, 0), r_mat(0, 1), r_mat(0, 2), cam_barrel_x},
+      {r_mat(1, 0), r_mat(1, 1), r_mat(1, 2), cam_barrel_y},
+      {r_mat(2, 0), r_mat(2, 1), r_mat(2, 2), cam_barrel_z},
+      {0, 0, 0, 1}
+    };
+
+    // Multiply cam -> target vector by transformation matrix to get barrel -> target vector
+    Eigen::Vector4d cam_to_target = {tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2), 1};
+    Eigen::Vector4d barrel_to_target = transform_mat * cam_to_target;
+
+    // Set tvec to contain the transformed xyz coordinates
+    tvec.at<double>(0) = barrel_to_target(0);
+    tvec.at<double>(1) = barrel_to_target(1);
+    tvec.at<double>(2) = barrel_to_target(2);
 
     // Publish the predicted armor if the pose is valid (we are tracking or firing)
     if (valid_pose_estimate)
@@ -183,4 +259,4 @@ int main(int argc, char *argv[])
 
     rclcpp::shutdown();
     return 0;
-}
+}