wk6 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
 }
 

mapping/src/mapping_node.cpp

@@ -26,6 +26,9 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <geometry_msgs/msg/point_stamped.hpp>
 // BEGIN STUDENT CODE
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 // END STUDENT CODE
 
 namespace mapping
@@ -46,8 +49,10 @@ class MappingNode : public rclcpp::Node
 public:
   explicit MappingNode(const rclcpp::NodeOptions & options)
   // BEGIN STUDENT CODE
-  : rclcpp::Node("mapping_node", options)
-    // END STUDENT CODE
+  : rclcpp::Node("mapping_node", options),
+    tf_buffer_(get_clock()),
+    tf_listener_(tf_buffer_)
+  // END STUDENT CODE
   {
     map_frame_id_ = declare_parameter<std::string>("map_frame", "map");
     robot_frame_id_ = declare_parameter<std::string>("robot_frame", "base_link");
@@ -74,11 +79,18 @@ class MappingNode : public rclcpp::Node
       rclcpp::SystemDefaultsQoS());
 
     // BEGIN STUDENT CODE
+    obstacles_subscription_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      "~/obstacles",
+      rclcpp::SystemDefaultsQoS(),
+      [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {ObstaclesCallback(msg);}
+      );
     // END STUDENT CODE
   }
 
 private:
   // BEGIN STUDENT CODE
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
   // END STUDENT CODE
   std::string map_frame_id_;
   std::string robot_frame_id_;
@@ -93,6 +105,12 @@ class MappingNode : public rclcpp::Node
   void ObstaclesCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr obstacles_msg)
   {
     // BEGIN STUDENT CODE
+    if (!tf_buffer_.canTransform(robot_frame_id_, map_frame_id_, tf2::TimePointZero) ||
+        !tf_buffer_.canTransform(obstacles_msg->header.frame_id, map_frame_id_, tf2::TimePointZero))
+    {
+      RCLCPP_INFO_ONCE(get_logger(), "Waiting for necessary TF data to be available.");
+      return;
+    }
     // END STUDENT CODE
 
     AddObstaclesToMap(*obstacles_msg);
@@ -116,7 +134,12 @@ class MappingNode : public rclcpp::Node
   geometry_msgs::msg::Point GetRobotLocation()
   {
     // BEGIN STUDENT CODE
-    return geometry_msgs::msg::Point{};
+    const auto robot_transform = tf_buffer_.lookupTransform(map_frame_id_, robot_frame_id_, tf2::TimePointZero);
+
+    geometry_msgs::msg::Point robot_location;
+    robot_location.x = robot_transform.transform.translation.x;
+    robot_location.y = robot_transform.transform.translation.y;
+    return robot_location;
     // END STUDENT CODE
   }
 
@@ -125,6 +148,32 @@ class MappingNode : public rclcpp::Node
     const auto robot_location = GetRobotLocation();
 
     // BEGIN STUDENT CODE
+    for (int y = 0; y <= obstacles.info.height - 1; ++y) {
+      for (int x = 0; x <= obstacles.info.width - 1; ++x) {
+
+        const auto obstacle_data_index = x + (y * obstacles.info.width);
+        const auto obstacle_data = obstacles.data.at(obstacle_data_index);
+
+        if (obstacle_data == -1) {
+          continue;
+        }
+
+        geometry_msgs::msg::PointStamped obstacle_location;
+        obstacle_location.header.frame_id = obstacles.header.frame_id;
+        obstacle_location.point.x = (x * obstacles.info.resolution) +
+          obstacles.info.origin.position.x;
+        obstacle_location.point.y = (y * obstacles.info.resolution) +
+          obstacles.info.origin.position.y;
+
+        const auto map_location = tf_buffer_.transform(obstacle_location, map_frame_id_);
+
+        if (!IsLocationInMapBounds(map_location.point)) {
+          continue;
+        }
+
+        UpdateProbability(robot_location, map_location.point, obstacle_data == 100);
+      }
+    }
     // END STUDENT CODE
   }
 
@@ -134,6 +183,19 @@ class MappingNode : public rclcpp::Node
     const bool & obstacle_detected)
   {
     // BEGIN STUDENT CODE
+    const int cell_x = (map_location.x - map_info_.origin.position.x) / map_info_.resolution;
+    const int cell_y = (map_location.y - map_info_.origin.position.y) / map_info_.resolution;
+    const auto data_index = MapDataIndexFromLocation(cell_x, cell_y);
+
+    const auto distance_robot_to_measurement = std::hypot(
+      robot_location.x - map_location.x,
+      robot_location.y - map_location.y);
+
+    auto probability = std::exp(-distance_coefficient_ * distance_robot_to_measurement);
+    probability *= (obstacle_detected ? hit_log_odds_ : miss_log_odds_);
+    map_data_[data_index] += probability;
+
+    map_data_[data_index] = std::clamp(map_data_[data_index], toLogOdds(0.01), toLogOdds(0.99));
     // END STUDENT CODE
   }