PC-57 (#3)

* Add controllers and implement PD Controller

* Added ap1_msgs dependency and updated topic paths

* Updated topic locations and message types according to pciv3. Needs ackermann to convert from acceleration to power/steer commands

* Added ackermann controller, car config, and reduced ugliness by 52%

* fix axes bug, add speed control

* Fix 80% bug.
Speed was converging to kp / (kd + kp) because the kd term was being calculated wrong

* Vector hpp file made

* PDController adjusted for vector class

* IController adjusted

* Control node adjuted

* Ackermann controller done

* Add clangd format

* fix vector compile errors

* mv control node -> control_node.cpp

* update build script

---------

Co-authored-by: Logan Ouellette <loganouellettetran@gmail.com>

Author: alyashour

.clang-format

@@ -0,0 +1,12 @@
+BasedOnStyle: LLVM
+IndentWidth: 4
+TabWidth: 4
+UseTab: Never
+ColumnLimit: 100
+BreakBeforeBraces: Allman
+AllowShortIfStatementsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+SpaceBeforeParens: ControlStatements
+SortIncludes: true
+PointerAlignment: Left
+

.github/workflows/colcon_build.yml

@@ -3,7 +3,6 @@ on:
   push:
     branches:
       - main
-      - colcon_script/logan
   pull_request:
     branches:
       - main
@@ -19,22 +18,27 @@ jobs:
     steps:
       - name: Checkout repository
         uses: actions/checkout@v4
+        with:
+          repository: WE-Autopilot/ap1
+          ref: main
 
       - name: Source ROS 2 environment
         run: |
           source /opt/ros/jazzy/setup.bash
           echo "ROS 2 environment sourced"
 
-      - name: Install workspace dependencies
+      - name: Import dependencies
         run: |
-          source /opt/ros/jazzy/setup.bash
-          rosdep update
-          rosdep install --from-paths src --ignore-src -iry
+          mkdir -p src
+          vcs import < .repos
 
-      - name: Build workspace with colcon
+      - name: Build control
         run: |
           source /opt/ros/jazzy/setup.bash
-          colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=${{ env.BUILD_TYPE }}
+          colcon build \
+          --packages-select ap1_control \
+          --symlink-install \
+          --cmake-args -DCMAKE_BUILD_TYPE=${{ env.BUILD_TYPE }}
 
       - name: Run tests
         if: ${{ env.RUN_TESTS == 'true' }}

.gitignore

@@ -1,4 +1,14 @@
 # ROS2 bbgl
 build/
 install/
-log/
+log/
+
+# Cache
+.cache/
+
+# IDEs
+.idea/
+.vscode/
+
+# CMake
+compile_commands.json

CMakeLists.txt

@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.8)
 project(ap1_control)
 
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
@@ -10,8 +14,13 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(ap1_msgs REQUIRED)
 
-add_executable(control_node src/main.cpp)
+add_executable(control_node 
+    src/main.cpp
+    src/control_node.cpp
+    src/ackermann_controller.cpp
+)
 target_include_directories(control_node PUBLIC 
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
@@ -21,6 +30,7 @@ ament_target_dependencies(
   "rclcpp"
   "std_msgs"
   "geometry_msgs"
+  "ap1_msgs"
 )
 
 install(TARGETS 
@@ -29,15 +39,7 @@ install(TARGETS
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  # no auto linting for now
 endif()
 
 ament_package()

control_node_cfg.csv

@@ -0,0 +1,4 @@
+wheelbase,2.5
+a_max,3.0
+delta_max,0.5
+throttle_gain,1.0

include/ap1/control/ackermann_controller.hpp

@@ -0,0 +1,44 @@
+/**
+ * Ackermann controller class.
+ * This is not a type of icontroller, probably should rename that to be clearer.
+ * Created: Nov. 11, 2025
+ * Author(s): Aly Ashour
+ */
+
+#ifndef AP1_ACKERMANN_CONTROLLER_HPP
+#define AP1_ACKERMANN_CONTROLLER_HPP
+
+#include <string>
+#include "vectors.hpp"
+
+namespace ap1::control
+{
+class AckermannController
+{
+  public:
+    struct Config
+    {
+        double L;             // wheelbase in meters
+        double a_max;         // max longitudinal accleration (ms^-2)
+        double delta_max;     // max steering angle
+        double throttle_gain; // scales accel to throttle [-1, 1]
+    };
+
+    explicit AckermannController(const Config &cfg);
+
+    struct Command
+    {
+        double throttle; // normalized [-1, 1]
+        double steering; // in rads
+    };
+
+    Command compute_command(const vec3f& acc, const vec3f& vel);
+
+    static Config load_config(const std::string &path);
+
+  private:
+    Config cfg_;
+};
+} // namespace ap1::control
+
+#endif // AP1_ACKERMANN_CONTROLLER

include/ap1/control/control_node.hpp

@@ -6,54 +6,63 @@
 #ifndef AP1_CONTROL_NODE_HPP
 #define AP1_CONTROL_NODE_HPP
 
+#include <iostream>
+#include <string>
+
 #include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/float32.hpp"
-#include "std_msgs/msg/float32_multi_array.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "geometry_msgs/msg/point32.hpp"
-
-namespace ap1::control {
-    class ControlNode : public rclcpp::Node {
-    private:
-        void on_speed_profile(const std_msgs::msg::Float32MultiArray::SharedPtr) {
-            // todo: implement
-            RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning");
-        }
-
-        void on_path(const std_msgs::msg::Float32MultiArray::SharedPtr) {
-            // todo: implement
-            RCLCPP_INFO(this->get_logger(), "Received new path from Planning");
-        }
-
-        // Fields
-        // Subs
-        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr speed_profile_sub_;
-        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr target_path_sub_;
-
-        // Pubs
-        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr turning_angle_pub_;
-        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr motor_power_pub_; // between -1 and 1? probably
-    public:
-        ControlNode() : Node("control_node") {
-            // # All inputs shabooya
-            // - SPEED PROFILE
-            speed_profile_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
-                "speed_profile", 10, std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)
-            );
-            // - TARGET PATH
-            target_path_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
-                "target_path", 10, std::bind(&ControlNode::on_path, this, std::placeholders::_1)
-            );
-        
-            // # Publishers
-            // - TURNING ANGLE
-            turning_angle_pub_ = this->create_publisher<std_msgs::msg::Float32>("turning_angle", 10);
-            // - MOTOR POWER
-            motor_power_pub_ = this->create_publisher<std_msgs::msg::Float32>("motor_power", 10);
-
-            RCLCPP_INFO(this->get_logger(), "Control Node initialized");
-        }
-    };
-}
-
-#endif // AP1_CONTROL_NODE_HPP
+
+#include "ap1_msgs/msg/motor_power_stamped.hpp"
+#include "ap1_msgs/msg/speed_profile_stamped.hpp"
+#include "ap1_msgs/msg/target_path_stamped.hpp"
+#include "ap1_msgs/msg/turn_angle_stamped.hpp"
+#include "ap1_msgs/msg/vehicle_speed_stamped.hpp"
+
+#include "ap1/control/ackermann_controller.hpp"
+#include "ap1/control/icontroller.hpp"
+
+namespace ap1::control
+{
+class ControlNode : public rclcpp::Node
+{
+  private:
+    // Fields
+
+    // Control Loop
+    const double rate_hz_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // Controller
+    std::unique_ptr<IController> controller_;
+    AckermannController ackermann_controller_;
+
+    // Memory
+    // half these types are very unnecessary, we should just have stampedfloat or
+    // stamped double or something
+    ap1_msgs::msg::SpeedProfileStamped speed_profile_;
+    ap1_msgs::msg::TargetPathStamped target_path_;
+    ap1_msgs::msg::VehicleSpeedStamped vehicle_speed_;
+    ap1_msgs::msg::TurnAngleStamped vehicle_turn_angle;
+
+    // Subs
+    rclcpp::Subscription<ap1_msgs::msg::TargetPathStamped>::SharedPtr target_path_sub_;
+    rclcpp::Subscription<ap1_msgs::msg::SpeedProfileStamped>::SharedPtr speed_profile_sub_;
+    rclcpp::Subscription<ap1_msgs::msg::VehicleSpeedStamped>::SharedPtr vehicle_speed_sub_;
+    rclcpp::Subscription<ap1_msgs::msg::TurnAngleStamped>::SharedPtr vehicle_turn_angle_sub_;
+
+    // Pubs
+    rclcpp::Publisher<ap1_msgs::msg::TurnAngleStamped>::SharedPtr turning_angle_pub_;
+    rclcpp::Publisher<ap1_msgs::msg::MotorPowerStamped>::SharedPtr motor_power_pub_; // between -1 and 1? probably
+
+    // Methods
+    void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile);
+    void on_path(const ap1_msgs::msg::TargetPathStamped target_path);
+    void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed);
+    void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle);
+    void control_loop_callback();
+
+  public:
+    ControlNode(const std::string& cfg_path, float rate_hz = 60);
+};
+} // namespace ap1::control
+
+#endif // AP1_CONTROL_NODE_HPP

include/ap1/control/icontroller.hpp

@@ -0,0 +1,25 @@
+/**
+ * Abstract class (interface) defining what a controller is.
+ * Date: Nov. 10, 2025
+ * Author(s): Aly Ashour
+ */
+
+#ifndef AP1_CONTROLLER_HPP
+#define AP1_CONTROLLER_HPP
+
+#include <vector>
+
+#include "vectors.hpp"
+#include "geometry_msgs/msg/point.hpp"
+
+namespace ap1::control
+{
+class IController
+{
+  public:
+    ~IController() = default;
+    virtual vec3f compute_acceleration(const vec3f& vel, const vec2f& target_pos, const float target_speed) = 0;
+};
+} // namespace ap1::control
+
+#endif // AP1_CONTROLLER_HPP

include/ap1/control/pd_controller.hpp

@@ -0,0 +1,71 @@
+/**
+ * PD Controller Implementation
+ * Date: Nov. 10, 2025
+ * Author(s): Aly Ashour
+ */
+
+#ifndef AP1_PD_CONTROLLER_HPP
+#define AP1_PD_CONTROLLER_HPP
+
+#include <cmath>
+#include <iostream>
+#include <stdexcept>
+#include <vector>
+
+#include "geometry_msgs/msg/point.hpp"
+
+#include "ap1/control/icontroller.hpp"
+#include "vectors.hpp"
+
+#define EPSILON 1e-6
+
+namespace ap1::control
+{
+class PDController : public IController
+{
+    // Stength of the proportional and derivative terms
+    float kp_, kd_;
+    // Last velocity vector
+    vec3f last_vel_;
+
+  public:
+    PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {};
+
+    // Computes acceleration based on current velocity, target position, and target speed. Returns
+    // acceleration vector.
+    virtual vec3f compute_acceleration(const vec3f& vel,
+                                       const vec2f& target_pos,
+                                       const float target_speed) override
+    {
+        // Convert target position to vec3f and calculate distance
+        const float distance = target_pos.length();
+
+        // targetDir for normalizing target position into unit direction
+        vec2f targetDir;
+        if (distance > EPSILON)
+            targetDir = target_pos.unit_vector();
+        else
+        {
+            targetDir = vec2f(0.0, 0.0);
+        }
+
+        // Scale direction vector by target speed for velocity
+        vec3f targetVel = targetDir * target_speed;
+
+        std::cout << "target_dir_x (SHOULD BE 1): " << targetDir.x
+                  << ", target_dir_y: " << targetDir.y
+                  << "\ntarget_vel_x (should be target speed): " << targetVel.x << "\n";
+
+        // Store current velocity for derivative approximation
+        last_vel_ = vel;
+
+        // Calculate velocity change (numerical derivative)
+        vec3f drv = vel - last_vel_;
+
+        // PD controller: proportional term tracks target velocity, derivative term adds damping
+        return kp_ * (targetVel - vel) - kd_ * drv;
+    };
+};
+} // namespace ap1::control
+
+#endif // AP1_PD_CONTROLLER_HPP