diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 39643d5..4ae7d6c 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -7,7 +7,7 @@ def generate_launch_description(): webcam_publisher = get_package_share_path('webcam_publisher') - video_path = "/home/purduerm/Videos/moving_but_no_spinning.avi" # example, can change to your liking + video_path = "/home/user-accounts/public/spintop/moving_but_no_spinning.avi" # example, can change to your liking return LaunchDescription([ Node( package='webcam_publisher', @@ -15,7 +15,7 @@ def generate_launch_description(): emulate_tty=True, executable='VideoCaptureNode', parameters=[{'source': str(video_path), - 'fps': 4, + 'fps': 1, 'frame_id': 'video', }] ), diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index 72ea64a..f5f6d23 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp @@ -40,19 +40,21 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) // Display the cropped frame for debugging #ifdef DEBUG - cv::resize(croppedFrame, croppedFrame, cv::Size(WIDTH / 2, HEIGHT / 2)); + if (_frame_count % 100 == 0) { + cv::resize(croppedFrame, croppedFrame, cv::Size(WIDTH / 2, HEIGHT / 2)); - // Create a static window name - const std::string window_name = "Detection Results"; - cv::imshow(window_name, croppedFrame); + // Create a static window name + const std::string window_name = "Detection Results"; + cv::imshow(window_name, croppedFrame); - // Update the window title - cv::setWindowTitle(window_name, - "detected: " + std::to_string(_detected_frame) + " / " + - std::to_string(_frame_count) + " (" + - std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames")); + // Update the window title + cv::setWindowTitle(window_name, + "detected: " + std::to_string(_detected_frame) + " / " + + std::to_string(_frame_count) + " (" + + std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames")); - cv::waitKey(30); + cv::waitKey(1); + } #endif // If we didn't find an armor for a few frames (ROS2 param), reset the search area diff --git a/src/prm_vision/pose_estimator/include/PoseEstimator.h b/src/prm_vision/pose_estimator/include/PoseEstimator.h index d2cbad8..ffc60f6 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimator.h +++ b/src/prm_vision/pose_estimator/include/PoseEstimator.h @@ -35,6 +35,9 @@ class PoseEstimator // Setters void setNumFramesToFireAfter(int num_frames_to_fire_after) { _num_frames_to_fire_after = num_frames_to_fire_after; } void setAllowedMissedFramesBeforeNoFire(int allowed_missed_frames_before_no_fire) { _allowed_missed_frames_before_no_fire = allowed_missed_frames_before_no_fire; } + void setEpsilon(double epsilon) { _epsilon = epsilon; } + void setMaxIterations(int max_iterations) { _max_iterations = max_iterations; } + void setPitch(double pitch) { _pitch = pitch; } private: // Class methods @@ -55,6 +58,11 @@ class PoseEstimator float _max_shift_distance = 150; int _prev_len = 5; + // Pose Estimator parameters (set dynamically) + double _epsilon = 0.4; + int _max_iterations = 50; + double _pitch = 15.0; + /** * @brief Functor for the loss function and gradient computation. * diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 0c56855..b1991d4 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -33,6 +33,7 @@ class PoseEstimatorNode : public rclcpp::Node // 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); + void drawLineWithAngle(cv::Mat &image, cv::Point2f start_pt, double angle, double length, cv::Scalar color); // Callbacks and publishers/subscribers rclcpp::Subscription::SharedPtr key_points_subscriber; diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp index ee8e54d..91545c3 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -39,8 +39,8 @@ double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector param; - param.epsilon = 0.4; - param.max_iterations = 50; + param.epsilon = _epsilon; + param.max_iterations = _max_iterations; LBFGSBSolver solver(param); LossAndGradient loss(*this, image_points, tvec); @@ -156,7 +156,7 @@ double PoseEstimator::gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec) { // We assume -15 deg pitch and 0 roll - double pitch = -15 * (M_PI / 180); + double pitch = _pitch * CV_PI / 180; cv::Mat Ry = (cv::Mat_(3, 3) << cos(yaw_guess), 0, sin(yaw_guess), 0, 1, 0, diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index e371fcf..4b06faf 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -11,6 +11,10 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node( // Dynamic parameters pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 150)); pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3)); + pose_estimator->setEpsilon(this->declare_parameter("_epsilon", 0.4)); + pose_estimator->setMaxIterations(this->declare_parameter("_max_iterations", 50)); + pose_estimator->setPitch(this->declare_parameter("_pitch", 15.0)); + validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3)); validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000)); validity_filter_.setMinDistance(this->declare_parameter("_min_distance", 10)); @@ -71,6 +75,21 @@ 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() == "_epsilon" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + { + pose_estimator->setEpsilon(param.as_double()); + RCLCPP_INFO(this->get_logger(), "Parameter '_epsilon' updated to: %f", param.as_double()); + } + else if (param.get_name() == "_max_iterations" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + pose_estimator->setMaxIterations(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_max_iterations' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_pitch" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + { + pose_estimator->setPitch(param.as_double()); + RCLCPP_INFO(this->get_logger(), "Parameter '_pitch' updated to: %f", param.as_double()); + } else { result.successful = false; @@ -91,6 +110,9 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha return; } + static int ctr = 0; + ctr++; + cv::Mat tvec, rvec; bool reset_kalman = false; std::string new_auto_aim_status; @@ -132,25 +154,78 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha // Draw top-down view #ifdef DEBUG - drawTopDownViewGivenRotation(_last_yaw_estimate, tvec.at(0), tvec.at(1), tvec.at(2)); + if (ctr % 100 == 0) { + drawTopDownViewGivenRotation(_last_yaw_estimate, tvec.at(0), tvec.at(1), tvec.at(2)); + } #endif } void PoseEstimatorNode::drawTopDownViewGivenRotation(double yaw, double X, double Y, double Z) { - // Draw a line rotated from the x-axis by yaw. - cv::Mat top_down_view = cv::Mat::zeros(1280, 720, CV_8UC3); + // Define image dimensions + const int imageWidth = 800; // Width of the top-down view image + const int imageHeight = 1000; // Height of the top-down view image + + // Create a blank image (white background) + cv::Mat topDownImage(imageHeight, imageWidth, CV_8UC3, cv::Scalar(255, 255, 255)); + + // Define scaling factor (pixels per millimeter) + // Since Z ranges from 500 mm to 4000 mm, we scale to fit within the image height + double scale = imageHeight / 4000.0; // Scale to fit 4000 mm (4 meters) in the image height + + // Map real-world coordinates (X, Z) to image pixel coordinates + int centerX = static_cast(imageWidth / 2 + X * scale); // Center X in image + int centerZ = static_cast(imageHeight - Z * scale); // Center Z in image (flip Y-axis for top-down view) + + // Define line length (adjustable) + double lineLength = 20.0; // Length of the line in pixels + double robot_diameter = 150.0; - // Draw the target offset from the center of the image - int target_x = Z; - int target_y = X; - cv::circle(top_down_view, cv::Point(target_x, target_y), 5, cv::Scalar(0, 255, 0), -1); + // Calculate line endpoints based on yaw angle + double angleRad = -yaw; // Convert yaw to radians - cv::resize(top_down_view, top_down_view, cv::Size(100, 100)); - cv::imshow("Top Down View", top_down_view); + // draw armor + drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad, lineLength, cv::Scalar(0, 0, 255)); + drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad + CV_PI, lineLength, cv::Scalar(0, 0, 255)); + + // draw line of typical robot diameter + drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad - CV_PI / 2, robot_diameter, cv::Scalar(0, 255, 0)); + + // armor at other end of line + drawLineWithAngle(topDownImage, cv::Point2f(centerX + robot_diameter * cos(angleRad - CV_PI / 2), centerZ + robot_diameter * sin(angleRad - CV_PI / 2)), angleRad, lineLength, cv::Scalar(0, 0, 255)); + drawLineWithAngle(topDownImage, cv::Point2f(centerX + robot_diameter * cos(angleRad - CV_PI / 2), centerZ + robot_diameter * sin(angleRad - CV_PI / 2)), angleRad + CV_PI, lineLength, cv::Scalar(0, 0, 255)); + + // more lines of typical robot diameter 90 degrees from the center line + cv::Point2f robot_center = cv::Point2f((centerX + robot_diameter / 2 * cos(angleRad - CV_PI / 2)), (centerZ + robot_diameter / 2 * sin(angleRad - CV_PI / 2))); + drawLineWithAngle(topDownImage, robot_center, angleRad - CV_PI, robot_diameter / 2, cv::Scalar(200, 255, 0)); + drawLineWithAngle(topDownImage, robot_center, angleRad, robot_diameter / 2, cv::Scalar(200, 255, 0)); + + // armor at other end of these two lines, perpendicular to the center line + drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x + robot_diameter / 2 * cos(angleRad), robot_center.y + robot_diameter / 2 * sin(angleRad)), angleRad - CV_PI / 2, lineLength, cv::Scalar(0, 0, 255)); + drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x + robot_diameter / 2 * cos(angleRad), robot_center.y + robot_diameter / 2 * sin(angleRad)), angleRad + CV_PI / 2, lineLength, cv::Scalar(0, 0, 255)); + + // same thing on the other side + drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x - robot_diameter / 2 * cos(angleRad), robot_center.y - robot_diameter / 2 * sin(angleRad)), angleRad - CV_PI / 2, lineLength, cv::Scalar(0, 0, 255)); + drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x - robot_diameter / 2 * cos(angleRad), robot_center.y - robot_diameter / 2 * sin(angleRad)), angleRad + CV_PI / 2, lineLength, cv::Scalar(0, 0, 255)); + + RCLCPP_INFO(get_logger(), "Drawing top-down view with yaw = %.2f, X = %.2f, Y = %.2f, Z = %.2f", 180.0 * yaw / CV_PI, X, Y, Z); + + // Display the image + cv::imshow("Top-Down View", topDownImage); cv::waitKey(1); } +void PoseEstimatorNode::drawLineWithAngle(cv::Mat &image, cv::Point2f start_pt, double angle, double length, cv::Scalar color) +{ + // Calculate endpoint of the line + cv::Point2f end_pt; + end_pt.x = start_pt.x + length * cos(angle); + end_pt.y = start_pt.y + length * sin(angle); + + // Draw the line + cv::line(image, start_pt, end_pt, color, 2); +} + void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status) { vision_msgs::msg::PredictedArmor predicted_armor_msg;