Split into header and source. Also, added to launch.

clearpath_generator_robot/clearpath_generator_robot/launch/generator.py

@@ -293,6 +293,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
                 ]
             ))
 
+        # A300 Fan Control Node
+        self.a300_fan_control = LaunchFile.Node(
+          package='clearpath_hardware_interfaces',
+          executable='fan_control_node',
+          name='a300_fan_control',
+          namespace=self.namespace,
+        )
+
         # Components required for each platform
         common_platform_components = [
             self.wireless_watcher_node,
@@ -321,6 +329,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
                 self.configure_mcu,
                 self.lighting_node,
                 self.lynx_node,
+                self.a300_fan_control,
             ],
             Platform.W200: common_platform_components + [
                 self.imu_0_filter_node,

clearpath_hardware_interfaces/CMakeLists.txt

@@ -37,10 +37,10 @@ find_package(tf2_ros REQUIRED)
 set(LIGHTING_EXECUTABLE lighting_node)
 set(LIGHTING_LIB clearpath_platform_lighting)
 set(LIGHTING_DEPENDENCIES
-    clearpath_motor_msgs
-    clearpath_platform_msgs
-    sensor_msgs
-    rclcpp
+  clearpath_motor_msgs
+  clearpath_platform_msgs
+  sensor_msgs
+  rclcpp
 )
 
 add_library(
@@ -72,13 +72,21 @@ target_include_directories(
 # Cooling
 set(FAN_CONTROL_EXECUTABLE fan_control_node)
 set(FAN_CONTROL_DEPENDENCIES
-    clearpath_motor_msgs
-    clearpath_platform_msgs
-    sensor_msgs
-    rclcpp
+  clearpath_motor_msgs
+  clearpath_platform_msgs
+  sensor_msgs
+  rclcpp
 )
 
 add_executable(${FAN_CONTROL_EXECUTABLE} src/a300/fan_control.cpp)
+
+target_include_directories(
+  ${FAN_CONTROL_EXECUTABLE}
+  PRIVATE
+  include
+)
+
+target_link_libraries(${FAN_CONTROL_EXECUTABLE})
 ament_target_dependencies(${FAN_CONTROL_EXECUTABLE} ${FAN_CONTROL_DEPENDENCIES})
 
 

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/fan_control.hpp

@@ -0,0 +1,172 @@
+#ifndef CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP
+#define CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include <clearpath_platform_msgs/msg/temperature.hpp>
+#include <clearpath_platform_msgs/msg/fans.hpp>
+#include <clearpath_motor_msgs/msg/lynx_multi_status.hpp>
+#include <mutex>
+#include <map>
+
+namespace a300_cooling
+{
+/// Fan control topic name
+const std::string FAN_CONTROL_TOPIC_NAME = "platform/mcu/cmd_fans";
+
+/// MCU temperature topic name
+const std::string MCU_TEMPERATURE_TOPIC = "platform/mcu/status/temperature";
+
+/// Motor temperature topic name
+const std::string MOTOR_TEMPERATURE_TOPIC = "platform/motors/status";
+
+/// Battery temperature topic name
+const std::string BATTERY_TEMPERATURE_TOPIC = "platform/bms/battery_state";
+
+/// Fan command for high error state, percent of maximum speed
+constexpr float FAN_CMD_HIGH_ERROR = 1.0f;
+
+/// Fan command for high warning state, percent of maximum speed
+constexpr float FAN_CMD_HIGH_WARNING = 0.75f;
+
+/// Fan command for low error state, percent of maximum speed
+constexpr float FAN_CMD_LOW_ERROR = 0.0f;
+
+/// Fan command for low warning state, percent of maximum speed
+constexpr float FAN_CMD_LOW_WARNING = 0.25f;
+
+/// Fan command for normal state, percent of maximum speed
+constexpr float FAN_CMD_NORMAL = 0.25f;
+
+/// Initial temperature reading when no data is available, degrees Celsius
+constexpr float INITIAL_READING = 30.0f;
+
+/// Enumeration to represent thermal status
+enum class ThermalStatus
+{
+  Normal,      ///< Thermal state is within normal range
+  LowError,    ///< Low error thermal state
+  LowWarning,  ///< Low warning thermal state
+  HighWarning, ///< High warning thermal state
+  HighError    ///< High error thermal state
+};
+
+/**
+ * @brief Convert thermal status to string
+ * 
+ * @param status The thermal status
+ * @return A string representing the thermal status
+ */
+std::string thermalStatusToString(ThermalStatus status);
+
+/**
+ * @brief Represents a thermal sensor with associated status thresholds
+ */
+class ThermalSensor
+{
+public:
+  /**
+   * @brief Construct a new ThermalSensor object with specific threshold values
+   * 
+   * @param low_error Temperature below this value is an error
+   * @param low_warning Temperature below this value is a warning
+   * @param high_warning Temperature above this value is a warning
+   * @param high_error Temperature above this value is an error
+   */
+  ThermalSensor(float low_error, float low_warning, float high_warning, float high_error);
+
+  /**
+   * @brief Construct a new ThermalSensor object with default values
+   */
+  ThermalSensor();
+
+  /**
+   * @brief Set the temperature reading of the sensor
+   * 
+   * @param value Temperature value to be set
+   */
+  void setValue(float value);
+
+  /**
+   * @brief Get the current thermal status of the sensor
+   * 
+   * @return The current thermal status
+   */
+  ThermalStatus getStatus() const;
+
+private:
+  float reading_;     ///< Current temperature reading
+  float low_error_;   ///< Threshold for low error
+  float low_warning_; ///< Threshold for low warning
+  float high_warning_;///< Threshold for high warning
+  float high_error_;  ///< Threshold for high error
+  ThermalStatus thermal_status_; ///< Current thermal status
+};
+
+/**
+ * @brief Collection of thermal sensors in the system
+ */
+class ThermalSensors
+{
+public:
+  /**
+   * @brief Default constructor that initializes the sensor list with default values
+   */
+  ThermalSensors();
+
+  /**
+   * @brief Set the value of a specific sensor
+   * 
+   * @param name The sensor's name
+   * @param value The temperature value to set
+   */
+  void setSensorValue(const std::string &name, float value);
+
+  /**
+   * @brief Get the highest thermal status from all sensors
+   * 
+   * @return The highest thermal status
+   */
+  ThermalStatus getHighestStatus() const;
+
+private:
+  std::map<std::string, ThermalSensor> sensors; ///< Map of sensor names to sensor objects
+};
+
+/**
+ * @brief Main class for managing fan control based on thermal sensor readings
+ */
+class FanController : public rclcpp::Node 
+{
+public:
+  /**
+   * @brief Construct a new FanController object
+   */
+  FanController();
+
+private:
+  /**
+   * @brief Timer callback to periodically check and control the fans
+   */
+  void timerCallback();
+
+  /**
+   * @brief Compute the fan value based on the thermal status
+   * 
+   * @param status The thermal status of the highest priority sensor
+   * @return Computed fan value as a uint8_t (0 to 255)
+   */
+  uint8_t computeFanValue(ThermalStatus status);
+
+  std::mutex update_mutex_;    ///< Mutex for updating thermal sensors
+  ThermalSensors thermal_sensors_; ///< Thermal sensor data
+  rclcpp::Publisher<clearpath_platform_msgs::msg::Fans>::SharedPtr fan_publisher_; ///< Fan control publisher
+  rclcpp::Subscription<clearpath_platform_msgs::msg::Temperature>::SharedPtr temp_subscription_; ///< Temperature subscription
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_subscription_; ///< Battery state subscription
+  rclcpp::Subscription<clearpath_motor_msgs::msg::LynxMultiStatus>::SharedPtr motor_subscription_; ///< Motor status subscription
+  rclcpp::TimerBase::SharedPtr control_timer_; ///< Timer for controlling the fan
+};
+
+}  // namespace a300_cooling
+
+#endif  // CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP