diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt
new file mode 100644
index 0000000..c47ea8d
--- /dev/null
+++ b/clearpath_diagnostics/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(clearpath_diagnostics)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(
+  PROGRAMS ${PROJECT_NAME}/diagnostics_updater
+           ${PROJECT_NAME}/battery_state/battery_state_estimator
+           ${PROJECT_NAME}/battery_state/battery_state_control
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/clearpath_diagnostics/clearpath_diagnostics/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py
new file mode 100644
index 0000000..2a8d9d9
--- /dev/null
+++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py
@@ -0,0 +1,306 @@
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <rkreinin@clearpathrobotics.com>
+# @copyright (c) 2023, 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 WARRANTIES, 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, INDIRECT, 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+from clearpath_platform_msgs.msg import Power
+from sensor_msgs.msg import BatteryState
+
+from clearpath_config.common.types.platform import Platform
+from clearpath_config.platform.battery import BatteryConfig
+
+from math import nan
+
+
+# Base Battery
+
+class Battery:
+    class BaseBattery:
+        """ Base Battery class. """
+
+        """Battery configuration. Represents number of battery cells in series and parallel."""
+        CONFIGURATIONS = {
+            BatteryConfig.S1P1: (1, 1),
+            BatteryConfig.S1P2: (1, 2),
+            BatteryConfig.S1P3: (1, 3),
+            BatteryConfig.S1P4: (1, 4),
+            BatteryConfig.S2P1: (2, 1),
+            BatteryConfig.S4P1: (4, 1),
+            BatteryConfig.S4P3: (4, 3),
+        }
+
+        # To be defined in child class
+        CAPACITY = 0.0
+        VOLTAGE = 0.0
+        LUT = []
+
+        def __init__(
+            self,
+            platform: Platform,
+            configuration: str,
+            rolling_average_period=30,
+        ) -> None:
+            self._rolling_average_period = rolling_average_period
+            self._platform = platform
+            self._configuration = self.CONFIGURATIONS[configuration]
+            self._msg = BatteryState()
+            self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN
+            self._msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
+            self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+            self._msg.present = True
+            self._msg.temperature = nan
+            self._readings: list[Power] = []
+
+            # Which power message indices to use
+            self.power_msg_voltage_index: int = 0
+            self.power_msg_current_index: int | list[int] = 0
+            match self._platform:
+                case Platform.J100:
+                    self.power_msg_voltage_index = Power.J100_MEASURED_BATTERY
+                    self.power_msg_current_index = Power.J100_TOTAL_CURRENT
+                case Platform.A200:
+                    self.power_msg_voltage_index = Power.A200_BATTERY_VOLTAGE
+                    self.power_msg_current_index = [
+                        Power.A200_MCU_AND_USER_PORT_CURRENT,
+                        Power.A200_LEFT_DRIVER_CURRENT,
+                        Power.A200_RIGHT_DRIVER_CURRENT,
+                    ]
+                case Platform.W200:
+                    self.power_msg_voltage_index = Power.W200_MEASURED_BATTERY
+                    self.power_msg_current_index = Power.W200_TOTAL_CURRENT
+                case Platform.DD100 | Platform.DO100:
+                    self.power_msg_voltage_index = Power.D100_MEASURED_BATTERY
+                    self.power_msg_current_index = Power.D100_TOTAL_CURRENT
+                case Platform.DD150 | Platform.DO150:
+                    self.power_msg_voltage_index = Power.D150_MEASURED_BATTERY
+                    self.power_msg_current_index = Power.D150_TOTAL_CURRENT
+
+
+            # System capacity
+            self._msg.capacity = self._msg.design_capacity = self.system_capacity
+
+        @property
+        def msg(self) -> BatteryState:
+            return self._msg
+
+        @property
+        def series(self) -> int:
+            return self._configuration[0]
+
+        @property
+        def parallel(self) -> int:
+            return self._configuration[1]
+
+        @property
+        def cell_count(self) -> int:
+            return self.series * self.parallel
+
+        @property
+        def system_capacity(self) -> float:
+            return self.CAPACITY * self.parallel
+
+        @property
+        def system_voltage(self) -> float:
+            return self.VOLTAGE * self.series
+
+        def update(self, power_msg: Power):
+            # Add new reading to rolling average
+            self._readings.append(power_msg)
+            self._msg.header = power_msg.header
+            # Remove oldest reading
+            if len(self._readings) > self._rolling_average_period:
+                self._readings.pop(0)
+
+            # Set battery voltage
+            self._msg.voltage = power_msg.measured_voltages[self.power_msg_voltage_index]
+
+            # Set battery current
+            if isinstance(self.power_msg_current_index, int):
+                self._msg.current = power_msg.measured_currents[
+                    self.power_msg_current_index
+                ]
+            elif isinstance(self.power_msg_current_index, list):
+                self._msg.current = 0.0
+                for i in self.power_msg_current_index:
+                    self._msg.current += power_msg.measured_currents[i]
+            # Set charging status
+            if power_msg.charger_connected == 1:
+                self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
+            else:
+                self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+            # Cells
+            self.update_cells()
+
+        def linear_interpolation(self, lut: list[list], v: float) -> float:
+            # Check if voltage is below minimum value
+            if v <= lut[0][0]:
+                return lut[0][1]
+
+            for i in range(0, len(lut)):
+                if v < lut[i][0]:
+                    return (v - lut[i - 1][0]) * (lut[i][1] - lut[i - 1][1]) / (
+                        lut[i][0] - lut[i - 1][0]
+                    ) + lut[i - 1][1]
+
+            # Return maximum value
+            return lut[len(lut) - 1][1]
+
+        def update_from_lut(self):
+            # Calculate state of charge
+            avg_voltage = 0
+            for reading in self._readings:
+                avg_voltage += reading.measured_voltages[self.power_msg_voltage_index]
+            avg_voltage /= len(self._readings)
+            self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage)
+            self._msg.charge = self._msg.capacity * self._msg.percentage
+
+        def update_cells(self):
+            self._msg.cell_voltage = [self._msg.voltage / self.series] * self.cell_count
+            self._msg.cell_temperature = [nan] * self.cell_count
+
+    # Battery Types
+
+    class LiION(BaseBattery):
+        """Base Lithium ION battery."""
+        LUT = []
+
+        def __init__(
+            self,
+            platform: Platform,
+            configuration: str,
+            rolling_average_period=30,
+        ) -> None:
+            super().__init__(platform, configuration, rolling_average_period)
+            self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
+            # Multiply LUT voltage by number of batteries in series
+            self.LUT = [(i * self.series, j) for (i, j) in self.LUT]
+
+        def update(self, power_msg: Power):
+            super().update(power_msg)
+            self.update_from_lut()
+
+    class SLA(BaseBattery):
+        """Base Lead Acid battery."""
+
+        # Rough estimate of 12V Lead-Acid SoC from voltage,
+        # scaled such that 11.6V is considered discharged.
+        # From https://iopscience.iop.org/article/10.1088/1742-6596/1367/1/012077/pdf
+        LUT = [
+            [11.6,  0.0],
+            [11.7,  0.1],
+            [11.9,  0.2],
+            [12.0,  0.3],
+            [12.2,  0.4],
+            [12.3,  0.5],
+            [12.4,  0.6],
+            [12.5,  0.7],
+            [12.55, 0.8],
+            [12.6,  0.90],
+            [12.65, 0.95],
+            [12.7,  1.00],
+        ]
+
+        def __init__(self,
+                     platform: Platform,
+                     configuration: str,
+                     rolling_average_period=30) -> None:
+            super().__init__(platform, configuration, rolling_average_period)
+            # Multiply LUT voltage by number of batteries in series
+            self.LUT = [(i * self.series, j) for (i, j) in self.LUT]
+
+        def update(self, power_msg: Power):
+            super().update(power_msg)
+            self.update_from_lut()
+
+    # Batteries
+
+    class HE2613(LiION):
+        CAPACITY = 12.8
+        VOLTAGE = 25.9
+        LUT = [
+            [21.00, 0.0],
+            [21.84, 0.1],
+            [22.68, 0.2],
+            [23.52, 0.3],
+            [24.36, 0.4],
+            [25.20, 0.5],
+            [26.04, 0.6],
+            [26.88, 0.7],
+            [27.72, 0.8],
+            [28.56, 0.9],
+            [29.40, 1.0],
+        ]
+
+    class ES20_12C(SLA):
+        CAPACITY = 20.0
+        VOLTAGE = 12.0
+
+    class U1_35(SLA):
+        CAPACITY = 35.0
+        VOLTAGE = 12.0
+
+    class TLV1222(SLA):
+        CAPACITY = 22.0
+        VOLTAGE = 12.0
+
+    class RB20(LiION):
+        CAPACITY = 20.0
+        VOLTAGE = 12.8
+        LUT = [
+            [10.5, 0.0],
+            [12.0, 0.05],
+            [13.0, 0.1],
+            [13.25, 0.2],
+            [13.3, 0.3],
+            [13.4, 0.4],
+            [13.45, 0.5],
+            [13.5, 0.6],
+            [13.6, 0.7],
+            [13.7, 0.8],
+            [13.8, 0.9],
+            [14.5, 1.0],
+        ]
+
+    # Match battery name to class
+    BATTERIES = {
+        BatteryConfig.HE2613: HE2613,
+        BatteryConfig.ES20_12C: ES20_12C,
+        BatteryConfig.U1_35: U1_35,
+        BatteryConfig.TLV1222: TLV1222,
+        BatteryConfig.RB20: RB20
+    }
+
+    def __new__(cls,
+                battery: str,
+                platform: Platform,
+                configuration: str,
+                rolling_average_period=30) -> BaseBattery:
+        return Battery.BATTERIES.setdefault(battery, Battery.BaseBattery)(
+            platform, configuration, rolling_average_period)
diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control
new file mode 100755
index 0000000..a4c0b33
--- /dev/null
+++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control
@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <rkreinin@clearpathrobotics.com>
+# @copyright (c) 2023, 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 WARRANTIES, 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, INDIRECT, 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+import os
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from std_msgs.msg import UInt8
+
+from sensor_msgs.msg import BatteryState
+
+from clearpath_config.clearpath_config import ClearpathConfig
+from clearpath_config.common.types.platform import Platform
+from clearpath_generator_common.common import BaseGenerator
+
+from enum import Enum
+
+
+class BatteryStateControl(Node):
+    class HmiColour(Enum):
+        GREEN = 0
+        YELLOW = 1
+        RED = 2
+
+    class HmiPattern(Enum):
+        OFF = 0
+        ON = 1
+        BLINK = 2
+
+    def __init__(self, setup_path='/etc/clearpath'):
+        super().__init__('battery_state_control')
+        self.setup_path = setup_path
+
+        # Define paths
+        self.config_path = os.path.join(self.setup_path, 'robot.yaml')
+
+        # Parse YAML into config
+        self.clearpath_config = ClearpathConfig(self.config_path)
+        self.platform = self.clearpath_config.platform.get_platform_model()
+
+        self.battery_state_sub = self.create_subscription(
+            BatteryState,
+            'platform/bms/state',
+            self.battery_state_callback,
+            qos_profile_sensor_data)
+
+        match self.platform:
+            case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150:
+                self.hmi_battery_pub = self.create_publisher(
+                    UInt8,
+                    'platform/mcu/_hmi',
+                    qos_profile_sensor_data)
+
+                self.hmi_msg = UInt8()
+
+    # UInt8 represents HMI state
+    # Bits 7-4: Unused
+    # Bits 3-2: Battery LED pattern
+    # Bits 1-0: Battery LED colour
+    def encode_hmi_msg(self, colour: HmiColour, pattern: HmiPattern) -> UInt8:
+        self.hmi_msg.data = (pattern.value << 2) | (colour.value)
+
+    def hmi_control(self, percentage: float):
+        if percentage >= 0.5:
+            self.encode_hmi_msg(
+                BatteryStateControl.HmiColour.GREEN,
+                BatteryStateControl.HmiPattern.ON)
+        elif percentage >= 0.2:
+            self.encode_hmi_msg(
+                BatteryStateControl.HmiColour.YELLOW,
+                BatteryStateControl.HmiPattern.ON)
+        elif percentage >= 0.1:
+            self.encode_hmi_msg(
+                BatteryStateControl.HmiColour.RED,
+                BatteryStateControl.HmiPattern.ON)
+        else:
+            self.encode_hmi_msg(
+                BatteryStateControl.HmiColour.RED,
+                BatteryStateControl.HmiPattern.BLINK)
+        self.hmi_battery_pub.publish(self.hmi_msg)
+
+    def battery_state_callback(self, msg: BatteryState):
+        match self.platform:
+            case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150:
+                self.hmi_control(msg.percentage)
+
+
+def main():
+    setup_path = BaseGenerator.get_args()
+    rclpy.init()
+
+    bsc = BatteryStateControl(setup_path)
+
+    rclpy.spin(bsc)
+
+    bsc.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator
new file mode 100755
index 0000000..464f049
--- /dev/null
+++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator
@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <rkreinin@clearpathrobotics.com>
+# @copyright (c) 2023, 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 WARRANTIES, 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, INDIRECT, 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+import os
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from clearpath_platform_msgs.msg import Power
+from sensor_msgs.msg import BatteryState
+
+from clearpath_config.clearpath_config import ClearpathConfig
+from clearpath_config.common.types.platform import Platform
+from clearpath_generator_common.common import BaseGenerator
+
+from clearpath_diagnostics.battery_state.battery import Battery
+
+
+class BatteryStateEstimator(Node):
+    def __init__(self, setup_path='/etc/clearpath'):
+        super().__init__('battery_state_estimator')
+        self.setup_path = setup_path
+
+        # Define paths
+        self.config_path = os.path.join(self.setup_path, 'robot.yaml')
+
+        # Parse YAML into config
+        self.clearpath_config = ClearpathConfig(self.config_path)
+        self.platform = self.clearpath_config.platform.get_platform_model()
+        self.battery_model = self.clearpath_config.platform.battery.model
+        self.battery_configuration = self.clearpath_config.platform.battery.configuration
+
+        self.battery = Battery(
+            battery=self.battery_model,
+            configuration=self.battery_configuration,
+            platform=self.platform)
+
+        self.battery_state_pub = self.create_publisher(
+            BatteryState,
+            'platform/bms/state',
+            qos_profile_sensor_data)
+
+        self.power_status_sub = self.create_subscription(
+            Power,
+            'platform/mcu/status/power',
+            self.power_status_callback,
+            qos_profile_sensor_data)
+
+    def power_status_callback(self, msg: Power):
+        self.battery.update(msg)
+        self.battery_state_pub.publish(self.battery.msg)
+
+
+def main():
+    setup_path = BaseGenerator.get_args()
+    rclpy.init()
+
+    bsp = BatteryStateEstimator(setup_path)
+
+    rclpy.spin(bsp)
+
+    bsp.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater
new file mode 100755
index 0000000..6060b6f
--- /dev/null
+++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater
@@ -0,0 +1,248 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <rkreinin@clearpathrobotics.com>
+# @copyright (c) 2023, 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 WARRANTIES, 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, INDIRECT, 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+import os
+
+from diagnostic_msgs.msg import DiagnosticStatus
+
+from diagnostic_updater import FrequencyStatusParam, HeaderlessTopicDiagnostic, Updater, DiagnosticStatusWrapper
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from sensor_msgs.msg import BatteryState, Image, Imu, LaserScan, MagneticField, NavSatFix, PointCloud2
+
+from clearpath_generator_common.common import BaseGenerator
+from clearpath_config.common.utils.yaml import read_yaml
+from clearpath_config.clearpath_config import ClearpathConfig
+from clearpath_config.sensors.types.sensor import BaseSensor
+from clearpath_config.sensors.types.cameras import BaseCamera, IntelRealsense, FlirBlackfly
+from clearpath_config.sensors.types.imu import BaseIMU, CHRoboticsUM6, Microstrain, RedshiftUM7
+from clearpath_config.sensors.types.lidars_2d import BaseLidar2D, HokuyoUST, SickLMS1XX
+from clearpath_config.sensors.types.lidars_3d import BaseLidar3D, VelodyneLidar
+from clearpath_config.sensors.types.gps import BaseGPS, SwiftNavDuro, NMEA
+
+from clearpath_platform_msgs.msg import Status
+
+from typing import List
+from apt import Cache, Version
+
+
+class ClearpathDiagnosticUpdater(Node):
+    def __init__(self, setup_path: str = '/etc/clearpath/'):
+        super().__init__('clearpath_diagnostics_updater')
+        self.setup_path = setup_path
+
+        # Define paths
+        self.config_path = os.path.join(setup_path, 'robot.yaml')
+        assert os.path.exists(self.config_path)
+
+        # # Read YAML
+        # self.config = read_yaml()
+        # Parse YAML into config
+        self.clearpath_config = ClearpathConfig(self.config_path)
+
+        self.serial_number = self.clearpath_config.serial_number
+        self.platform_model = self.clearpath_config.platform.get_platform_model()
+        self.namespace = self.clearpath_config.system.namespace
+
+        # Create updater
+        self.updater = Updater(self)
+        self.updater.setHardwareID(self.serial_number)
+
+        # Subscribe to topics
+        self.status_sub = self.create_subscription(
+            Status,
+            'platform/mcu/status',
+            self.status_callback,
+            qos_profile_sensor_data
+        )
+
+        self.camera_freq_bounds = {'min': 5, 'max': 10}
+        self.lidar2d_freq_bounds = {'min': 5, 'max': 10}
+        self.lidar3d_freq_bounds = {'min': 5, 'max': 10}
+
+        for sensor in self.clearpath_config.sensors.get_all_sensors():
+            self.add_sensor_diagnostic(sensor)
+
+        # Check current firmware version
+        cache = Cache()
+        try:
+            pkg = cache['ros-humble-clearpath-firmware']
+            versions = pkg.versions
+            newest: Version = versions[0]
+            self.newest_firmware_version = newest.version.split('-')[0]
+        except KeyError:
+            self.get_logger().warn('ros-humble-clearpath-firmware package not found')
+            self.newest_firmware_version = 'Not found'
+
+        self.firmware_version = '0.0.0'
+        self.battery_percentage = 0.0
+        self.battery_voltage = 0.0
+
+        # Add Diagnostic status
+        self.updater.add('Firmware Version', self.check_firmware_version)
+
+        self.updater.force_update()
+
+    def status_callback(self, msg: Status):
+        self.firmware_version = msg.firmware_version
+        self.mcu_temperature = msg.mcu_temperature
+        self.pcb_temperature = msg.pcb_temperature
+
+    def check_firmware_version(self, stat: DiagnosticStatusWrapper):
+        if self.newest_firmware_version == 'Not found':
+            stat.summary(DiagnosticStatus.ERROR, 'ros-humble-clearpath-firmware package not found')
+        elif self.firmware_version == self.newest_firmware_version:
+            stat.summary(DiagnosticStatus.OK, f'Firmware is up to date (v{self.firmware_version})')
+        elif self.firmware_version < self.newest_firmware_version:
+            stat.summary(DiagnosticStatus.WARN, f'New firmware available. (v{self.firmware_version}) -> (v{self.newest_firmware_version})')
+        else:
+            stat.summary(DiagnosticStatus.WARN, 'ros-humble-clearpath-firmware package is outdated.')
+        return stat
+
+    def get_headerless_topic_diagnostic(self,
+                                        sensor: BaseSensor,
+                                        topic: str) -> HeaderlessTopicDiagnostic:
+        sensor_topic = sensor.get_topic(topic)
+        rate = sensor.get_topic_rate(topic)
+        freq_bounds = FrequencyStatusParam({'min': rate, 'max': rate})
+        return HeaderlessTopicDiagnostic(sensor_topic, self.updater, freq_bounds)
+
+    def add_sensor_diagnostic(self, sensor: BaseSensor):
+        diagnostics: List[tuple] = []
+
+        if not sensor.launch_enabled:
+            return
+
+        match sensor:
+            case IntelRealsense():
+                if sensor.color_enabled:
+                    diagnostics.append((
+                        Image,
+                        sensor.get_topic(IntelRealsense.TOPICS.COLOR_IMAGE),
+                        self.get_headerless_topic_diagnostic(
+                            sensor, IntelRealsense.TOPICS.COLOR_IMAGE)
+                    ))
+                if sensor.depth_enabled:
+                    diagnostics.append((
+                        Image,
+                        sensor.get_topic(IntelRealsense.TOPICS.DEPTH_IMAGE),
+                        self.get_headerless_topic_diagnostic(
+                            sensor, IntelRealsense.TOPICS.DEPTH_IMAGE)
+                    ))
+                if sensor.pointcloud_enabled:
+                    diagnostics.append((
+                        PointCloud2,
+                        sensor.get_topic(IntelRealsense.TOPICS.POINTCLOUD),
+                        self.get_headerless_topic_diagnostic(
+                            sensor, IntelRealsense.TOPICS.POINTCLOUD)
+                    ))
+
+            case BaseCamera():
+                diagnostics.append((
+                    Image,
+                    sensor.get_topic(BaseCamera.TOPICS.COLOR_IMAGE),
+                    self.get_headerless_topic_diagnostic(
+                        sensor, BaseCamera.TOPICS.COLOR_IMAGE)
+                ))
+
+            case BaseLidar2D():
+                diagnostics.append((
+                  LaserScan,
+                  sensor.get_topic(BaseLidar2D.TOPICS.SCAN),
+                  self.get_headerless_topic_diagnostic(
+                      sensor, BaseLidar2D.TOPICS.SCAN)
+                ))
+
+            case BaseLidar3D():
+                diagnostics.append((
+                  LaserScan,
+                  sensor.get_topic(BaseLidar3D.TOPICS.SCAN),
+                  self.get_headerless_topic_diagnostic(
+                      sensor, BaseLidar3D.TOPICS.SCAN)
+                ))
+                diagnostics.append((
+                    PointCloud2,
+                    sensor.get_topic(BaseLidar3D.TOPICS.POINTS),
+                    self.get_headerless_topic_diagnostic(
+                        sensor, BaseLidar3D.TOPICS.POINTS)
+                ))
+
+            case BaseIMU():
+                diagnostics.append((
+                  Imu,
+                  sensor.get_topic(BaseIMU.TOPICS.DATA),
+                  self.get_headerless_topic_diagnostic(
+                      sensor, BaseIMU.TOPICS.DATA)
+                ))
+                diagnostics.append((
+                  MagneticField,
+                  sensor.get_topic(BaseIMU.TOPICS.MAG),
+                  self.get_headerless_topic_diagnostic(
+                      sensor, BaseIMU.TOPICS.MAG)
+                ))
+
+            case BaseGPS():
+                diagnostics.append((
+                  NavSatFix,
+                  sensor.get_topic(BaseGPS.TOPICS.FIX),
+                  self.get_headerless_topic_diagnostic(
+                      sensor, BaseGPS.TOPICS.FIX)
+                ))
+
+        for d in diagnostics:
+            self.create_subscription(
+                d[0],
+                d[1],
+                lambda msg: d[2].tick(),
+                qos_profile_sensor_data
+            )
+
+
+def main():
+    setup_path = BaseGenerator.get_args()
+
+    rclpy.init()
+
+    node = ClearpathDiagnosticUpdater(setup_path)
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/clearpath_diagnostics/config/diagnostics.yaml b/clearpath_diagnostics/config/diagnostics.yaml
new file mode 100644
index 0000000..0121fbe
--- /dev/null
+++ b/clearpath_diagnostics/config/diagnostics.yaml
@@ -0,0 +1,36 @@
+/**:
+  analyzers:
+    ros__parameters:
+      path: Clearpath Diagnostics
+      platform:
+        type: diagnostic_aggregator/AnalyzerGroup
+        path: Platform
+        analyzers:
+          firmware:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: Firmware
+            contains: [ 'Firmware' ]
+      sensors:
+        type: diagnostic_aggregator/AnalyzerGroup
+        path: Sensors
+        analyzers:
+          cameras:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: Cameras
+            contains: [ 'camera' ]
+          lidar2d:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: Lidar2D
+            contains: [ 'lidar2d' ]
+          lidar3d:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: Lidar3D
+            contains: [ 'lidar3d' ]
+          imu:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: IMU
+            contains: [ 'imu' ]
+          gps:
+            type: diagnostic_aggregator/GenericAnalyzer
+            path: GPS
+            contains: [ 'gps' ]
diff --git a/clearpath_diagnostics/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py
new file mode 100644
index 0000000..4395465
--- /dev/null
+++ b/clearpath_diagnostics/launch/diagnostics.launch.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <rkreinin@clearpathrobotics.com>
+# @copyright (c) 2023, 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 WARRANTIES, 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, INDIRECT, 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+from ament_index_python.packages import get_package_share_directory
+
+from clearpath_config.common.utils.yaml import read_yaml
+from clearpath_config.clearpath_config import ClearpathConfig
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    OpaqueFunction,
+)
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node, PushRosNamespace
+
+
+ARGUMENTS = [
+    DeclareLaunchArgument(
+        "setup_path",
+        default_value="/etc/clearpath/",
+        description="Clearpath setup path",
+    )
+]
+
+
+def launch_setup(context, *args, **kwargs):
+    pkg_clearpath_diagnostics = get_package_share_directory("clearpath_diagnostics")
+
+    setup_path = LaunchConfiguration("setup_path")
+
+    analyzer_params_filepath = PathJoinSubstitution(
+        [pkg_clearpath_diagnostics, "config", "diagnostics.yaml"]
+    )
+
+    # Read robot YAML
+    config = read_yaml(setup_path.perform(context) + "robot.yaml")
+    # Parse robot YAML into config
+    clearpath_config = ClearpathConfig(config)
+
+    namespace = clearpath_config.system.namespace
+    diagnostics = GroupAction(
+        [
+            PushRosNamespace(namespace),
+            # Aggregator
+            Node(
+                package="diagnostic_aggregator",
+                executable="aggregator_node",
+                output="screen",
+                parameters=[analyzer_params_filepath],
+                remappings=[
+                    ("/diagnostics", "diagnostics"),
+                    ("/diagnostics_agg", "diagnostics_agg"),
+                    ("/diagnostics_toplevel_state", "diagnostics_toplevel_state"),
+                ],
+            ),
+            # Updater
+            Node(
+                package="clearpath_diagnostics",
+                executable="diagnostics_updater",
+                output="screen",
+                remappings=[
+                    ("/diagnostics", "diagnostics"),
+                    ("/diagnostics_agg", "diagnostics_agg"),
+                    ("/diagnostics_toplevel_state", "diagnostics_toplevel_state"),
+                ],
+                arguments=['-s', setup_path]
+            ),
+        ]
+    )
+
+    return [diagnostics]
+
+
+def generate_launch_description():
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(OpaqueFunction(function=launch_setup))
+    return ld
diff --git a/clearpath_diagnostics/package.xml b/clearpath_diagnostics/package.xml
new file mode 100644
index 0000000..e89a63a
--- /dev/null
+++ b/clearpath_diagnostics/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>clearpath_diagnostics</name>
+  <version>0.0.0</version>
+  <description>Clearpath Robot Diagnostics scripts</description>
+  <maintainer email="rkreinin@clearpathrobotics.com">rkreinin</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>clearpath_config</exec_depend>
+  <exec_depend>clearpath_generator_common</exec_depend>
+  <exec_depend>clearpath_platform_msgs</exec_depend>
+  <exec_depend>diagnostic_updater</exec_depend>
+  <exec_depend>diagnostic_aggregator</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py
index 0f099be..1e946c2 100644
--- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py
+++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py
@@ -32,7 +32,7 @@
 # modification, is not permitted without the express permission
 # of Clearpath Robotics.
 
-from clearpath_generator_common.common import LaunchFile
+from clearpath_generator_common.common import LaunchFile, Package
 from clearpath_generator_common.launch.writer import LaunchWriter
 from clearpath_generator_common.launch.generator import LaunchGenerator
 from clearpath_generator_robot.launch.sensors import SensorLaunch
@@ -79,8 +79,17 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
             ]
         )
 
-        # J100 Add micro ros agent
-        self.uros_node = LaunchFile.Node(
+        # Ethernet MicroROS Agent
+        self.eth_uros_node = LaunchFile.Node(
+            name='micro_ros_agent',
+            package='micro_ros_agent',
+            executable='micro_ros_agent',
+            namespace=self.namespace,
+            arguments=['udp4', '--port', '11411'],
+        )
+
+        # J100 MicroROS Agent
+        self.j100_uros_node = LaunchFile.Node(
             name='micro_ros_agent',
             package='micro_ros_agent',
             executable='micro_ros_agent',
@@ -119,6 +128,44 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
             ],
         )
 
+        # Diagnostics
+        clearpath_diagnostics_package = Package('clearpath_diagnostics')
+        self.diagnostics_launch = LaunchFile('diagnostics', package=clearpath_diagnostics_package)
+
+        # Battery state
+        self.battery_state_estimator = LaunchFile.Node(
+            package='clearpath_diagnostics',
+            executable='battery_state_estimator',
+            name='battery_state_estimator',
+            namespace=self.namespace,
+            arguments=['-s', setup_path]
+        )
+
+        self.battery_state_control = LaunchFile.Node(
+            package='clearpath_diagnostics',
+            executable='battery_state_control',
+            name='battery_state_control',
+            namespace=self.namespace,
+            arguments=['-s', setup_path]
+        )
+
+        # Lighting
+        self.lighting_node = LaunchFile.Node(
+          package='clearpath_platform',
+          executable='lighting_node',
+          name='lighting_node',
+          namespace=self.namespace,
+          parameters=[{'platform': self.platform_model}]
+        )
+
+        # Sevcon
+        self.sevcon_node = LaunchFile.Node(
+          package='sevcon_traction',
+          executable='sevcon_traction_node',
+          name='sevcon_traction_node',
+          namespace=self.namespace,
+        )
+
         # Static transform from <namespace>/odom to odom
         # See https://github.com/ros-controls/ros2_controllers/pull/533
         self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node(
@@ -139,17 +186,57 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
         )
 
         # Components required for each platform
+        common_platform_components = [
+            self.wireless_watcher_node,
+            self.diagnostics_launch,
+            self.battery_state_estimator,
+            self.battery_state_control
+        ]
+
         self.platform_components = {
-            Platform.J100: [
+            Platform.J100: common_platform_components + [
                 self.imu_0_filter_node,
                 self.imu_0_filter_config,
                 self.configure_mcu,
-                self.uros_node,
-                self.nmea_driver_node,
-                self.wireless_watcher_node,
+                self.j100_uros_node,
+                self.nmea_driver_node
             ],
-            Platform.A200: [
-                self.wireless_watcher_node
+            Platform.A200: common_platform_components,
+            Platform.W200: common_platform_components + [
+                self.imu_0_filter_node,
+                self.imu_0_filter_config,
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
+                self.sevcon_node
+            ],
+            Platform.DD100: common_platform_components + [
+                self.imu_0_filter_node,
+                self.imu_0_filter_config,
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
+            ],
+            Platform.DO100: common_platform_components + [
+                self.imu_0_filter_node,
+                self.imu_0_filter_config,
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
+            ],
+            Platform.DD150: common_platform_components + [
+                self.imu_0_filter_node,
+                self.imu_0_filter_config,
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
+            ],
+            Platform.DO150: common_platform_components + [
+                self.imu_0_filter_node,
+                self.imu_0_filter_config,
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
             ],
         }
 
diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml
index 41f3335..01a1006 100644
--- a/clearpath_generator_robot/package.xml
+++ b/clearpath_generator_robot/package.xml
@@ -16,6 +16,8 @@
   <exec_depend>clearpath_sensors</exec_depend>
   <exec_depend>imu_filter_madgwick</exec_depend>
   <exec_depend>micro_ros_agent</exec_depend>
+  <!-- Disabled for now.-->
+  <!-- <exec_depend>sevcon_traction</exec_depend> -->
   <exec_depend>wireless_watcher</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/clearpath_robot/debian/udev b/clearpath_robot/debian/udev
index 66184ce..cd67145 100644
--- a/clearpath_robot/debian/udev
+++ b/clearpath_robot/debian/udev
@@ -13,4 +13,8 @@ SUBSYSTEM=="tty", KERNEL=="ttyUSB[0-9]*", ATTRS{idProduct}=="6001", ATTRS{idVend
 # Udev rule for the Logitech controllers
 KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21f", SYMLINK+="input/f710"
 KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c219", SYMLINK+="input/f710"
-KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310"
\ No newline at end of file
+KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310"
+
+# CAN rule for PCI CANBUS card
+SUBSYSTEM=="net", KERNEL == "can0", RUN+="/bin/sh -c 'ip link set can0 type can bitrate 1000000; ip link set can0 up'"
+SUBSYSTEM=="net", KERNEL == "can1", RUN+="/bin/sh -c 'ip link set can1 type can bitrate 500000;  ip link set can1 up'"
\ No newline at end of file
diff --git a/clearpath_robot/package.xml b/clearpath_robot/package.xml
index d6f856a..91b6305 100644
--- a/clearpath_robot/package.xml
+++ b/clearpath_robot/package.xml
@@ -14,6 +14,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>can-utils</exec_depend>
   <!-- Metapackage dependencies-->
   <exec_depend>clearpath_generator_robot</exec_depend>
   <exec_depend>clearpath_sensors</exec_depend>