From 8f1d516b14ecbd3c78327267385a0251e56a08f9 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Mon, 10 Nov 2025 13:57:00 -0500 Subject: [PATCH 01/15] Add controllers and implement PD Controller --- .gitignore | 12 ++- CMakeLists.txt | 4 +- include/ap1/control/control_node.hpp | 113 +++++++++++++++---------- include/ap1/control/controller.hpp | 25 ++++++ include/ap1/control/pd_controller.hpp | 82 ++++++++++++++++++ include/ap1/control/pid_controller.hpp | 0 6 files changed, 188 insertions(+), 48 deletions(-) create mode 100644 include/ap1/control/controller.hpp create mode 100644 include/ap1/control/pd_controller.hpp create mode 100644 include/ap1/control/pid_controller.hpp diff --git a/.gitignore b/.gitignore index 8b2e828..f700eed 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,14 @@ # ROS2 bbgl build/ install/ -log/ \ No newline at end of file +log/ + +# Cache +.cache/ + +# IDEs +.idea/ +.vscode/ + +# CMake +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 854a311..5cd9efa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -add_executable(control_node src/main.cpp) +add_executable(control_node + src/main.cpp +) target_include_directories(control_node PUBLIC $ $ diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 1e68857..4224666 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -6,54 +6,75 @@ #ifndef AP1_CONTROL_NODE_HPP #define AP1_CONTROL_NODE_HPP +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point32.hpp" #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" + +#include "ap1/control/controller.hpp" +#include "ap1/control/pd_controller.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::SharedPtr speed_profile_sub_; - rclcpp::Subscription::SharedPtr target_path_sub_; - - // Pubs - rclcpp::Publisher::SharedPtr turning_angle_pub_; - rclcpp::Publisher::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( - "speed_profile", 10, std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1) - ); - // - TARGET PATH - target_path_sub_ = this->create_subscription( - "target_path", 10, std::bind(&ControlNode::on_path, this, std::placeholders::_1) - ); - - // # Publishers - // - TURNING ANGLE - turning_angle_pub_ = this->create_publisher("turning_angle", 10); - // - MOTOR POWER - motor_power_pub_ = this->create_publisher("motor_power", 10); - - RCLCPP_INFO(this->get_logger(), "Control Node initialized"); - } - }; -} - -#endif // AP1_CONTROL_NODE_HPP \ No newline at end of file +class ControlNode : public rclcpp::Node { +private: + // Fields + + // Controller + // this should be a sharedptr or something NOT raw. + ap1::control::IController *controller_; + + // Memory + + // Subs + rclcpp::Subscription::SharedPtr + speed_profile_sub_; + rclcpp::Subscription::SharedPtr + target_path_sub_; + + // Pubs + rclcpp::Publisher::SharedPtr turning_angle_pub_; + rclcpp::Publisher::SharedPtr + motor_power_pub_; // between -1 and 1? probably + + 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"); + } + +public: + ControlNode() : Node("control_node") { + controller_ = new ap1::control::PDController(); + + // # All inputs shabooya + // - SPEED PROFILE + speed_profile_sub_ = + this->create_subscription( + "speed_profile", 10, + std::bind(&ControlNode::on_speed_profile, this, + std::placeholders::_1)); + // - TARGET PATH + target_path_sub_ = + this->create_subscription( + "target_path", 10, + std::bind(&ControlNode::on_path, this, std::placeholders::_1)); + + // # Publishers + // - TURNING ANGLE + turning_angle_pub_ = + this->create_publisher("turning_angle", 10); + // - MOTOR POWER + motor_power_pub_ = + this->create_publisher("motor_power", 10); + + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); + } +}; +} // namespace ap1::control + +#endif // AP1_CONTROL_NODE_HPP diff --git a/include/ap1/control/controller.hpp b/include/ap1/control/controller.hpp new file mode 100644 index 0000000..91531ca --- /dev/null +++ b/include/ap1/control/controller.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 + +#include "geometry_msgs/msg/point.hpp" + +namespace ap1::control { +class IController { +public: + ~IController() = default; + virtual std::vector + compute_acceleration(std::vector vel, + geometry_msgs::msg::Point target_pos, + float target_speed) = 0; +}; +} // namespace ap1::control + +#endif // AP1_CONTROLLER_HPP diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp new file mode 100644 index 0000000..5feeea2 --- /dev/null +++ b/include/ap1/control/pd_controller.hpp @@ -0,0 +1,82 @@ +/** + * PD Controller Implementation + * Date: Nov. 10, 2025 + * Author(s): Aly Ashour + */ + +#ifndef AP1_PD_CONTROLLER_HPP +#define AP1_PD_CONTROLLER_HPP + +#include +#include +#include + +#include "geometry_msgs/msg/point.hpp" + +#include "ap1/control/controller.hpp" + +#define EPSILON 1e-6 + +// THESE SHOULD NOT BE HERE GANG +// very uncouth. +float length(geometry_msgs::msg::Point p) { + return std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2)); +} + +geometry_msgs::msg::Point get_norm(geometry_msgs::msg::Point p) { + const float l = length(p); + geometry_msgs::msg::Point p2; + p2.x = p.x / l; + p2.y = p.y / l; + p2.z = p.z / l; + return p2; +} + +namespace ap1::control { +class PDController : public IController { + float kp_, kd_; + +public: + PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; + + // computes accceleration according to a pid controller + // currently stores vectors half in the ROS *Point* format and half in + // std::vectors. This is super not sigma and we should isolate out both with a + // vec class for all of ap1 (since ros2 points don't have scalar mult, dot, or + // cross or anything). + virtual std::vector + compute_acceleration(std::vector vel, + geometry_msgs::msg::Point target_pos, + float target_speed) override { + // CHECKS + // if vel contains less than 3 vals throw an error + if (vel.size() < 3) { + throw std::invalid_argument( + "vel must have at least three elements (x, y, z)."); + } + + const float distance = length(target_pos); + geometry_msgs::msg::Point target_dir; + + if (distance > EPSILON) + target_dir = get_norm(target_pos); + else { + target_dir.x = 0; + target_dir.y = 0; + target_dir.z = 0; + } + + float target_vel_x = target_dir.x * target_speed; + float target_vel_y = target_dir.y * target_speed; + float target_vel_z = target_dir.z * target_speed; + + std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * vel.at(0), + kp_ * (target_vel_y - vel.at(1)) - kd_ * vel.at(1), + kp_ * (target_vel_z - vel.at(2)) - kd_ * vel.at(2)}; + + return acc; + }; +}; +} // namespace ap1::control + +#endif // AP1_PD_CONTROLLER_HPP diff --git a/include/ap1/control/pid_controller.hpp b/include/ap1/control/pid_controller.hpp new file mode 100644 index 0000000..e69de29 From 238725c11a22e9e89ff8318e46437adf0ea27c70 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Mon, 10 Nov 2025 16:33:40 -0500 Subject: [PATCH 02/15] Added ap1_msgs dependency and updated topic paths --- CMakeLists.txt | 2 ++ include/ap1/control/control_node.hpp | 29 +++++++++++++++------------- package.xml | 5 +++++ 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cd9efa..af681ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ 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 @@ -23,6 +24,7 @@ ament_target_dependencies( "rclcpp" "std_msgs" "geometry_msgs" + "ap1_msgs" ) install(TARGETS diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 4224666..b81c011 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -12,6 +12,9 @@ #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/float32_multi_array.hpp" +#include "ap1_msgs/msg/speed_profile_stamped.hpp" +#include "ap1_msgs/msg/target_path_stamped.hpp" + #include "ap1/control/controller.hpp" #include "ap1/control/pd_controller.hpp" @@ -27,22 +30,22 @@ class ControlNode : public rclcpp::Node { // Memory // Subs - rclcpp::Subscription::SharedPtr - speed_profile_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr target_path_sub_; + rclcpp::Subscription::SharedPtr + speed_profile_sub_; // Pubs rclcpp::Publisher::SharedPtr turning_angle_pub_; rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably - void on_speed_profile(const std_msgs::msg::Float32MultiArray::SharedPtr) { + void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped) { // todo: implement RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning"); } - void on_path(const std_msgs::msg::Float32MultiArray::SharedPtr) { + void on_path(const ap1_msgs::msg::TargetPathStamped) { // todo: implement RCLCPP_INFO(this->get_logger(), "Received new path from Planning"); } @@ -54,23 +57,23 @@ class ControlNode : public rclcpp::Node { // # All inputs shabooya // - SPEED PROFILE speed_profile_sub_ = - this->create_subscription( - "speed_profile", 10, + this->create_subscription( + "ap1/planning/speed_profile", 10, std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); // - TARGET PATH target_path_sub_ = - this->create_subscription( - "target_path", 10, + this->create_subscription( + "ap1/planning/speed_profile", 10, std::bind(&ControlNode::on_path, this, std::placeholders::_1)); // # Publishers // - TURNING ANGLE - turning_angle_pub_ = - this->create_publisher("turning_angle", 10); + turning_angle_pub_ = this->create_publisher( + "ap1/control/motor_power", 10); // - MOTOR POWER - motor_power_pub_ = - this->create_publisher("motor_power", 10); + motor_power_pub_ = this->create_publisher( + "ap1/control/turn_angle", 10); RCLCPP_INFO(this->get_logger(), "Control Node initialized"); } diff --git a/package.xml b/package.xml index 0bf67dd..8116814 100644 --- a/package.xml +++ b/package.xml @@ -9,6 +9,11 @@ ament_cmake + rclcpp + std_msgs + geometry_msgs + ap1_msgs + ament_lint_auto ament_lint_common From e6546c22c0444e25c0c9b6a76141d0f7750d20a8 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Mon, 10 Nov 2025 17:26:14 -0500 Subject: [PATCH 03/15] Updated topic locations and message types according to pciv3. Needs ackermann to convert from acceleration to power/steer commands --- include/ap1/control/control_node.hpp | 86 ++++++++++++++++++++++------ 1 file changed, 70 insertions(+), 16 deletions(-) diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index b81c011..5a78f09 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -6,14 +6,13 @@ #ifndef AP1_CONTROL_NODE_HPP #define AP1_CONTROL_NODE_HPP -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/point32.hpp" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/float32_multi_array.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/controller.hpp" #include "ap1/control/pd_controller.hpp" @@ -23,35 +22,74 @@ class ControlNode : public rclcpp::Node { private: // Fields + // # Create Control Loop + // fire at rate_hz + const double rate_hz_; + rclcpp::TimerBase::SharedPtr timer_; + // Controller // this should be a sharedptr or something NOT raw. ap1::control::IController *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; + + // target speed // Subs rclcpp::Subscription::SharedPtr target_path_sub_; rclcpp::Subscription::SharedPtr speed_profile_sub_; + rclcpp::Subscription::SharedPtr + vehicle_speed_sub_; + rclcpp::Subscription::SharedPtr + vehicle_turn_angle_sub_; // Pubs - rclcpp::Publisher::SharedPtr turning_angle_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr + turning_angle_pub_; + rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably - void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped) { - // todo: implement - RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning"); + void + on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile) { + speed_profile_ = speed_profile; + } + + void on_path(const ap1_msgs::msg::TargetPathStamped target_path) { + target_path_ = target_path; + } + + void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed) { + vehicle_speed_ = speed; } - void on_path(const ap1_msgs::msg::TargetPathStamped) { - // todo: implement - RCLCPP_INFO(this->get_logger(), "Received new path from Planning"); + void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle) { + vehicle_turn_angle = turn_angle; + } + + void control_loop_callback() { + const std::vector velocity{this->vehicle_speed_.speed, 0, + 0}; // which direction is up? assuming +x + + const std::vector acc = controller_->compute_acceleration( + velocity, + target_path_.path.at(0), // for now get the first path waypoint + speed_profile_.speeds.at(0) // and the first speed value + ); + + // send the acceleration through publisher topics + // we need to implement ackermann steering here. } public: - ControlNode() : Node("control_node") { + ControlNode(float rate_hz = 60) : Node("control_node"), rate_hz_(rate_hz) { controller_ = new ap1::control::PDController(); // # All inputs shabooya @@ -66,15 +104,31 @@ class ControlNode : public rclcpp::Node { this->create_subscription( "ap1/planning/speed_profile", 10, std::bind(&ControlNode::on_path, this, std::placeholders::_1)); + vehicle_speed_sub_ = + this->create_subscription( + "ap1/actuation/speed_actual", 10, + std::bind(&ControlNode::on_speed, this, std::placeholders::_1)); + vehicle_turn_angle_sub_ = + this->create_subscription( + "ap1/actuation/turn_angle_actual", 10, + std::bind(&ControlNode::on_turn_angle, this, + std::placeholders::_1)); // # Publishers // - TURNING ANGLE - turning_angle_pub_ = this->create_publisher( - "ap1/control/motor_power", 10); + turning_angle_pub_ = + this->create_publisher( + "ap1/control/motor_power", 10); // - MOTOR POWER - motor_power_pub_ = this->create_publisher( + motor_power_pub_ = this->create_publisher( "ap1/control/turn_angle", 10); + // # Create Control Loop + // fire at rate_hz + timer_ = this->create_wall_timer( + std::chrono::duration(1.0 / rate_hz), + std::bind(&ControlNode::control_loop_callback, this)); + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); } }; From af40ef27c85bace223ae414c047122b9494ffe3a Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Tue, 11 Nov 2025 23:14:14 -0500 Subject: [PATCH 04/15] Added ackermann controller, car config, and reduced ugliness by 52% --- CMakeLists.txt | 1 + control_node_cfg.csv | 4 + include/ap1/control/ackermann_controller.hpp | 44 ++++ include/ap1/control/control_node.hpp | 243 ++++++++++--------- include/ap1/control/controller.hpp | 25 -- include/ap1/control/icontroller.hpp | 26 ++ include/ap1/control/pd_controller.hpp | 113 ++++----- src/ackermann_controller.cpp | 121 +++++++++ src/main.cpp | 26 +- 9 files changed, 411 insertions(+), 192 deletions(-) create mode 100644 control_node_cfg.csv create mode 100644 include/ap1/control/ackermann_controller.hpp delete mode 100644 include/ap1/control/controller.hpp create mode 100644 include/ap1/control/icontroller.hpp create mode 100644 src/ackermann_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index af681ca..8b2fc4e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ap1_msgs REQUIRED) add_executable(control_node src/main.cpp + src/ackermann_controller.cpp ) target_include_directories(control_node PUBLIC $ diff --git a/control_node_cfg.csv b/control_node_cfg.csv new file mode 100644 index 0000000..7adc715 --- /dev/null +++ b/control_node_cfg.csv @@ -0,0 +1,4 @@ +wheelbase,2.5 +a_max,3.0 +delta_max,0.5 +throttle_gain,1.0 diff --git a/include/ap1/control/ackermann_controller.hpp b/include/ap1/control/ackermann_controller.hpp new file mode 100644 index 0000000..3264b15 --- /dev/null +++ b/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 +#include + +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 std::vector &acc, const std::vector &vel); + + static Config load_config(const std::string &path); + + private: + Config cfg_; +}; +} // namespace ap1::control + +#endif // AP1_ACKERMANN_CONTROLLER diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 5a78f09..0ad57ea 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -14,123 +14,146 @@ #include "ap1_msgs/msg/turn_angle_stamped.hpp" #include "ap1_msgs/msg/vehicle_speed_stamped.hpp" -#include "ap1/control/controller.hpp" +#include "ap1/control/ackermann_controller.hpp" +#include "ap1/control/icontroller.hpp" #include "ap1/control/pd_controller.hpp" -namespace ap1::control { -class ControlNode : public rclcpp::Node { -private: - // Fields - - // # Create Control Loop - // fire at rate_hz - const double rate_hz_; - rclcpp::TimerBase::SharedPtr timer_; - - // Controller - // this should be a sharedptr or something NOT raw. - ap1::control::IController *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; - - // target speed - - // Subs - rclcpp::Subscription::SharedPtr - target_path_sub_; - rclcpp::Subscription::SharedPtr - speed_profile_sub_; - rclcpp::Subscription::SharedPtr - vehicle_speed_sub_; - rclcpp::Subscription::SharedPtr - vehicle_turn_angle_sub_; - - // Pubs - rclcpp::Publisher::SharedPtr - turning_angle_pub_; - rclcpp::Publisher::SharedPtr - motor_power_pub_; // between -1 and 1? probably - - void - on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile) { - speed_profile_ = speed_profile; - } - - void on_path(const ap1_msgs::msg::TargetPathStamped target_path) { - target_path_ = target_path; - } - - void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed) { - vehicle_speed_ = speed; - } - - void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle) { - vehicle_turn_angle = turn_angle; - } - - void control_loop_callback() { - const std::vector velocity{this->vehicle_speed_.speed, 0, - 0}; // which direction is up? assuming +x - - const std::vector acc = controller_->compute_acceleration( - velocity, - target_path_.path.at(0), // for now get the first path waypoint - speed_profile_.speeds.at(0) // and the first speed value - ); - - // send the acceleration through publisher topics - // we need to implement ackermann steering here. - } - -public: - ControlNode(float rate_hz = 60) : Node("control_node"), rate_hz_(rate_hz) { - controller_ = new ap1::control::PDController(); - - // # All inputs shabooya - // - SPEED PROFILE - speed_profile_sub_ = - this->create_subscription( - "ap1/planning/speed_profile", 10, - std::bind(&ControlNode::on_speed_profile, this, - std::placeholders::_1)); - // - TARGET PATH - target_path_sub_ = - this->create_subscription( +namespace ap1::control +{ +class ControlNode : public rclcpp::Node +{ + private: + // Fields + + // # Create Control Loop + // fire at rate_hz + const double rate_hz_; + rclcpp::TimerBase::SharedPtr timer_; + + // Controller + // these should be a sharedptr or something NOT raw. + 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::SharedPtr target_path_sub_; + rclcpp::Subscription::SharedPtr speed_profile_sub_; + rclcpp::Subscription::SharedPtr vehicle_speed_sub_; + rclcpp::Subscription::SharedPtr vehicle_turn_angle_sub_; + + // Pubs + rclcpp::Publisher::SharedPtr turning_angle_pub_; + rclcpp::Publisher::SharedPtr + motor_power_pub_; // between -1 and 1? probably + + void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile) + { + speed_profile_ = speed_profile; + } + + void on_path(const ap1_msgs::msg::TargetPathStamped target_path) + { + target_path_ = target_path; + } + + void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed) + { + vehicle_speed_ = speed; + } + + void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle) + { + vehicle_turn_angle = turn_angle; + } + + void control_loop_callback() + { + const std::vector velocity{this->vehicle_speed_.speed, 0, + 0}; // which direction is up? assuming +x + + const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP + + // if path has no waypoints OR path is too old + if (target_path_.path.size() < 1 || PATH_IS_STALE) + return; // don't update + // todo: ideally this should actually still + // send through the speed control + // but we'll implement that later + + // if speed profile has no waypoints or speed profile is too old + if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE) + return; + + const std::vector acc = controller_->compute_acceleration( + velocity, + target_path_.path.at(0), // for now get the first path waypoint + speed_profile_.speeds.at(0) // and the first speed value + ); + + // compute acc and throttle using ackermann controller + AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity); + + // pack the turn angle into a message + ap1_msgs::msg::TurnAngleStamped turn_msg; + turn_msg.header.stamp = this->now(); + turn_msg.header.frame_id = "base_link"; + turn_msg.angle = cmd.steering; // rads + + // pack the power into a message + ap1_msgs::msg::MotorPowerStamped pwr_msg; + pwr_msg.header.stamp = this->now(); + pwr_msg.header.frame_id = "base_link"; + pwr_msg.power = cmd.throttle; // [-1, 1] + + // send both messages out + turning_angle_pub_->publish(turn_msg); + motor_power_pub_->publish(pwr_msg); + } + + public: + ControlNode(const std::string& cfg_path, float rate_hz = 60) + : Node("control_node"), rate_hz_(rate_hz), controller_(new PDController()), + ackermann_controller_(AckermannController(AckermannController::load_config(cfg_path))) + { + // # All inputs shabooya + // - SPEED PROFILE + speed_profile_sub_ = this->create_subscription( "ap1/planning/speed_profile", 10, + std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); + // - TARGET PATH + target_path_sub_ = this->create_subscription( + "ap1/planning/target_path", 10, std::bind(&ControlNode::on_path, this, std::placeholders::_1)); - vehicle_speed_sub_ = - this->create_subscription( + vehicle_speed_sub_ = this->create_subscription( "ap1/actuation/speed_actual", 10, std::bind(&ControlNode::on_speed, this, std::placeholders::_1)); - vehicle_turn_angle_sub_ = - this->create_subscription( + vehicle_turn_angle_sub_ = this->create_subscription( "ap1/actuation/turn_angle_actual", 10, - std::bind(&ControlNode::on_turn_angle, this, - std::placeholders::_1)); - - // # Publishers - // - TURNING ANGLE - turning_angle_pub_ = - this->create_publisher( - "ap1/control/motor_power", 10); - // - MOTOR POWER - motor_power_pub_ = this->create_publisher( - "ap1/control/turn_angle", 10); - - // # Create Control Loop - // fire at rate_hz - timer_ = this->create_wall_timer( - std::chrono::duration(1.0 / rate_hz), - std::bind(&ControlNode::control_loop_callback, this)); - - RCLCPP_INFO(this->get_logger(), "Control Node initialized"); - } + std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1)); + + // # Publishers + // - TURNING ANGLE + turning_angle_pub_ = + this->create_publisher("ap1/control/motor_power", 10); + // - MOTOR POWER + motor_power_pub_ = + this->create_publisher("ap1/control/turn_angle", 10); + + // # Create Control Loop + // fire at rate_hz + timer_ = this->create_wall_timer(std::chrono::duration(1.0 / rate_hz), + std::bind(&ControlNode::control_loop_callback, this)); + + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); + } }; } // namespace ap1::control diff --git a/include/ap1/control/controller.hpp b/include/ap1/control/controller.hpp deleted file mode 100644 index 91531ca..0000000 --- a/include/ap1/control/controller.hpp +++ /dev/null @@ -1,25 +0,0 @@ -/** - * 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 - -#include "geometry_msgs/msg/point.hpp" - -namespace ap1::control { -class IController { -public: - ~IController() = default; - virtual std::vector - compute_acceleration(std::vector vel, - geometry_msgs::msg::Point target_pos, - float target_speed) = 0; -}; -} // namespace ap1::control - -#endif // AP1_CONTROLLER_HPP diff --git a/include/ap1/control/icontroller.hpp b/include/ap1/control/icontroller.hpp new file mode 100644 index 0000000..a87bad4 --- /dev/null +++ b/include/ap1/control/icontroller.hpp @@ -0,0 +1,26 @@ +/** + * 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 + +#include "geometry_msgs/msg/point.hpp" + +namespace ap1::control +{ +class IController +{ + public: + ~IController() = default; + virtual std::vector compute_acceleration(std::vector vel, + geometry_msgs::msg::Point target_pos, + float target_speed) = 0; +}; +} // namespace ap1::control + +#endif // AP1_CONTROLLER_HPP diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index 5feeea2..651f618 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -13,69 +13,74 @@ #include "geometry_msgs/msg/point.hpp" -#include "ap1/control/controller.hpp" +#include "ap1/control/icontroller.hpp" #define EPSILON 1e-6 // THESE SHOULD NOT BE HERE GANG // very uncouth. -float length(geometry_msgs::msg::Point p) { - return std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2)); +inline float length(geometry_msgs::msg::Point p) +{ + return std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2)); } -geometry_msgs::msg::Point get_norm(geometry_msgs::msg::Point p) { - const float l = length(p); - geometry_msgs::msg::Point p2; - p2.x = p.x / l; - p2.y = p.y / l; - p2.z = p.z / l; - return p2; +inline geometry_msgs::msg::Point get_norm(geometry_msgs::msg::Point p) +{ + const float l = length(p); + geometry_msgs::msg::Point p2; + p2.x = p.x / l; + p2.y = p.y / l; + p2.z = p.z / l; + return p2; } -namespace ap1::control { -class PDController : public IController { - float kp_, kd_; - -public: - PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; - - // computes accceleration according to a pid controller - // currently stores vectors half in the ROS *Point* format and half in - // std::vectors. This is super not sigma and we should isolate out both with a - // vec class for all of ap1 (since ros2 points don't have scalar mult, dot, or - // cross or anything). - virtual std::vector - compute_acceleration(std::vector vel, - geometry_msgs::msg::Point target_pos, - float target_speed) override { - // CHECKS - // if vel contains less than 3 vals throw an error - if (vel.size() < 3) { - throw std::invalid_argument( - "vel must have at least three elements (x, y, z)."); - } - - const float distance = length(target_pos); - geometry_msgs::msg::Point target_dir; - - if (distance > EPSILON) - target_dir = get_norm(target_pos); - else { - target_dir.x = 0; - target_dir.y = 0; - target_dir.z = 0; - } - - float target_vel_x = target_dir.x * target_speed; - float target_vel_y = target_dir.y * target_speed; - float target_vel_z = target_dir.z * target_speed; - - std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * vel.at(0), - kp_ * (target_vel_y - vel.at(1)) - kd_ * vel.at(1), - kp_ * (target_vel_z - vel.at(2)) - kd_ * vel.at(2)}; - - return acc; - }; +namespace ap1::control +{ +class PDController : public IController +{ + float kp_, kd_; + + public: + PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd){}; + + // computes accceleration according to a pid controller + // currently stores vectors half in the ROS *Point* format and half in + // std::vectors. This is super not sigma and we should isolate out both with a + // vec class for all of ap1 (since ros2 points don't have scalar mult, dot, or + // cross or anything). + virtual std::vector compute_acceleration(std::vector vel, + geometry_msgs::msg::Point target_pos, + float target_speed) override + { + // CHECKS + // if vel contains less than 3 vals throw an error + if (vel.size() < 3) + { + throw std::invalid_argument("vel must have at least three elements (x, y, z)."); + } + + const float distance = length(target_pos); + geometry_msgs::msg::Point target_dir; + + if (distance > EPSILON) + target_dir = get_norm(target_pos); + else + { + target_dir.x = 0; + target_dir.y = 0; + target_dir.z = 0; + } + + float target_vel_x = target_dir.x * target_speed; + float target_vel_y = target_dir.y * target_speed; + float target_vel_z = target_dir.z * target_speed; + + std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * vel.at(0), + kp_ * (target_vel_y - vel.at(1)) - kd_ * vel.at(1), + kp_ * (target_vel_z - vel.at(2)) - kd_ * vel.at(2)}; + + return acc; + }; }; } // namespace ap1::control diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp new file mode 100644 index 0000000..b0c618b --- /dev/null +++ b/src/ackermann_controller.cpp @@ -0,0 +1,121 @@ +/** + * Ackermann Controller implementation bbgl + * Created: Nov. 11, 2025 + * Author(s): Aly Ashour + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "ap1/control/ackermann_controller.hpp" + +#define EPSILON 1e-3 + +namespace ap1::control +{ + +// ideally we should have a vector class with all of these functions defined in it +float mag(const std::vector& v) +{ + if (v.size() < 3) + return 0; + + return std::sqrt(std::pow(v.at(0), 2) + std::pow(v.at(1), 2) + std::pow(v.at(2), 2)); +} + +std::vector norm(const std::vector& v) +{ + if (v.size() < 3) + return {}; + + const float m = mag(v); + + if (m == 0) + return {}; + + return {v.at(0) / m, v.at(1) / m, v.at(2) / m}; +} + +// also this should be moved to a csv/config loader +std::unordered_map load_csv_config(const std::string& path) +{ + std::ifstream file(path); + if (!file.is_open()) + { + throw std::runtime_error("Failed to open config file: " + path); + } + + std::unordered_map config; + std::string line; + while (std::getline(file, line)) + { + std::istringstream ss(line); + std::string key, value_str; + if (std::getline(ss, key, ',') && std::getline(ss, value_str)) + { + try + { + double value = std::stod(value_str); + config[key] = value; + } + catch (const std::exception& e) + { + throw std::runtime_error("Invalid vlaue for key " + key + ": " + value_str); + } + } + } + + return config; +} + +AckermannController::AckermannController(const Config& cfg) : cfg_(cfg) {} + +// we should move everything over to double but I already wrote all the message types in float +// and I'm too lazy to switch so we'll do it later +AckermannController::Command AckermannController::compute_command( + // notice all vectors are literall std::vector's in the codebase so far + // this is beyond cooked but will do for now. + // vec[0] = x, vec[1] = y, vec[2] = z, and anything else is wabisabi + const std::vector& acc, const std::vector& vel) +{ + Command cmd{}; + + double a_long = std::clamp((double)acc.at(0), -cfg_.a_max, cfg_.a_max); + double a_lat = acc[1]; + + double v = mag(vel); + double kappa = (v > EPSILON) ? a_lat / (v * v) : 0.0; + double delta = std::atan(cfg_.L * kappa); + delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max); + + double throttle = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, -1.0, 1.0); + + cmd.throttle = throttle; + cmd.steering = delta; + + return cmd; +} + +AckermannController::Config AckermannController::load_config(const std::string& path) +{ + Config cfg; + + auto map = load_csv_config(path); + + if (map.count("wheelbase")) + cfg.L = map["wheelbase"]; + if (map.count("a_max")) + cfg.a_max = map["a_max"]; + if (map.count("delta_max")) + cfg.delta_max = map["delta_max"]; + if (map.count("throttle_gain")) + cfg.throttle_gain = map["throttle_gain"]; + + return cfg; +} +} // namespace ap1::control diff --git a/src/main.cpp b/src/main.cpp index db19483..596b78e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,9 +1,29 @@ +#include +#include + #include "ap1/control/control_node.hpp" -int main(int argc, char** argv) { +const char* LOGGER_NAME = "main"; + +int main(int argc, char** argv) +{ + // init ros & ros logger rclcpp::init(argc, argv); - auto node = std::make_shared(); + + // get control node config path from args + std::string config_path = ""; + if (argc > 1) + { + config_path = argv[1]; + } + else + { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Usage: control_node "); + return 1; + } + + auto node = std::make_shared(config_path); rclcpp::spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 44dc67221a9af673ddc333c4b70980c32fc66b47 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Thu, 13 Nov 2025 01:09:29 -0500 Subject: [PATCH 05/15] fix axes bug, add speed control --- include/ap1/control/control_node.hpp | 15 +++++++++++++-- include/ap1/control/pd_controller.hpp | 2 +- src/ackermann_controller.cpp | 8 +++++++- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 0ad57ea..e000136 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -6,6 +6,10 @@ #ifndef AP1_CONTROL_NODE_HPP #define AP1_CONTROL_NODE_HPP +#include +#include +#include + #include "rclcpp/rclcpp.hpp" #include "ap1_msgs/msg/motor_power_stamped.hpp" @@ -97,9 +101,15 @@ class ControlNode : public rclcpp::Node target_path_.path.at(0), // for now get the first path waypoint speed_profile_.speeds.at(0) // and the first speed value ); + std::string s = "ACC: " + std::to_string(acc.at(0)) + ", " + std::to_string(acc.at(1)) + + ", " + std::to_string(acc.at(2)); + RCLCPP_INFO(this->get_logger(), s.c_str()); // compute acc and throttle using ackermann controller AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity); + s = "CMD: {throttle:" + std::to_string(cmd.throttle) + + ", steering:" + std::to_string(cmd.steering) + "}"; + RCLCPP_INFO(this->get_logger(), s.c_str()); // pack the turn angle into a message ap1_msgs::msg::TurnAngleStamped turn_msg; @@ -112,6 +122,7 @@ class ControlNode : public rclcpp::Node pwr_msg.header.stamp = this->now(); pwr_msg.header.frame_id = "base_link"; pwr_msg.power = cmd.throttle; // [-1, 1] + // pwr_msg.power = 1.0f; // send both messages out turning_angle_pub_->publish(turn_msg); @@ -142,10 +153,10 @@ class ControlNode : public rclcpp::Node // # Publishers // - TURNING ANGLE turning_angle_pub_ = - this->create_publisher("ap1/control/motor_power", 10); + this->create_publisher("ap1/control/turn_angle", 10); // - MOTOR POWER motor_power_pub_ = - this->create_publisher("ap1/control/turn_angle", 10); + this->create_publisher("ap1/control/motor_power", 10); // # Create Control Loop // fire at rate_hz diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index 651f618..8f5b6d2 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -41,7 +41,7 @@ class PDController : public IController float kp_, kd_; public: - PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd){}; + PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; // computes accceleration according to a pid controller // currently stores vectors half in the ROS *Point* format and half in diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp index b0c618b..2b3195c 100644 --- a/src/ackermann_controller.cpp +++ b/src/ackermann_controller.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -73,7 +74,12 @@ std::unordered_map load_csv_config(const std::string& path) return config; } -AckermannController::AckermannController(const Config& cfg) : cfg_(cfg) {} +AckermannController::AckermannController(const Config& cfg) : cfg_(cfg) +{ + printf("Created Ackermann Controller with config: a_max=%f, delta_max=%f, L=%f, " + "throttle_gain=%f\n", + cfg_.a_max, cfg_.delta_max, cfg_.L, cfg_.throttle_gain); +} // we should move everything over to double but I already wrote all the message types in float // and I'm too lazy to switch so we'll do it later From 9e29d8119399250c20416124025cf4321b0e583f Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Sat, 15 Nov 2025 12:26:03 -0500 Subject: [PATCH 06/15] Fix 80% bug. Speed was converging to kp / (kd + kp) because the kd term was being calculated wrong --- include/ap1/control/control_node.hpp | 1 + include/ap1/control/pd_controller.hpp | 18 +++++++++++++++--- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index e000136..2071d55 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -80,6 +80,7 @@ class ControlNode : public rclcpp::Node void control_loop_callback() { + // the car's current velocity. we only support moving forward atp const std::vector velocity{this->vehicle_speed_.speed, 0, 0}; // which direction is up? assuming +x diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index 8f5b6d2..47ce117 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -8,6 +8,7 @@ #define AP1_PD_CONTROLLER_HPP #include +#include #include #include @@ -39,6 +40,7 @@ namespace ap1::control class PDController : public IController { float kp_, kd_; + std::vector last_vel_; public: PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; @@ -72,12 +74,22 @@ class PDController : public IController } float target_vel_x = target_dir.x * target_speed; + std::cout << "target_dir_x (SHOULD BE 1): " << target_dir.x + << ". target_vel_x (should be target speed): " << target_vel_x << "\n"; float target_vel_y = target_dir.y * target_speed; float target_vel_z = target_dir.z * target_speed; - std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * vel.at(0), - kp_ * (target_vel_y - vel.at(1)) - kd_ * vel.at(1), - kp_ * (target_vel_z - vel.at(2)) - kd_ * vel.at(2)}; + // keep track of last vel + // so we can approximate derivative of v + // as v2 - v1 + last_vel_ = vel; + + std::vector drv{vel.at(0) - last_vel_.at(0), vel.at(1) - last_vel_.at(1), + vel.at(2) - last_vel_.at(2)}; + + std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * drv.at(0), + kp_ * (target_vel_y - vel.at(1)) - kd_ * drv.at(1), + kp_ * (target_vel_z - vel.at(2)) - kd_ * drv.at(2)}; return acc; }; From 5b0abe4979bfe360892f94829d95d2b2cf86988f Mon Sep 17 00:00:00 2001 From: Logan Ouellette Date: Sat, 15 Nov 2025 13:15:45 -0500 Subject: [PATCH 07/15] Vector hpp file made --- include/ap1/control/vectors.hpp | 268 ++++++++++++++++++++++++++++++++ 1 file changed, 268 insertions(+) create mode 100644 include/ap1/control/vectors.hpp diff --git a/include/ap1/control/vectors.hpp b/include/ap1/control/vectors.hpp new file mode 100644 index 0000000..8b1ab2a --- /dev/null +++ b/include/ap1/control/vectors.hpp @@ -0,0 +1,268 @@ +#ifndef vectors_hpp +#define vectors_hpp + +/** + * Class for 2d and 3d vectos + * Created: Oct. 11, 2025 + * Author(s): Logan Ouellette + */ + +#include +#include +#include + + +class vec3 { + public: + float e[3]; + + vec3() {} + vec3(float e0, float e1, float e2) { e[0] = e0; e[1] = e1; e[2] = e2; } + inline float x() const { return e[0]; } + inline float y() const { return e[1]; } + inline float z() const { return e[2]; } + + inline const vec3& operator+() const { return *this; } + inline vec3 operator-() const { return vec3(-e[0], -e[1], -e[2]); } + inline float operator[](int i) const { return e[i]; } + inline float& operator[](int i) { return e[i]; } + + inline vec3& operator+=(const vec3 &v2); + inline vec3& operator-=(const vec3 &v2); + inline vec3& operator*=(const vec3 &v2); + inline vec3& operator/=(const vec3 &v2); + inline vec3& operator*=(const float t); + inline vec3& operator/=(const float t); + + inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); } + inline float squared_length() const { return e[0]*e[0] + e[1]*e[1] + e[2]*e[2]; } + inline void make_unit_vector(); +}; + + +inline std::istream& operator>>(std::istream &is, vec3 &t) { + is >> t.e[0] >> t.e[1] >> t.e[2]; + return is; +} + +inline std::ostream& operator<<(std::ostream &os, const vec3 &t) { + os << t.e[0] << " " << t.e[1] << " " << t.e[2]; + return os; +} + +inline void vec3::make_unit_vector() { + float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); + e[0] *= k; e[1] *= k; e[2] *= k; +} + +inline vec3 operator+(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1], v1.e[2] + v2.e[2]); +} + +inline vec3 operator-(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1], v1.e[2] - v2.e[2]); +} + +inline vec3 operator*(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1], v1.e[2] * v2.e[2]); +} + +inline vec3 operator/(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1], v1.e[2] / v2.e[2]); +} + +inline vec3 operator*(float t, const vec3 &v) { + return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); +} + +inline vec3 operator/(vec3 v, float t) { + return vec3(v.e[0]/t, v.e[1]/t, v.e[2]/t); +} + +inline vec3 operator*(const vec3 &v, float t) { + return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); +} + +inline float dot(const vec3 &v1, const vec3 &v2) { + return v1.e[0] * v2.e[0] + + v1.e[1] * v2.e[1] + + v1.e[2] * v2.e[2]; +} + +inline vec3 cross(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[1] * v2.e[2] - v1.e[2] * v2.e[1], + v1.e[2] * v2.e[0] - v1.e[0] * v2.e[2], + v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]); +} + +inline vec3& vec3::operator+=(const vec3 &v){ + e[0] += v.e[0]; + e[1] += v.e[1]; + e[2] += v.e[2]; + return *this; +} + +inline vec3& vec3::operator*=(const vec3 &v){ + e[0] *= v.e[0]; + e[1] *= v.e[1]; + e[2] *= v.e[2]; + return *this; +} + +inline vec3& vec3::operator/=(const vec3 &v){ + e[0] /= v.e[0]; + e[1] /= v.e[1]; + e[2] /= v.e[2]; + return *this; +} + +inline vec3& vec3::operator-=(const vec3& v) { + e[0] -= v.e[0]; + e[1] -= v.e[1]; + e[2] -= v.e[2]; + return *this; +} + +inline vec3& vec3::operator*=(const float t) { + e[0] *= t; + e[1] *= t; + e[2] *= t; + return *this; +} + +inline vec3& vec3::operator/=(const float t) { + float k = 1.0f/t; + + e[0] *= k; + e[1] *= k; + e[2] *= k; + return *this; +} + +inline vec3 unit_vector(vec3 v) { + return v / v.length(); +} + +class vec2 { + public: + float e[2]; + + vec2() {} + vec2(float e0, float e1) { e[0] = e0; e[1] = e1; } + inline float x() const { return e[0]; } + inline float y() const { return e[1]; } + + inline const vec2& operator+() const { return *this; } + inline vec2 operator-() const { return vec2(-e[0], -e[1]); } + inline float operator[](int i) const { return e[i]; } + inline float& operator[](int i) { return e[i]; } + + inline vec2& operator+=(const vec2 &v2); + inline vec2& operator-=(const vec2 &v2); + inline vec2& operator*=(const vec2 &v2); + inline vec2& operator/=(const vec2 &v2); + inline vec2& operator*=(const float t); + inline vec2& operator/=(const float t); + + inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1]); } + inline float squared_length() const { return e[0]*e[0] + e[1]*e[1]; } + inline void make_unit_vector(); +}; + + +inline std::istream& operator>>(std::istream &is, vec2 &t) { + is >> t.e[0] >> t.e[1]; + return is; +} + +inline std::ostream& operator<<(std::ostream &os, const vec2 &t) { + os << t.e[0] << " " << t.e[1]; + return os; +} + +inline void vec2::make_unit_vector() { + float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1]); + e[0] *= k; e[1] *= k; +} + +inline vec2 operator+(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1]); +} + +inline vec2 operator-(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1]); +} + +inline vec2 operator*(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1]); +} + +inline vec2 operator/(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1]); +} + +inline vec2 operator*(float t, const vec2 &v) { + return vec2(t*v.e[0], t*v.e[1]); +} + +inline vec2 operator/(vec2 v, float t) { + return vec2(v.e[0]/t, v.e[1]/t); +} + +inline vec2 operator*(const vec2 &v, float t) { + return vec2(t*v.e[0], t*v.e[1]); +} + +inline float dot(const vec2 &v1, const vec2 &v2) { + return v1.e[0] * v2.e[0] + + v1.e[1] * v2.e[1]; +} + +inline float cross(const vec2 &v1, const vec2 &v2) { + return v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]; +} + +inline vec2& vec2::operator+=(const vec2 &v){ + e[0] += v.e[0]; + e[1] += v.e[1]; + return *this; +} + +inline vec2& vec2::operator*=(const vec2 &v){ + e[0] *= v.e[0]; + e[1] *= v.e[1]; + return *this; +} + +inline vec2& vec2::operator/=(const vec2 &v){ + e[0] /= v.e[0]; + e[1] /= v.e[1]; + return *this; +} + +inline vec2& vec2::operator-=(const vec2& v) { + e[0] -= v.e[0]; + e[1] -= v.e[1]; + return *this; +} + +inline vec2& vec2::operator*=(const float t) { + e[0] *= t; + e[1] *= t; + return *this; +} + +inline vec2& vec2::operator/=(const float t) { + float k = 1.0f/t; + + e[0] *= k; + e[1] *= k; + return *this; +} + +inline vec2 unit_vector(vec2 v) { + return v / v.length(); +} + + +#endif From 3e4c37575f22b73d97a6b31c5087df04b580f6f3 Mon Sep 17 00:00:00 2001 From: Logan Ouellette Date: Sat, 15 Nov 2025 14:02:51 -0500 Subject: [PATCH 08/15] PDController adjusted for vector class --- include/ap1/control/pd_controller.hpp | 78 +++++++++------------------ include/ap1/control/vectors.hpp | 4 +- 2 files changed, 27 insertions(+), 55 deletions(-) diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index 47ce117..dd34a70 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -10,7 +10,9 @@ #include #include #include -#include +# include + +#include "vectors.hpp" #include "geometry_msgs/msg/point.hpp" @@ -18,78 +20,48 @@ #define EPSILON 1e-6 -// THESE SHOULD NOT BE HERE GANG -// very uncouth. -inline float length(geometry_msgs::msg::Point p) -{ - return std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2)); -} - -inline geometry_msgs::msg::Point get_norm(geometry_msgs::msg::Point p) -{ - const float l = length(p); - geometry_msgs::msg::Point p2; - p2.x = p.x / l; - p2.y = p.y / l; - p2.z = p.z / l; - return p2; -} - namespace ap1::control { class PDController : public IController { + // Stength of the proportional and derivative terms float kp_, kd_; - std::vector last_vel_; + //Last velocoty vector + vec3 last_vel_; public: PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; - // computes accceleration according to a pid controller - // currently stores vectors half in the ROS *Point* format and half in - // std::vectors. This is super not sigma and we should isolate out both with a - // vec class for all of ap1 (since ros2 points don't have scalar mult, dot, or - // cross or anything). - virtual std::vector compute_acceleration(std::vector vel, - geometry_msgs::msg::Point target_pos, - float target_speed) override + // Computes acceleration based on current velocity, target position, and target speed. Returns acceleration vector. + virtual vec3 compute_acceleration(vec3 vel, geometry_msgs::msg::Point target_pos, float target_speed) override { - // CHECKS - // if vel contains less than 3 vals throw an error - if (vel.size() < 3) - { - throw std::invalid_argument("vel must have at least three elements (x, y, z)."); - } - - const float distance = length(target_pos); - geometry_msgs::msg::Point target_dir; + // Convert target position to vec3 and calculate distance + vec3 vecTargetPos(target_pos.x, target_pos.y, target_pos.z); + const float distance = vecTargetPos.length(); + // targetDir for normalizing target position into unit direction + vec3 targetDir; if (distance > EPSILON) - target_dir = get_norm(target_pos); + targetDir = unit_vector(vecTargetPos); else { - target_dir.x = 0; - target_dir.y = 0; - target_dir.z = 0; + targetDir = vec3(0.0, 0.0, 0.0); } - float target_vel_x = target_dir.x * target_speed; - std::cout << "target_dir_x (SHOULD BE 1): " << target_dir.x - << ". target_vel_x (should be target speed): " << target_vel_x << "\n"; - float target_vel_y = target_dir.y * target_speed; - float target_vel_z = target_dir.z * target_speed; + // Scale direction vector by target speed for velocity + vec3 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"; - // keep track of last vel - // so we can approximate derivative of v - // as v2 - v1 + // Store current velocity for derivative approximation last_vel_ = vel; - std::vector drv{vel.at(0) - last_vel_.at(0), vel.at(1) - last_vel_.at(1), - vel.at(2) - last_vel_.at(2)}; + // Calculate velocity change (numerical derivative) + vec3 drv = vel - last_vel_; - std::vector acc{kp_ * (target_vel_x - vel.at(0)) - kd_ * drv.at(0), - kp_ * (target_vel_y - vel.at(1)) - kd_ * drv.at(1), - kp_ * (target_vel_z - vel.at(2)) - kd_ * drv.at(2)}; + // PD controller: proportional term tracks target velocity, derivative term adds damping + vec3 acc = kp_* (targetVel - vel) - kd_ * drv; return acc; }; diff --git a/include/ap1/control/vectors.hpp b/include/ap1/control/vectors.hpp index 8b1ab2a..c0cce62 100644 --- a/include/ap1/control/vectors.hpp +++ b/include/ap1/control/vectors.hpp @@ -1,5 +1,5 @@ -#ifndef vectors_hpp -#define vectors_hpp +#ifndef AP1_vectors_hpp +#define AP1_vectors_hpp /** * Class for 2d and 3d vectos From 688cf240170963a7df3cf04a06098c8f451a9a31 Mon Sep 17 00:00:00 2001 From: Logan Ouellette Date: Sat, 15 Nov 2025 14:03:53 -0500 Subject: [PATCH 09/15] IController adjusted --- include/ap1/control/icontroller.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/ap1/control/icontroller.hpp b/include/ap1/control/icontroller.hpp index a87bad4..304e68e 100644 --- a/include/ap1/control/icontroller.hpp +++ b/include/ap1/control/icontroller.hpp @@ -9,6 +9,7 @@ #include +#include "vectors.hpp" #include "geometry_msgs/msg/point.hpp" namespace ap1::control @@ -17,9 +18,7 @@ class IController { public: ~IController() = default; - virtual std::vector compute_acceleration(std::vector vel, - geometry_msgs::msg::Point target_pos, - float target_speed) = 0; + virtual vec3 compute_acceleration(vec3 vel, geometry_msgs::msg::Point target_pos, float target_speed) = 0; }; } // namespace ap1::control From 00db23651a62d1f95435fc75c8493f70e826ab1f Mon Sep 17 00:00:00 2001 From: Logan Ouellette Date: Sat, 15 Nov 2025 14:11:42 -0500 Subject: [PATCH 10/15] Control node adjuted --- include/ap1/control/ackermann_controller.hpp | 3 ++- include/ap1/control/control_node.hpp | 10 +++++----- src/ackermann_controller.cpp | 2 +- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/include/ap1/control/ackermann_controller.hpp b/include/ap1/control/ackermann_controller.hpp index 3264b15..af1555a 100644 --- a/include/ap1/control/ackermann_controller.hpp +++ b/include/ap1/control/ackermann_controller.hpp @@ -10,6 +10,7 @@ #include #include +#include "vectors.hpp" namespace ap1::control { @@ -32,7 +33,7 @@ class AckermannController double steering; // in rads }; - Command compute_command(const std::vector &acc, const std::vector &vel); + Command compute_command(const vec3 acc, const vec3 vel); static Config load_config(const std::string &path); diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 2071d55..9de8db4 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -10,6 +10,7 @@ #include #include +#include "vectors.hpp" #include "rclcpp/rclcpp.hpp" #include "ap1_msgs/msg/motor_power_stamped.hpp" @@ -81,8 +82,7 @@ class ControlNode : public rclcpp::Node void control_loop_callback() { // the car's current velocity. we only support moving forward atp - const std::vector velocity{this->vehicle_speed_.speed, 0, - 0}; // which direction is up? assuming +x + const vec3 velocity(this->vehicle_speed_.speed, 0, 0); // which direction is up? assuming +x const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP @@ -97,13 +97,13 @@ class ControlNode : public rclcpp::Node if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE) return; - const std::vector acc = controller_->compute_acceleration( + const vec3 acc = controller_->compute_acceleration( velocity, target_path_.path.at(0), // for now get the first path waypoint speed_profile_.speeds.at(0) // and the first speed value ); - std::string s = "ACC: " + std::to_string(acc.at(0)) + ", " + std::to_string(acc.at(1)) + - ", " + std::to_string(acc.at(2)); + std::string s = "ACC: " + std::to_string(acc.x) + ", " + std::to_string(acc.y) + + ", " + std::to_string(acc.z); RCLCPP_INFO(this->get_logger(), s.c_str()); // compute acc and throttle using ackermann controller diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp index 2b3195c..22fa5e4 100644 --- a/src/ackermann_controller.cpp +++ b/src/ackermann_controller.cpp @@ -87,7 +87,7 @@ AckermannController::Command AckermannController::compute_command( // notice all vectors are literall std::vector's in the codebase so far // this is beyond cooked but will do for now. // vec[0] = x, vec[1] = y, vec[2] = z, and anything else is wabisabi - const std::vector& acc, const std::vector& vel) + const vec3& acc, const vec3& vel) { Command cmd{}; From d1cb95d92b35f147d3ce20db45a5c0e2203920bc Mon Sep 17 00:00:00 2001 From: Logan Ouellette Date: Sun, 16 Nov 2025 12:47:33 -0500 Subject: [PATCH 11/15] Ackermann controller done --- include/ap1/control/ackermann_controller.hpp | 2 +- include/ap1/control/control_node.hpp | 2 +- include/ap1/control/pd_controller.hpp | 2 +- include/ap1/control/vectors.hpp | 514 ++++++++++--------- src/ackermann_controller.cpp | 38 +- 5 files changed, 272 insertions(+), 286 deletions(-) diff --git a/include/ap1/control/ackermann_controller.hpp b/include/ap1/control/ackermann_controller.hpp index af1555a..11ebb5d 100644 --- a/include/ap1/control/ackermann_controller.hpp +++ b/include/ap1/control/ackermann_controller.hpp @@ -33,7 +33,7 @@ class AckermannController double steering; // in rads }; - Command compute_command(const vec3 acc, const vec3 vel); + Command compute_command(const vec3& acc, const vec3& vel); static Config load_config(const std::string &path); diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 9de8db4..f6da19a 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -10,7 +10,6 @@ #include #include -#include "vectors.hpp" #include "rclcpp/rclcpp.hpp" #include "ap1_msgs/msg/motor_power_stamped.hpp" @@ -22,6 +21,7 @@ #include "ap1/control/ackermann_controller.hpp" #include "ap1/control/icontroller.hpp" #include "ap1/control/pd_controller.hpp" +#include "ap1/control/vectors.hpp" namespace ap1::control { diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index dd34a70..6a64280 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -12,7 +12,7 @@ #include # include -#include "vectors.hpp" +#include "ap1/control/vectors.hpp" #include "geometry_msgs/msg/point.hpp" diff --git a/include/ap1/control/vectors.hpp b/include/ap1/control/vectors.hpp index c0cce62..0097465 100644 --- a/include/ap1/control/vectors.hpp +++ b/include/ap1/control/vectors.hpp @@ -11,258 +11,266 @@ #include #include - -class vec3 { - public: - float e[3]; - - vec3() {} - vec3(float e0, float e1, float e2) { e[0] = e0; e[1] = e1; e[2] = e2; } - inline float x() const { return e[0]; } - inline float y() const { return e[1]; } - inline float z() const { return e[2]; } - - inline const vec3& operator+() const { return *this; } - inline vec3 operator-() const { return vec3(-e[0], -e[1], -e[2]); } - inline float operator[](int i) const { return e[i]; } - inline float& operator[](int i) { return e[i]; } - - inline vec3& operator+=(const vec3 &v2); - inline vec3& operator-=(const vec3 &v2); - inline vec3& operator*=(const vec3 &v2); - inline vec3& operator/=(const vec3 &v2); - inline vec3& operator*=(const float t); - inline vec3& operator/=(const float t); - - inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); } - inline float squared_length() const { return e[0]*e[0] + e[1]*e[1] + e[2]*e[2]; } - inline void make_unit_vector(); -}; - - -inline std::istream& operator>>(std::istream &is, vec3 &t) { - is >> t.e[0] >> t.e[1] >> t.e[2]; - return is; -} - -inline std::ostream& operator<<(std::ostream &os, const vec3 &t) { - os << t.e[0] << " " << t.e[1] << " " << t.e[2]; - return os; -} - -inline void vec3::make_unit_vector() { - float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); - e[0] *= k; e[1] *= k; e[2] *= k; -} - -inline vec3 operator+(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1], v1.e[2] + v2.e[2]); -} - -inline vec3 operator-(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1], v1.e[2] - v2.e[2]); -} - -inline vec3 operator*(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1], v1.e[2] * v2.e[2]); -} - -inline vec3 operator/(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1], v1.e[2] / v2.e[2]); -} - -inline vec3 operator*(float t, const vec3 &v) { - return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); -} - -inline vec3 operator/(vec3 v, float t) { - return vec3(v.e[0]/t, v.e[1]/t, v.e[2]/t); -} - -inline vec3 operator*(const vec3 &v, float t) { - return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); -} - -inline float dot(const vec3 &v1, const vec3 &v2) { - return v1.e[0] * v2.e[0] - + v1.e[1] * v2.e[1] - + v1.e[2] * v2.e[2]; -} - -inline vec3 cross(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[1] * v2.e[2] - v1.e[2] * v2.e[1], - v1.e[2] * v2.e[0] - v1.e[0] * v2.e[2], - v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]); -} - -inline vec3& vec3::operator+=(const vec3 &v){ - e[0] += v.e[0]; - e[1] += v.e[1]; - e[2] += v.e[2]; - return *this; -} - -inline vec3& vec3::operator*=(const vec3 &v){ - e[0] *= v.e[0]; - e[1] *= v.e[1]; - e[2] *= v.e[2]; - return *this; -} - -inline vec3& vec3::operator/=(const vec3 &v){ - e[0] /= v.e[0]; - e[1] /= v.e[1]; - e[2] /= v.e[2]; - return *this; -} - -inline vec3& vec3::operator-=(const vec3& v) { - e[0] -= v.e[0]; - e[1] -= v.e[1]; - e[2] -= v.e[2]; - return *this; -} - -inline vec3& vec3::operator*=(const float t) { - e[0] *= t; - e[1] *= t; - e[2] *= t; - return *this; -} - -inline vec3& vec3::operator/=(const float t) { - float k = 1.0f/t; - - e[0] *= k; - e[1] *= k; - e[2] *= k; - return *this; -} - -inline vec3 unit_vector(vec3 v) { - return v / v.length(); -} - -class vec2 { - public: - float e[2]; - - vec2() {} - vec2(float e0, float e1) { e[0] = e0; e[1] = e1; } - inline float x() const { return e[0]; } - inline float y() const { return e[1]; } - - inline const vec2& operator+() const { return *this; } - inline vec2 operator-() const { return vec2(-e[0], -e[1]); } - inline float operator[](int i) const { return e[i]; } - inline float& operator[](int i) { return e[i]; } - - inline vec2& operator+=(const vec2 &v2); - inline vec2& operator-=(const vec2 &v2); - inline vec2& operator*=(const vec2 &v2); - inline vec2& operator/=(const vec2 &v2); - inline vec2& operator*=(const float t); - inline vec2& operator/=(const float t); - - inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1]); } - inline float squared_length() const { return e[0]*e[0] + e[1]*e[1]; } - inline void make_unit_vector(); -}; - - -inline std::istream& operator>>(std::istream &is, vec2 &t) { - is >> t.e[0] >> t.e[1]; - return is; -} - -inline std::ostream& operator<<(std::ostream &os, const vec2 &t) { - os << t.e[0] << " " << t.e[1]; - return os; -} - -inline void vec2::make_unit_vector() { - float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1]); - e[0] *= k; e[1] *= k; -} - -inline vec2 operator+(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1]); -} - -inline vec2 operator-(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1]); -} - -inline vec2 operator*(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1]); -} - -inline vec2 operator/(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1]); -} - -inline vec2 operator*(float t, const vec2 &v) { - return vec2(t*v.e[0], t*v.e[1]); -} - -inline vec2 operator/(vec2 v, float t) { - return vec2(v.e[0]/t, v.e[1]/t); -} - -inline vec2 operator*(const vec2 &v, float t) { - return vec2(t*v.e[0], t*v.e[1]); -} - -inline float dot(const vec2 &v1, const vec2 &v2) { - return v1.e[0] * v2.e[0] - + v1.e[1] * v2.e[1]; -} - -inline float cross(const vec2 &v1, const vec2 &v2) { - return v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]; -} - -inline vec2& vec2::operator+=(const vec2 &v){ - e[0] += v.e[0]; - e[1] += v.e[1]; - return *this; -} - -inline vec2& vec2::operator*=(const vec2 &v){ - e[0] *= v.e[0]; - e[1] *= v.e[1]; - return *this; -} - -inline vec2& vec2::operator/=(const vec2 &v){ - e[0] /= v.e[0]; - e[1] /= v.e[1]; - return *this; -} - -inline vec2& vec2::operator-=(const vec2& v) { - e[0] -= v.e[0]; - e[1] -= v.e[1]; - return *this; -} - -inline vec2& vec2::operator*=(const float t) { - e[0] *= t; - e[1] *= t; - return *this; -} - -inline vec2& vec2::operator/=(const float t) { - float k = 1.0f/t; - - e[0] *= k; - e[1] *= k; - return *this; -} - -inline vec2 unit_vector(vec2 v) { - return v / v.length(); -} - +namespace ap1::control +{ + // 3D vector class + class vec3 { + public: + float e[3]; + + vec3() {} + vec3(float e0, float e1, float e2) { e[0] = e0; e[1] = e1; e[2] = e2; } + inline float x() const { return e[0]; } + inline float y() const { return e[1]; } + inline float z() const { return e[2]; } + + inline const vec3& operator+() const { return *this; } + inline vec3 operator-() const { return vec3(-e[0], -e[1], -e[2]); } + inline float operator[](int i) const { return e[i]; } + inline float& operator[](int i) { return e[i]; } + + inline vec3& operator+=(const vec3 &v2); + inline vec3& operator-=(const vec3 &v2); + inline vec3& operator*=(const vec3 &v2); + inline vec3& operator/=(const vec3 &v2); + inline vec3& operator*=(const float t); + inline vec3& operator/=(const float t); + + inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); } + inline float squared_length() const { return e[0]*e[0] + e[1]*e[1] + e[2]*e[2]; } + inline void make_unit_vector(); + }; + + + inline std::istream& operator>>(std::istream &is, vec3 &t) { + is >> t.e[0] >> t.e[1] >> t.e[2]; + return is; + } + + inline std::ostream& operator<<(std::ostream &os, const vec3 &t) { + os << t.e[0] << " " << t.e[1] << " " << t.e[2]; + return os; + } + + inline void vec3::make_unit_vector() { + float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); + e[0] *= k; e[1] *= k; e[2] *= k; + } + + inline vec3 operator+(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1], v1.e[2] + v2.e[2]); + } + + inline vec3 operator-(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1], v1.e[2] - v2.e[2]); + } + + inline vec3 operator*(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1], v1.e[2] * v2.e[2]); + } + + inline vec3 operator/(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1], v1.e[2] / v2.e[2]); + } + + inline vec3 operator*(float t, const vec3 &v) { + return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); + } + + inline vec3 operator/(vec3 v, float t) { + return vec3(v.e[0]/t, v.e[1]/t, v.e[2]/t); + } + + inline vec3 operator*(const vec3 &v, float t) { + return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); + } + + inline float dot(const vec3 &v1, const vec3 &v2) { + return v1.e[0] * v2.e[0] + + v1.e[1] * v2.e[1] + + v1.e[2] * v2.e[2]; + } + + inline vec3 cross(const vec3 &v1, const vec3 &v2) { + return vec3(v1.e[1] * v2.e[2] - v1.e[2] * v2.e[1], + v1.e[2] * v2.e[0] - v1.e[0] * v2.e[2], + v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]); + } + + inline vec3& vec3::operator+=(const vec3 &v){ + e[0] += v.e[0]; + e[1] += v.e[1]; + e[2] += v.e[2]; + return *this; + } + + inline vec3& vec3::operator*=(const vec3 &v){ + e[0] *= v.e[0]; + e[1] *= v.e[1]; + e[2] *= v.e[2]; + return *this; + } + + inline vec3& vec3::operator/=(const vec3 &v){ + e[0] /= v.e[0]; + e[1] /= v.e[1]; + e[2] /= v.e[2]; + return *this; + } + + inline vec3& vec3::operator-=(const vec3& v) { + e[0] -= v.e[0]; + e[1] -= v.e[1]; + e[2] -= v.e[2]; + return *this; + } + + inline vec3& vec3::operator*=(const float t) { + e[0] *= t; + e[1] *= t; + e[2] *= t; + return *this; + } + + inline vec3& vec3::operator/=(const float t) { + float k = 1.0f/t; + + e[0] *= k; + e[1] *= k; + e[2] *= k; + return *this; + } + + inline vec3 unit_vector(vec3 v) { + return v / v.length(); + } + +} // namespace ap1::control + +namespace ap1::control +{ + // 2D vector class + class vec2 { + public: + float e[2]; + + vec2() {} + vec2(float e0, float e1) { e[0] = e0; e[1] = e1; } + inline float x() const { return e[0]; } + inline float y() const { return e[1]; } + + inline const vec2& operator+() const { return *this; } + inline vec2 operator-() const { return vec2(-e[0], -e[1]); } + inline float operator[](int i) const { return e[i]; } + inline float& operator[](int i) { return e[i]; } + + inline vec2& operator+=(const vec2 &v2); + inline vec2& operator-=(const vec2 &v2); + inline vec2& operator*=(const vec2 &v2); + inline vec2& operator/=(const vec2 &v2); + inline vec2& operator*=(const float t); + inline vec2& operator/=(const float t); + + inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1]); } + inline float squared_length() const { return e[0]*e[0] + e[1]*e[1]; } + inline void make_unit_vector(); + }; + + + inline std::istream& operator>>(std::istream &is, vec2 &t) { + is >> t.e[0] >> t.e[1]; + return is; + } + + inline std::ostream& operator<<(std::ostream &os, const vec2 &t) { + os << t.e[0] << " " << t.e[1]; + return os; + } + + inline void vec2::make_unit_vector() { + float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1]); + e[0] *= k; e[1] *= k; + } + + inline vec2 operator+(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1]); + } + + inline vec2 operator-(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1]); + } + + inline vec2 operator*(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1]); + } + + inline vec2 operator/(const vec2 &v1, const vec2 &v2) { + return vec2(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1]); + } + + inline vec2 operator*(float t, const vec2 &v) { + return vec2(t*v.e[0], t*v.e[1]); + } + + inline vec2 operator/(vec2 v, float t) { + return vec2(v.e[0]/t, v.e[1]/t); + } + + inline vec2 operator*(const vec2 &v, float t) { + return vec2(t*v.e[0], t*v.e[1]); + } + + inline float dot(const vec2 &v1, const vec2 &v2) { + return v1.e[0] * v2.e[0] + + v1.e[1] * v2.e[1]; + } + + inline float cross(const vec2 &v1, const vec2 &v2) { + return v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]; + } + + inline vec2& vec2::operator+=(const vec2 &v){ + e[0] += v.e[0]; + e[1] += v.e[1]; + return *this; + } + + inline vec2& vec2::operator*=(const vec2 &v){ + e[0] *= v.e[0]; + e[1] *= v.e[1]; + return *this; + } + + inline vec2& vec2::operator/=(const vec2 &v){ + e[0] /= v.e[0]; + e[1] /= v.e[1]; + return *this; + } + + inline vec2& vec2::operator-=(const vec2& v) { + e[0] -= v.e[0]; + e[1] -= v.e[1]; + return *this; + } + + inline vec2& vec2::operator*=(const float t) { + e[0] *= t; + e[1] *= t; + return *this; + } + + inline vec2& vec2::operator/=(const float t) { + float k = 1.0f/t; + + e[0] *= k; + e[1] *= k; + return *this; + } + + inline vec2 unit_vector(vec2 v) { + return v / v.length(); + } + +} // namespace ap1::control #endif diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp index 22fa5e4..d3fe540 100644 --- a/src/ackermann_controller.cpp +++ b/src/ackermann_controller.cpp @@ -19,29 +19,6 @@ namespace ap1::control { - -// ideally we should have a vector class with all of these functions defined in it -float mag(const std::vector& v) -{ - if (v.size() < 3) - return 0; - - return std::sqrt(std::pow(v.at(0), 2) + std::pow(v.at(1), 2) + std::pow(v.at(2), 2)); -} - -std::vector norm(const std::vector& v) -{ - if (v.size() < 3) - return {}; - - const float m = mag(v); - - if (m == 0) - return {}; - - return {v.at(0) / m, v.at(1) / m, v.at(2) / m}; -} - // also this should be moved to a csv/config loader std::unordered_map load_csv_config(const std::string& path) { @@ -83,18 +60,19 @@ AckermannController::AckermannController(const Config& cfg) : cfg_(cfg) // we should move everything over to double but I already wrote all the message types in float // and I'm too lazy to switch so we'll do it later -AckermannController::Command AckermannController::compute_command( - // notice all vectors are literall std::vector's in the codebase so far - // this is beyond cooked but will do for now. - // vec[0] = x, vec[1] = y, vec[2] = z, and anything else is wabisabi - const vec3& acc, const vec3& vel) +AckermannController::Command AckermannController::compute_command(const vec3& acc, const vec3& vel) { Command cmd{}; - double a_long = std::clamp((double)acc.at(0), -cfg_.a_max, cfg_.a_max); + double a_long = std::clamp( + (double)acc.x(), + -cfg_.a_max, + cfg_.a_max + ); + double a_lat = acc[1]; - double v = mag(vel); + double v = vel.length(); double kappa = (v > EPSILON) ? a_lat / (v * v) : 0.0; double delta = std::atan(cfg_.L * kappa); delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max); From 153e70ff60d2e0bc4c12d2f4656cba51caba98a1 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Sun, 16 Nov 2025 22:36:59 -0500 Subject: [PATCH 12/15] Add clangd format --- .clang-format | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..27f84ec --- /dev/null +++ b/.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 + From 012adac4b2e869f3bc2fea0d50c68a754b16ab65 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Sun, 23 Nov 2025 11:37:34 -0500 Subject: [PATCH 13/15] fix vector compile errors --- CMakeLists.txt | 14 +- include/ap1/control/ackermann_controller.hpp | 3 +- include/ap1/control/control_node.hpp | 9 +- include/ap1/control/icontroller.hpp | 2 +- include/ap1/control/pd_controller.hpp | 38 +-- include/ap1/control/vectors.hpp | 276 ------------------- include/vectors.hpp | 257 +++++++++++++++++ src/ackermann_controller.cpp | 11 +- 8 files changed, 294 insertions(+), 316 deletions(-) delete mode 100644 include/ap1/control/vectors.hpp create mode 100644 include/vectors.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b2fc4e..01ba0bb 100644 --- a/CMakeLists.txt +++ b/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() @@ -34,15 +38,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() diff --git a/include/ap1/control/ackermann_controller.hpp b/include/ap1/control/ackermann_controller.hpp index 11ebb5d..3c88484 100644 --- a/include/ap1/control/ackermann_controller.hpp +++ b/include/ap1/control/ackermann_controller.hpp @@ -9,7 +9,6 @@ #define AP1_ACKERMANN_CONTROLLER_HPP #include -#include #include "vectors.hpp" namespace ap1::control @@ -33,7 +32,7 @@ class AckermannController double steering; // in rads }; - Command compute_command(const vec3& acc, const vec3& vel); + Command compute_command(const vec3f& acc, const vec3f& vel); static Config load_config(const std::string &path); diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index f6da19a..324f615 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -21,7 +21,7 @@ #include "ap1/control/ackermann_controller.hpp" #include "ap1/control/icontroller.hpp" #include "ap1/control/pd_controller.hpp" -#include "ap1/control/vectors.hpp" +#include "vectors.hpp" namespace ap1::control { @@ -82,7 +82,7 @@ class ControlNode : public rclcpp::Node void control_loop_callback() { // the car's current velocity. we only support moving forward atp - const vec3 velocity(this->vehicle_speed_.speed, 0, 0); // which direction is up? assuming +x + const vec3f velocity(this->vehicle_speed_.speed, 0, 0); // which direction is up? assuming +x const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP @@ -97,9 +97,10 @@ class ControlNode : public rclcpp::Node if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE) return; - const vec3 acc = controller_->compute_acceleration( + auto next_waypoint = target_path_.path.at(0); + const vec3f acc = controller_->compute_acceleration( velocity, - target_path_.path.at(0), // for now get the first path waypoint + vec2f(next_waypoint.x, next_waypoint.y), // for now get the first path waypoint speed_profile_.speeds.at(0) // and the first speed value ); std::string s = "ACC: " + std::to_string(acc.x) + ", " + std::to_string(acc.y) + diff --git a/include/ap1/control/icontroller.hpp b/include/ap1/control/icontroller.hpp index 304e68e..4c6b36d 100644 --- a/include/ap1/control/icontroller.hpp +++ b/include/ap1/control/icontroller.hpp @@ -18,7 +18,7 @@ class IController { public: ~IController() = default; - virtual vec3 compute_acceleration(vec3 vel, geometry_msgs::msg::Point target_pos, float target_speed) = 0; + virtual vec3f compute_acceleration(const vec3f& vel, const vec2f& target_pos, const float target_speed) = 0; }; } // namespace ap1::control diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp index 6a64280..5d677bb 100644 --- a/include/ap1/control/pd_controller.hpp +++ b/include/ap1/control/pd_controller.hpp @@ -10,13 +10,12 @@ #include #include #include -# include - -#include "ap1/control/vectors.hpp" +#include #include "geometry_msgs/msg/point.hpp" #include "ap1/control/icontroller.hpp" +#include "vectors.hpp" #define EPSILON 1e-6 @@ -26,44 +25,45 @@ class PDController : public IController { // Stength of the proportional and derivative terms float kp_, kd_; - //Last velocoty vector - vec3 last_vel_; + // 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 vec3 compute_acceleration(vec3 vel, geometry_msgs::msg::Point target_pos, float target_speed) override + // 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 vec3 and calculate distance - vec3 vecTargetPos(target_pos.x, target_pos.y, target_pos.z); - const float distance = vecTargetPos.length(); + // Convert target position to vec3f and calculate distance + const float distance = target_pos.length(); // targetDir for normalizing target position into unit direction - vec3 targetDir; + vec2f targetDir; if (distance > EPSILON) - targetDir = unit_vector(vecTargetPos); + targetDir = target_pos.unit_vector(); else { - targetDir = vec3(0.0, 0.0, 0.0); + targetDir = vec2f(0.0, 0.0); } // Scale direction vector by target speed for velocity - vec3 targetVel = targetDir * target_speed; + vec3f targetVel = targetDir * target_speed; - std::cout << "target_dir_x (SHOULD BE 1): " << targetDir.x << ", target_dir_y: " << targetDir.y + 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) - vec3 drv = vel - last_vel_; + vec3f drv = vel - last_vel_; // PD controller: proportional term tracks target velocity, derivative term adds damping - vec3 acc = kp_* (targetVel - vel) - kd_ * drv; - - return acc; + return kp_ * (targetVel - vel) - kd_ * drv; }; }; } // namespace ap1::control diff --git a/include/ap1/control/vectors.hpp b/include/ap1/control/vectors.hpp deleted file mode 100644 index 0097465..0000000 --- a/include/ap1/control/vectors.hpp +++ /dev/null @@ -1,276 +0,0 @@ -#ifndef AP1_vectors_hpp -#define AP1_vectors_hpp - -/** - * Class for 2d and 3d vectos - * Created: Oct. 11, 2025 - * Author(s): Logan Ouellette - */ - -#include -#include -#include - -namespace ap1::control -{ - // 3D vector class - class vec3 { - public: - float e[3]; - - vec3() {} - vec3(float e0, float e1, float e2) { e[0] = e0; e[1] = e1; e[2] = e2; } - inline float x() const { return e[0]; } - inline float y() const { return e[1]; } - inline float z() const { return e[2]; } - - inline const vec3& operator+() const { return *this; } - inline vec3 operator-() const { return vec3(-e[0], -e[1], -e[2]); } - inline float operator[](int i) const { return e[i]; } - inline float& operator[](int i) { return e[i]; } - - inline vec3& operator+=(const vec3 &v2); - inline vec3& operator-=(const vec3 &v2); - inline vec3& operator*=(const vec3 &v2); - inline vec3& operator/=(const vec3 &v2); - inline vec3& operator*=(const float t); - inline vec3& operator/=(const float t); - - inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); } - inline float squared_length() const { return e[0]*e[0] + e[1]*e[1] + e[2]*e[2]; } - inline void make_unit_vector(); - }; - - - inline std::istream& operator>>(std::istream &is, vec3 &t) { - is >> t.e[0] >> t.e[1] >> t.e[2]; - return is; - } - - inline std::ostream& operator<<(std::ostream &os, const vec3 &t) { - os << t.e[0] << " " << t.e[1] << " " << t.e[2]; - return os; - } - - inline void vec3::make_unit_vector() { - float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]); - e[0] *= k; e[1] *= k; e[2] *= k; - } - - inline vec3 operator+(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1], v1.e[2] + v2.e[2]); - } - - inline vec3 operator-(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1], v1.e[2] - v2.e[2]); - } - - inline vec3 operator*(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1], v1.e[2] * v2.e[2]); - } - - inline vec3 operator/(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1], v1.e[2] / v2.e[2]); - } - - inline vec3 operator*(float t, const vec3 &v) { - return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); - } - - inline vec3 operator/(vec3 v, float t) { - return vec3(v.e[0]/t, v.e[1]/t, v.e[2]/t); - } - - inline vec3 operator*(const vec3 &v, float t) { - return vec3(t*v.e[0], t*v.e[1], t*v.e[2]); - } - - inline float dot(const vec3 &v1, const vec3 &v2) { - return v1.e[0] * v2.e[0] - + v1.e[1] * v2.e[1] - + v1.e[2] * v2.e[2]; - } - - inline vec3 cross(const vec3 &v1, const vec3 &v2) { - return vec3(v1.e[1] * v2.e[2] - v1.e[2] * v2.e[1], - v1.e[2] * v2.e[0] - v1.e[0] * v2.e[2], - v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]); - } - - inline vec3& vec3::operator+=(const vec3 &v){ - e[0] += v.e[0]; - e[1] += v.e[1]; - e[2] += v.e[2]; - return *this; - } - - inline vec3& vec3::operator*=(const vec3 &v){ - e[0] *= v.e[0]; - e[1] *= v.e[1]; - e[2] *= v.e[2]; - return *this; - } - - inline vec3& vec3::operator/=(const vec3 &v){ - e[0] /= v.e[0]; - e[1] /= v.e[1]; - e[2] /= v.e[2]; - return *this; - } - - inline vec3& vec3::operator-=(const vec3& v) { - e[0] -= v.e[0]; - e[1] -= v.e[1]; - e[2] -= v.e[2]; - return *this; - } - - inline vec3& vec3::operator*=(const float t) { - e[0] *= t; - e[1] *= t; - e[2] *= t; - return *this; - } - - inline vec3& vec3::operator/=(const float t) { - float k = 1.0f/t; - - e[0] *= k; - e[1] *= k; - e[2] *= k; - return *this; - } - - inline vec3 unit_vector(vec3 v) { - return v / v.length(); - } - -} // namespace ap1::control - -namespace ap1::control -{ - // 2D vector class - class vec2 { - public: - float e[2]; - - vec2() {} - vec2(float e0, float e1) { e[0] = e0; e[1] = e1; } - inline float x() const { return e[0]; } - inline float y() const { return e[1]; } - - inline const vec2& operator+() const { return *this; } - inline vec2 operator-() const { return vec2(-e[0], -e[1]); } - inline float operator[](int i) const { return e[i]; } - inline float& operator[](int i) { return e[i]; } - - inline vec2& operator+=(const vec2 &v2); - inline vec2& operator-=(const vec2 &v2); - inline vec2& operator*=(const vec2 &v2); - inline vec2& operator/=(const vec2 &v2); - inline vec2& operator*=(const float t); - inline vec2& operator/=(const float t); - - inline float length() const { return sqrt(e[0]*e[0] + e[1]*e[1]); } - inline float squared_length() const { return e[0]*e[0] + e[1]*e[1]; } - inline void make_unit_vector(); - }; - - - inline std::istream& operator>>(std::istream &is, vec2 &t) { - is >> t.e[0] >> t.e[1]; - return is; - } - - inline std::ostream& operator<<(std::ostream &os, const vec2 &t) { - os << t.e[0] << " " << t.e[1]; - return os; - } - - inline void vec2::make_unit_vector() { - float k = 1.0 / sqrt(e[0]*e[0] + e[1]*e[1]); - e[0] *= k; e[1] *= k; - } - - inline vec2 operator+(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] + v2.e[0], v1.e[1] + v2.e[1]); - } - - inline vec2 operator-(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] - v2.e[0], v1.e[1] - v2.e[1]); - } - - inline vec2 operator*(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] * v2.e[0], v1.e[1] * v2.e[1]); - } - - inline vec2 operator/(const vec2 &v1, const vec2 &v2) { - return vec2(v1.e[0] / v2.e[0], v1.e[1] / v2.e[1]); - } - - inline vec2 operator*(float t, const vec2 &v) { - return vec2(t*v.e[0], t*v.e[1]); - } - - inline vec2 operator/(vec2 v, float t) { - return vec2(v.e[0]/t, v.e[1]/t); - } - - inline vec2 operator*(const vec2 &v, float t) { - return vec2(t*v.e[0], t*v.e[1]); - } - - inline float dot(const vec2 &v1, const vec2 &v2) { - return v1.e[0] * v2.e[0] - + v1.e[1] * v2.e[1]; - } - - inline float cross(const vec2 &v1, const vec2 &v2) { - return v1.e[0] * v2.e[1] - v1.e[1] * v2.e[0]; - } - - inline vec2& vec2::operator+=(const vec2 &v){ - e[0] += v.e[0]; - e[1] += v.e[1]; - return *this; - } - - inline vec2& vec2::operator*=(const vec2 &v){ - e[0] *= v.e[0]; - e[1] *= v.e[1]; - return *this; - } - - inline vec2& vec2::operator/=(const vec2 &v){ - e[0] /= v.e[0]; - e[1] /= v.e[1]; - return *this; - } - - inline vec2& vec2::operator-=(const vec2& v) { - e[0] -= v.e[0]; - e[1] -= v.e[1]; - return *this; - } - - inline vec2& vec2::operator*=(const float t) { - e[0] *= t; - e[1] *= t; - return *this; - } - - inline vec2& vec2::operator/=(const float t) { - float k = 1.0f/t; - - e[0] *= k; - e[1] *= k; - return *this; - } - - inline vec2 unit_vector(vec2 v) { - return v / v.length(); - } - -} // namespace ap1::control - -#endif diff --git a/include/vectors.hpp b/include/vectors.hpp new file mode 100644 index 0000000..9bfebec --- /dev/null +++ b/include/vectors.hpp @@ -0,0 +1,257 @@ +/** + * Class for 2d and 3d vectos + * Created: Oct. 11, 2025 + * Author(s): Logan Ouellette + */ + +#ifndef AP1_VECTOR_HPP +#define AP1_VECTOR_HPP + +#include +#include +#include +#include +#include + +// 2D vector class +class vec2f +{ + public: + float x, y; + + vec2f() : x(0.f), y(0.f) {} + vec2f(float x, float y) : x(x), y(y) {}; + + inline vec2f operator-() const + { + return vec2f(-x, -y); + } + + inline float operator[](const int i) const + { + if (i == 0) + return x; + else if (i == 1) + return y; + else + { + throw std::out_of_range(std::format("Index %d out of bounds for vec3f", i)); + } + } + + inline vec2f operator+(const vec2f& v) const + { + return vec2f(x + v.x, y + v.y); + } + + inline vec2f operator-(const vec2f& v) const + { + return vec2f(x - v.x, y - v.y); + } + + inline vec2f operator*(const float f) const + { + return vec2f(x * f, y * f); + } + + inline vec2f operator/(const float f) const + { + return vec2f(x / f, y / f); + } + + inline vec2f& operator+=(const vec2f& v) + { + x += v.x; + y += v.y; + return *this; + } + + inline vec2f& operator-=(const vec2f& v) + { + x -= v.x; + y -= v.y; + return *this; + } + + inline vec2f& operator*=(const float t) + { + x *= t; + y *= t; + return *this; + } + + inline vec2f& operator/=(const float t) + { + x /= t; + y /= t; + return *this; + } + + inline float length() const + { + return sqrt(x * x + y * y); + } + + inline float squared_length() const + { + return x * x + y * y; + } + + inline void make_unit_vector() + { + auto len = this->length(); + x /= len; + y /= len; + } + + inline vec2f unit_vector() const + { + auto len = this->length(); + return vec2f(x / len, y / len); + } + + inline vec2f dot(const vec2f& v) const + { + return vec2f(x * v.x, y * v.y); + } + + inline float cross(const vec2f& v) const + { + return x * v.y - y * v.x; + } +}; + +// 3D vector class +class vec3f +{ + public: + float x, y, z; + + vec3f() : x(0.f), y(0.f), z(0.f) {} + vec3f(float x, float y, float z) : x(x), y(y), z(z) {} + vec3f(const vec2f& v): x(v.x), y(v.y), z(0) {} + + float operator[](int i) const + { + if (i == 0) + return x; + else if (i == 1) + return y; + else if (i == 2) + return z; + else + { + throw std::out_of_range(std::format("Index %d out of bounds for vec3f", i)); + } + } + + inline vec3f operator-() const + { + return vec3f(-x, -y, -z); + } + + inline vec3f operator+(const vec3f& v) const + { + return vec3f(x + v.x, y + v.y, z + v.z); + } + + inline vec3f operator-(const vec3f& v) const + { + return vec3f(x - v.x, y - v.y, z - v.z); + } + + inline vec3f operator*(const float f) const + { + return vec3f(x * f, y * f, z * f); + } + + inline vec3f operator/(const float f) const + { + return vec3f(x / f, y / f, z / f); + } + + inline vec3f& operator+=(const vec3f& v) + { + x += v.x; + y += v.y; + z += v.z; + return *this; + } + + inline vec3f& operator-=(const vec3f& v) + { + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + + inline vec3f& operator*=(const float t) + { + x *= t; + y *= t; + z *= t; + return *this; + } + + inline vec3f& operator/=(const float t) + { + x /= t; + y /= t; + z /= t; + return *this; + } + + inline float length() const + { + return sqrt(x * x + y * y + z * z); + } + + inline float squared_length() const + { + return x * x + y * y + z * z; + } + + inline void make_unit_vector() + { + auto len = this->length(); + x /= len; + y /= len; + z /= len; + } + + inline vec3f unit_vector() const + { + auto len = this->length(); + return vec3f(x / len, y / len, z / len); + } + + inline vec3f dot(const vec3f& v) const + { + return vec3f(x * v.x, y * v.y, z * v.z); + } + + inline vec3f cross(const vec3f& v) const + { + return vec3f(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + } +}; + +// free function overloads +inline vec2f operator*(float s, const vec2f& v) { + return v * s; +} + +inline vec3f operator*(float s, const vec3f& v) { + return v * s; +} + +inline vec2f operator/(float s, const vec2f& v) { + return v / s; +} + +inline vec3f operator/(float s, const vec3f& v) { + return v / s; +} + +#endif diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp index d3fe540..25a92c6 100644 --- a/src/ackermann_controller.cpp +++ b/src/ackermann_controller.cpp @@ -13,6 +13,7 @@ #include #include +#include "vectors.hpp" #include "ap1/control/ackermann_controller.hpp" #define EPSILON 1e-3 @@ -60,20 +61,20 @@ AckermannController::AckermannController(const Config& cfg) : cfg_(cfg) // we should move everything over to double but I already wrote all the message types in float // and I'm too lazy to switch so we'll do it later -AckermannController::Command AckermannController::compute_command(const vec3& acc, const vec3& vel) +AckermannController::Command AckermannController::compute_command(const vec3f& acc, const vec3f& vel) { Command cmd{}; double a_long = std::clamp( - (double)acc.x(), + (double)acc.x, -cfg_.a_max, cfg_.a_max ); - double a_lat = acc[1]; + double a_lat = acc.y; - double v = vel.length(); - double kappa = (v > EPSILON) ? a_lat / (v * v) : 0.0; + double speed = vel.length(); + double kappa = (speed > EPSILON) ? a_lat / (speed * speed) : 0.0; double delta = std::atan(cfg_.L * kappa); delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max); From 98c067305908d1a8f3f4de9222b9fa74baf41c7a Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Sun, 23 Nov 2025 12:03:45 -0500 Subject: [PATCH 14/15] mv control node -> control_node.cpp --- CMakeLists.txt | 1 + include/ap1/control/control_node.hpp | 125 ++---------------------- include/ap1/control/pid_controller.hpp | 0 src/control_node.cpp | 129 +++++++++++++++++++++++++ 4 files changed, 140 insertions(+), 115 deletions(-) delete mode 100644 include/ap1/control/pid_controller.hpp create mode 100644 src/control_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 01ba0bb..61854ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ap1_msgs REQUIRED) add_executable(control_node src/main.cpp + src/control_node.cpp src/ackermann_controller.cpp ) target_include_directories(control_node PUBLIC diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 324f615..a130b04 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -6,7 +6,6 @@ #ifndef AP1_CONTROL_NODE_HPP #define AP1_CONTROL_NODE_HPP -#include #include #include @@ -20,8 +19,6 @@ #include "ap1/control/ackermann_controller.hpp" #include "ap1/control/icontroller.hpp" -#include "ap1/control/pd_controller.hpp" -#include "vectors.hpp" namespace ap1::control { @@ -30,14 +27,12 @@ class ControlNode : public rclcpp::Node private: // Fields - // # Create Control Loop - // fire at rate_hz + // Control Loop const double rate_hz_; rclcpp::TimerBase::SharedPtr timer_; // Controller - // these should be a sharedptr or something NOT raw. - IController* controller_; + std::unique_ptr controller_; AckermannController ackermann_controller_; // Memory @@ -56,117 +51,17 @@ class ControlNode : public rclcpp::Node // Pubs rclcpp::Publisher::SharedPtr turning_angle_pub_; - rclcpp::Publisher::SharedPtr - motor_power_pub_; // between -1 and 1? probably + rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably - void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile) - { - speed_profile_ = speed_profile; - } - - void on_path(const ap1_msgs::msg::TargetPathStamped target_path) - { - target_path_ = target_path; - } - - void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed) - { - vehicle_speed_ = speed; - } - - void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle) - { - vehicle_turn_angle = turn_angle; - } - - void control_loop_callback() - { - // the car's current velocity. we only support moving forward atp - const vec3f velocity(this->vehicle_speed_.speed, 0, 0); // which direction is up? assuming +x - - const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP - - // if path has no waypoints OR path is too old - if (target_path_.path.size() < 1 || PATH_IS_STALE) - return; // don't update - // todo: ideally this should actually still - // send through the speed control - // but we'll implement that later - - // if speed profile has no waypoints or speed profile is too old - if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE) - return; - - auto next_waypoint = target_path_.path.at(0); - const vec3f acc = controller_->compute_acceleration( - velocity, - vec2f(next_waypoint.x, next_waypoint.y), // for now get the first path waypoint - speed_profile_.speeds.at(0) // and the first speed value - ); - std::string s = "ACC: " + std::to_string(acc.x) + ", " + std::to_string(acc.y) + - ", " + std::to_string(acc.z); - RCLCPP_INFO(this->get_logger(), s.c_str()); - - // compute acc and throttle using ackermann controller - AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity); - s = "CMD: {throttle:" + std::to_string(cmd.throttle) + - ", steering:" + std::to_string(cmd.steering) + "}"; - RCLCPP_INFO(this->get_logger(), s.c_str()); - - // pack the turn angle into a message - ap1_msgs::msg::TurnAngleStamped turn_msg; - turn_msg.header.stamp = this->now(); - turn_msg.header.frame_id = "base_link"; - turn_msg.angle = cmd.steering; // rads - - // pack the power into a message - ap1_msgs::msg::MotorPowerStamped pwr_msg; - pwr_msg.header.stamp = this->now(); - pwr_msg.header.frame_id = "base_link"; - pwr_msg.power = cmd.throttle; // [-1, 1] - // pwr_msg.power = 1.0f; - - // send both messages out - turning_angle_pub_->publish(turn_msg); - motor_power_pub_->publish(pwr_msg); - } + // 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) - : Node("control_node"), rate_hz_(rate_hz), controller_(new PDController()), - ackermann_controller_(AckermannController(AckermannController::load_config(cfg_path))) - { - // # All inputs shabooya - // - SPEED PROFILE - speed_profile_sub_ = this->create_subscription( - "ap1/planning/speed_profile", 10, - std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); - // - TARGET PATH - target_path_sub_ = this->create_subscription( - "ap1/planning/target_path", 10, - std::bind(&ControlNode::on_path, this, std::placeholders::_1)); - vehicle_speed_sub_ = this->create_subscription( - "ap1/actuation/speed_actual", 10, - std::bind(&ControlNode::on_speed, this, std::placeholders::_1)); - vehicle_turn_angle_sub_ = this->create_subscription( - "ap1/actuation/turn_angle_actual", 10, - std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1)); - - // # Publishers - // - TURNING ANGLE - turning_angle_pub_ = - this->create_publisher("ap1/control/turn_angle", 10); - // - MOTOR POWER - motor_power_pub_ = - this->create_publisher("ap1/control/motor_power", 10); - - // # Create Control Loop - // fire at rate_hz - timer_ = this->create_wall_timer(std::chrono::duration(1.0 / rate_hz), - std::bind(&ControlNode::control_loop_callback, this)); - - RCLCPP_INFO(this->get_logger(), "Control Node initialized"); - } + ControlNode(const std::string& cfg_path, float rate_hz = 60); }; } // namespace ap1::control diff --git a/include/ap1/control/pid_controller.hpp b/include/ap1/control/pid_controller.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/control_node.cpp b/src/control_node.cpp new file mode 100644 index 0000000..b158038 --- /dev/null +++ b/src/control_node.cpp @@ -0,0 +1,129 @@ +/** + * Created: Nov. 23, 2025 + * Author(s): Aly Ashour + */ + +#include +#include +#include + +#include "rclcpp/rclcpp.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/control_node.hpp" +#include "ap1/control/icontroller.hpp" +#include "ap1/control/pd_controller.hpp" +#include "vectors.hpp" + +using namespace ap1_msgs::msg; +using namespace ap1::control; + +void ControlNode::on_speed_profile(const SpeedProfileStamped speed_profile) +{ + speed_profile_ = speed_profile; +} + +void ControlNode::on_path(const TargetPathStamped target_path) +{ + target_path_ = target_path; +} + +void ControlNode::on_speed(const VehicleSpeedStamped speed) +{ + vehicle_speed_ = speed; +} + +void ControlNode::on_turn_angle(const TurnAngleStamped turn_angle) +{ + vehicle_turn_angle = turn_angle; +} + +void ControlNode::control_loop_callback() +{ + // the car's current velocity. we only support moving forward atp + const vec3f velocity(this->vehicle_speed_.speed, 0, 0); // +x is always forward on the car + + const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP + + // if path has no waypoints OR path is too old + if (target_path_.path.size() < 1 || PATH_IS_STALE) + return; // don't update + // todo: ideally this should actually still + // send through the speed control + // but we'll implement that later + + // if speed profile has no waypoints or speed profile is too old + if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE) + return; + + // ask the controller to calculate the acceleration needed + // for now we'll only consider the very next waypoint & speed value + auto next_waypoint = target_path_.path.at(0); + const vec3f acc = controller_->compute_acceleration( + velocity, vec2f(next_waypoint.x, next_waypoint.y), speed_profile_.speeds.at(0)); + + // log + std::string s = "ACC: " + std::to_string(acc.x) + ", " + std::to_string(acc.y) + ", " + + std::to_string(acc.z); + RCLCPP_INFO(this->get_logger(), s.c_str()); + + // compute acc and throttle using ackermann controller + AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity); + + // log + s = "CMD: {throttle:" + std::to_string(cmd.throttle) + + ", steering:" + std::to_string(cmd.steering) + "}"; + RCLCPP_INFO(this->get_logger(), s.c_str()); + + // pack the turn angle into a message + TurnAngleStamped turn_msg; + turn_msg.header.stamp = this->now(); + turn_msg.header.frame_id = "base_link"; + turn_msg.angle = cmd.steering; // rads + + // pack the power into a message + MotorPowerStamped pwr_msg; + pwr_msg.header.stamp = this->now(); + pwr_msg.header.frame_id = "base_link"; + pwr_msg.power = cmd.throttle; // [-1, 1] + // pwr_msg.power = 1.0f; + + // send both messages out + turning_angle_pub_->publish(turn_msg); + motor_power_pub_->publish(pwr_msg); +} + +ControlNode::ControlNode(const std::string& cfg_path, float rate_hz) + : Node("control_node"), rate_hz_(rate_hz), controller_(new PDController()), + ackermann_controller_(AckermannController(AckermannController::load_config(cfg_path))) +{ + // Subs + speed_profile_sub_ = this->create_subscription( + "ap1/planning/speed_profile", 10, + std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); + target_path_sub_ = this->create_subscription( + "ap1/planning/target_path", 10, + std::bind(&ControlNode::on_path, this, std::placeholders::_1)); + vehicle_speed_sub_ = this->create_subscription( + "ap1/actuation/speed_actual", 10, + std::bind(&ControlNode::on_speed, this, std::placeholders::_1)); + vehicle_turn_angle_sub_ = this->create_subscription( + "ap1/actuation/turn_angle_actual", 10, + std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1)); + + // Pubs + turning_angle_pub_ = this->create_publisher("ap1/control/turn_angle", 10); + motor_power_pub_ = this->create_publisher("ap1/control/motor_power", 10); + + // Create Control Loop + timer_ = this->create_wall_timer(std::chrono::duration(1.0 / rate_hz), + std::bind(&ControlNode::control_loop_callback, this)); + + // Log completion + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); +} From 8f736ec5c2f6167230e34f0ce25910556dbebe28 Mon Sep 17 00:00:00 2001 From: Aly Ashour Date: Sun, 23 Nov 2025 12:14:00 -0500 Subject: [PATCH 15/15] update build script --- .github/workflows/colcon_build.yml | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/.github/workflows/colcon_build.yml b/.github/workflows/colcon_build.yml index 5731215..36edf1c 100644 --- a/.github/workflows/colcon_build.yml +++ b/.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' }}