diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp index a94725d0..0832bc03 100644 --- 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 index 6dc4eaa3..f3603b87 100644 --- 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("roboteq/driver_state_timeout", 0.2); + + const RoboteqBatteryParams battery_params = { + static_cast(this->get_parameter("roboteq/driver_state_timeout").as_double()), + static_cast(this->get_parameter("ma_window_len/voltage").as_int()), + static_cast(this->get_parameter("ma_window_len/current").as_int()), + }; + + driver_state_sub_ = this->create_subscription( + "hardware/robot_driver_state", 5, + [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; }); + + battery_1_ = std::make_shared([&]() { return driver_state_; }, battery_params); + + battery_publisher_ = std::make_shared( + 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 index 414fce6b..740ba838 100644 --- 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 index 21f6995f..cc26bfe7 100644 --- 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 index 611b5d64..84eb00b9 100644 --- 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 index 7441af08..bad9542c 100644 --- 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::SharedPtr driver_state_pub_; }; -TestBatteryNode::TestBatteryNode(const bool dual_battery) +TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery) { std::vector 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(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw")); - WriteNumberToFile(800, std::filesystem::path(device0_path_ / "in_voltage1_raw")); - WriteNumberToFile(2, std::filesystem::path(device0_path_ / "in_voltage2_raw")); - WriteNumberToFile(2, std::filesystem::path(device0_path_ / "in_voltage3_raw")); - WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale")); - WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale")); - WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale")); - WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale")); - - WriteNumberToFile(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw")); - WriteNumberToFile(600, std::filesystem::path(device1_path_ / "in_voltage1_raw")); - WriteNumberToFile(600, std::filesystem::path(device1_path_ / "in_voltage2_raw")); - WriteNumberToFile(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw")); - WriteNumberToFile(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale")); - WriteNumberToFile(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale")); - WriteNumberToFile(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale")); - WriteNumberToFile(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(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw")); + WriteNumberToFile(800, std::filesystem::path(device0_path_ / "in_voltage1_raw")); + WriteNumberToFile(2, std::filesystem::path(device0_path_ / "in_voltage2_raw")); + WriteNumberToFile(2, std::filesystem::path(device0_path_ / "in_voltage3_raw")); + WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale")); + WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale")); + WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale")); + WriteNumberToFile(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale")); + + WriteNumberToFile(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw")); + WriteNumberToFile(600, std::filesystem::path(device1_path_ / "in_voltage1_raw")); + WriteNumberToFile(600, std::filesystem::path(device1_path_ / "in_voltage2_raw")); + WriteNumberToFile(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw")); + WriteNumberToFile(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale")); + WriteNumberToFile(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale")); + WriteNumberToFile(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale")); + WriteNumberToFile(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 index 61e8f946..adf83965 100644 --- 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 index 067db90c..6dea3a07 100644 --- a/panther_controller/package.xml +++ b/panther_controller/package.xml @@ -22,6 +22,7 @@ joint_state_broadcaster launch launch_ros + lynx_description mecanum_drive_controller panther_description panther_hardware_interfaces diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index ef2d3d6b..faa23cdd 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -31,6 +31,7 @@ husarion_gz_worlds launch launch_ros + lynx_description nav2_common panther_controller panther_description