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 + 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' }} 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..61854ed 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() @@ -10,8 +14,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(ap1_msgs REQUIRED) -add_executable(control_node src/main.cpp) +add_executable(control_node + src/main.cpp + src/control_node.cpp + src/ackermann_controller.cpp +) target_include_directories(control_node PUBLIC $ $ @@ -21,6 +30,7 @@ ament_target_dependencies( "rclcpp" "std_msgs" "geometry_msgs" + "ap1_msgs" ) install(TARGETS @@ -29,15 +39,7 @@ install(TARGETS ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + # no auto linting for now endif() ament_package() 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..3c88484 --- /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 "vectors.hpp" + +namespace ap1::control +{ +class AckermannController +{ + public: + struct Config + { + double L; // wheelbase in meters + double a_max; // max longitudinal accleration (ms^-2) + double delta_max; // max steering angle + double throttle_gain; // scales accel to throttle [-1, 1] + }; + + explicit AckermannController(const Config &cfg); + + struct Command + { + double throttle; // normalized [-1, 1] + double steering; // in rads + }; + + Command compute_command(const vec3f& acc, const vec3f& vel); + + static Config load_config(const std::string &path); + + private: + Config cfg_; +}; +} // namespace ap1::control + +#endif // AP1_ACKERMANN_CONTROLLER diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 1e68857..a130b04 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -6,54 +6,63 @@ #ifndef AP1_CONTROL_NODE_HPP #define AP1_CONTROL_NODE_HPP +#include +#include + #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/float32_multi_array.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/point32.hpp" - -namespace ap1::control { - class ControlNode : public rclcpp::Node { - private: - void on_speed_profile(const std_msgs::msg::Float32MultiArray::SharedPtr) { - // todo: implement - RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning"); - } - - void on_path(const std_msgs::msg::Float32MultiArray::SharedPtr) { - // todo: implement - RCLCPP_INFO(this->get_logger(), "Received new path from Planning"); - } - - // Fields - // Subs - rclcpp::Subscription::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 + +#include "ap1_msgs/msg/motor_power_stamped.hpp" +#include "ap1_msgs/msg/speed_profile_stamped.hpp" +#include "ap1_msgs/msg/target_path_stamped.hpp" +#include "ap1_msgs/msg/turn_angle_stamped.hpp" +#include "ap1_msgs/msg/vehicle_speed_stamped.hpp" + +#include "ap1/control/ackermann_controller.hpp" +#include "ap1/control/icontroller.hpp" + +namespace ap1::control +{ +class ControlNode : public rclcpp::Node +{ + private: + // Fields + + // Control Loop + const double rate_hz_; + rclcpp::TimerBase::SharedPtr timer_; + + // Controller + std::unique_ptr 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 + + // Methods + void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile); + void on_path(const ap1_msgs::msg::TargetPathStamped target_path); + void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed); + void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle); + void control_loop_callback(); + + public: + ControlNode(const std::string& cfg_path, float rate_hz = 60); +}; +} // namespace ap1::control + +#endif // AP1_CONTROL_NODE_HPP diff --git a/include/ap1/control/icontroller.hpp b/include/ap1/control/icontroller.hpp new file mode 100644 index 0000000..4c6b36d --- /dev/null +++ b/include/ap1/control/icontroller.hpp @@ -0,0 +1,25 @@ +/** + * Abstract class (interface) defining what a controller is. + * Date: Nov. 10, 2025 + * Author(s): Aly Ashour + */ + +#ifndef AP1_CONTROLLER_HPP +#define AP1_CONTROLLER_HPP + +#include + +#include "vectors.hpp" +#include "geometry_msgs/msg/point.hpp" + +namespace ap1::control +{ +class IController +{ + public: + ~IController() = default; + virtual vec3f compute_acceleration(const vec3f& vel, const vec2f& target_pos, const float target_speed) = 0; +}; +} // namespace ap1::control + +#endif // AP1_CONTROLLER_HPP diff --git a/include/ap1/control/pd_controller.hpp b/include/ap1/control/pd_controller.hpp new file mode 100644 index 0000000..5d677bb --- /dev/null +++ b/include/ap1/control/pd_controller.hpp @@ -0,0 +1,71 @@ +/** + * PD Controller Implementation + * Date: Nov. 10, 2025 + * Author(s): Aly Ashour + */ + +#ifndef AP1_PD_CONTROLLER_HPP +#define AP1_PD_CONTROLLER_HPP + +#include +#include +#include +#include + +#include "geometry_msgs/msg/point.hpp" + +#include "ap1/control/icontroller.hpp" +#include "vectors.hpp" + +#define EPSILON 1e-6 + +namespace ap1::control +{ +class PDController : public IController +{ + // Stength of the proportional and derivative terms + float kp_, kd_; + // Last velocity vector + vec3f last_vel_; + + public: + PDController(float kp = 2.0, float kd = 0.5) : kp_(kp), kd_(kd) {}; + + // Computes acceleration based on current velocity, target position, and target speed. Returns + // acceleration vector. + virtual vec3f compute_acceleration(const vec3f& vel, + const vec2f& target_pos, + const float target_speed) override + { + // Convert target position to vec3f and calculate distance + const float distance = target_pos.length(); + + // targetDir for normalizing target position into unit direction + vec2f targetDir; + if (distance > EPSILON) + targetDir = target_pos.unit_vector(); + else + { + targetDir = vec2f(0.0, 0.0); + } + + // Scale direction vector by target speed for velocity + vec3f targetVel = targetDir * target_speed; + + std::cout << "target_dir_x (SHOULD BE 1): " << targetDir.x + << ", target_dir_y: " << targetDir.y + << "\ntarget_vel_x (should be target speed): " << targetVel.x << "\n"; + + // Store current velocity for derivative approximation + last_vel_ = vel; + + // Calculate velocity change (numerical derivative) + vec3f drv = vel - last_vel_; + + // PD controller: proportional term tracks target velocity, derivative term adds damping + return kp_ * (targetVel - vel) - kd_ * drv; + }; +}; +} // namespace ap1::control + +#endif // AP1_PD_CONTROLLER_HPP 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/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 diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp new file mode 100644 index 0000000..25a92c6 --- /dev/null +++ b/src/ackermann_controller.cpp @@ -0,0 +1,106 @@ +/** + * Ackermann Controller implementation bbgl + * Created: Nov. 11, 2025 + * Author(s): Aly Ashour + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "vectors.hpp" +#include "ap1/control/ackermann_controller.hpp" + +#define EPSILON 1e-3 + +namespace ap1::control +{ +// 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) +{ + 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 +AckermannController::Command AckermannController::compute_command(const vec3f& acc, const vec3f& vel) +{ + Command cmd{}; + + double a_long = std::clamp( + (double)acc.x, + -cfg_.a_max, + cfg_.a_max + ); + + double a_lat = acc.y; + + 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); + + 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/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"); +} 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 +}