Skip to content

Commit 8be9a33

Browse files
tonybaltovskitonybaltovski
authored
Added a software based low voltage cut-off for the A300. (#185)
1 parent 8d8d9cd commit 8be9a33

File tree

4 files changed

+204
-0
lines changed

4 files changed

+204
-0
lines changed

‎clearpath_generator_robot/clearpath_generator_robot/launch/generator.py

+9
Original file line numberDiff line numberDiff line change
@@ -337,6 +337,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
337337
remappings=[('/diagnostics', 'diagnostics')],
338338
)
339339

340+
# A300 SW LVC Noode
341+
self.a300_sw_lvc = LaunchFile.Node(
342+
package='clearpath_hardware_interfaces',
343+
executable='sw_lvc_node',
344+
name='a300_sw_lvc',
345+
namespace=self.namespace,
346+
)
347+
340348
# Components required for each platform
341349
common_platform_components = [
342350
self.wireless_watcher_node,
@@ -368,6 +376,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
368376
self.lighting_node,
369377
self.lynx_node,
370378
self.a300_fan_control,
379+
self.a300_sw_lvc
371380
self.pinout_node,
372381
],
373382
Platform.W200: common_platform_components + [

‎clearpath_hardware_interfaces/CMakeLists.txt

+20
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,25 @@ target_include_directories(
112112
target_link_libraries(${PINOUT_CONTROL_EXECUTABLE})
113113
ament_target_dependencies(${PINOUT_CONTROL_EXECUTABLE} ${PINOUT_CONTROL_DEPENDENCIES})
114114

115+
# SW LVC
116+
set(SW_LVC_EXECUTABLE sw_lvc_node)
117+
set(SW_LVC_DEPENDENCIES
118+
sensor_msgs
119+
std_srvs
120+
rclcpp
121+
)
122+
123+
add_executable(${SW_LVC_EXECUTABLE} src/a300/software_lvc.cpp)
124+
125+
target_include_directories(
126+
${SW_LVC_EXECUTABLE}
127+
PRIVATE
128+
include
129+
)
130+
131+
target_link_libraries(${SW_LVC_EXECUTABLE})
132+
ament_target_dependencies(${SW_LVC_EXECUTABLE} ${SW_LVC_DEPENDENCIES})
133+
115134
# A200 Hardware
116135
add_library(
117136
a200_hardware
@@ -254,6 +273,7 @@ install(
254273
${PINOUT_CONTROL_EXECUTABLE}
255274
${LIGHTING_EXECUTABLE}
256275
${LIGHTING_LIB}
276+
${SW_LVC_EXECUTABLE}
257277
LIBRARY DESTINATION lib
258278
RUNTIME DESTINATION lib/${PROJECT_NAME}
259279
)

‎clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/software_lvc.hpp

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
/**
2+
Software License Agreement (BSD)
3+
\file software_lvc.hpp
4+
\authors Tony Baltovski <[email protected]>
5+
\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
6+
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
7+
the following conditions are met:
8+
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
9+
following disclaimer.
10+
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
11+
following disclaimer in the documentation and/or other materials provided with the distribution.
12+
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
13+
products derived from this software without specific prior written permission.
14+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
15+
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
16+
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
17+
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
18+
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
19+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
20+
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
21+
*/
22+
23+
#ifndef CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_
24+
#define CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_
25+
26+
#include "rclcpp/rclcpp.hpp"
27+
#include "sensor_msgs/msg/battery_state.hpp"
28+
#include "std_srvs/srv/empty.hpp"
29+
30+
namespace a300_slvc
31+
{
32+
33+
/**
34+
* @class SoftwareLowVoltageCutoff
35+
* @brief Monitors battery state and issues warnings based on charge level.
36+
*
37+
* This node subscribes to the "battery_state" topic and checks the battery charge percentage.
38+
* It logs warnings at 12% and calls a service when the charge drops to 8% or below.
39+
*/
40+
class SoftwareLowVoltageCutoff : public rclcpp::Node
41+
{
42+
public:
43+
static constexpr float WARNING_THRESHOLD = 12.0; ///< Warning threshold percentage
44+
static constexpr float CRITICAL_THRESHOLD = 8.0; ///< Critical threshold percentage
45+
static constexpr int CRITICAL_READINGS_REQUIRED = 30; ///< Number of CRITICAL_THRESHOLD readings required before issuing the command
46+
47+
48+
/// Battery state topic name
49+
const std::string BATTERY_STATE_TOPIC = "platform/bms/state";
50+
51+
/// LVC service name
52+
const std::string SW_LVC_SERVICE_NAME = "platform/mcu/cmd_shutdown";
53+
54+
/**
55+
* @brief Constructor for SoftwareLowVoltageCutoff node.
56+
*
57+
* Initializes the subscriber to "battery_state" and the service client for low battery handling.
58+
*/
59+
SoftwareLowVoltageCutoff();
60+
61+
private:
62+
/**
63+
* @brief Callback function for battery state messages.
64+
*
65+
* @param msg The battery state message.
66+
*
67+
* This function checks if the battery is present and evaluates its charge percentage.
68+
* Warnings are issued at 12%, and a critical service call is made at 8%.
69+
*/
70+
void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
71+
72+
/**
73+
* @brief Calls the low battery service when charge reaches the critical level.
74+
*
75+
* Attempts to call the "low_battery_service" if available.
76+
*/
77+
void call_low_battery_service();
78+
79+
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_subscriber_; ///< Battery state subscriber
80+
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_; ///< Client for low battery service
81+
int consecutive_low_readings_; ///< Counter for consecutive low readings
82+
};
83+
84+
} // namespace a300_slvc
85+
86+
#endif // CLEARPATH_HARDWARE_INTERFACES_A300_SOFTWARE_LOW_VOLTAGE_CUTOFF_HPP_

‎clearpath_hardware_interfaces/src/a300/software_lvc.cpp

+89
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
/**
2+
Software License Agreement (BSD)
3+
\file software_lvc.hpp
4+
\authors Tony Baltovski <[email protected]>
5+
\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
6+
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
7+
the following conditions are met:
8+
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
9+
following disclaimer.
10+
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
11+
following disclaimer in the documentation and/or other materials provided with the distribution.
12+
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
13+
products derived from this software without specific prior written permission.
14+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
15+
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
16+
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
17+
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
18+
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
19+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
20+
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
21+
*/
22+
23+
#include "clearpath_hardware_interfaces/a300/software_lvc.hpp"
24+
25+
26+
a300_slvc::SoftwareLowVoltageCutoff::SoftwareLowVoltageCutoff() : Node("software_low_voltage_cutoff")
27+
{
28+
battery_subscriber_ = this->create_subscription<sensor_msgs::msg::BatteryState>(
29+
BATTERY_STATE_TOPIC, 10,
30+
std::bind(&SoftwareLowVoltageCutoff::battery_callback, this, std::placeholders::_1));
31+
32+
client_ = this->create_client<std_srvs::srv::Empty>("low_battery_service");
33+
}
34+
35+
void a300_slvc::SoftwareLowVoltageCutoff::battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg)
36+
{
37+
if (!msg->present)
38+
{
39+
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
40+
"Battery not present. Ignoring message.");
41+
return;
42+
}
43+
44+
float charge_percentage = msg->percentage * 100.0;
45+
46+
if (charge_percentage <= WARNING_THRESHOLD && charge_percentage > CRITICAL_THRESHOLD)
47+
{
48+
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
49+
"Battery charge low: %.2f%%", charge_percentage);
50+
}
51+
else if (charge_percentage <= CRITICAL_THRESHOLD)
52+
{
53+
RCLCPP_ERROR_THROTTLE(
54+
this->get_logger(), *this->get_clock(), 1000, "Battery critically low: %.2f%%", charge_percentage);
55+
this->consecutive_low_readings_++;
56+
57+
// Gather many readings before calling the service
58+
if (this->consecutive_low_readings_ >= CRITICAL_READINGS_REQUIRED)
59+
{
60+
RCLCPP_FATAL(this->get_logger(),
61+
"Battery critically low, calling cmd shutdown service on MCU to power off robot.");
62+
call_low_battery_service();
63+
}
64+
}
65+
else
66+
{
67+
this->consecutive_low_readings_ = 0; // Reset counter if charge goes above critical level
68+
}
69+
}
70+
71+
void a300_slvc::SoftwareLowVoltageCutoff::call_low_battery_service()
72+
{
73+
if (!client_->wait_for_service(std::chrono::seconds(1)))
74+
{
75+
RCLCPP_ERROR(this->get_logger(), "Low battery service unavailable");
76+
return;
77+
}
78+
79+
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
80+
auto future = client_->async_send_request(request);
81+
}
82+
83+
int main(int argc, char **argv)
84+
{
85+
rclcpp::init(argc, argv);
86+
rclcpp::spin(std::make_shared<a300_slvc::SoftwareLowVoltageCutoff>());
87+
rclcpp::shutdown();
88+
return 0;
89+
}

0 commit comments

Comments
 (0)