super capacitor working now

Author: Raventhatfly

execution/super_capacitor/CMakeLists.txt

@@ -27,7 +27,7 @@ target_include_directories(super_capacitor PRIVATE
   $<INSTALL_INTERFACE:include>)
 
 rclcpp_components_register_node(super_capacitor
-  PLUGIN "XidiCapacitorDriver"
+  PLUGIN "SuperCapacitorController"
   EXECUTABLE super_capacitor_node)
 
 ament_target_dependencies(super_capacitor
@@ -39,24 +39,12 @@ ament_target_dependencies(super_capacitor
   meta_hardware
 )
 
-# ament_export_libraries(
-#   super_capacitor
-# )
-# ament_export_targets(
-#   export_${PROJECT_NAME}
-# )
-
-
-
 pluginlib_export_plugin_description_file(super_capacitor plugins.xml)
 
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}/
 )
 
-# install(TARGETS super_capacitor
-#   LIBRARY DESTINATION lib
-# )
 install(
   TARGETS super_capacitor
   EXPORT export_${PROJECT_NAME}

execution/super_capacitor/include/super_capacitor/super_capacitor_base.h

@@ -10,7 +10,7 @@ namespace super_capacitor
 class SuperCapacitorBase
 {
 public:
-    SuperCapacitorBase();
+    SuperCapacitorBase() = default;
 
     virtual void init(std::string can_interface) = 0;
 

execution/super_capacitor/include/super_capacitor/super_capacitor_controller.h

@@ -15,13 +15,15 @@
 #include "device_interface/msg/capacitor_state.hpp"
 #include "meta_hardware/can_driver/can_driver.hpp"
 
-class SuperCapacitorController : public rclcpp::Node
+class SuperCapacitorController
 {
 public:
-    SuperCapacitorController();
+    SuperCapacitorController(const rclcpp::NodeOptions & options);
     ~SuperCapacitorController();
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
 
 private:
+    rclcpp::Node::SharedPtr node_;
     std::shared_ptr<super_capacitor::SuperCapacitorBase> capacitor_; 
     // double target_power_, referee_power_;
     std::atomic<double> target_power_;

execution/super_capacitor/package.xml

@@ -13,6 +13,7 @@
   <test_depend>ament_lint_common</test_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>serial_driver</depend>
   <depend>pluginlib</depend>
   <depend>device_interface</depend>

execution/super_capacitor/plugins.xml

@@ -1,5 +1,5 @@
 <library path="super_capacitor">
-  <class type="super_capacitor::XidiSuperCapacitorDriver" base_class_type="super_capacitor::SuperCapacitorBase">
+  <class type="super_capacitor::XidiCapacitorDriver" base_class_type="super_capacitor::SuperCapacitorBase">
     <description>This is a super capacitor plugin.</description>
   </class>
 </library>

execution/super_capacitor/src/super_capacitor_controller.cpp

@@ -9,50 +9,60 @@
 #include "super_capacitor/super_capacitor_base.h"
 #include "super_capacitor/super_capacitor_controller.h"
 #include "meta_hardware/can_driver/can_driver.hpp"
+#include "super_capacitor/xidi_capacitor_driver.h"
 
 
-SuperCapacitorController::SuperCapacitorController() : Node("SuperCapacitorController")
+SuperCapacitorController::SuperCapacitorController(const rclcpp::NodeOptions & options)
 {
+    node_ = rclcpp::Node::make_shared("super_capacitor_controller", options);
 
     // get parameters
-    std::string can_interface = this->declare_parameter("can_interface", std::string("can0"));
-
+    std::string can_interface = node_->declare_parameter("can_interface", std::string("can0"));
+    std::string capacitor_device = node_->declare_parameter("capacitor_device", std::string("xidi"));
     // create subscriptions
-    goal_sub_ = this->create_subscription<device_interface::msg::CapacitorCmd>(
-        "super_capacitor_goal", 10, [this](const device_interface::msg::CapacitorCmd::SharedPtr msg){
-            goal_sub_callback(msg);
-        });
-
+    // goal_sub_ = node_->create_subscription<device_interface::msg::CapacitorCmd>(
+    //     "super_capacitor_goal", 10, [node_](const device_interface::msg::CapacitorCmd::SharedPtr msg){
+    //         goal_sub_callback(msg);
+    //     });
+    goal_sub_ = node_->create_subscription<device_interface::msg::CapacitorCmd>(
+        "super_capacitor_goal", 10, std::bind(&SuperCapacitorController::goal_sub_callback, this, std::placeholders::_1));
     // create a timer for publishing super capacitor state
-    state_pub_ = this->create_publisher<device_interface::msg::CapacitorState>("super_capacitor_state", 10);
-    pub_timer_ = this->create_wall_timer(
-        std::chrono::milliseconds(100), [this](){
-            pub_state();
-        });
+    state_pub_ = node_->create_publisher<device_interface::msg::CapacitorState>("super_capacitor_state", 10);
+    pub_timer_ = node_->create_wall_timer(
+        std::chrono::milliseconds(100), std::bind(&SuperCapacitorController::pub_state, this));
 
     // complete initialization
-    tx_timer_ = this->create_wall_timer(
-        std::chrono::milliseconds(100), [this](){
-            send_command();
-        });
+    tx_timer_ = node_->create_wall_timer(
+        std::chrono::milliseconds(100), std::bind(&SuperCapacitorController::send_command, this));
 
 
-    pluginlib::ClassLoader<super_capacitor::SuperCapacitorBase> capacitor_loader("super_capacitor","super_capacitor::SuperCapacitorBase");
+    // pluginlib::ClassLoader<super_capacitor::SuperCapacitorBase> capacitor_loader("super_capacitor","super_capacitor::SuperCapacitorBase");
 
     try
     {
-        capacitor_ = capacitor_loader.createSharedInstance("SuperCapacitorBase");
-        capacitor_->init(can_interface);
+        // capacitor_ = capacitor_loader.createSharedInstance("super_capacitor::XidiCapacitorDriver");
+        // capacitor_->init(can_interface);
+        if(capacitor_device == "xidi"){
+            capacitor_ = std::make_shared<super_capacitor::XidiCapacitorDriver>();
+            capacitor_->init(can_interface);
+        }else{
+            RCLCPP_ERROR(node_->get_logger(), "Unknown capacitor device: %s", capacitor_device.c_str());
+        }
+        
     }
     catch(pluginlib::PluginlibException& ex)
     {
         printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
     }
-    RCLCPP_INFO(this->get_logger(), "SuperCapacitorController initialized");
+    RCLCPP_INFO(node_->get_logger(), "SuperCapacitorController initialized");
+}
+
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr SuperCapacitorController::get_node_base_interface() const
+{
+    return node_->get_node_base_interface();
 }
 
 void SuperCapacitorController::goal_sub_callback(const device_interface::msg::CapacitorCmd::SharedPtr msg){
-    // RCLCPP_INFO(this->get_logger(), "Received super capacitor goal");
     target_power_.store(msg->target_power,std::memory_order_relaxed);
     referee_power_.store(msg->referee_power,std::memory_order_relaxed);
     capacitor_->set_target_power(target_power_.load(std::memory_order_relaxed));
@@ -79,4 +89,8 @@ void SuperCapacitorController::pub_state() {
 
 SuperCapacitorController::~SuperCapacitorController() {
     // Destructor implementation
-}
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(SuperCapacitorController)