wk3 submission

coordinate_transform/src/coordinate_transform_component.cpp

@@ -23,6 +23,8 @@
 #include <tf2_ros/transform_listener.h>
 #include <string>
 // BEGIN STUDENT CODE
+#include <array>
+#include <vector>
 // END STUDENT CODE
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
@@ -80,6 +82,33 @@ class CoordinateTransformComponent : public rclcpp::Node
       getTransformationMatrixForOpticalFrame();
 
     // BEGIN STUDENT CODE
+    std::vector<stsl_interfaces::msg::Tag> 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<double, 16> 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<double, 16> 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)

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

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')

localization/src/odometry_sensor_model.cpp

@@ -0,0 +1,51 @@
+#include "odometry_sensor_model.hpp"
+#include <cmath>
+#include <vector>
+
+namespace localization
+{
+
+OdometrySensorModel::OdometrySensorModel(rclcpp::Node & node)
+{
+    covariance_ = node.declare_parameter<std::vector<double>>("sensors.odom.covariance", {0.1, 0.1});
+    timeout_ = node.declare_parameter<double>("sensors.odom.measurement_timeout", 0.1);
+
+    odom_sub_ = node.create_subscription<nav_msgs::msg::Odometry>(
+        "/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

localization/src/odometry_sensor_model.hpp

@@ -0,0 +1,30 @@
+#ifndef ODOMETRY_SENSOR_MODEL_HPP_
+#define ODOMETRY_SENSOR_MODEL_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#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<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+};
+
+}  // namespace localization
+
+#endif  // ODOMETRY_SENSOR_MODEL_HPP_

localization/src/particle_filter_localizer.cpp

@@ -27,6 +27,7 @@
 #include <rclcpp_components/register_node_macro.hpp>
 #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<ArucoSensorModel>(*this));
   // BEGIN STUDENT CODE
+  sensor_models_.push_back(std::make_unique<OdometrySensorModel>(*this));
   // END STUDENT CODE
 }
 

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()

obstacle_detector/src/obstacle_detector.cpp

@@ -31,6 +31,7 @@
 #include <opencv2/core/eigen.hpp>
 
 // 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<nav_msgs::msg::OccupancyGrid>("~/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<int>(
@@ -64,6 +72,8 @@ class ObstacleDetector : public rclcpp::Node
 
   // BEGIN STUDENT CODE
   // Declare subscriber and publisher members
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::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
   }
 

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;
+}

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 <opencv2/opencv.hpp>
+
+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

rj_training_bringup/launch/projects/week_3.launch.xml

@@ -13,7 +13,7 @@
   </node>
 
   <!-- BEGIN STUDENT CODE -->
-  <include file="$(find-pkg-share rj_training_bringup)/launch/fake_localizer.launch.py">
+  <include file="$(find-pkg-share localization)/launch/particle_filter_localizer.launch.py">
     <arg name="use_sim_time" value="$(var simulation)"/>
   </include>
   <!-- END STUDENT CODE -->