Merge branch 'ros2-lynx-devel' into ros2-lynx-lights-animations

husarion · Sep 23, 2024 · f831319 · f831319
2 parents da0175f + 08ec819
commit f831319
Show file tree

Hide file tree

Showing 9 changed files with 70 additions and 36 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
@@ -55,14 +55,18 @@ BatteryDriverNode::BatteryDriverNode(
 
 void BatteryDriverNode::Initialize()
 {
+  RCLCPP_INFO(this->get_logger(), "Initializing.");
+
   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 +124,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/test_battery_driver_node_adc_single.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCSingle : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {}
+  TestBatteryNodeADCSingle() : TestBatteryNode(true, false) {}
 };
 
 TEST_F(TestBatteryNodeADCSingle, BatteryValues)

diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -27,7 +27,7 @@
 class TestBatteryNodeRoboteq : public TestBatteryNode
 {
 public:
-  TestBatteryNodeRoboteq() : TestBatteryNode(1.0) {}
+  TestBatteryNodeRoboteq() : TestBatteryNode(false) {}
 };
 
 TEST_F(TestBatteryNodeRoboteq, 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,42 @@ 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"));
+  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);

diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
@@ -112,12 +112,13 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_description"),
+                FindPackageShare(robot_description_pkg),
                 "config",
                 PythonExpression(["'", wheel_type, ".yaml'"]),
             ]

diff --git a/panther_controller/package.xml b/panther_controller/package.xml
@@ -22,6 +22,7 @@
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>lynx_description</exec_depend>
   <exec_depend>mecanum_drive_controller</exec_depend>
   <exec_depend>panther_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_hardware_interfaces</exec_depend>

diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml
@@ -31,6 +31,7 @@
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_gz_worlds</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch_ros</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">lynx_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>