diff --git a/CMakeLists.txt b/CMakeLists.txt index 61854ed..7f29db4 100644 --- a/CMakeLists.txt +++ b/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 $ $ ) + 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() diff --git a/config.yaml b/config.yaml new file mode 100644 index 0000000..c545b74 --- /dev/null +++ b/config.yaml @@ -0,0 +1 @@ +rate_hz: 20 diff --git a/config/control.yaml b/config/control.yaml new file mode 100644 index 0000000..97b4222 --- /dev/null +++ b/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 diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index a130b04..9b6eaca 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -8,8 +8,12 @@ #include #include +#include +#include +#include #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" @@ -17,52 +21,59 @@ #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 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::SharedPtr target_path_sub_; rclcpp::Subscription::SharedPtr speed_profile_sub_; rclcpp::Subscription::SharedPtr vehicle_speed_sub_; rclcpp::Subscription::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::SharedPtr turning_angle_pub_; - rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably + rclcpp::Publisher::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::SharedPtr reset_service_; + + void handle_reset( + const std::shared_ptr, + std::shared_ptr response); }; + } // namespace ap1::control #endif // AP1_CONTROL_NODE_HPP diff --git a/package.xml b/package.xml index 8116814..4a79ac0 100644 --- a/package.xml +++ b/package.xml @@ -1,9 +1,9 @@ - ap1_control 0.1.0 AP1 Control Node + Aly Ashour MIT @@ -13,9 +13,8 @@ std_msgs geometry_msgs ap1_msgs - - ament_lint_auto - ament_lint_common + std_srvs + yaml-cpp ament_cmake diff --git a/src/control_node.cpp b/src/control_node.cpp index 29d9812..c6752b1 100644 --- a/src/control_node.cpp +++ b/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(); + + 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( "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); + // Publishers + 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)); + // Load config at startup + load_config(); + + // Reset service + reset_service_ = this->create_service( + "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(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::shared_ptr 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."); +} \ No newline at end of file