A300 (#106)

* Added lynx hardware interface

* Lynx motor driver
Rename clearpath_platform namespace to clearpath_hardware_interfaces

* Added A300 and Inventus battery to generator

* A300 lighting

* Dependencies and README

* Rename platform to hardware_interfaces in hardware.xml

* Fix append of bms in generator

* Removed wheel_joints_ map

---------

Co-authored-by: Luis Camero <[email protected]>

Author: roni-kreinin

clearpath_generator_robot/clearpath_generator_robot/launch/generator.py

@@ -35,7 +35,7 @@
 
 from clearpath_config.common.types.platform import Platform
 from clearpath_config.platform.battery import BatteryConfig
-from clearpath_generator_common.common import LaunchFile, Package
+from clearpath_generator_common.common import LaunchFile, Package, ParamFile
 from clearpath_generator_common.launch.generator import LaunchGenerator
 from clearpath_generator_common.launch.writer import LaunchWriter
 from clearpath_generator_robot.launch.sensors import SensorLaunch
@@ -149,8 +149,11 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
             arguments=['-s', setup_path]
         )
 
-        # Valence BMS
+        # BMS
         self.bms_launch_file = None
+        self.bms_node = None
+
+        # Valence BMS
         if (self.clearpath_config.platform.battery.model in
                 [BatteryConfig.VALENCE_U24_12XP, BatteryConfig.VALENCE_U27_12XP]):
 
@@ -176,6 +179,40 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
                 package=Package('valence_bms_driver'),
                 args=bms_launch_args
                 )
+        # Inventus BMS
+        elif (self.clearpath_config.platform.battery.model in
+              [BatteryConfig.S_24V20_U1]):
+
+            pkg_inventus_bmu = Package('inventus_bmu')
+            launch_args = self.clearpath_config.platform.battery.launch_args
+
+            inventus_bmu_params_file = ParamFile('default', package=pkg_inventus_bmu)
+            inventus_bmu_params = inventus_bmu_params_file.full_path
+
+            can_dev = 'can1'
+
+            if launch_args:
+                if 'params' in launch_args:
+                    inventus_bmu_params = launch_args['params']
+                if 'can_device' in launch_args:
+                    can_dev = launch_args['can_device']
+
+            self.bms_node = LaunchFile.Node(
+                'inventus_bmu',
+                'inventus_bmu',
+                'inventus_bmu_driver',
+                self.namespace,
+                parameters=[
+                    inventus_bmu_params,
+                    {'can_device': can_dev}
+                ],
+                remappings=[
+                    ('bms/battery_state', 'platform/bms/state'),
+                    ('modules', 'platform/bms/modules'),
+                    ('bms/low_soc_alarm', 'platform/bms/low_soc_alarm'),
+                    ('bms/soc_difference_alarm', 'platform/bms/soc_difference_alarm')
+                ]
+            )
 
         # Lighting
         self.lighting_node = LaunchFile.Node(
@@ -203,6 +240,15 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
           namespace=self.namespace,
         )
 
+        # BLDC Multi-Drive Node
+        self.lynx_node = LaunchFile.Node(
+          package='lynx_motor_driver',
+          executable='lynx_motor_driver',
+          parameters=[os.path.join(self.platform_params_path, 'control.yaml')],
+          name='lynx_control',
+          namespace=self.namespace,
+        )
+
         # ROS2 socketcan bridges
         ros2_socketcan_package = Package('clearpath_ros2_socketcan_interface')
         self.can_bridges = []
@@ -231,10 +277,13 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
         common_platform_components = [
             self.wireless_watcher_node,
             self.diagnostics_launch,
-            self.battery_state_estimator,
             self.battery_state_control,
         ]
 
+        # Only add estimator when no BMS is present
+        if self.bms_launch_file is None and self.bms_node is None:
+            common_platform_components.append(self.battery_state_estimator)
+
         if len(self.can_bridges) > 0:
             common_platform_components.extend(self.can_bridges)
 
@@ -247,6 +296,12 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
                 self.nmea_driver_node
             ],
             Platform.A200: common_platform_components,
+            Platform.A300: common_platform_components + [
+                self.eth_uros_node,
+                self.configure_mcu,
+                self.lighting_node,
+                self.lynx_node,
+            ],
             Platform.W200: common_platform_components + [
                 self.imu_0_filter_node,
                 self.imu_0_filter_config,
@@ -336,6 +391,9 @@ def generate_platform(self) -> None:
         if self.bms_launch_file:
             platform_service_launch_writer.add(self.bms_launch_file)
 
+        if self.bms_node:
+            platform_service_launch_writer.add(self.bms_node)
+
         platform_service_launch_writer.generate_file()
 
     def generate_manipulators(self) -> None:

clearpath_generator_robot/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>micro_ros_agent</exec_depend>
   <!--<exec_depend>sevcon_traction</exec_depend>-->
   <exec_depend>valence_bms_driver</exec_depend>
+  <exec_depend>wireless_msgs</exec_depend>
   <exec_depend>wireless_watcher</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

clearpath_hardware_interfaces/CMakeLists.txt

@@ -16,10 +16,10 @@ find_package(ament_cmake_python REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(controller_manager_msgs REQUIRED)
+find_package(clearpath_motor_msgs REQUIRED)
 find_package(clearpath_platform_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(clearpath_motor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 
 find_package(geometry_msgs REQUIRED)
@@ -37,6 +37,7 @@ 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
@@ -170,17 +171,42 @@ ament_target_dependencies(
   rclcpp
 )
 
+# Lynx Hardware
+add_library(
+  lynx_hardware
+  SHARED
+  src/lynx/hardware.cpp
+  src/lynx/hardware_interface.cpp
+)
+
+target_include_directories(
+  lynx_hardware
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(
+  lynx_hardware
+  clearpath_motor_msgs
+  clearpath_platform_msgs
+  hardware_interface
+  pluginlib
+  rclcpp
+)
+
 pluginlib_export_plugin_description_file(hardware_interface src/a200/hardware.xml)
 pluginlib_export_plugin_description_file(hardware_interface src/j100/hardware.xml)
 pluginlib_export_plugin_description_file(hardware_interface src/w200/hardware.xml)
 pluginlib_export_plugin_description_file(hardware_interface src/puma/hardware.xml)
+pluginlib_export_plugin_description_file(hardware_interface src/lynx/hardware.xml)
 
 # INSTALL
 install(
   TARGETS a200_hardware
           j100_hardware
           w200_hardware
           puma_hardware
+          lynx_hardware
           ${LIGHTING_EXECUTABLE}
           ${LIGHTING_LIB}
   LIBRARY DESTINATION lib
@@ -208,6 +234,7 @@ ament_export_libraries(
   j100_hardware
   w200_hardware
   puma_hardware
+  lynx_hardware
   ${LIGHTING_LIB}
 )
 ament_export_dependencies(

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp

@@ -62,18 +62,18 @@ struct hsv_t {
   }
 };
 
-static const hsv_t COLOR_RED = hsv_t(0.0, 100.0, 100.0);
-static const hsv_t COLOR_RED_DIM = hsv_t(0.0, 100.0, 10.0);
-static const hsv_t COLOR_MAGENTA = hsv_t(300.0, 100.0, 100.0);
-static const hsv_t COLOR_BLUE = hsv_t(240.0, 100.0, 100.0);
-static const hsv_t COLOR_BLUE_DIM = hsv_t(240.0, 100.0, 10.0);
-static const hsv_t COLOR_CYAN = hsv_t(180.0, 100.0, 100.0);
-static const hsv_t COLOR_GREEN = hsv_t(120.0, 100.0, 100.0);
-static const hsv_t COLOR_GREEN_DIM = hsv_t(120.0, 100.0, 10.0);
-static const hsv_t COLOR_YELLOW = hsv_t(60.0, 100.0, 100.0);
-static const hsv_t COLOR_ORANGE = hsv_t(30.0, 100.0, 100.0);
-static const hsv_t COLOR_WHITE = hsv_t(0.0, 0.0, 100.0);
-static const hsv_t COLOR_WHITE_DIM = hsv_t(0.0, 0.0, 10.0);
+static const hsv_t COLOR_RED = hsv_t(0.0, 100.0, 37.5);
+static const hsv_t COLOR_RED_DIM = hsv_t(0.0, 100.0, 18.75);
+static const hsv_t COLOR_MAGENTA = hsv_t(300.0, 100.0, 25.0);
+static const hsv_t COLOR_BLUE = hsv_t(240.0, 100.0, 25.0);
+static const hsv_t COLOR_BLUE_DIM = hsv_t(240.0, 100.0, 12.5);
+static const hsv_t COLOR_CYAN = hsv_t(180.0, 100.0, 25.0);
+static const hsv_t COLOR_GREEN = hsv_t(120.0, 100.0, 25.0);
+static const hsv_t COLOR_GREEN_DIM = hsv_t(120.0, 100.0, 12.5);
+static const hsv_t COLOR_YELLOW = hsv_t(60.0, 100.0, 25.0);
+static const hsv_t COLOR_ORANGE = hsv_t(30.0, 100.0, 25.0);
+static const hsv_t COLOR_WHITE = hsv_t(0.0, 0.0, 25.0);
+static const hsv_t COLOR_WHITE_DIM = hsv_t(0.0, 0.0, 12.5);
 static const hsv_t COLOR_BLACK = hsv_t(0.0, 0.0, 0.0);
 
 
@@ -87,6 +87,22 @@ class ColorHSV
   double h() { return hsv_.h; };
   double s() { return hsv_.s; };
   double v() { return hsv_.v; };
+
+  void setH(const double h)
+  {
+    hsv_.h = h;
+  }
+
+  void setS(const double s)
+  {
+    hsv_.s = s;
+  }
+
+  void setV(const double v)
+  {
+    hsv_.v = v;
+  }
+
 private:
   hsv_t hsv_;
 };

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/lighting.hpp

@@ -44,6 +44,8 @@
 #include "clearpath_platform_msgs/msg/power.hpp"
 #include "clearpath_platform_msgs/msg/stop_status.hpp"
 
+#include "clearpath_motor_msgs/msg/lynx_system_protection.hpp"
+
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "std_msgs/msg/bool.hpp"
@@ -68,7 +70,9 @@ class Lighting : public rclcpp::Node
   {
     BatteryFault = 0,
     ShoreFault,
-    //PumaFault,
+    MotorFault,
+    MotorOverheated,
+    MotorThrottled,
     ShoreAndCharged,
     ShoreAndCharging,
     ShorePower,
@@ -101,6 +105,7 @@ class Lighting : public rclcpp::Node
   void batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
   void stopEngagedCallback(const std_msgs::msg::Bool::SharedPtr msg);
   void cmdVelCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
+  void systemProtectionCallback(const clearpath_motor_msgs::msg::LynxSystemProtection::SharedPtr msg);
 
   /** Updates the current lighting state based on all inputs */
   void setState(Lighting::State new_state);
@@ -117,6 +122,7 @@ class Lighting : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_sub_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stop_engaged_sub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_sub_;
+  rclcpp::Subscription<clearpath_motor_msgs::msg::LynxSystemProtection>::SharedPtr system_protection_sub_;
 
   // Timers
   rclcpp::TimerBase::SharedPtr lighting_timer_;
@@ -130,6 +136,7 @@ class Lighting : public rclcpp::Node
   sensor_msgs::msg::BatteryState battery_state_msg_;
   std_msgs::msg::Bool stop_engaged_msg_;
   geometry_msgs::msg::TwistStamped cmd_vel_msg_;
+  clearpath_motor_msgs::msg::LynxSystemProtection system_protection_msg_;
 
   // Variables
   Platform platform_;

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/platform.hpp

@@ -43,6 +43,7 @@ namespace clearpath_lighting
 
 enum Platform
 {
+  A300,
   DD100,
   DO100,
   DD150,
@@ -53,6 +54,7 @@ enum Platform
 
 static std::map<std::string, Platform> ClearpathPlatforms
 {
+  {"a300", Platform::A300},
   {"dd100", Platform::DD100},
   {"do100", Platform::DO100},
   {"dd150", Platform::DD150},
@@ -63,6 +65,7 @@ static std::map<std::string, Platform> ClearpathPlatforms
 
 static std::map<Platform, int> PlatformNumLights
 {
+  {Platform::A300, 4},
   {Platform::DD100, 4},
   {Platform::DO100, 4},
   {Platform::DD150, 4},
@@ -71,6 +74,17 @@ static std::map<Platform, int> PlatformNumLights
   {Platform::W200, 4},
 };
 
+static std::map<Platform, float> PlatformBrightness
+{
+  {Platform::A300, 0.5f},
+  {Platform::DD100, 1.0f},
+  {Platform::DO100, 1.0f},
+  {Platform::DD150, 1.0f},
+  {Platform::DO150, 1.0f},
+  {Platform::R100, 1.0f},
+  {Platform::W200, 1.0f},
+};
+
 }
 
 #endif  // CLEARPATH_HARDWARE_INTERFACES__LIGHTING__PLATFORM_HPP_

clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp

@@ -61,13 +61,38 @@ class Sequence
 {
 
 public:
+  Sequence();
+
   clearpath_platform_msgs::msg::Lights getLightsMsg();
   void reset();
   static LightingState fillLightingState(ColorHSV color, clearpath_lighting::Platform platform);
   static LightingState fillFrontRearLightingState(ColorHSV front_color, ColorHSV rear_color, clearpath_lighting::Platform platform);
   static LightingState fillLeftRightLightingState(ColorHSV left_color, ColorHSV right_color, clearpath_lighting::Platform platform);
   static LightingState fillOppositeCornerLightingState(ColorHSV front_left_color, ColorHSV front_right_color, clearpath_lighting::Platform platform);
-  Sequence();
+  const LightingSequence& getSequence() const;
+  void setSequence(LightingSequence sequence);
+
+  uint16_t getNumStates() const;
+  void setNumStates(const uint16_t num_states);
+
+  Sequence operator+(Sequence const& other)
+  {
+    Sequence s;
+    LightingSequence ls, other_ls;
+
+    ls = sequence_;
+    other_ls = other.getSequence();
+    
+    for (std::size_t i = 0; i < sequence_.size(); i++)
+    {
+      ls.at(i).insert(ls.at(i).end(), other_ls.at(i).begin(), other_ls.at(i).end());
+    }
+
+    s.setSequence(ls);
+    s.setNumStates(num_states_ + other.getNumStates());
+    
+    return s;
+  }
 
 protected:
   LightingSequence sequence_;