Week7

controllers/CMakeLists.txt

@@ -14,6 +14,7 @@ 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
@@ -33,6 +34,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,110 @@
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <nav2_msgs/action/follow_path.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include "test_path_generator.hpp"
+
+namespace controllers
+{
+
+class ControllerTestClient : public rclcpp::Node
+{
+using FollowPath = nav2_msgs::action::FollowPath;
+using GoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
+
+public:
+
+    explicit ControllerTestClient(const rclcpp::NodeOptions & options)
+    : rclcpp::Node("controller_test_client", options)
+    {
+        client_ = rclcpp_action::create_client<FollowPath>(this, "follow_path");
+
+        rclcpp::QoS qos_profile(1);
+        qos_profile.transient_local();
+
+        publisher_ = create_publisher<nav_msgs::msg::Path>(
+            "/plan",
+            qos_profile);
+
+        std::thread(&ControllerTestClient::SendGoal, this).detach();
+    }
+
+private:
+    void SendGoal()
+    {
+        if (!client_->wait_for_action_server(std::chrono::seconds(10))) {
+            RCLCPP_ERROR(get_logger(), "Action server waited too long");
+            rclcpp::shutdown();
+            return;
+        }
+
+        FollowPath::Goal goal;
+        goal.path = controllers::TestPathGenerator(20).BuildPath();
+        goal.path.header.frame_id = "map";
+        goal.path.header.stamp = now();
+
+        publisher_->publish(goal.path);
+
+        auto send_goal_options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
+
+        send_goal_options.goal_response_callback = std::bind(
+            &ControllerTestClient::GoalResponseCallback,
+            this,
+            std::placeholders::_1);
+
+        send_goal_options.feedback_callback = std::bind(
+            &ControllerTestClient::FeedbackCallback, 
+            this,
+            std::placeholders::_1,
+            std::placeholders::_2);
+
+        send_goal_options.result_callback = std::bind(
+            &ControllerTestClient::ResultCallback,
+            this,
+            std::placeholders::_1);
+
+        client_->async_send_goal(goal, send_goal_options);
+    }
+
+private:
+    rclcpp_action::Client<FollowPath>::SharedPtr client_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr publisher_;
+
+    void GoalResponseCallback(const GoalHandle::SharedPtr & goal_handle)
+    {
+        if (goal_handle) {
+            RCLCPP_INFO(get_logger(), "Goal ACCEPTED");
+        } else {
+            RCLCPP_INFO(get_logger(), "Goal REJECTED");
+        }
+    }
+
+    void FeedbackCallback(
+        GoalHandle::SharedPtr, 
+        const std::shared_ptr<const FollowPath::Feedback> feedback)
+    {
+        RCLCPP_INFO(this->get_logger(), "%f distance left to the goal", feedback->distance_to_goal);
+    }
+
+    void ResultCallback(const GoalHandle::WrappedResult & result)
+    {
+        switch (result.code) {
+            case rclcpp_action::ResultCode::SUCCEEDED:
+                RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
+                break;
+            case rclcpp_action::ResultCode::ABORTED:
+                RCLCPP_INFO(this->get_logger(), "Goal aborted!");
+                break;
+            case rclcpp_action::ResultCode::CANCELED:
+                RCLCPP_INFO(this->get_logger(), "Goal cancelled!");
+                break;
+            case rclcpp_action::ResultCode::UNKNOWN:
+                RCLCPP_INFO(this->get_logger(), "Goal unkown!");
+                break;
+        }
+        rclcpp::shutdown();
+    }
+};
+
+}  // namespace controllers
+RCLCPP_COMPONENTS_REGISTER_NODE(controllers::ControllerTestClient)

controllers/src/lqr_controller.cpp

@@ -53,6 +53,42 @@ class LqrController : public nav2_core::Controller
       rclcpp::SystemDefaultsQoS());
 
     // BEGIN STUDENT CODE
+    T_ = node_shared->declare_parameter<double>(name + ".T", 1.0);
+    dt_ = node_shared->declare_parameter<double>(name + ".dt", 0.1);
+    time_between_states_ = node_shared->declare_parameter<double>(name + ".time_between_states", 3.0);
+    iterations_ = node_shared->declare_parameter<int>(name + ".iterations", 1);
+
+    std::vector<double> Q_temp = node_shared->declare_parameter<std::vector<double>>(name+".Q", {1.0, 1.0, 0.3});
+    if(Q_temp.size() != 3) {
+      RCLCPP_ERROR(node_shared->get_logger(), "incorrect size Q, must be 3 values");
+      exit(0);
+    }
+    Q_(0, 0) = Q_temp[0];
+    Q_(1, 1) = Q_temp[1];
+    Q_(2, 2) = Q_temp[2];
+
+    std::vector<double> Qf_temp = node_shared->declare_parameter<std::vector<double>>(name+".Qf", {10.0, 10.0, 0.1});
+    if(Qf_temp.size() != 3) {
+      RCLCPP_ERROR(node_shared->get_logger(), "incorrect size Qf, must be 3 values");
+      exit(0);
+    }
+    Qf_(0, 0) = Qf_temp[0];
+    Qf_(1, 1) = Qf_temp[1];
+    Qf_(2, 2) = Qf_temp[2];
+
+    std::vector<double> R_temp = node_shared->declare_parameter<std::vector<double>>(name+".R", {0.1, 0.05});
+    if(R_temp.size() != 2) {
+      RCLCPP_ERROR(node_shared->get_logger(), "incorrect size R, must be 2 values");
+      exit(0);
+    }
+    R_(0, 0) = R_temp[0];
+    R_(1, 1) = R_temp[1];
+
+    int N = static_cast<int>(T_ / dt_);
+
+    prev_x_ = std::vector<Eigen::Vector3d>(N, Eigen::Vector3d::Zero());
+    prev_u_ = std::vector<Eigen::Vector2d>(N, Eigen::Vector2d::Zero());
+    S_      = std::vector<Eigen::Matrix3d>(N, Eigen::Matrix3d::Zero());
     // END STUDENT CODE
   }
 

controllers/src/pid_controller.cpp

@@ -53,6 +53,26 @@ class PIDController : public nav2_core::Controller
       rclcpp::SystemDefaultsQoS());
 
     // BEGIN STUDENT CODE
+    bx_P_ = node_shared->declare_parameter<double>(name + ".bx.P", 1.0);
+    by_P_ = node_shared->declare_parameter<double>(name + ".by.P", 1.0);
+    yaw_P_ = node_shared->declare_parameter<double>(name + ".yaw.P", 1.0);
+    bx_D_ = node_shared->declare_parameter<double>(name + ".bx.D", 0.0);
+    by_D_ = node_shared->declare_parameter<double>(name + ".by.D", 0.0);
+    yaw_D_ = node_shared->declare_parameter<double>(name + ".yaw.D", 0.0);
+    bx_I_ = node_shared->declare_parameter<double>(name + ".bx.I", 0.0);
+    by_I_ = node_shared->declare_parameter<double>(name + ".by.I", 0.0);
+    yaw_I_ = node_shared->declare_parameter<double>(name + ".yaw.I", 0.0);
+
+    time_between_states_ = node_shared->declare_parameter<double>(name + ".time_between_states", 3.0);
+
+    integral_max_ = node_shared->declare_parameter<std::vector<double>>(name + ".integral_max", {1.0, 1.0, 1.0});
+    if (integral_max_.size() != 3) {
+      RCLCPP_ERROR(node_shared->get_logger(), "incorrect size integral_max, must be 3 values");
+      exit(0);
+    }
+
+    prev_error_ = Eigen::Vector3d::Zero();
+    integral_error_ = Eigen::Vector3d::Zero();
     // END STUDENT CODE
   }
 
@@ -131,6 +151,13 @@ class PIDController : public nav2_core::Controller
     R << cos(state(2)), -sin(state(2)), 0, sin(state(2)), cos(state(2)), 0, 0, 0, 1;
 
     // BEGIN STUDENT CODE
+    error = target_state - state;
+    error(2) = angles::shortest_angular_distance(state(2), target_state(2));
+    error = R.transpose() * error;
+
+    error_delta = (error - prev_error_) / dt;
+    integral_error_ += error * dt;
+    prev_error_ = error;
     // END STUDENT CODE
   }
 
@@ -139,7 +166,7 @@ class PIDController : public nav2_core::Controller
     double P, double D, double I)
   {
     // BEGIN STUDENT CODE
-    return 0;
+    return (error * P) + (error_delta * D) + (integral_error * I);
     // END STUDENT CODE
   }
 

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