Added a software based low voltage cut-off for the A300.

clearpath_generator_robot/clearpath_generator_robot/launch/generator.py

@@ -337,6 +337,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
           remappings=[('/diagnostics', 'diagnostics')],
         )
 
+        # A300 SW LVC Noode
+        self.a300_sw_lvc = LaunchFile.Node(
+          package='clearpath_hardware_interfaces',
+          executable='sw_lvc_node',
+          name='a300_sw_lvc',
+          namespace=self.namespace,
+        )
+
         # Components required for each platform
         common_platform_components = [
             self.wireless_watcher_node,
@@ -368,6 +376,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
                 self.lighting_node,
                 self.lynx_node,
                 self.a300_fan_control,
+                self.a300_sw_lvc
                 self.pinout_node,
             ],
             Platform.W200: common_platform_components + [

clearpath_hardware_interfaces/CMakeLists.txt

@@ -112,6 +112,25 @@ target_include_directories(
 target_link_libraries(${PINOUT_CONTROL_EXECUTABLE})
 ament_target_dependencies(${PINOUT_CONTROL_EXECUTABLE} ${PINOUT_CONTROL_DEPENDENCIES})
 
+# SW LVC
+set(SW_LVC_EXECUTABLE sw_lvc_node)
+set(SW_LVC_DEPENDENCIES
+  sensor_msgs
+  std_srvs
+  rclcpp
+)
+
+add_executable(${SW_LVC_EXECUTABLE} src/a300/software_lvc.cpp)
+
+target_include_directories(
+  ${SW_LVC_EXECUTABLE}
+  PRIVATE
+  include
+)
+
+target_link_libraries(${SW_LVC_EXECUTABLE})
+ament_target_dependencies(${SW_LVC_EXECUTABLE} ${SW_LVC_DEPENDENCIES})
+
 # A200 Hardware
 add_library(
   a200_hardware
@@ -254,6 +273,7 @@ install(
           ${PINOUT_CONTROL_EXECUTABLE}
           ${LIGHTING_EXECUTABLE}
           ${LIGHTING_LIB}
+          ${SW_LVC_EXECUTABLE}
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/software_lvc.hpp

@@ -0,0 +1,86 @@
+/**
+Software License Agreement (BSD)
+\file      software_lvc.hpp
+\authors   Tony Baltovski <[email protected]>
+\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_
+#define CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+namespace a300_slvc
+{
+
+/**
+ * @class SoftwareLowVoltageCutoff
+ * @brief Monitors battery state and issues warnings based on charge level.
+ *
+ * This node subscribes to the "battery_state" topic and checks the battery charge percentage.
+ * It logs warnings at 12% and calls a service when the charge drops to 8% or below.
+ */
+class SoftwareLowVoltageCutoff : public rclcpp::Node
+{
+public:
+  static constexpr float WARNING_THRESHOLD = 12.0; ///< Warning threshold percentage
+  static constexpr float CRITICAL_THRESHOLD = 8.0; ///< Critical threshold percentage
+  static constexpr int CRITICAL_READINGS_REQUIRED = 30; ///< Number of CRITICAL_THRESHOLD readings required before issuing the command
+
+
+  /// Battery state topic name
+  const std::string BATTERY_STATE_TOPIC = "platform/bms/state";
+
+  /// LVC service name
+  const std::string SW_LVC_SERVICE_NAME = "platform/mcu/cmd_shutdown";
+
+  /**
+   * @brief Constructor for SoftwareLowVoltageCutoff node.
+   *
+   * Initializes the subscriber to "battery_state" and the service client for low battery handling.
+   */
+  SoftwareLowVoltageCutoff();
+
+private:
+  /**
+   * @brief Callback function for battery state messages.
+   *
+   * @param msg The battery state message.
+   *
+   * This function checks if the battery is present and evaluates its charge percentage.
+   * Warnings are issued at 12%, and a critical service call is made at 8%.
+   */
+  void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
+
+  /**
+   * @brief Calls the low battery service when charge reaches the critical level.
+   *
+   * Attempts to call the "low_battery_service" if available.
+   */
+  void call_low_battery_service();
+
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_subscriber_; ///< Battery state subscriber
+  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_; ///< Client for low battery service
+  int consecutive_low_readings_; ///< Counter for consecutive low readings
+};
+
+}  // namespace a300_slvc
+
+#endif  // CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_

clearpath_hardware_interfaces/src/a300/software_lvc.cpp

@@ -0,0 +1,89 @@
+/**
+Software License Agreement (BSD)
+\file      software_lvc.hpp
+\authors   Tony Baltovski <[email protected]>
+\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "clearpath_hardware_interfaces/a300/software_lvc.hpp"
+
+
+a300_slvc::SoftwareLowVoltageCutoff::SoftwareLowVoltageCutoff() : Node("software_low_voltage_cutoff")
+{
+  battery_subscriber_ = this->create_subscription<sensor_msgs::msg::BatteryState>(
+      BATTERY_STATE_TOPIC, 10,
+      std::bind(&SoftwareLowVoltageCutoff::battery_callback, this, std::placeholders::_1));
+
+  client_ = this->create_client<std_srvs::srv::Empty>("low_battery_service");
+}
+
+void a300_slvc::SoftwareLowVoltageCutoff::battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  if (!msg->present)
+  {
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+        "Battery not present. Ignoring message.");
+    return;
+  }
+
+  float charge_percentage = msg->percentage * 100.0;
+
+  if (charge_percentage <= WARNING_THRESHOLD && charge_percentage > CRITICAL_THRESHOLD)
+  {
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+        "Battery charge low: %.2f%%", charge_percentage);
+  }
+  else if (charge_percentage <= CRITICAL_THRESHOLD)
+  {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "Battery critically low: %.2f%%", charge_percentage);
+    this->consecutive_low_readings_++;
+
+    // Gather many readings before calling the service
+    if (this->consecutive_low_readings_ >= CRITICAL_READINGS_REQUIRED) 
+    {
+      RCLCPP_FATAL(this->get_logger(),
+          "Battery critically low, calling cmd shutdown service on MCU to power off robot.");
+      call_low_battery_service();
+    }
+  }
+  else
+  {
+    this->consecutive_low_readings_ = 0;  // Reset counter if charge goes above critical level
+  }
+}
+
+void a300_slvc::SoftwareLowVoltageCutoff::call_low_battery_service() 
+{
+  if (!client_->wait_for_service(std::chrono::seconds(1)))
+  {
+    RCLCPP_ERROR(this->get_logger(), "Low battery service unavailable");
+    return;
+  }
+
+  auto request = std::make_shared<std_srvs::srv::Empty::Request>();
+  auto future = client_->async_send_request(request);
+}
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<a300_slvc::SoftwareLowVoltageCutoff>());
+  rclcpp::shutdown();
+  return 0;
+}