From cfd335177cb233ee06a38a4f8267ea54e1b19f8e Mon Sep 17 00:00:00 2001 From: bkuangs Date: Tue, 20 Jan 2026 23:03:00 -0500 Subject: [PATCH 1/3] 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/3] 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 From 1980ca5f13515a897ddd63a815c84c4d0933a9e5 Mon Sep 17 00:00:00 2001 From: bkuangs Date: Wed, 21 Jan 2026 14:42:24 -0500 Subject: [PATCH 3/3] wk3 submission --- localization/CMakeLists.txt | 1 + .../particle_filter_localizer.launch.py | 9 ++++ localization/src/odometry_sensor_model.cpp | 51 +++++++++++++++++++ localization/src/odometry_sensor_model.hpp | 30 +++++++++++ .../src/particle_filter_localizer.cpp | 2 + .../launch/projects/week_3.launch.xml | 2 +- 6 files changed, 94 insertions(+), 1 deletion(-) create mode 100644 localization/src/odometry_sensor_model.cpp create mode 100644 localization/src/odometry_sensor_model.hpp diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt index bc664aa9c..ef2203bdc 100644 --- a/localization/CMakeLists.txt +++ b/localization/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME}_component SHARED src/random_helpers.cpp src/particle_filter_localizer.cpp # BEGIN STUDENT CODE + src/odometry_sensor_model.cpp # END STUDENT CODE ) ament_target_dependencies(${PROJECT_NAME}_component diff --git a/localization/launch/particle_filter_localizer.launch.py b/localization/launch/particle_filter_localizer.launch.py index 75c61c3b9..da4a56487 100644 --- a/localization/launch/particle_filter_localizer.launch.py +++ b/localization/launch/particle_filter_localizer.launch.py @@ -22,11 +22,16 @@ from launch import LaunchDescription from launch_ros.actions import Node # BEGIN STUDENT CODE +import os +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration # END STUDENT CODE def generate_launch_description(): # BEGIN STUDENT CODE + parameters_file_path = os.path.join(get_package_share_directory( + 'localization'), 'config', 'localizer_params.yaml') # END STUDENT CODE return LaunchDescription([ @@ -35,6 +40,10 @@ def generate_launch_description(): executable='localization_node', output='screen', # BEGIN STUDENT CODE + parameters=[ + parameters_file_path, + {'use_sim_time': LaunchConfiguration('use_sim_time', default='false')} + ], # END STUDENT CODE remappings=[ ('/tags', '/coordinate_transformer/tags_transformed') diff --git a/localization/src/odometry_sensor_model.cpp b/localization/src/odometry_sensor_model.cpp new file mode 100644 index 000000000..4d9f1b638 --- /dev/null +++ b/localization/src/odometry_sensor_model.cpp @@ -0,0 +1,51 @@ +#include "odometry_sensor_model.hpp" +#include +#include + +namespace localization +{ + +OdometrySensorModel::OdometrySensorModel(rclcpp::Node & node) +{ + covariance_ = node.declare_parameter>("sensors.odom.covariance", {0.1, 0.1}); + timeout_ = node.declare_parameter("sensors.odom.measurement_timeout", 0.1); + + odom_sub_ = node.create_subscription( + "/odom", + rclcpp::SystemDefaultsQoS(), + std::bind(&OdometrySensorModel::UpdateMeasurement, this, std::placeholders::_1)); +} + +void OdometrySensorModel::UpdateMeasurement(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + last_msg_ = *msg; +} + +bool OdometrySensorModel::IsMeasurementAvailable(const rclcpp::Time & current_time) +{ + if(last_msg_.header.stamp.sec == 0) + return false; + + const auto time_since_last_msg = current_time - rclcpp::Time(last_msg_.header.stamp); + + return time_since_last_msg.seconds() < timeout_; +} + +double OdometrySensorModel::ComputeLogProb(const Particle& particle) +{ + double log_prob = 0.0; + + log_prob += pow(last_msg_.twist.twist.linear.x - particle.x_vel, 2) / covariance_[0]; + + log_prob += pow((-last_msg_.twist.twist.angular.z) - particle.yaw_vel, 2) / covariance_[1]; + + return log_prob; +} + +double OdometrySensorModel::ComputeLogNormalizer() +{ + return log(sqrt(pow(2 * M_PI, 2))) + + log(sqrt(covariance_[0])) + log(sqrt(covariance_[1])); +} + +} // namespace localization \ No newline at end of file diff --git a/localization/src/odometry_sensor_model.hpp b/localization/src/odometry_sensor_model.hpp new file mode 100644 index 000000000..40f861c47 --- /dev/null +++ b/localization/src/odometry_sensor_model.hpp @@ -0,0 +1,30 @@ +#ifndef ODOMETRY_SENSOR_MODEL_HPP_ +#define ODOMETRY_SENSOR_MODEL_HPP_ + +#include +#include +#include "sensor_model.hpp" + +namespace localization +{ + +class OdometrySensorModel : public SensorModel +{ +public: + OdometrySensorModel(rclcpp::Node& node); + + void UpdateMeasurement(const nav_msgs::msg::Odometry::SharedPtr msg); + + double ComputeLogProb(const Particle& particle) override; + double ComputeLogNormalizer() override; + bool IsMeasurementAvailable(const rclcpp::Time& current_time) override; + +private: + nav_msgs::msg::Odometry last_msg_; + + rclcpp::Subscription::SharedPtr odom_sub_; +}; + +} // namespace localization + +#endif // ODOMETRY_SENSOR_MODEL_HPP_ \ No newline at end of file diff --git a/localization/src/particle_filter_localizer.cpp b/localization/src/particle_filter_localizer.cpp index 4be6d51d3..a42475f14 100644 --- a/localization/src/particle_filter_localizer.cpp +++ b/localization/src/particle_filter_localizer.cpp @@ -27,6 +27,7 @@ #include #include "aruco_sensor_model.hpp" // BEGIN STUDENT CODE +#include "odometry_sensor_model.hpp" // END STUDENT CODE namespace localization @@ -71,6 +72,7 @@ ParticleFilterLocalizer::ParticleFilterLocalizer(const rclcpp::NodeOptions & opt sensor_models_.push_back(std::make_unique(*this)); // BEGIN STUDENT CODE + sensor_models_.push_back(std::make_unique(*this)); // END STUDENT CODE } diff --git a/rj_training_bringup/launch/projects/week_3.launch.xml b/rj_training_bringup/launch/projects/week_3.launch.xml index d40c2ad88..6c9eb5e44 100644 --- a/rj_training_bringup/launch/projects/week_3.launch.xml +++ b/rj_training_bringup/launch/projects/week_3.launch.xml @@ -13,7 +13,7 @@ - +