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/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 0c56855..5ef580f 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -18,6 +18,9 @@ // Pose Estimator classes #include "PoseEstimator.h" +// Standard cpp math +#include + 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::SharedPtr key_points_subscriber; std::shared_ptr> predicted_armor_publisher; @@ -42,4 +53,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 e371fcf..ddf1eac 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/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("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(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 + + // Transform camera coordinates to barrel coordinates + + // Set up transformation matrices + 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; + + 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}, + {0, 0, 0, 1} + }; + + // Multiply cam -> target vector by transformation matrix to get barrel -> target vector + 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); + 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) @@ -183,4 +259,4 @@ int main(int argc, char *argv[]) rclcpp::shutdown(); return 0; -} \ No newline at end of file +}