Pre ackermann (and twist)

CMakeLists.txt

@@ -5,6 +5,17 @@ set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
+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()
@@ -14,6 +25,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)
 find_package(ap1_msgs REQUIRED)
 
 add_executable(control_node 
@@ -25,12 +37,19 @@ target_include_directories(control_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
+
+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"
+  rclcpp
+  std_msgs
+  geometry_msgs
+  ap1_msgs
 )
 
 install(TARGETS 

include/ap1/control/control_node.hpp

@@ -20,6 +20,14 @@
 #include "ap1/control/ackermann_controller.hpp"
 #include "ap1/control/icontroller.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
+
 namespace ap1::control
 {
 class ControlNode : public rclcpp::Node
@@ -52,6 +60,14 @@ class ControlNode : public rclcpp::Node
     // Pubs
     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
+
+    #ifdef AP1_CONTROL_SUPPORT_ACKERMANN
+    rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr ackermann_pub_;
+    #endif
+
+    #ifdef AP1_CONTROL_SUPPORT_TWIST
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+    #endif 
 
     // Methods
     void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile);

package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>ackermann_msgs</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>

src/ackermann_controller.cpp

@@ -16,7 +16,7 @@
 #include "vectors.hpp"
 #include "ap1/control/ackermann_controller.hpp"
 
-#define EPSILON 1e-3
+const float EPSILON = 1e-3;
 
 namespace ap1::control
 {

src/control_node.cpp

@@ -9,17 +9,28 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+// messages
 #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"
 
+// control
 #include "ap1/control/control_node.hpp"
 #include "ap1/control/icontroller.hpp"
 #include "ap1/control/pd_controller.hpp"
+#include "ap1/control/ackermann_controller.hpp"
+#include "ap1/control/icontroller.hpp"
 #include "vectors.hpp"
 
+// special types
+#ifdef AP1_CONTROL_SUPPORT_TWIST
+#include "geometry_msgs/msg/twist.hpp"
+#elif defined(AP1_CONTROL_SUPPORT_ACKERMANN)
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#endif
+
 using namespace ap1_msgs::msg;
 using namespace ap1::control;
 
@@ -80,6 +91,23 @@ void ControlNode::control_loop_callback()
         ", steering:" + std::to_string(cmd.steering) + "}";
     RCLCPP_INFO(this->get_logger(), s.c_str());
 
+#ifdef AP1_CONTROL_SUPPORT_ACKERMANN
+    // only send out an ackermann message
+    ackermann_msgs::msg::AckermannDriveStamped msg;
+    msg.header.stamp = this->now();
+    msg.drive.speed = speed_profile_.speeds.at(0);
+    msg.drive.steering_angle = cmd.steering; // rads
+    ackermann_pub_->publish(msg);
+#elif defined(AP1_CONTROL_SUPPORT_TWIST)
+    // only send out a twist message
+    geometry_msgs::msg::Twist msg;
+    msg.linear.x = speed_profile_.speeds.at(0);
+    // msg.angular.z = cmd.steering; STILL NEEDS TO BE IMPLEMENTED. THIS IS TARGET ANGLE NOT RAD/S
+    // ANGULAR CONTROL.
+    throw "Not yet implemented";
+    twist_pub_->publish(msg);
+#else
+    // send out both a TurnAngleStamped and MotorPowerStamped message
     // pack the turn angle into a message
     TurnAngleStamped turn_msg;
     turn_msg.header.stamp = this->now();
@@ -91,11 +119,11 @@ void ControlNode::control_loop_callback()
     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);
+#endif
 }
 
 ControlNode::ControlNode(const std::string& cfg_path, float rate_hz)
@@ -119,6 +147,14 @@ ControlNode::ControlNode(const std::string& cfg_path, float rate_hz)
     // 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);
+    #ifdef AP1_CONTROL_SUPPORT_ACKERMANN
+    ackermann_pub_ = this->create_publisher<ackermann_msgs::msg::AckermannDriveStamped>(
+        "/ap1/control/ackermann_cmd", 10);
+    #endif
+
+    #ifdef AP1_CONTROL_SUPPORT_TWIST
+    twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/ap1/control/twist_cmd", 10);
+    #endif
 
     // Create Control Loop
     timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / rate_hz),