Merge branch 'master' of github.com:Meta-Team/Meta-ROS

Author: nice-mee

decomposition/metav_description/urdf/playground/dm_motor.xacro

@@ -38,21 +38,21 @@
             <hardware>
                 <plugin>meta_hardware/MetaRobotDmMotorNetwork</plugin>
                 <param name="can_network_name">can0</param>
-                <param name="master_id">0</param>
+                <param name="master_id">0x30</param>
             </hardware>
             <joint name="motor_joint">
                 <command_interface name="position" />
                 <state_interface name="position" />
                 <state_interface name="velocity" />
                 <state_interface name="effort" />
-                <param name="motor_model">4310</param>
+                <param name="motor_model">DaMiao</param>
                 <param name="motor_id">2</param>
                 <param name="mechanical_reduction">1.0</param>
                 <param name="max_pos">3.141593</param>
                 <param name="max_vel">30</param>
                 <param name="max_effort">10</param>
                 <param name="offset">0.0</param>
-                <param name="control_mode">position</param>
+                <param name="control_mode">mit</param>
                 <param name="Kp">150.0</param>
                 <param name="Kd">0.5</param>
             </joint>

execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp

@@ -33,7 +33,7 @@ DmMotor::DmMotor(const std::unordered_map<std::string, std::string> &motor_param
     max_effort_ = std::stod(motor_param.at("max_effort"));
 
     std::string control_mode = motor_param.at("control_mode");
-    uint32_t id_offeset = 0;
+    uint32_t id_offset = 0;
 
     if (control_mode == "mit") {
         Kp_ = std::stod(motor_param.at("Kp"));
@@ -46,16 +46,16 @@ DmMotor::DmMotor(const std::unordered_map<std::string, std::string> &motor_param
 
     } else if (control_mode == "position") {
         run_mode_ = RunMode::POSITION;
-        id_offeset = 0x100;
+        id_offset = 0x100;
     } else if (control_mode == "velocity") {
         run_mode_ = RunMode::VELOCITY;
-        id_offeset = 0x200;
+        id_offset = 0x200;
     } else {
         throw std::runtime_error("Unknown motor mode: " + control_mode);
     }
 
-    if (motor_model_ == "4310") {
-        tx_can_id_ = dm_motor_id_ + id_offeset;
+    if (motor_model_ == "DaMiao") {
+        tx_can_id_ = dm_motor_id_ + id_offset;
     } else {
         throw std::runtime_error("Unknown motor model: " + motor_model_);
     }

execution/super_capacitor/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(rclcpp_components)
 add_library(super_capacitor SHARED
   src/super_capacitor_controller.cpp
   src/xidi_capacitor_driver.cpp
+  src/pacific_spirit_capacitor_driver.cpp
 )
 
 target_include_directories(super_capacitor PRIVATE

execution/super_capacitor/include/super_capacitor/pacific_spirit_capacitor_driver.h

@@ -0,0 +1,44 @@
+#ifndef PACIFIC_SPIRIT_CAPACITOR_DRIVER_H
+#define PACIFIC_SPIRIT_CAPACITOR_DRIVER_H
+
+#include <thread>
+#include <vector>
+#include <unordered_map>
+#include <stop_token>
+#include <memory>
+#include "super_capacitor/super_capacitor_base.h"
+#include "meta_hardware/can_driver/can_driver.hpp"
+
+namespace super_capacitor {
+
+class PacificSpiritCapacitorDriver : public super_capacitor::SuperCapacitorBase
+{
+public:
+    explicit PacificSpiritCapacitorDriver();
+
+    void init(std::string can_interface) override;
+
+    void set_target_power(double target_power) override;
+
+    void set_referee_power(double referee_power) override;
+
+    std::string get_device_name() override;
+
+    std::unordered_map<std::string, double> get_state() override;
+
+    ~PacificSpiritCapacitorDriver() override;
+    
+protected:
+    double target_power_, referee_power_;
+    std::vector<can_filter> can_filters_;
+    std::unique_ptr<meta_hardware::CanDriver> can_driver_;
+    void rx_loop(std::stop_token stop_token) override;
+    std::unique_ptr<std::jthread> rx_thread_;
+    double max_discharge_power_, base_power_, cap_energy_percentage_, cap_state_;
+    void tx() override;
+
+};
+}
+
+
+#endif // PACIFIC_SPIRIT_CAPACITOR_DRIVER_H

execution/super_capacitor/src/pacific_spirit_capacitor_driver.cpp

@@ -0,0 +1,95 @@
+#include <thread>
+#include <iostream>
+#include <exception>
+#include <linux/can.h>
+#include <stdexcept>
+#include <string>
+
+#include "meta_hardware/can_driver/can_driver.hpp"
+#include "meta_hardware/can_driver/can_exceptions.hpp"
+#include "super_capacitor/super_capacitor_base.h"
+#include "super_capacitor/pacific_spirit_capacitor_driver.h"
+
+namespace super_capacitor {
+PacificSpiritCapacitorDriver::PacificSpiritCapacitorDriver() 
+    : super_capacitor::SuperCapacitorBase(){}
+
+void PacificSpiritCapacitorDriver::init(std::string can_interface){
+    can_filters_.push_back({.can_id = 0x2c8, .can_mask = CAN_EFF_MASK});
+    // Initialize CAN driver
+    can_driver_ = std::make_unique<meta_hardware::CanDriver>(can_interface, false, can_filters_);
+
+    // Initialize RX thread
+    rx_thread_ =
+        std::make_unique<std::jthread>([this](std::stop_token s) { rx_loop(s); });
+}
+
+void PacificSpiritCapacitorDriver::set_target_power(double power) {
+    target_power_ = power;
+}
+
+void PacificSpiritCapacitorDriver::set_referee_power(double power) {
+    referee_power_ = power;
+}
+
+std::unordered_map<std::string, double> PacificSpiritCapacitorDriver::get_state() {
+    return {
+        {"max_discharge_power", max_discharge_power_},
+        {"base_power", base_power_},
+        {"cap_energy_percentage", cap_energy_percentage_}, 
+        {"cap_state", cap_state_}
+    };
+}
+
+std::string PacificSpiritCapacitorDriver::get_device_name() {
+    return "Xidi Capacitor";
+}
+
+void PacificSpiritCapacitorDriver::rx_loop(std::stop_token stop_token) {
+    while (!stop_token.stop_requested()) {
+        try {
+            can_frame can_msg = can_driver_->read(2000);
+
+            max_discharge_power_ = static_cast<double>(static_cast<uint16_t>(static_cast<uint16_t>(can_msg.data[0]) |
+                            (static_cast<uint16_t>(can_msg.data[1]) << 8)) / 100.0);
+            base_power_ = static_cast<double>(static_cast<uint16_t>(static_cast<uint16_t>(can_msg.data[2]) |
+                            (static_cast<uint16_t>(can_msg.data[3]) << 8)) / 100.0);
+            cap_energy_percentage_ = static_cast<double>(static_cast<uint16_t>(static_cast<uint16_t>(can_msg.data[4]) |
+                            (static_cast<uint16_t>(can_msg.data[5]) << 8)) / 100.0);
+            cap_state_ = static_cast<double>(static_cast<uint16_t>(static_cast<uint16_t>(can_msg.data[6]) |
+                            (static_cast<uint16_t>(can_msg.data[7]) << 8)) / 100.0);
+        } catch (const meta_hardware::CanIOException &e) {
+            std::cerr << "Error reading super capacitor CAN message: " << e.what() << std::endl;
+        }
+    }
+
+}
+
+void PacificSpiritCapacitorDriver::tx(){
+    can_frame tx_frame{.can_id = 0x2c7, .len = 8, .data = {0}};
+    tx_frame.data[0] = static_cast<uint8_t>(static_cast<uint32_t>(target_power_ * 100.0) & 0xFF);
+    tx_frame.data[1] = static_cast<uint8_t>(static_cast<uint32_t>(target_power_ * 100.0) >> 8);
+    tx_frame.data[2] = static_cast<uint8_t>(static_cast<uint32_t>(referee_power_ * 100.0) & 0xFF);
+    tx_frame.data[3] = static_cast<uint8_t>(static_cast<uint32_t>(referee_power_ * 100.0) >> 8);
+    tx_frame.data[4] = 0x12;
+    tx_frame.data[5] = 0x20;
+    tx_frame.data[6] = 0x12;
+    tx_frame.data[7] = 0x07;
+    try {
+        can_driver_->write(tx_frame);
+    } catch (meta_hardware::CanIOException &e) {
+        std::cerr << "Error writing CAN message: " << e.what() << std::endl;
+    }
+}
+
+PacificSpiritCapacitorDriver::~PacificSpiritCapacitorDriver() {
+    if (rx_thread_ && rx_thread_->joinable()) {
+        rx_thread_->request_stop();  // Gracefully stop the thread
+        rx_thread_->join();
+    }
+}
+
+}
+
+// #include <pluginlib/class_list_macros.hpp>
+// PLUGINLIB_EXPORT_CLASS(super_capacitor::PacificSpiritCapacitorDriver, super_capacitor::SuperCapacitorBase)

execution/super_capacitor/src/super_capacitor_controller.cpp

@@ -10,6 +10,7 @@
 #include "super_capacitor/super_capacitor_controller.h"
 #include "meta_hardware/can_driver/can_driver.hpp"
 #include "super_capacitor/xidi_capacitor_driver.h"
+#include "super_capacitor/pacific_spirit_capacitor_driver.h"
 
 
 SuperCapacitorController::SuperCapacitorController(const rclcpp::NodeOptions & options)
@@ -45,6 +46,9 @@ SuperCapacitorController::SuperCapacitorController(const rclcpp::NodeOptions & o
         if(capacitor_device == "xidi"){
             capacitor_ = std::make_shared<super_capacitor::XidiCapacitorDriver>();
             capacitor_->init(can_interface);
+        }else if(capacitor_device == "pacific_spirit"){
+            capacitor_ = std::make_shared<super_capacitor::PacificSpiritCapacitorDriver>();
+            capacitor_->init(can_interface);
         }else{
             RCLCPP_ERROR(node_->get_logger(), "Unknown capacitor device: %s", capacitor_device.c_str());
         }