Week8

astar_path_planner/src/astar_path_planner.cpp

@@ -65,7 +65,35 @@ std::vector<Point> AStarPathPlanner::Plan(const Point & start, const Point & goa
   goal_ = goal;
 
   // BEGIN STUDENT CODE
+  frontier_.push({{start}, GetHeuristicCost(start)});
+
+  while (!frontier_.empty()) {
+    const auto entry = frontier_.top();
+    frontier_.pop();
+    const auto path = entry.path;
+    const auto cost = entry.cost;
+    const auto last_state = path.back();
+
+    if (expanded_.count(last_state) > 0) {
+      continue;
+    }
+
+    expanded_.insert(last_state);
+
+    if (IsGoal(last_state)) {
+      return path;
+    }
+
+    std::vector<Point> neighbors = GetAdjacentPoints(last_state);
+
+    std::for_each(neighbors.begin(), neighbors.end(),
+      [this, &path, &cost](const Point& neighbor) {
+        this->ExtendPathAndAddToFrontier(path, cost, neighbor);
+      }
+    );
+  }
 
+  RCLCPP_ERROR(logger_, "No path found after exhausting search space.");
   return {};
   // END STUDENT CODE
 }
@@ -75,36 +103,61 @@ void AStarPathPlanner::ExtendPathAndAddToFrontier(
   const Point & next_point)
 {
   // BEGIN STUDENT CODE
+  std::vector<Point> new_path = path;
+  new_path.push_back(next_point);
+
+  double old_heuristic = GetHeuristicCost(path.back());
+  double step_cost = GetStepCost(path.back(), next_point);
+  double new_heuristic = GetHeuristicCost(next_point);
 
+  double new_cost = (path_cost - old_heuristic) + step_cost + new_heuristic;
+
+  frontier_.push({new_path, new_cost});
   // END STUDENT CODE
 }
 
 std::vector<Point> AStarPathPlanner::GetAdjacentPoints(const Point & point)
 {
   // BEGIN STUDENT CODE
-  return {};
+  std::vector<Point> neighbors;
+
+  for (double dx = -grid_size_; dx <= grid_size_; dx += grid_size_) {
+    for (double dy = -grid_size_; dy <= grid_size_; dy += grid_size_) {
+      if (std::abs(dx) < 1e-4 && std::abs(dy) < 1e-4) {
+        continue;
+      }
+
+      Point neighbor = point + Point{dx, dy};
+
+      if (!IsPointInCollision(neighbor)) {
+        neighbors.push_back(neighbor);
+      }
+    }
+  }
+
+  return neighbors;
   // END STUDENT CODE
 }
 
 
 double AStarPathPlanner::GetHeuristicCost(const Point & point)
 {
   // BEGIN STUDENT CODE
-  return 0.0;
+  return (point - goal_).norm();
   // END STUDENT CODE
 }
 
 double AStarPathPlanner::GetStepCost(const Point & point, const Point & next)
 {
   // BEGIN STUDENT CODE
-  return 0.0;
+  return (point - next).norm();
   // END STUDENT CODE
 }
 
 bool AStarPathPlanner::IsGoal(const Point & point)
 {
   // BEGIN STUDENT CODE
-  return false;
+  return (point - goal_).norm() < goal_threshold_;
   // END STUDENT CODE
 }
 

controllers/CMakeLists.txt

@@ -14,11 +14,13 @@ find_package(angles REQUIRED)
 
 add_library(controllers SHARED
   # BEGIN STUDENT CODE
+  src/controller_test_client.cpp
   # END STUDENT CODE
   src/lqr_controller.cpp
   src/pid_controller.cpp
   src/test_path_generator.cpp
 )
+
 ament_target_dependencies(controllers
   "rclcpp"
   "rclcpp_action"
@@ -33,6 +35,11 @@ ament_target_dependencies(controllers
 set_property(TARGET controllers PROPERTY CXX_STANDARD 17)
 
 # BEGIN STUDENT CODE
+rclcpp_components_register_node(
+  controllers
+  PLUGIN "controllers::ControllerTestClient"
+  EXECUTABLE controller_test_client
+)
 # END STUDENT CODE
 
 install(TARGETS controllers

controllers/src/controller_test_client.cpp

@@ -0,0 +1,18 @@
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+namespace controller_test_client
+{
+
+class ControllerTestClient : public rclcpp::Node
+{
+public:
+
+explicit ControllerTestClient(const rclcpp::NodeOptions & options)
+: rclcpp::Node("controller_test_client", options)
+{
+}
+
+}; 
+} // namespace controller_test_client
+RCLCPP_COMPONENTS_REGISTER_NODE(controller_test_client::ControllerTestClient)

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
 }