From ea0a71cad6d9adebf240521da37b12a9ffe9ad68 Mon Sep 17 00:00:00 2001 From: ComradeHibiscus Date: Mon, 27 Jan 2025 10:37:42 -0500 Subject: [PATCH 01/11] Added coordinate transform branch --- src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 3aef3a9..d1cb87a 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -106,6 +106,8 @@ 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(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); + + // TODO:: The coordinate transform should be around here // Publish the predicted armor if the pose is valid (we are tracking or firing) if (valid_pose_estimate) @@ -163,4 +165,4 @@ int main(int argc, char *argv[]) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 4f36dfe66f8ed9129acc0bf2aa277be86343f39f Mon Sep 17 00:00:00 2001 From: Mark Antonov Date: Sat, 8 Feb 2025 15:05:31 -0500 Subject: [PATCH 02/11] Set up control stuff after this. Other than that, transform is in the code --- src/prm_launch/launch/video2detector.py | 4 ++++ .../pose_estimator/include/PoseEstimatorNode.hpp | 10 ++++++++-- .../pose_estimator/src/PoseEstimatorNode.cpp | 13 ++++++++++++- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index eadd279..fce6f38 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -17,6 +17,7 @@ def generate_launch_description(): parameters=[{'source': str(video_path), 'fps': 4, 'frame_id': 'video', + 'cam_barrel_angle': 1.0; }] ), Node( @@ -26,5 +27,8 @@ def generate_launch_description(): Node( package='pose_estimator', executable='PoseEstimatorNode', + parameters=[{'cam_barrel_angle: 1.0'}] ), + + //TODO:: get actual angle, potentially set up the parameter to change based on control ]) diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 53ade18..01ddff1 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -18,18 +18,24 @@ // Pose Estimator classes #include "PoseEstimator.h" +// Math for trig +#include + class PoseEstimatorNode : public rclcpp::Node { public: PoseEstimatorNode(const rclcpp::NodeOptions &options); ~PoseEstimatorNode(); + + // Declare ros2 parameter for camera barrel angle + this -> declare_parameter("cam_barrel_angle", rclcpp::PARAMETER_DOUBLE); PoseEstimator *pose_estimator = new PoseEstimator(); ValidityFilter &validity_filter_ = pose_estimator->validity_filter_; // Reference to the validity filter private: double _last_yaw_estimate = 0.0; - + // Class methods void publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status); @@ -41,4 +47,4 @@ class PoseEstimatorNode : public rclcpp::Node rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector ¶meters); }; -#endif // POSE_ESTIMATOR_NODE_HPP \ No newline at end of file +#endif // POSE_ESTIMATOR_NODE_HPP diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index d1cb87a..6c48deb 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -107,7 +107,18 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha _last_yaw_estimate = pose_estimator->estimateYaw(_last_yaw_estimate, image_points, tvec); bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); - // TODO:: The coordinate transform should be around here + // TODO:: Ensure that the tvec is order x, y, z + + // Perform coordinate transform to account for offset of camera -> barrel. y is not being adjusted because of vertical fix of camera and barrel + double x_temp = tvec.at(0); + double z_temp = tvec.at(2); + + // Get angle between camera and barrel from ros2 parameters + rclpp::Parameter cam_barrel_angle_param = this -> get_parameter("cam_barrel_angle"); + double cam_barrel_angle = cam_barrel_angle_param(); + + tvec.at(0) = (x_temp * cos(cam_barrel_angle)) + (z_temp * sin(cam_barrel_angle)); + tvec.at(2) = (-1 * x_temp * sin(cam_barrel_angle)) + (z_temp * cos(cam_barrel_angle)); // Publish the predicted armor if the pose is valid (we are tracking or firing) if (valid_pose_estimate) From f2c1a97c53ca11645ffa57839f7377c7fea653f6 Mon Sep 17 00:00:00 2001 From: ComradeHibiscus Date: Wed, 12 Feb 2025 16:50:01 -0500 Subject: [PATCH 03/11] 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). --- .idea/.gitignore | 8 + .idea/auto-aiming.iml | 8 + .idea/editor.xml | 584 ++++++++++++++++++ .idea/misc.xml | 6 + .idea/modules.xml | 8 + .idea/vcs.xml | 6 + src/prm_launch/launch/video2detector.py | 8 +- .../include/PoseEstimatorNode.hpp | 4 +- .../pose_estimator/src/PoseEstimatorNode.cpp | 60 +- 9 files changed, 681 insertions(+), 11 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/auto-aiming.iml create mode 100644 .idea/editor.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/auto-aiming.iml b/.idea/auto-aiming.iml new file mode 100644 index 0000000..bc2cd87 --- /dev/null +++ b/.idea/auto-aiming.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/editor.xml b/.idea/editor.xml new file mode 100644 index 0000000..7fb72a2 --- /dev/null +++ b/.idea/editor.xml @@ -0,0 +1,584 @@ + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..04273fd --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..039ab0a --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index fce6f38..4228a7c 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -27,7 +27,13 @@ def generate_launch_description(): Node( package='pose_estimator', executable='PoseEstimatorNode', - parameters=[{'cam_barrel_angle: 1.0'}] + parameters[{'cam_barrel_roll': 1.0, # camera -> barrel rotations and translations. (radians and meters) + 'cam_barrel_pitch': 1.0, + 'cam_barrel_yaw': 1.0, + 'cam_barrel_x': 0.1, + 'cam_barrel_y': 0.1, + 'cam_barrel_z': 0.1, + }] ), //TODO:: get actual angle, potentially set up the parameter to change based on control diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 01ddff1..16ee30f 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -18,8 +18,8 @@ // Pose Estimator classes #include "PoseEstimator.h" -// Math for trig -#include +// Standard cpp math +#include class PoseEstimatorNode : public rclcpp::Node { diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 6c48deb..1fbff31 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -108,17 +108,61 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); // TODO:: Ensure that the tvec is order x, y, z + // TODO:: Verify units and set parameters - // Perform coordinate transform to account for offset of camera -> barrel. y is not being adjusted because of vertical fix of camera and barrel - double x_temp = tvec.at(0); - double z_temp = tvec.at(2); + // Transform camera coordinates to barrel coordinates + + // Get ros2 parameters + rclpp:Parameter cam_barrel_roll_param = this -> get_parameter("cam_barrel_roll"); + rclpp:Parameter cam_barrel_pitch_param = this -> get_parameter("cam_barrel_pitch"); + rclpp:Parameter cam_barrel_yaw_param = this -> get_parameter("cam_barrel_yaw"); + rclpp:Parameter cam_barrel_x_param = this -> get_parameter("cam_barrel_x"); + rclpp:Parameter cam_barrel_y_param = this -> get_parameter("cam_barrel_y"); + rclpp:Parameter cam_barrel_z_param = this -> get_parameter("cam_barrel_z"); + + double cam_barrel_roll = cam_barrel_roll_param.as_double(); + double cam_barrel_pitch = cam_barrel_pitch_param.as_double(); + double cam_barrel_yaw = cam_barrel_yaw_param.as_double(); + double cam_barrel_x = cam_barrel_x_param.as_double(); + double cam_barrel_y = cam_barrel_y_param.as_double(); + double cam_barrel_z = cam_barrel_z_param.as_double(); + + // Set up transformation matrices + Eigin::Matrix r_roll = { + {1, 0, 0}, + {0, cos(cam_barrel_roll), -sin(cam_barrel_roll)}, + {0, sin(cam_barrel_roll, cos(cam_barrel_roll))} + }; + + Eigin::Matrix r_pitch = { + {cos(cam_barrel_pitch), 0, -1 * sin(cam_barrel_pitch)}, + {0, 1, 0}, + { -1 * sin(cam_barrel_roll, 0, cos(cam_barrel_roll))} + }; + + Eigin::Matrix r_yaw = { + {cos(cam_barrel_yaw), sin(cam_barrel_yaw), 0}, + {sin(cam_barrel_yaw), cos(cam_barrel_yaw), 0}, + {0, 0, 1} + }; + + Eigin::Matrix r_mat = r_roll * r_pitch * r_yaw; + + Eigin::Matrix 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} + } - // Get angle between camera and barrel from ros2 parameters - rclpp::Parameter cam_barrel_angle_param = this -> get_parameter("cam_barrel_angle"); - double cam_barrel_angle = cam_barrel_angle_param(); + // Multiply cam -> target vector by transformation matrix to get barrel -> target vector + Eigin::Vector3d cam_to_target = {tvec.at(0), tvec.at(1), tvec.at(2)}; + Eigin::Vector3d barrel_to_target = transform_mat * cam_to_target; - tvec.at(0) = (x_temp * cos(cam_barrel_angle)) + (z_temp * sin(cam_barrel_angle)); - tvec.at(2) = (-1 * x_temp * sin(cam_barrel_angle)) + (z_temp * cos(cam_barrel_angle)); + // Set tvec to contain the transformed xyz coordinates + tvec.at(0) = barrel_to_target(0); + tvec.at(1) = barrel_to_target(1); + tvec.at(2) = barrel_to_target(2); // Publish the predicted armor if the pose is valid (we are tracking or firing) if (valid_pose_estimate) From 9dc71d2aad71d027ae6b5026cad9d90c3055e34e Mon Sep 17 00:00:00 2001 From: ComradeHibiscus Date: Wed, 12 Feb 2025 20:52:24 -0500 Subject: [PATCH 04/11] Updated camera -> barrel tip params --- src/prm_launch/launch/video2detector.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 4228a7c..651fccf 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -27,12 +27,12 @@ def generate_launch_description(): Node( package='pose_estimator', executable='PoseEstimatorNode', - parameters[{'cam_barrel_roll': 1.0, # camera -> barrel rotations and translations. (radians and meters) - 'cam_barrel_pitch': 1.0, - 'cam_barrel_yaw': 1.0, - 'cam_barrel_x': 0.1, - 'cam_barrel_y': 0.1, - 'cam_barrel_z': 0.1, + parameters[{'cam_barrel_roll': 0.0, # camera -> barrel rotations and translations. (radians and mm) + 'cam_barrel_pitch': 0.0, # angles are 0 bc camera is parallel at the moment + 'cam_barrel_yaw': 0.0, + 'cam_barrel_x': 88, # camera is 88 mm to left of barrel tip + 'cam_barrel_y': -73, # camera is 73 mm above barrel tip + 'cam_barrel_z': 80, # camera is 80 mm behind barrel tip }] ), From 17bf1576ebe94815f9a198582a34abdf444b865d Mon Sep 17 00:00:00 2001 From: Mark Antonov Date: Sat, 15 Feb 2025 16:16:35 -0500 Subject: [PATCH 05/11] Swapped translations to (-) Translations should be negative for coordinate transform. Sanjay and Millan should be co-authors. --- src/prm_launch/launch/video2detector.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 651fccf..4f05949 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -30,9 +30,9 @@ def generate_launch_description(): parameters[{'cam_barrel_roll': 0.0, # camera -> barrel rotations and translations. (radians and mm) 'cam_barrel_pitch': 0.0, # angles are 0 bc camera is parallel at the moment 'cam_barrel_yaw': 0.0, - 'cam_barrel_x': 88, # camera is 88 mm to left of barrel tip - 'cam_barrel_y': -73, # camera is 73 mm above barrel tip - 'cam_barrel_z': 80, # camera is 80 mm behind barrel tip + 'cam_barrel_x': -88, # camera is 88 mm to left of barrel tip + 'cam_barrel_y': 73, # camera is 73 mm above barrel tip + 'cam_barrel_z': -80, # camera is 80 mm behind barrel tip }] ), From 2fd65a75b644197d375ae75c5c324d69d7e90053 Mon Sep 17 00:00:00 2001 From: Mark Antonov Date: Sat, 15 Feb 2025 16:28:00 -0500 Subject: [PATCH 06/11] Z param should be negated --- src/prm_launch/launch/video2detector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 4f05949..0e813e7 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -32,7 +32,7 @@ def generate_launch_description(): 'cam_barrel_yaw': 0.0, 'cam_barrel_x': -88, # camera is 88 mm to left of barrel tip 'cam_barrel_y': 73, # camera is 73 mm above barrel tip - 'cam_barrel_z': -80, # camera is 80 mm behind barrel tip + 'cam_barrel_z': 80, # camera is 80 mm behind barrel tip }] ), From 47e6e5fe4398a5308b520083716c1e3ed4c673ad Mon Sep 17 00:00:00 2001 From: purduerm Date: Sat, 15 Feb 2025 16:42:56 -0500 Subject: [PATCH 07/11] fix compile :( --- .../include/PoseEstimatorNode.hpp | 3 -- .../pose_estimator/src/PoseEstimatorNode.cpp | 36 +++++++++---------- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 16ee30f..3e6a624 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -26,9 +26,6 @@ class PoseEstimatorNode : public rclcpp::Node public: PoseEstimatorNode(const rclcpp::NodeOptions &options); ~PoseEstimatorNode(); - - // Declare ros2 parameter for camera barrel angle - this -> declare_parameter("cam_barrel_angle", rclcpp::PARAMETER_DOUBLE); PoseEstimator *pose_estimator = new PoseEstimator(); ValidityFilter &validity_filter_ = pose_estimator->validity_filter_; // Reference to the validity filter diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 1fbff31..de08087 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -113,12 +113,12 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha // Transform camera coordinates to barrel coordinates // Get ros2 parameters - rclpp:Parameter cam_barrel_roll_param = this -> get_parameter("cam_barrel_roll"); - rclpp:Parameter cam_barrel_pitch_param = this -> get_parameter("cam_barrel_pitch"); - rclpp:Parameter cam_barrel_yaw_param = this -> get_parameter("cam_barrel_yaw"); - rclpp:Parameter cam_barrel_x_param = this -> get_parameter("cam_barrel_x"); - rclpp:Parameter cam_barrel_y_param = this -> get_parameter("cam_barrel_y"); - rclpp:Parameter cam_barrel_z_param = this -> get_parameter("cam_barrel_z"); + rclcpp::Parameter cam_barrel_roll_param = this -> get_parameter("cam_barrel_roll"); + rclcpp::Parameter cam_barrel_pitch_param = this -> get_parameter("cam_barrel_pitch"); + rclcpp::Parameter cam_barrel_yaw_param = this -> get_parameter("cam_barrel_yaw"); + rclcpp::Parameter cam_barrel_x_param = this -> get_parameter("cam_barrel_x"); + rclcpp::Parameter cam_barrel_y_param = this -> get_parameter("cam_barrel_y"); + rclcpp::Parameter cam_barrel_z_param = this -> get_parameter("cam_barrel_z"); double cam_barrel_roll = cam_barrel_roll_param.as_double(); double cam_barrel_pitch = cam_barrel_pitch_param.as_double(); @@ -128,36 +128,36 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha double cam_barrel_z = cam_barrel_z_param.as_double(); // Set up transformation matrices - Eigin::Matrix r_roll = { + Eigen::Matrix r_roll { {1, 0, 0}, {0, cos(cam_barrel_roll), -sin(cam_barrel_roll)}, - {0, sin(cam_barrel_roll, cos(cam_barrel_roll))} + {0, sin(cam_barrel_roll), cos(cam_barrel_roll)} }; - Eigin::Matrix r_pitch = { + Eigen::Matrix r_pitch { {cos(cam_barrel_pitch), 0, -1 * sin(cam_barrel_pitch)}, {0, 1, 0}, - { -1 * sin(cam_barrel_roll, 0, cos(cam_barrel_roll))} + { -1 * sin(cam_barrel_roll), 0, cos(cam_barrel_roll)} }; - Eigin::Matrix r_yaw = { + Eigen::Matrix r_yaw { {cos(cam_barrel_yaw), sin(cam_barrel_yaw), 0}, {sin(cam_barrel_yaw), cos(cam_barrel_yaw), 0}, {0, 0, 1} }; - Eigin::Matrix r_mat = r_roll * r_pitch * r_yaw; + Eigen::Matrix r_mat = r_roll * r_pitch * r_yaw; - Eigin::Matrix transform_mat = { + Eigen::Matrix 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} + {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 - Eigin::Vector3d cam_to_target = {tvec.at(0), tvec.at(1), tvec.at(2)}; - Eigin::Vector3d barrel_to_target = transform_mat * cam_to_target; + Eigen::Vector4d cam_to_target = {tvec.at(0), tvec.at(1), tvec.at(2), 1}; + Eigen::Vector4d barrel_to_target = transform_mat * cam_to_target; // Set tvec to contain the transformed xyz coordinates tvec.at(0) = barrel_to_target(0); From 8d6728b0f77a7fe156d6dfa41614d7d361ef75c2 Mon Sep 17 00:00:00 2001 From: purduerm Date: Sat, 15 Feb 2025 16:56:56 -0500 Subject: [PATCH 08/11] try fix fmt --- .../pose_estimator/src/PoseEstimatorNode.cpp | 31 +++++++++---------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index de08087..6f8854f 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -128,23 +128,20 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha double cam_barrel_z = cam_barrel_z_param.as_double(); // Set up transformation matrices - Eigen::Matrix 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 r_pitch { - {cos(cam_barrel_pitch), 0, -1 * sin(cam_barrel_pitch)}, - {0, 1, 0}, - { -1 * sin(cam_barrel_roll), 0, cos(cam_barrel_roll)} - }; - - Eigen::Matrix 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 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 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 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 r_mat = r_roll * r_pitch * r_yaw; From 71d96ef19984ee4b391492075a07d5847544d091 Mon Sep 17 00:00:00 2001 From: purduerm Date: Sat, 15 Feb 2025 17:23:48 -0500 Subject: [PATCH 09/11] change params to be delcared in header and defined in constructor --- src/prm_launch/launch/video2detector.py | 9 +------- .../include/PoseEstimatorNode.hpp | 8 +++++++ .../pose_estimator/src/PoseEstimatorNode.cpp | 22 ++++++------------- 3 files changed, 16 insertions(+), 23 deletions(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 0e813e7..27ed111 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -27,14 +27,7 @@ def generate_launch_description(): Node( package='pose_estimator', executable='PoseEstimatorNode', - parameters[{'cam_barrel_roll': 0.0, # camera -> barrel rotations and translations. (radians and mm) - 'cam_barrel_pitch': 0.0, # angles are 0 bc camera is parallel at the moment - 'cam_barrel_yaw': 0.0, - 'cam_barrel_x': -88, # camera is 88 mm to left of barrel tip - 'cam_barrel_y': 73, # camera is 73 mm above barrel tip - 'cam_barrel_z': 80, # camera is 80 mm behind barrel tip - }] ), - //TODO:: get actual angle, potentially set up the parameter to change based on control + # TODO:: get actual angle, potentially set up the parameter to change based on control ]) diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 3e6a624..2339033 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -36,6 +36,14 @@ class PoseEstimatorNode : public rclcpp::Node // Class methods void publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status); + // 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::SharedPtr key_points_subscriber; std::shared_ptr> predicted_armor_publisher; diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 6f8854f..531e569 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -9,6 +9,13 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node( predicted_armor_publisher = this->create_publisher("predicted_armor", 10); // Dynamic parameters + // 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)); @@ -111,21 +118,6 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha // TODO:: Verify units and set parameters // Transform camera coordinates to barrel coordinates - - // Get ros2 parameters - rclcpp::Parameter cam_barrel_roll_param = this -> get_parameter("cam_barrel_roll"); - rclcpp::Parameter cam_barrel_pitch_param = this -> get_parameter("cam_barrel_pitch"); - rclcpp::Parameter cam_barrel_yaw_param = this -> get_parameter("cam_barrel_yaw"); - rclcpp::Parameter cam_barrel_x_param = this -> get_parameter("cam_barrel_x"); - rclcpp::Parameter cam_barrel_y_param = this -> get_parameter("cam_barrel_y"); - rclcpp::Parameter cam_barrel_z_param = this -> get_parameter("cam_barrel_z"); - - double cam_barrel_roll = cam_barrel_roll_param.as_double(); - double cam_barrel_pitch = cam_barrel_pitch_param.as_double(); - double cam_barrel_yaw = cam_barrel_yaw_param.as_double(); - double cam_barrel_x = cam_barrel_x_param.as_double(); - double cam_barrel_y = cam_barrel_y_param.as_double(); - double cam_barrel_z = cam_barrel_z_param.as_double(); // Set up transformation matrices Eigen::Matrix r_roll; From cef72cad311d68f7202ae503cf4428ed204dad4b Mon Sep 17 00:00:00 2001 From: purduerm Date: Sat, 15 Feb 2025 17:43:36 -0500 Subject: [PATCH 10/11] change params to dynamic and fix max_dt bug --- .../pose_estimator/src/PoseEstimatorNode.cpp | 34 +++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 531e569..3853589f 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -14,7 +14,7 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node( 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_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)); @@ -68,7 +68,7 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback( validity_filter_.setPrevLen(param.as_int()); RCLCPP_INFO(this->get_logger(), "Parameter '_prev_len' updated to: %d", param.as_int()); } - else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE || param.get_type()) + else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { validity_filter_.setMaxDt(param.as_double()); RCLCPP_INFO(this->get_logger(), "Parameter '_max_dt' updated to: %f", param.as_double()); @@ -78,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; From 47096d578d5fac967c2e80252f0de2ccdd160645 Mon Sep 17 00:00:00 2001 From: purduerm Date: Sat, 15 Feb 2025 17:57:46 -0500 Subject: [PATCH 11/11] change video2detector, remove .idea --- .gitignore | 3 +- .idea/.gitignore | 8 - .idea/auto-aiming.iml | 8 - .idea/editor.xml | 584 ------------------ .idea/misc.xml | 6 - .idea/modules.xml | 8 - .idea/vcs.xml | 6 - src/prm_launch/launch/video2detector.py | 3 - .../pose_estimator/src/PoseEstimatorNode.cpp | 1 - 9 files changed, 2 insertions(+), 625 deletions(-) delete mode 100644 .idea/.gitignore delete mode 100644 .idea/auto-aiming.iml delete mode 100644 .idea/editor.xml delete mode 100644 .idea/misc.xml delete mode 100644 .idea/modules.xml delete mode 100644 .idea/vcs.xml diff --git a/.gitignore b/.gitignore index dbe9c82..93c700e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -.vscode/ \ No newline at end of file +.vscode/ +.idea/ diff --git a/.idea/.gitignore b/.idea/.gitignore deleted file mode 100644 index 13566b8..0000000 --- a/.idea/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml -# Editor-based HTTP Client requests -/httpRequests/ -# Datasource local storage ignored files -/dataSources/ -/dataSources.local.xml diff --git a/.idea/auto-aiming.iml b/.idea/auto-aiming.iml deleted file mode 100644 index bc2cd87..0000000 --- a/.idea/auto-aiming.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/editor.xml b/.idea/editor.xml deleted file mode 100644 index 7fb72a2..0000000 --- a/.idea/editor.xml +++ /dev/null @@ -1,584 +0,0 @@ - - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml deleted file mode 100644 index 04273fd..0000000 --- a/.idea/misc.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 039ab0a..0000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml deleted file mode 100644 index 35eb1dd..0000000 --- a/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 87bb4a4..39643d5 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -17,7 +17,6 @@ def generate_launch_description(): parameters=[{'source': str(video_path), 'fps': 4, 'frame_id': 'video', - 'cam_barrel_angle': 1.0; }] ), Node( @@ -28,6 +27,4 @@ def generate_launch_description(): package='pose_estimator', executable='PoseEstimatorNode', ), - - # TODO:: get actual angle, potentially set up the parameter to change based on control ]) diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 086e458..ddf1eac 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -69,7 +69,6 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback( RCLCPP_INFO(this->get_logger(), "Parameter '_prev_len' updated to: %d", param.as_int()); } else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) - else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { validity_filter_.setMaxDt(param.as_double()); RCLCPP_INFO(this->get_logger(), "Parameter '_max_dt' updated to: %f", param.as_double());