From 1f07fb66f94c8ece5263aff0771d30c417cc73db Mon Sep 17 00:00:00 2001 From: cadence-m Date: Sat, 22 Nov 2025 17:43:32 -0500 Subject: [PATCH 1/5] updating CMakeLists.txt --- CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 854a311..272ff50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 3.8) project(ap1_control) +option(AP1_CONTROL_SUPPORT_ACKERMANN "Enable ackermann command output" ON) + +if(AP1_CONTROL_SUPPORT_ACKERMANN) + add_compile_definitions(AP1_CONTROL_SUPPORT_ACKERMANN) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -10,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) add_executable(control_node src/main.cpp) target_include_directories(control_node PUBLIC @@ -21,6 +28,7 @@ ament_target_dependencies( "rclcpp" "std_msgs" "geometry_msgs" + "ackermann_msgs" ) install(TARGETS From 643c8323d60d22789582f3b020b42004e4beccc1 Mon Sep 17 00:00:00 2001 From: cadence-m Date: Sat, 22 Nov 2025 20:16:40 -0500 Subject: [PATCH 2/5] Added control node .cpp, modified CMakeLists.txt and package.xml --- CMakeLists.txt | 20 +++++++---- include/ap1/control/control_node.hpp | 14 ++++++++ package.xml | 5 +++ src/control_node.cpp | 52 ++++++++++++++++++++++++++++ 4 files changed, 85 insertions(+), 6 deletions(-) create mode 100644 src/control_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 272ff50..27fabbb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,19 +16,27 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(ap1_msgs REQUIRED) find_package(ackermann_msgs REQUIRED) -add_executable(control_node src/main.cpp) +add_executable(control_node + src/main.cpp + src/control_node.cpp +) + target_include_directories(control_node PUBLIC $ $ ) + +target_compile_features(control_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 ament_target_dependencies( - control_node - "rclcpp" - "std_msgs" - "geometry_msgs" - "ackermann_msgs" + control_node + rclcpp + std_msgs + geometry_msgs + ap1_msgs + ackermann_msgs ) install(TARGETS diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 1e68857..22fff48 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -12,6 +12,10 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" +#ifdef AP1_CONTROL_SUPPORT_ACKERMANN +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#endif + namespace ap1::control { class ControlNode : public rclcpp::Node { private: @@ -33,6 +37,9 @@ namespace ap1::control { // Pubs rclcpp::Publisher::SharedPtr turning_angle_pub_; rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably + #ifdef AP1_CONTROL_SUPPORT_ACKERMANN + rclcpp::Publisher::SharedPtr ackermann_pub_; + #endif public: ControlNode() : Node("control_node") { // # All inputs shabooya @@ -50,6 +57,13 @@ namespace ap1::control { turning_angle_pub_ = this->create_publisher("turning_angle", 10); // - MOTOR POWER motor_power_pub_ = this->create_publisher("motor_power", 10); + + #ifdef AP1_CONTROL_SUPPORT_ACKERMANN + ackermann_pub_ = this->create_publisher( + "/ap1/control/ackermann_cmd", 10 + ); + #endif + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); } diff --git a/package.xml b/package.xml index 0bf67dd..a01bcf4 100644 --- a/package.xml +++ b/package.xml @@ -9,6 +9,11 @@ ament_cmake + rclcpp + std_msgs + geometry_msgs + ackermann_msgs + ament_lint_auto ament_lint_common diff --git a/src/control_node.cpp b/src/control_node.cpp new file mode 100644 index 0000000..6221ba8 --- /dev/null +++ b/src/control_node.cpp @@ -0,0 +1,52 @@ +#include "ap1/control/control_node.hpp" + +namespace ap1::control { + +ControlNode::ControlNode() +: Node("control_node") +{ + speed_profile_sub_ = this->create_subscription( + "speed_profile", 10, + std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); + + target_path_sub_ = this->create_subscription( + "target_path", 10, + std::bind(&ControlNode::on_path, this, std::placeholders::_1)); + + turning_angle_pub_ = this->create_publisher("turning_angle", 10); + motor_power_pub_ = this->create_publisher("motor_power", 10); + +#ifdef AP1_CONTROL_SUPPORT_ACKERMANN + ackermann_pub_ = this->create_publisher( + "/ap1/control/ackermann_cmd", 10); +#endif + + RCLCPP_INFO(this->get_logger(), "Control Node initialized"); +} + +void ControlNode::on_speed_profile(const std_msgs::msg::Float32MultiArray::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning"); + + float motor_output = 0.0f; // TODO compute + float turning_output = 0.0f; // TODO compute + + // Standard outputs + std_msgs::msg::Float32 motor_msg; + motor_msg.data = motor_output; + motor_power_pub_->publish(motor_msg); + + std_msgs::msg::Float32 turn_msg; + turn_msg.data = turning_output; + turning_angle_pub_->publish(turn_msg); + +#ifdef AP1_CONTROL_SUPPORT_ACKERMANN + ackermann_msgs::msg::AckermannDriveStamped ack_msg; + ack_msg.header.stamp = this->now(); + ack_msg.drive.speed = motor_output; + ack_msg.drive.steering_angle = turning_output; + ackermann_pub_->publish(ack_msg); +#endif +} + +} // namespace ap1::control From 765d89d3599b6c5023f2bbad74d1720f8afc6a37 Mon Sep 17 00:00:00 2001 From: cadence-m Date: Sun, 23 Nov 2025 19:43:00 -0500 Subject: [PATCH 3/5] Got rid of unnecessary control_node.cpp, added publish_output method in the header --- CMakeLists.txt | 3 -- include/ap1/control/control_node.hpp | 28 +++++++++++++-- src/control_node.cpp | 52 ---------------------------- 3 files changed, 26 insertions(+), 57 deletions(-) delete mode 100644 src/control_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 27fabbb..f24c166 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,12 +16,10 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(ap1_msgs REQUIRED) find_package(ackermann_msgs REQUIRED) add_executable(control_node src/main.cpp - src/control_node.cpp ) target_include_directories(control_node PUBLIC @@ -35,7 +33,6 @@ ament_target_dependencies( rclcpp std_msgs geometry_msgs - ap1_msgs ackermann_msgs ) diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 22fff48..0ce907a 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -39,6 +39,8 @@ namespace ap1::control { rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably #ifdef AP1_CONTROL_SUPPORT_ACKERMANN rclcpp::Publisher::SharedPtr ackermann_pub_; + // timer for publishing outputs + rclcpp::TimerBase::SharedPtr timer_; #endif public: ControlNode() : Node("control_node") { @@ -60,13 +62,35 @@ namespace ap1::control { #ifdef AP1_CONTROL_SUPPORT_ACKERMANN ackermann_pub_ = this->create_publisher( - "/ap1/control/ackermann_cmd", 10 + "/ap1/control/ackermann_cmd", 10 ); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + [this]() { publish_outputs(1.0, 0.1); } + ); + #endif + + } + + void publish_outputs(float motor_output, float turning_output) { + #ifdef AP1_CONTROL_SUPPORT_ACKERMANN + ackermann_msgs::msg::AckermannDriveStamped msg; + msg.header.stamp = this->now(); + msg.drive.speed = motor_output; + msg.drive.steering_angle = turning_output; + ackermann_pub_->publish(msg); #endif + std_msgs::msg::Float32 motor_msg; + motor_msg.data = motor_output; + motor_power_pub_->publish(motor_msg); - RCLCPP_INFO(this->get_logger(), "Control Node initialized"); + std_msgs::msg::Float32 turning_msg; + turning_msg.data = turning_output; + turning_angle_pub_->publish(turning_msg); } + }; } diff --git a/src/control_node.cpp b/src/control_node.cpp deleted file mode 100644 index 6221ba8..0000000 --- a/src/control_node.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "ap1/control/control_node.hpp" - -namespace ap1::control { - -ControlNode::ControlNode() -: Node("control_node") -{ - speed_profile_sub_ = this->create_subscription( - "speed_profile", 10, - std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)); - - target_path_sub_ = this->create_subscription( - "target_path", 10, - std::bind(&ControlNode::on_path, this, std::placeholders::_1)); - - turning_angle_pub_ = this->create_publisher("turning_angle", 10); - motor_power_pub_ = this->create_publisher("motor_power", 10); - -#ifdef AP1_CONTROL_SUPPORT_ACKERMANN - ackermann_pub_ = this->create_publisher( - "/ap1/control/ackermann_cmd", 10); -#endif - - RCLCPP_INFO(this->get_logger(), "Control Node initialized"); -} - -void ControlNode::on_speed_profile(const std_msgs::msg::Float32MultiArray::SharedPtr msg) -{ - RCLCPP_INFO(this->get_logger(), "Received Speed Profile from Planning"); - - float motor_output = 0.0f; // TODO compute - float turning_output = 0.0f; // TODO compute - - // Standard outputs - std_msgs::msg::Float32 motor_msg; - motor_msg.data = motor_output; - motor_power_pub_->publish(motor_msg); - - std_msgs::msg::Float32 turn_msg; - turn_msg.data = turning_output; - turning_angle_pub_->publish(turn_msg); - -#ifdef AP1_CONTROL_SUPPORT_ACKERMANN - ackermann_msgs::msg::AckermannDriveStamped ack_msg; - ack_msg.header.stamp = this->now(); - ack_msg.drive.speed = motor_output; - ack_msg.drive.steering_angle = turning_output; - ackermann_pub_->publish(ack_msg); -#endif -} - -} // namespace ap1::control From 4213b03148b8d96d52823b4cf66a97b19be4c670 Mon Sep 17 00:00:00 2001 From: cadence-m Date: Tue, 25 Nov 2025 11:51:21 -0500 Subject: [PATCH 4/5] Added twist publisher --- CMakeLists.txt | 5 +++++ include/ap1/control/control_node.hpp | 23 ++++++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f24c166..9f5e3db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,11 +2,16 @@ cmake_minimum_required(VERSION 3.8) project(ap1_control) option(AP1_CONTROL_SUPPORT_ACKERMANN "Enable ackermann command output" ON) +option(AP1_CONTROL_SUPPORT_TWIST "Enable Twist publisher" OFF) if(AP1_CONTROL_SUPPORT_ACKERMANN) add_compile_definitions(AP1_CONTROL_SUPPORT_ACKERMANN) endif() +if(AP1_CONTROL_SUPPORT_TWIST) + add_compile_definitions(AP1_CONTROL_SUPPORT_TWIST) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 0ce907a..20c5e4f 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -12,6 +12,10 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" +#ifdef AP1_CONTROL_SUPPORT_TWIST +#include "geometry_msgs/msg/twist.hpp" +#endif + #ifdef AP1_CONTROL_SUPPORT_ACKERMANN #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #endif @@ -39,9 +43,14 @@ namespace ap1::control { rclcpp::Publisher::SharedPtr motor_power_pub_; // between -1 and 1? probably #ifdef AP1_CONTROL_SUPPORT_ACKERMANN rclcpp::Publisher::SharedPtr ackermann_pub_; - // timer for publishing outputs + // timer for publishing ackermann outputs rclcpp::TimerBase::SharedPtr timer_; #endif + + #ifdef AP1_CONTROL_SUPPORT_TWIST + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::TimerBase::SharedPtr twist_timer_; + #endif public: ControlNode() : Node("control_node") { // # All inputs shabooya @@ -71,6 +80,18 @@ namespace ap1::control { ); #endif + #ifdef AP1_CONTROL_SUPPORT_TWIST + twist_pub_ = this->create_publisher("/ap1/control/twist_cmd", 10); + twist_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + [this]() { + geometry_msgs::msg::Twist msg; + msg.linear.x = 1.0; // example forward velocity + msg.angular.z = 0.5; // example turning rate + twist_pub_->publish(msg); + }); + #endif + } void publish_outputs(float motor_output, float turning_output) { From 60d05d5c2ccbd927982efe24f5ccd10cf179bad5 Mon Sep 17 00:00:00 2001 From: cadence-m Date: Tue, 2 Dec 2025 14:35:18 -0500 Subject: [PATCH 5/5] Consolidated extra timers into one timer for publishers --- include/ap1/control/control_node.hpp | 5 +---- src/control_node.cpp | 28 ++++++++-------------------- 2 files changed, 9 insertions(+), 24 deletions(-) diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index 1dcdb8f..8329c04 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -63,14 +63,11 @@ class ControlNode : public rclcpp::Node #ifdef AP1_CONTROL_SUPPORT_ACKERMANN rclcpp::Publisher::SharedPtr ackermann_pub_; - // timer for publishing ackermann outputs - rclcpp::TimerBase::SharedPtr timer_; #endif #ifdef AP1_CONTROL_SUPPORT_TWIST rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::TimerBase::SharedPtr twist_timer_; - #endif + #endif // Methods void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile); diff --git a/src/control_node.cpp b/src/control_node.cpp index 2bb1bc8..9241891 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -147,30 +147,18 @@ ControlNode::ControlNode(const std::string& cfg_path, float rate_hz) // 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)); - -#ifdef AP1_CONTROL_SUPPORT_ACKERMANN + #ifdef AP1_CONTROL_SUPPORT_ACKERMANN ackermann_pub_ = this->create_publisher( "/ap1/control/ackermann_cmd", 10); + #endif -// timer_ = this->create_wall_timer(std::chrono::milliseconds(500), -// [this]() { publish_outputs(1.0, 0.1); }); -#endif - -#ifdef AP1_CONTROL_SUPPORT_TWIST + #ifdef AP1_CONTROL_SUPPORT_TWIST twist_pub_ = this->create_publisher("/ap1/control/twist_cmd", 10); - /*twist_timer_ = this->create_wall_timer(std::chrono::milliseconds(500), - [this]() - { - geometry_msgs::msg::Twist msg; - msg.linear.x = 1.0; // example forward velocity - msg.angular.z = 0.5; // example turning rate - twist_pub_->publish(msg); - });*/ -#endif + #endif + + // 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");