panther-verion -> use_adc_battery

husarion · Sep 20, 2024 · 21570ee · 21570ee
1 parent 77ad3f6
commit 21570ee
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 41 deletions.
diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp
@@ -42,6 +42,7 @@ class BatteryDriverNode : public rclcpp::Node
   void BatteryPubTimerCB();
   void Initialize();
   void InitializeWithADCBattery();
+  void InitializeWithRoboteqBattery();
 
   static constexpr int kADCCurrentOffset = 625;
 

diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
@@ -41,6 +41,7 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
+  this->declare_parameter<bool>("use_adc_battery", true);
   this->declare_parameter<int>("ma_window_len/voltage", 10);
   this->declare_parameter<int>("ma_window_len/current", 10);
 
@@ -55,14 +56,21 @@ BatteryDriverNode::BatteryDriverNode(
 
 void BatteryDriverNode::Initialize()
 {
-  try {
-    InitializeWithADCBattery();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_WARN_STREAM(
-      this->get_logger(), "An exception occurred while initializing with ADC: "
-                            << e.what()
-                            << " Falling back to using Roboteq drivers to publish battery data.");
+  RCLCPP_INFO(this->get_logger(), "Initializing.");
+
+  const float use_adc_battery = this->get_parameter("use_adc_battery").as_bool();
+  if (use_adc_battery) {
+    try {
+      InitializeWithADCBattery();
+      return;
+    } catch (const std::runtime_error & e) {
+      RCLCPP_WARN_STREAM(
+        this->get_logger(), "An exception occurred while initializing with ADC: "
+                              << e.what()
+                              << " Falling back to using Roboteq drivers to publish battery data.");
+    }
   }
+  InitializeWithRoboteqBattery();
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -120,6 +128,30 @@ void BatteryDriverNode::InitializeWithADCBattery()
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
 }
 
+void BatteryDriverNode::InitializeWithRoboteqBattery()
+{
+  RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
+
+  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
+
+  const RoboteqBatteryParams battery_params = {
+    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
+    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
+    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
+  };
+
+  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
+    "hardware/robot_driver_state", 5,
+    [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
+
+  battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
+
+  battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
+    this->shared_from_this(), diagnostic_updater_, battery_1_);
+
+  RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
+}
+
 void BatteryDriverNode::BatteryPubTimerCB()
 {
   if (!battery_publisher_) {

diff --git a/panther_battery/test/test_battery_driver_node_adc_dual.cpp b/panther_battery/test/test_battery_driver_node_adc_dual.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCDual : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {}
+  TestBatteryNodeADCDual() : TestBatteryNode(true, true) {}
 };
 
 TEST_F(TestBatteryNodeADCDual, BatteryValues)

diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -40,7 +40,7 @@ using IOStateMsg = panther_msgs::msg::IOState;
 class TestBatteryNode : public testing::Test
 {
 public:
-  TestBatteryNode(const bool dual_battery = false);
+  TestBatteryNode(const bool use_adc_battery = true, const bool dual_battery = false);
   ~TestBatteryNode();
 
 protected:
@@ -63,40 +63,43 @@ class TestBatteryNode : public testing::Test
   rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_pub_;
 };
 
-TestBatteryNode::TestBatteryNode(const bool dual_battery)
+TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery)
 {
   std::vector<rclcpp::Parameter> params;
-
-  device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
-  device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
-
-  params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
-  params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
-  params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
-
-  // Create the device0 and device1 directories if they do not exist
-  std::filesystem::create_directory(device0_path_);
-  std::filesystem::create_directory(device1_path_);
-
-  // Create only files that are required for adc_node to start
-  int dual_bat_volt = dual_battery ? 800 : 1600;
-  WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
-  WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
-  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
-  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
-
-  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
-  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
-  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
-  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
-  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
-  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
-  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
+  params.push_back(rclcpp::Parameter("use_adc_battery", use_adc_battery));
+
+  if (use_adc_battery) {
+    device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
+    device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
+
+    params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
+    params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
+    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
+
+    // Create the device0 and device1 directories if they do not exist
+    std::filesystem::create_directory(device0_path_);
+    std::filesystem::create_directory(device1_path_);
+
+    // Create only files that are required for adc_node to start
+    int dual_bat_volt = dual_battery ? 800 : 1600;
+    WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
+    WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
+    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
+    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
+
+    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
+    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
+    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
+    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
+    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
+    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
+    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
+  }
 
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);