Skip to content

Commit 688842b

Browse files
iBoot32ComradeHibiscuspurduerm
authored
Uart read nonblock, and coordinate transform (#8)
* Added coordinate transform branch * Set up control stuff after this. Other than that, transform is in the code * Moved over to eigin + 3d -Moved transform code over to eigin library. -Set up transform to accept all 3 axes of rotation and coordinates to make it more portable/future-proof. -Can't find Sanjay and Millan but they should be co-authors (github can't find their names). * Updated camera -> barrel tip params * Swapped translations to (-) Translations should be negative for coordinate transform. Sanjay and Millan should be co-authors. * Z param should be negated * fix compile :( * try fix fmt * change params to be delcared in header and defined in constructor * change params to dynamic and fix max_dt bug * change video2detector, remove .idea * change from blocking to non blocking uart, no more alignment in favor of syncronizing send times (4ms on control and 4ms wall timer) * less frequent printing --------- Co-authored-by: ComradeHibiscus <mantonov@purdue.edu> Co-authored-by: Mark Antonov <mark.vladimir.antonov@gmail.com> Co-authored-by: purduerm <rmc@purdue.edu>
1 parent 55f779f commit 688842b

5 files changed

Lines changed: 155 additions & 96 deletions

File tree

‎.gitignore‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1,2 @@
1-
.vscode/
1+
.vscode/
2+
.idea/

‎src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -88,16 +88,14 @@ class ControlCommunicatorNode : public rclcpp::Node
8888
rclcpp::TimerBase::SharedPtr uart_read_timer;
8989
rclcpp::TimerBase::SharedPtr heart_beat_timer;
9090

91-
void start_uart(const char *port);
91+
bool start_uart(const char *port);
9292

9393
void publish_static_tf(float, float, float, float, float, float, const char *, const char *);
9494

9595
void auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg);
9696
void nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
9797
void heart_beat_handler();
9898

99-
bool read_alignment();
100-
10199
void read_uart();
102100
};
103101

‎src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp‎

Lines changed: 61 additions & 88 deletions
Original file line numberDiff line numberDiff line change
@@ -25,15 +25,6 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr
2525

2626
this->start_uart(port);
2727

28-
if (this->read_alignment())
29-
{
30-
RCLCPP_INFO(this->get_logger(), "Inital Read alignment success.");
31-
}
32-
else
33-
{
34-
RCLCPP_WARN(this->get_logger(), "Inital Read alignment failed.");
35-
}
36-
3728
RCLCPP_INFO(this->get_logger(), "should have printed.");
3829

3930
this->tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
@@ -60,7 +51,7 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr
6051
this->target_robot_color_publisher = this->create_publisher<std_msgs::msg::String>("color_set", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
6152
this->match_status_publisher = this->create_publisher<std_msgs::msg::Bool>("match_start", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
6253

63-
// this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));
54+
this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));
6455

6556
RCLCPP_INFO(this->get_logger(), "Control Communicator Node Started.");
6657
}
@@ -92,60 +83,69 @@ void ControlCommunicatorNode::publish_static_tf(float x, float y, float z, float
9283
tf_static_broadcaster->sendTransform(t);
9384
}
9485

95-
void ControlCommunicatorNode::start_uart(const char *port)
86+
bool ControlCommunicatorNode::start_uart(const char *port)
9687
{
88+
// Open the serial port in read/write mode, no controlling terminal, and no delay
89+
int baud_rate = B1152000;
9790
this->is_connected = false;
98-
this->port_fd = open(port, O_RDWR);
99-
100-
// Check for errors
101-
if (this->port_fd < 0)
102-
{
103-
RCLCPP_ERROR(this->get_logger(), "Failed to open: %s, %s", port, strerror(errno));
104-
return;
105-
}
106-
107-
struct termios tty;
108-
109-
// Set UART TTY to 8n1
110-
tty.c_cflag &= ~PARENB;
111-
tty.c_cflag &= ~CSTOPB;
112-
tty.c_cflag &= ~CSIZE;
113-
tty.c_cflag |= CS8;
114-
115-
tty.c_cflag &= ~CRTSCTS; // No RTS/CTS flow control
116-
tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines
117-
tty.c_lflag &= ~ICANON; // Disable canonical mode
118-
119-
// Disable echo, erasure and newline echo
120-
tty.c_lflag &= ~ECHO;
121-
tty.c_lflag &= ~ECHOE;
122-
tty.c_lflag &= ~ECHONL;
123-
124-
// Disable interpretation of INTR, QUIT and SUSP
125-
tty.c_lflag &= ~ISIG;
126-
127-
// Disable special handling, interpretation, S/W flow control, \n conv.
128-
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
129-
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
130-
tty.c_oflag &= ~OPOST;
131-
tty.c_oflag &= ~ONLCR;
132-
133-
tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds)
134-
tty.c_cc[VMIN] = sizeof(PackageOut); // Block for sizeof(PackageOut) bits
135-
136-
// Set the baud rate
137-
cfsetispeed(&tty, B1152000);
138-
139-
// Save tty settings, also checking for error
91+
this->port_fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
92+
93+
if (this->port_fd == -1)
94+
{
95+
RCLCPP_ERROR(this->get_logger(), "Error %i from open: %s", errno, strerror(errno));
96+
return false;
97+
}
98+
else
99+
{
100+
tcflush(this->port_fd, TCIOFLUSH); // Flush both input and output buffers
101+
}
102+
103+
// Configure serial port settings
104+
struct termios tty;
105+
if (tcgetattr(this->port_fd, &tty) != 0)
106+
{
107+
RCLCPP_ERROR(this->get_logger(), "Error %i from tcgetattr: %s", errno, strerror(errno));
108+
close(this->port_fd);
109+
return false;
110+
}
111+
112+
// Set baud rate
113+
if (cfsetispeed(&tty, baud_rate) != 0 || cfsetospeed(&tty, baud_rate) != 0)
114+
{
115+
RCLCPP_ERROR(this->get_logger(), "Error setting baud rate: %s", strerror(errno));
116+
close(this->port_fd);
117+
return false;
118+
}
119+
120+
// Set 8N1 (8 data bits, no parity, 1 stop bit)
121+
tty.c_cflag &= ~PARENB; // Disable parity
122+
tty.c_cflag &= ~CSTOPB; // Use one stop bit
123+
tty.c_cflag &= ~CSIZE; // Clear current data size setting
124+
tty.c_cflag |= CS8; // 8 data bits
125+
126+
// Disable hardware flow control
127+
tty.c_cflag &= ~CRTSCTS;
128+
129+
// Enable reading and ignore modem control lines
130+
tty.c_cflag |= CREAD | CLOCAL;
131+
132+
// Set raw mode (disable canonical mode, echo, signals, etc.)
133+
cfmakeraw(&tty);
134+
135+
// Set blocking mode with a 1-second read timeout
136+
tty.c_cc[VMIN] = 1; // Minimum 1 character to read
137+
tty.c_cc[VTIME] = 10; // Timeout in deciseconds (1 second)
138+
139+
// Apply the configuration to the serial port
140140
if (tcsetattr(this->port_fd, TCSANOW, &tty) != 0)
141141
{
142142
RCLCPP_ERROR(this->get_logger(), "Error %i from tcsetattr: %s", errno, strerror(errno));
143-
return;
143+
return false;
144144
}
145145
this->is_connected = true;
146146
RCLCPP_INFO(this->get_logger(), "UART Connected");
147147

148-
return;
148+
return true;
149149
}
150150

151151
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
198198
package.autoAimPackage.fire = 1;
199199
write(this->port_fd, &package, sizeof(PackageOut));
200200
fsync(this->port_fd);
201-
if (auto_aim_frame_id % 1000 == 0)
201+
if (this->auto_aim_frame_id % 1000 == 0)
202202
{
203203
RCLCPP_INFO(this->get_logger(), "Yaw, Pitch: %.3f, %.3f, FIRE=%i", package.autoAimPackage.yaw, package.autoAimPackage.pitch, package.autoAimPackage.fire);
204-
}
205-
// RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent, pkg is %i bytes", sizeof(PackageOut));
204+
} // RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent, pkg is %i bytes", sizeof(PackageOut));
206205
}
207206

208207
void ControlCommunicatorNode::nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg)
@@ -259,38 +258,20 @@ void ControlCommunicatorNode::heart_beat_handler()
259258
}
260259
}
261260

262-
bool ControlCommunicatorNode::read_alignment()
263-
{
264-
RCLCPP_INFO(this->get_logger(), "Attemp to alignment.");
265-
uint8_t i = 0;
266-
uint8_t buffer[33];
267-
do
268-
{
269-
int success = read(this->port_fd, &(buffer[0]), sizeof(buffer[0]));
270-
if (success)
271-
{
272-
i++;
273-
}
274-
} while (buffer[0] != 0xAA || i > sizeof(PackageIn) * 2);
275-
read(this->port_fd, &buffer, sizeof(PackageIn) - 1);
276-
277-
return i <= sizeof(PackageIn) * 2;
278-
}
279261

280262
void ControlCommunicatorNode::read_uart()
281263
{
282-
RCLCPP_INFO(this->get_logger(), "reading uart");
283264
PackageIn package;
284265
int success = read(this->port_fd, &package, sizeof(PackageIn));
285266

286267
rclcpp::Time curr_time = this->now();
287268
if (success == -1)
288269
{
289-
RCLCPP_ERROR(this->get_logger(), "Error %i from read: %s", errno, strerror(errno));
290-
return;
270+
RCLCPP_INFO(this->get_logger(), "DEBUG: Nothing in buffer, err %i from read: %s", errno, strerror(errno));
271+
return; // No data to read, try again later
291272
}
292273

293-
// // print each field of package
274+
// print each field of package
294275
// RCLCPP_INFO(this->get_logger(), "\n");
295276
// RCLCPP_INFO(this->get_logger(), "head: %x", package.head);
296277
// RCLCPP_INFO(this->get_logger(), "pitch: %f", package.pitch);
@@ -306,14 +287,6 @@ void ControlCommunicatorNode::read_uart()
306287
if (package.head != 0xAA) // Package validation
307288
{
308289
RCLCPP_WARN(this->get_logger(), "Packet miss aligned.");
309-
if (this->read_alignment())
310-
{
311-
RCLCPP_INFO(this->get_logger(), "Read alignment success.");
312-
}
313-
else
314-
{
315-
RCLCPP_WARN(this->get_logger(), "Read alignment failed.");
316-
}
317290
return;
318291
}
319292

@@ -414,7 +387,7 @@ void ControlCommunicatorNode::read_uart()
414387
int main(int argc, char **argv)
415388
{
416389
rclcpp::init(argc, argv);
417-
auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS0");
390+
auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS1");
418391

419392
rclcpp::spin(node);
420393
rclcpp::shutdown();

‎src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,9 @@
1818
// Pose Estimator classes
1919
#include "PoseEstimator.h"
2020

21+
// Standard cpp math
22+
#include <cmath>
23+
2124
class PoseEstimatorNode : public rclcpp::Node
2225
{
2326
public:
@@ -29,11 +32,19 @@ class PoseEstimatorNode : public rclcpp::Node
2932

3033
private:
3134
double _last_yaw_estimate = 0.0;
32-
35+
3336
// Class methods
3437
void publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status);
3538
void drawTopDownViewGivenRotation(double yaw, double X, double Y, double Z);
3639

40+
// dynamic parameters
41+
double cam_barrel_roll;
42+
double cam_barrel_pitch;
43+
double cam_barrel_yaw;
44+
double cam_barrel_x;
45+
double cam_barrel_y;
46+
double cam_barrel_z;
47+
3748
// Callbacks and publishers/subscribers
3849
rclcpp::Subscription<vision_msgs::msg::KeyPoints>::SharedPtr key_points_subscriber;
3950
std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::PredictedArmor>> predicted_armor_publisher;
@@ -42,4 +53,4 @@ class PoseEstimatorNode : public rclcpp::Node
4253
rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter> &parameters);
4354
};
4455

45-
#endif // POSE_ESTIMATOR_NODE_HPP
56+
#endif // POSE_ESTIMATOR_NODE_HPP

‎src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp‎

Lines changed: 78 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,14 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node(
99
predicted_armor_publisher = this->create_publisher<vision_msgs::msg::PredictedArmor>("predicted_armor", 10);
1010

1111
// Dynamic parameters
12-
pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 150));
12+
// Get ros2 parameters
13+
cam_barrel_roll = this -> declare_parameter("cam_barrel_roll", 0.0);
14+
cam_barrel_pitch = this -> declare_parameter("cam_barrel_pitch", 0.0);
15+
cam_barrel_yaw = this -> declare_parameter("cam_barrel_yaw", 0.0);
16+
cam_barrel_x = this -> declare_parameter("cam_barrel_x", -88.0);
17+
cam_barrel_y = this -> declare_parameter("cam_barrel_y", -73.0);
18+
cam_barrel_z = this -> declare_parameter("cam_barrel_z", 80.0);
19+
pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 15));
1320
pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3));
1421
validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3));
1522
validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000));
@@ -71,6 +78,36 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback(
7178
pose_estimator->setAllowedMissedFramesBeforeNoFire(param.as_int());
7279
RCLCPP_INFO(this->get_logger(), "Parameter '_allowed_missed_frames_before_no_fire' updated to: %d", param.as_int());
7380
}
81+
else if (param.get_name() == "cam_barrel_roll" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
82+
{
83+
cam_barrel_roll = param.as_double();
84+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_roll' updated to: %f", param.as_double());
85+
}
86+
else if (param.get_name() == "cam_barrel_pitch" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
87+
{
88+
cam_barrel_pitch = param.as_double();
89+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_pitch' updated to: %f", param.as_double());
90+
}
91+
else if (param.get_name() == "cam_barrel_yaw" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
92+
{
93+
cam_barrel_yaw = param.as_double();
94+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_yaw' updated to: %f", param.as_double());
95+
}
96+
else if (param.get_name() == "cam_barrel_x" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
97+
{
98+
cam_barrel_x = param.as_double();
99+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_x' updated to: %f", param.as_double());
100+
}
101+
else if (param.get_name() == "cam_barrel_y" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
102+
{
103+
cam_barrel_y = param.as_double();
104+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_y' updated to: %f", param.as_double());
105+
}
106+
else if (param.get_name() == "cam_barrel_z" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
107+
{
108+
cam_barrel_z = param.as_double();
109+
RCLCPP_INFO(this->get_logger(), "Parameter 'cam_barrel_z' updated to: %f", param.as_double());
110+
}
74111
else
75112
{
76113
result.successful = false;
@@ -106,6 +143,45 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
106143
pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec);
107144
_last_yaw_estimate = pose_estimator->estimateYaw(_last_yaw_estimate, image_points, tvec);
108145
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);
146+
147+
// TODO:: Ensure that the tvec is order x, y, z
148+
// TODO:: Verify units and set parameters
149+
150+
// Transform camera coordinates to barrel coordinates
151+
152+
// Set up transformation matrices
153+
Eigen::Matrix<double, 3, 3> r_roll;
154+
r_roll << 1, 0, 0,
155+
0, cos(cam_barrel_roll), -sin(cam_barrel_roll),
156+
0, sin(cam_barrel_roll), cos(cam_barrel_roll);
157+
158+
Eigen::Matrix<double, 3, 3> r_pitch;
159+
r_pitch << cos(cam_barrel_pitch), 0, -sin(cam_barrel_pitch),
160+
0, 1, 0,
161+
sin(cam_barrel_pitch), 0, cos(cam_barrel_pitch);
162+
163+
Eigen::Matrix<double, 3, 3> r_yaw;
164+
r_yaw << cos(cam_barrel_yaw), sin(cam_barrel_yaw), 0,
165+
-sin(cam_barrel_yaw), cos(cam_barrel_yaw), 0,
166+
0, 0, 1;
167+
168+
Eigen::Matrix<double, 3, 3> r_mat = r_roll * r_pitch * r_yaw;
169+
170+
Eigen::Matrix<double, 4, 4> transform_mat {
171+
{r_mat(0, 0), r_mat(0, 1), r_mat(0, 2), cam_barrel_x},
172+
{r_mat(1, 0), r_mat(1, 1), r_mat(1, 2), cam_barrel_y},
173+
{r_mat(2, 0), r_mat(2, 1), r_mat(2, 2), cam_barrel_z},
174+
{0, 0, 0, 1}
175+
};
176+
177+
// Multiply cam -> target vector by transformation matrix to get barrel -> target vector
178+
Eigen::Vector4d cam_to_target = {tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2), 1};
179+
Eigen::Vector4d barrel_to_target = transform_mat * cam_to_target;
180+
181+
// Set tvec to contain the transformed xyz coordinates
182+
tvec.at<double>(0) = barrel_to_target(0);
183+
tvec.at<double>(1) = barrel_to_target(1);
184+
tvec.at<double>(2) = barrel_to_target(2);
109185

110186
// Publish the predicted armor if the pose is valid (we are tracking or firing)
111187
if (valid_pose_estimate)
@@ -183,4 +259,4 @@ int main(int argc, char *argv[])
183259

184260
rclcpp::shutdown();
185261
return 0;
186-
}
262+
}

0 commit comments

Comments
 (0)