From cfd335177cb233ee06a38a4f8267ea54e1b19f8e Mon Sep 17 00:00:00 2001 From: bkuangs Date: Tue, 20 Jan 2026 23:03:00 -0500 Subject: [PATCH 1/2] cleanup wk2 --- obstacle_detector/CMakeLists.txt | 3 ++- obstacle_detector/src/obstacle_detector.cpp | 15 ++++++++++-- obstacle_detector/src/student_functions.cpp | 26 +++++++++++++++++++++ obstacle_detector/src/student_functions.hpp | 14 +++++++++++ 4 files changed, 55 insertions(+), 3 deletions(-) create mode 100644 obstacle_detector/src/student_functions.cpp create mode 100644 obstacle_detector/src/student_functions.hpp diff --git a/obstacle_detector/CMakeLists.txt b/obstacle_detector/CMakeLists.txt index 4410ef272..8f0232c1b 100644 --- a/obstacle_detector/CMakeLists.txt +++ b/obstacle_detector/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(OpenCV 4 REQUIRED) add_library(${PROJECT_NAME} SHARED src/obstacle_detector.cpp # BEGIN STUDENT CODE + src/student_functions.cpp # END STUDENT CODE ) ament_target_dependencies(${PROJECT_NAME} @@ -50,4 +51,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/obstacle_detector/src/obstacle_detector.cpp b/obstacle_detector/src/obstacle_detector.cpp index bf6ebac84..e2e963281 100644 --- a/obstacle_detector/src/obstacle_detector.cpp +++ b/obstacle_detector/src/obstacle_detector.cpp @@ -31,6 +31,7 @@ #include // BEGIN STUDENT CODE +#include "student_functions.hpp" // END STUDENT CODE namespace obstacle_detector @@ -44,6 +45,13 @@ class ObstacleDetector : public rclcpp::Node { // BEGIN STUDENT CODE // Initialize publisher and subscriber + occupancy_grid_publisher_ = create_publisher("~/occupancy_grid", rclcpp::SystemDefaultsQoS()); + camera_subscriber_ = image_transport::create_camera_subscription( + this, "/camera/image_raw", + std::bind( + &ObstacleDetector::ImageCallback, this, std::placeholders::_1, + std::placeholders::_2), + "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile()); // END STUDENT CODE declare_parameters( @@ -64,6 +72,8 @@ class ObstacleDetector : public rclcpp::Node // BEGIN STUDENT CODE // Declare subscriber and publisher members + rclcpp::Publisher::SharedPtr occupancy_grid_publisher_ {}; + image_transport::CameraSubscriber camera_subscriber_ {}; // END STUDENT CODE void ImageCallback( @@ -83,7 +93,7 @@ class ObstacleDetector : public rclcpp::Node // BEGIN STUDENT CODE // Call FindColors() - cv::Mat detected_colors; + cv::Mat detected_colors = FindColors(cv_image->image, min_color, max_color); // END STUDENT CODE std::string tf_error_string; @@ -117,7 +127,7 @@ class ObstacleDetector : public rclcpp::Node // BEGIN STUDENT CODE // Call ReprojectToGroundPlane - cv::Mat projected_colors; + cv::Mat projected_colors = ReprojectToGroundPlane(detected_colors, homography, map_size); // END STUDENT CODE cv::rotate(projected_colors, projected_colors, cv::ROTATE_90_CLOCKWISE); @@ -141,6 +151,7 @@ class ObstacleDetector : public rclcpp::Node // BEGIN STUDENT CODE // Publish occupancy_grid_msg + occupancy_grid_publisher_->publish(occupancy_grid_msg); // END STUDENT CODE } diff --git a/obstacle_detector/src/student_functions.cpp b/obstacle_detector/src/student_functions.cpp new file mode 100644 index 000000000..84caf88b6 --- /dev/null +++ b/obstacle_detector/src/student_functions.cpp @@ -0,0 +1,26 @@ +#include "student_functions.hpp" + +cv::Mat FindColors(const cv::Mat input, const cv::Scalar range_min, const cv::Scalar range_max) +{ + cv::Mat input_hsv; + cv::cvtColor(input, input_hsv, cv::COLOR_BGR2HSV); + + cv::Mat output(input.size(), CV_8UC1); + + cv::inRange(input_hsv, range_min, range_max, output); + + return output; +} + +cv::Mat ReprojectToGroundPlane(const cv::Mat input, + const cv::Mat homography, + const cv::Size map_size) +{ + cv::Mat output(map_size, CV_8UC1); + + cv::warpPerspective( + input, output, homography, map_size, cv::INTER_NEAREST, cv::BORDER_CONSTANT, + cv::Scalar(127)); + + return output; +} \ No newline at end of file diff --git a/obstacle_detector/src/student_functions.hpp b/obstacle_detector/src/student_functions.hpp new file mode 100644 index 000000000..fb311bfac --- /dev/null +++ b/obstacle_detector/src/student_functions.hpp @@ -0,0 +1,14 @@ +#ifndef STUDENT_FUNCTIONS_HPP // Check that the unique macro has not already been defined +#define STUDENT_FUNCTIONS_HPP // Define the unique macro + +#include + +cv::Mat FindColors(const cv::Mat input, + const cv::Scalar range_min, + const cv::Scalar range_max); + +cv::Mat ReprojectToGroundPlane(const cv::Mat input, + const cv::Mat homography, + const cv::Size map_size); + +#endif // Close the pre-processor if block \ No newline at end of file From 41a3a6786f3079d00a3380d18a3747ffdb4efaa9 Mon Sep 17 00:00:00 2001 From: bkuangs Date: Tue, 20 Jan 2026 23:05:37 -0500 Subject: [PATCH 2/2] wk1 submission --- .../src/coordinate_transform_component.cpp | 53 ++++++++++++++++++- 1 file changed, 51 insertions(+), 2 deletions(-) diff --git a/coordinate_transform/src/coordinate_transform_component.cpp b/coordinate_transform/src/coordinate_transform_component.cpp index 38ba747a4..aaed41e62 100644 --- a/coordinate_transform/src/coordinate_transform_component.cpp +++ b/coordinate_transform/src/coordinate_transform_component.cpp @@ -23,6 +23,8 @@ #include #include // BEGIN STUDENT CODE +#include +#include // END STUDENT CODE #include #include @@ -80,6 +82,33 @@ class CoordinateTransformComponent : public rclcpp::Node getTransformationMatrixForOpticalFrame(); // BEGIN STUDENT CODE + std::vector new_tags; + + for(const auto tag : tag_array_msg->tags) { + stsl_interfaces::msg::Tag new_tag; + new_tag.id = tag.id; + + Eigen::Vector4d position( + tag.pose.position.x, + tag.pose.position.y, + tag.pose.position.z, + 1 + ); + + position = camera_to_base_transform * camera_optical_to_conventional_transform * position; + + new_tag.pose.position.x = position.x(); + new_tag.pose.position.y = position.y(); + new_tag.pose.position.z = position.z(); + + Eigen::Matrix4d tag_orientation = quaternionMessageToTransformationMatrix(tag.pose.orientation); + + tag_orientation = camera_to_base_transform * camera_optical_to_conventional_transform * tag_orientation; + + new_tag.pose.orientation = transformationMatrixToQuaternionMessage(tag_orientation); + + new_tags.push_back(new_tag); + } // END STUDENT CODE // create a new tag array message @@ -91,6 +120,7 @@ class CoordinateTransformComponent : public rclcpp::Node // BEGIN STUDENT CODE // set message tags to new_tags vector + new_tag_array_msg.tags = new_tags; // END STUDENT CODE // publish new tag message @@ -100,7 +130,26 @@ class CoordinateTransformComponent : public rclcpp::Node Eigen::Matrix4d getTransformationMatrixForOpticalFrame() { // BEGIN STUDENT CODE - return {}; + // Rotation about the X axis by pi/2 + std::array R_roll_data = { + 1, 0, 0, 0, + 0, 0, -1, 0, + 0, 1, 0, 0, + 0, 0, 0, 1 + }; + + // Rotation about the Z axis by pi/2 + std::array R_yaw_data = { + 0, -1, 0, 0, + 1, 0, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + }; + + Eigen::Matrix4d R_roll(R_roll_data.data()); + Eigen::Matrix4d R_yaw(R_yaw_data.data()); + + return R_yaw * R_roll; // END STUDENT CODE } @@ -121,4 +170,4 @@ class CoordinateTransformComponent : public rclcpp::Node }; } // namespace coordinate_transform -RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent) \ No newline at end of file