Implement reset functionality for control module

CMakeLists.txt

@@ -9,37 +9,54 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
+# Required packages
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(ap1_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
-add_executable(control_node 
-    src/main.cpp
-    src/control_node.cpp
-    src/ackermann_controller.cpp
+include_directories(
+  include
 )
+
+# ------------------------------------------------------------------------------
+# EXECUTABLE: control_node
+# Merge both versions: include your controllers + upstream structure
+# ------------------------------------------------------------------------------
+
+add_executable(control_node
+  src/main.cpp
+  src/control_node.cpp
+  src/ackermann_controller.cpp
+  src/pd_controller.cpp
+)
+
 target_include_directories(control_node PUBLIC 
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
+
 ament_target_dependencies(
-  control_node 
-  "rclcpp"
-  "std_msgs"
-  "geometry_msgs"
-  "ap1_msgs"
+  control_node
+  rclcpp
+  std_msgs
+  geometry_msgs
+  std_srvs
+  ap1_msgs
+)
+
+target_link_libraries(control_node
+  yaml-cpp
 )
 
-install(TARGETS 
-  control_node 
+install(TARGETS control_node
   DESTINATION lib/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
-  # no auto linting for now
 endif()
 
 ament_package()

config.yaml

@@ -0,0 +1 @@
+rate_hz: 20

config/control.yaml

@@ -0,0 +1,6 @@
+control_node:
+  ros__parameters:
+    rate_hz: 20
+    turn_kp: 0.5
+    turn_ki: 0.1
+    turn_kd: 0.05

include/ap1/control/control_node.hpp

@@ -8,61 +8,72 @@
 
 #include <iostream>
 #include <string>
+#include <memory>
+#include <filesystem>
+#include <chrono>
 
 #include "rclcpp/rclcpp.hpp"
+#include "yaml-cpp/yaml.h"
 
 #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 "std_srvs/srv/trigger.hpp"
+
 #include "ap1/control/ackermann_controller.hpp"
 #include "ap1/control/icontroller.hpp"
 
 namespace ap1::control
 {
+
 class ControlNode : public rclcpp::Node
 {
-  private:
-    // Fields
+public:
+    // Constructor with config path + default rate
+    ControlNode(const std::string &cfg_path, float rate_hz = 60);
 
-    // Control Loop
-    const double rate_hz_;
-    rclcpp::TimerBase::SharedPtr timer_;
+private:
+    void load_config();
+
+    std::string config_path_;
+    int rate_hz_{60};
 
-    // Controller
     std::unique_ptr<IController> controller_;
     AckermannController ackermann_controller_;
 
-    // Memory
-    // half these types are very unnecessary, we should just have stampedfloat or
-    // stamped double or something
     ap1_msgs::msg::SpeedProfileStamped speed_profile_;
     ap1_msgs::msg::TargetPathStamped target_path_;
     ap1_msgs::msg::VehicleSpeedStamped vehicle_speed_;
-    ap1_msgs::msg::TurnAngleStamped vehicle_turn_angle;
+    ap1_msgs::msg::TurnAngleStamped vehicle_turn_angle_;
 
-    // Subs
     rclcpp::Subscription<ap1_msgs::msg::TargetPathStamped>::SharedPtr target_path_sub_;
     rclcpp::Subscription<ap1_msgs::msg::SpeedProfileStamped>::SharedPtr speed_profile_sub_;
     rclcpp::Subscription<ap1_msgs::msg::VehicleSpeedStamped>::SharedPtr vehicle_speed_sub_;
     rclcpp::Subscription<ap1_msgs::msg::TurnAngleStamped>::SharedPtr vehicle_turn_angle_sub_;
 
-    // Pubs
+    void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped msg);
+    void on_path(const ap1_msgs::msg::TargetPathStamped msg);
+    void on_speed(const ap1_msgs::msg::VehicleSpeedStamped msg);
+    void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped msg);
+
+
     rclcpp::Publisher<ap1_msgs::msg::TurnAngleStamped>::SharedPtr turning_angle_pub_;
-    rclcpp::Publisher<ap1_msgs::msg::MotorPowerStamped>::SharedPtr motor_power_pub_; // between -1 and 1? probably
+    rclcpp::Publisher<ap1_msgs::msg::MotorPowerStamped>::SharedPtr motor_power_pub_;
 
-    // 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);
+    rclcpp::TimerBase::SharedPtr timer_;
     void control_loop_callback();
 
-  public:
-    ControlNode(const std::string& cfg_path, float rate_hz = 60);
+
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
+
+    void handle_reset(
+        const std::shared_ptr<std_srvs::srv::Trigger::Request>,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> response);
 };
+
 } // namespace ap1::control
 
 #endif // AP1_CONTROL_NODE_HPP

package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ap1_control</name>
   <version>0.1.0</version>
   <description>AP1 Control Node</description>
+
   <maintainer email="alyashour1@gmail.com">Aly Ashour</maintainer>
   <license>MIT</license>
 
@@ -13,9 +13,8 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>ap1_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <depend>std_srvs</depend>
+  <depend>yaml-cpp</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/control_node.cpp

@@ -23,103 +23,146 @@
 using namespace ap1_msgs::msg;
 using namespace ap1::control;
 
-void ControlNode::on_speed_profile(const SpeedProfileStamped speed_profile)
+void ControlNode::on_speed_profile(const SpeedProfileStamped msg)
 {
-    speed_profile_ = speed_profile;
+    speed_profile_ = msg;
 }
 
-void ControlNode::on_path(const TargetPathStamped target_path)
+void ControlNode::on_path(const TargetPathStamped msg)
 {
-    target_path_ = target_path;
+    target_path_ = msg;
 }
 
-void ControlNode::on_speed(const VehicleSpeedStamped speed)
+void ControlNode::on_speed(const VehicleSpeedStamped msg)
 {
-    vehicle_speed_ = speed;
+    vehicle_speed_ = msg;
 }
 
-void ControlNode::on_turn_angle(const TurnAngleStamped turn_angle)
+void ControlNode::on_turn_angle(const TurnAngleStamped msg)
 {
-    vehicle_turn_angle = turn_angle;
+    vehicle_turn_angle_ = msg;
 }
 
 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 vec3f velocity(this->vehicle_speed_.speed, 0, 0);
 
-    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
+    // Require valid path and speed profile
+    if (target_path_.path.empty())
+        return;
 
-    // if speed profile has no waypoints or speed profile is too old
-    if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE)
+    if (speed_profile_.speeds.empty())
         return;
 
-    // ask the controller to calculate the acceleration needed
-    // for now we'll only consider the very next waypoint & speed value
+    // Compute acceleration
     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));
+        velocity,
+        vec2f(next_waypoint.x, next_waypoint.y),
+        speed_profile_.speeds.at(0));
 
-    // log
-    RCLCPP_INFO(this->get_logger(), "ACC: %.2f, %.2f, %.2f", acc.x, acc.y, acc.z);
+    RCLCPP_INFO(this->get_logger(), "ACC: %.2f %.2f %.2f", acc.x, acc.y, acc.z);
 
-    // compute acc and throttle using ackermann controller
-    AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity);
+    // Convert to Ackermann steering + throttle
+    AckermannController::Command cmd =
+        ackermann_controller_.compute_command(acc, velocity);
 
-    // log
-    RCLCPP_INFO(this->get_logger(), "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);
+    RCLCPP_INFO(this->get_logger(),
+        "CMD: {throttle: %.2f, steering: %.2f}",
+        cmd.throttle, cmd.steering);
 
-    // pack the turn angle into a message
+    // Publish steering
     TurnAngleStamped turn_msg;
     turn_msg.header.stamp = this->now();
     turn_msg.header.frame_id = "base_link";
-    turn_msg.angle = cmd.steering; // rads
+    turn_msg.angle = cmd.steering;
+    turning_angle_pub_->publish(turn_msg);
 
-    // pack the power into a message
+    // Publish throttle
     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);
+    pwr_msg.power = cmd.throttle;
     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)))
+void ControlNode::load_config()
+{
+    YAML::Node config = YAML::LoadFile(config_path_);
+
+    if (config["rate_hz"])
+        rate_hz_ = config["rate_hz"].as<int>();
+
+    RCLCPP_INFO(this->get_logger(), "Config reloaded: rate_hz=%d", rate_hz_);
+}
+
+ControlNode::ControlNode(const std::string &cfg_path, float rate_hz)
+    : Node("control_node"),
+      config_path_(cfg_path),
+      rate_hz_(rate_hz),
+      controller_(new PDController()),
+      ackermann_controller_(
+          AckermannController(AckermannController::load_config(cfg_path)))
 {
-    // Subs
+    // Subscriptions
     speed_profile_sub_ = this->create_subscription<SpeedProfileStamped>(
         "ap1/planning/speed_profile", 10,
         std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1));
+
     target_path_sub_ = this->create_subscription<TargetPathStamped>(
         "ap1/planning/target_path", 10,
         std::bind(&ControlNode::on_path, this, std::placeholders::_1));
+
     vehicle_speed_sub_ = this->create_subscription<VehicleSpeedStamped>(
         "ap1/actuation/speed_actual", 10,
         std::bind(&ControlNode::on_speed, this, std::placeholders::_1));
+
     vehicle_turn_angle_sub_ = this->create_subscription<TurnAngleStamped>(
         "ap1/actuation/turn_angle_actual", 10,
         std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1));
 
-    // Pubs
-    turning_angle_pub_ = this->create_publisher<TurnAngleStamped>("ap1/control/turn_angle", 10);
-    motor_power_pub_ = this->create_publisher<MotorPowerStamped>("ap1/control/motor_power", 10);
+    // Publishers
+    turning_angle_pub_ =
+        this->create_publisher<TurnAngleStamped>("ap1/control/turn_angle", 10);
+
+    motor_power_pub_ =
+        this->create_publisher<MotorPowerStamped>("ap1/control/motor_power", 10);
 
-    // Create Control Loop
-    timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / rate_hz),
-                                     std::bind(&ControlNode::control_loop_callback, this));
+    // Load config at startup
+    load_config();
+
+    // Reset service
+    reset_service_ = this->create_service<std_srvs::srv::Trigger>(
+        "ap1/control/reset",
+        std::bind(
+            &ControlNode::handle_reset,
+            this,
+            std::placeholders::_1,
+            std::placeholders::_2));
+
+    // Create control loop timer
+    timer_ = this->create_wall_timer(
+        std::chrono::duration<double>(1.0 / rate_hz_),
+        std::bind(&ControlNode::control_loop_callback, this));
 
-    // Log completion
     RCLCPP_INFO(this->get_logger(), "Control Node initialized");
 }
+
+void ControlNode::handle_reset(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request>,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+{
+    try
+    {
+        load_config();
+        response->success = true;
+        response->message = "Control reset: config reloaded";
+    }
+    catch (const std::exception &e)
+    {
+        response->success = false;
+        response->message = std::string("Reset failed: ") + e.what();
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Control reset triggered.");
+}