Ros2 lights controller

.pre-commit-config.yaml

@@ -39,7 +39,7 @@ repos:
       - id: clang-format
 
   - repo: https://github.com/codespell-project/codespell
-    rev: v1.16.0
+    rev: v2.2.6
     hooks:
       - id: codespell
         name: codespell

panther_bringup/README.md

@@ -21,10 +21,12 @@ The package contains the default configuration and launch files necessary to sta
 - `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only.
 - `controller_config_path` [*string*, default: **panther_controller/config/<wheel_type arg>_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here.
 - `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file.
+- `led_config_file` [*string*, default: **panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
 - `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description.
 - `simulation_engine` [*string*, default: **ignition-gazebo**]: simulation engine to use when running Gazebo.
 - `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter.
 - `use_sim` [*bool*, default: **false**]: whether simulation is used.
+- `user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations.
 - `wheel_config_path` [*string*, default: **$(find panther_description)/config/<wheel_type arg>.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**.
 - `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - offroad, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly).
 

panther_bringup/launch/bringup.launch.py

@@ -141,6 +141,26 @@ def generate_launch_description():
         default_value="",
     )
 
+    led_config_file = LaunchConfiguration("led_config_file")
+    declare_led_config_file_arg = DeclareLaunchArgument(
+        "led_config_file",
+        default_value=PathJoinSubstitution(
+            [
+                get_package_share_directory("panther_lights"),
+                "config",
+                PythonExpression(["'led_config.yaml'"]),
+            ]
+        ),
+        description="Path to a YAML file with a description of led configuration",
+    )
+
+    user_led_animations_file = LaunchConfiguration("user_led_animations_file")
+    declare_user_led_animations_file_arg = DeclareLaunchArgument(
+        "user_led_animations_file",
+        default_value="",
+        description="Path to a YAML file with a description of the user defined animations",
+    )
+
     simulation_engine = LaunchConfiguration("simulation_engine")
     declare_simulation_engine_arg = DeclareLaunchArgument(
         "simulation_engine",
@@ -226,6 +246,10 @@ def generate_launch_description():
             )
         ),
         condition=UnlessCondition(use_sim),
+        launch_arguments={
+            "led_config_file": led_config_file,
+            "user_led_animations_file": user_led_animations_file,
+        }.items(),
     )
 
     battery_launch = IncludeLaunchDescription(
@@ -282,6 +306,8 @@ def generate_launch_description():
         declare_wheel_config_path_arg,
         declare_controller_config_path_arg,
         declare_battery_config_path_arg,
+        declare_led_config_file_arg,
+        declare_user_led_animations_file_arg,
         declare_simulation_engine_arg,
         declare_publish_robot_state_arg,
         declare_use_ekf_arg,

panther_gpiod/test/test_gpio_driver.cpp

@@ -98,9 +98,9 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail)
 
 TEST_F(TestGPIODriver, SetWrongPinValue)
 {
-  panther_utils::test_utils::ExpectThrowWithDescription<std::invalid_argument>(
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(static_cast<GPIOPin>(-1), true); },
-    "Pin not found in GPIO info storage.");
+    "Pin not found in GPIO info storage."));
 }
 
 TEST_F(TestGPIODriver, IsPinAvailable)
@@ -136,12 +136,12 @@ TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT)
 
 TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread)
 {
-  panther_utils::test_utils::ExpectThrowWithDescription<std::runtime_error>(
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::runtime_error>(
     [&]() {
       this->gpio_driver_->ConfigureEdgeEventCallback(
         std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1));
     },
-    "GPIO monitor thread is not running!");
+    "GPIO monitor thread is not running!"));
 }
 
 TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState)
@@ -170,9 +170,9 @@ TEST_F(TestGPIODriver, ChangePinDirection)
   this->gpio_driver_->GPIOMonitorEnable();
   this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT);
 
-  panther_utils::test_utils::ExpectThrowWithDescription<std::invalid_argument>(
+  EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown<std::invalid_argument>(
     [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); },
-    "Cannot set value for INPUT pin.");
+    "Cannot set value for INPUT pin."));
 
   this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT);
 

panther_hardware_interfaces/README.md

@@ -33,7 +33,7 @@ That said apart from the usual interface provided by the ros2_control, this plug
 [//]: # (ROS_API_NODE_PUBLISHERS_START)
 
 - `/diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages.
-- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags.
+- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers state and error flags.
 
 [//]: # (ROS_API_NODE_PUBLISHERS_END)
 

panther_lights/CMakeLists.txt

@@ -10,11 +10,22 @@ find_package(diagnostic_updater REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(panther_gpiod REQUIRED)
 find_package(panther_msgs REQUIRED)
+find_package(panther_utils REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 include_directories(include)
 
+pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml)
+
+add_library(
+  panther_animation_plugins SHARED src/animation/image_animation.cpp
+                                   src/animation/charging_animation.cpp)
+ament_target_dependencies(panther_animation_plugins panther_utils pluginlib)
+target_link_libraries(panther_animation_plugins png yaml-cpp)
+
 add_executable(driver_node src/driver_node_main.cpp src/driver_node.cpp
                            src/apa102.cpp)
 
@@ -27,24 +38,132 @@ ament_target_dependencies(
   rclcpp
   sensor_msgs)
 
+add_executable(
+  controller_node
+  src/controller_node_main.cpp src/controller_node.cpp src/led_segment.cpp
+  src/led_panel.cpp src/segment_converter.cpp src/led_animations_queue.cpp)
+
+ament_target_dependencies(controller_node rclcpp pluginlib panther_msgs
+                          panther_utils sensor_msgs)
+target_link_libraries(controller_node yaml-cpp)
+
 add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp
                                     src/dummy_scheduler_node.cpp)
 
 ament_target_dependencies(dummy_scheduler_node image_transport rclcpp
-                          sensor_msgs)
+                          panther_msgs sensor_msgs)
 
-install(TARGETS driver_node dummy_scheduler_node
+install(TARGETS driver_node controller_node dummy_scheduler_node
         DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib)
+
+install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY include/ DESTINATION include)
+ament_export_include_directories(include)
+
+ament_export_libraries(panther_animation_plugins)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}_test_animation test/test_animation.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_animation
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_animation panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_animation yaml-cpp)
 
-# 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_add_gtest(
+    ${PROJECT_NAME}_test_image_animation test/test_image_animation.cpp
+    src/animation/image_animation.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_image_animation
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_image_animation
+                            ament_index_cpp panther_utils pluginlib)
+  target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_charging_animation test/test_charging_animation.cpp
+    src/animation/charging_animation.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_charging_animation
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_charging_animation
+                            ament_index_cpp panther_utils pluginlib)
+  target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp)
+
+  ament_add_gtest(${PROJECT_NAME}_test_led_panel test/test_led_panel.cpp
+                  src/led_panel.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_panel
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+
+  ament_add_gtest(${PROJECT_NAME}_test_led_segment test/test_led_segment.cpp
+                  src/led_segment.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_segment
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_led_segment pluginlib
+                            panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_led_segment yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_segment_converter test/test_segment_converter.cpp
+    src/segment_converter.cpp src/led_panel.cpp src/led_segment.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_segment_converter
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib
+                            panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_led_animation test/test_led_animation.cpp
+    src/led_panel.cpp src/led_segment.cpp src/led_animations_queue.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_animation
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_led_animation pluginlib
+                            panther_utils rclcpp)
+  target_link_libraries(${PROJECT_NAME}_test_led_animation yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_led_animations_queue
+    test/test_led_animations_queue.cpp src/led_panel.cpp src/led_segment.cpp
+    src/led_animations_queue.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_animations_queue
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_led_animations_queue pluginlib
+                            panther_utils rclcpp)
+  target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_controller_node
+    test/test_controller_node.cpp
+    src/controller_node.cpp
+    src/led_segment.cpp
+    src/led_panel.cpp
+    src/segment_converter.cpp
+    src/led_animations_queue.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_controller_node
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_controller_node pluginlib
+                            panther_msgs panther_utils rclcpp sensor_msgs)
+  target_link_libraries(${PROJECT_NAME}_test_controller_node yaml-cpp)
+endif()
 
 ament_package()

panther_lights/LIGHTS_API.md

@@ -0,0 +1,99 @@
+# Animation API
+
+## Animation Class
+
+Basic animation type. This class consists of:
+
+Arguments:
+
+- `animation_description` [*dict*]: a dictionary with animation description. Contain following keys:
+  - `brightness` [*float*, optional]: will be assigned to the `brightness_` variable as a value in a range **[0, 255]**.
+  - `duration` [*float*]: will be assigned to `duration_` variable.
+  - `repeat` [*int*, optional]: will be assigned to `loops_` variable.
+- `num_led` [*int*]: number of LEDs in a bumper.
+- `controller_frequency` [*float*]: controller frequency **[Hz]** at which animation frames will be processed.
+
+Functions:
+
+- `Initialize` - this method handles animation initialization. It should be invoked right after the animation is created, as the constructors for pluginlib classes cannot have parameters.
+- `Update` - returns a list of length `num_led` with **RGBA** values of colors to be displayed on the Bumper Lights based on the `UpdateFrame` method. Handles looping over animation based on `repeating` parameter, iterating over animation, and updating its progress.
+- `UpdateFrame` - returns a list of shape (`num_led`, 4) with **RGBA** values of colors to be displayed on the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. By default, not implemented.
+- `Reset` - resets animation to its initial state. If overwritten, it requires calling the parent class implementation first.
+
+Setters:
+
+- `SetParam` - allows processing an additional parameter passed to the animation when it is created.
+
+Getters:
+
+- `GetBrightness` [*std::uint8_t*]: returns animation brightness.
+- `IsFinished` [*bool*]: returns a value indicating if the animation execution process has finished.
+- `GetNumberOfLeds` [*std::size_t*]: returns the number of LEDs.
+- `GetProgress` [*float*]: returns animation execution progress.
+- `GetAnimationLength` [*std::size_t*]: returns animation length.
+- `GetAnimationIteration` [*std::size_t*]: returns current animation iteration.
+
+## Defining a New Animation Type
+
+It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html).
+
+Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animation_description` and processed inside `Initialize` method. See the example below or other animation definitions.
+
+Create a New Animation Type:
+
+```c++
+# my_cool_animation.hpp
+
+#ifndef MY_PACKAGE_MY_COOL_ANIMATION_HPP_
+#define MY_PACKAGE_MY_COOL_ANIMATION_HPP_
+
+#include <yaml-cpp/yaml.h>
+
+#include <panther_lights/animation/animation.hpp>
+
+class MyCoolAnimation : public Animation
+{
+public:
+  MyCoolAnimation() {}
+  ~MyCoolAnimation() {}
+
+  void Initialize(
+    const YAML::Node & animation_description, const std::size_t num_led,
+    const float controller_frequency) override;
+
+private:
+  std::uint8_t value_;
+};
+
+#endif  // MY_PACKAGE_MY_COOL_ANIMATION_HPP_
+```
+
+```c++
+# my_cool_animation.cpp
+
+#include <yaml-cpp/yaml.h>
+
+#include <my_package/animation/my_cool_animation.hpp>
+
+void MyCoolAnimation::Initialize(
+  const YAML::Node & animation_description, const std::size_t num_led,
+  const float controller_frequency)
+{
+  Animation::Initialize(animation_description, num_led, controller_frequency);
+}
+
+void MyCoolAnimation::SetParam(const std::string & param)
+{
+  value_ = static_cast<std::uint8_t>(param);
+}
+
+std::vector<uint8_t> MyCoolAnimation::UpdateFrame()
+{
+  return std::vector<std::uint8_t>(this->GetNumberOfLeds() * 4, value_);
+}
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(my_package::MyCoolAnimation, my_package::Animation)
+
+```