diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6f979246b..250258515 100644 --- a/.pre-commit-config.yaml +++ b/.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 diff --git a/panther_bringup/README.md b/panther_bringup/README.md index d61031d4c..c5510f8ee 100644 --- a/panther_bringup/README.md +++ b/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/_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/.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). diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index fd76f0bdd..108a462a0 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/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, diff --git a/panther_gpiod/test/test_gpio_driver.cpp b/panther_gpiod/test/test_gpio_driver.cpp index d24fbeb53..b69565dda 100644 --- a/panther_gpiod/test/test_gpio_driver.cpp +++ b/panther_gpiod/test/test_gpio_driver.cpp @@ -98,9 +98,9 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail) TEST_F(TestGPIODriver, SetWrongPinValue) { - panther_utils::test_utils::ExpectThrowWithDescription( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->SetPinValue(static_cast(-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( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { 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( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { 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); diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md index d87f5d8f7..5307180e2 100644 --- a/panther_hardware_interfaces/README.md +++ b/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) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 5bb267eee..2e1e9a475 100644 --- a/panther_lights/CMakeLists.txt +++ b/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 $ + $) + 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 $ + $) + 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 $ + $) + 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 $ + $) + + 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 $ + $) + 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 $ + $) + 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 $ + $) + 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 $ + $) + 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 $ + $) + 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() diff --git a/panther_lights/LIGHTS_API.md b/panther_lights/LIGHTS_API.md new file mode 100644 index 000000000..a9089f35a --- /dev/null +++ b/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 + +#include + +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 + +#include + +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(param); +} + +std::vector MyCoolAnimation::UpdateFrame() +{ + return std::vector(this->GetNumberOfLeds() * 4, value_); +} + +#include + +PLUGINLIB_EXPORT_CLASS(my_package::MyCoolAnimation, my_package::Animation) + +``` diff --git a/panther_lights/README.md b/panther_lights/README.md index fdf385599..27d0d7744 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -59,6 +59,217 @@ This node is responsible for displaying frames on the Husarion Panther robot's B - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**. - `~num_led` [*int*, default: **46**]: number of LEDs in a single bumper. +[//]: # (ROS_API_NODE_PARAMETERS_END) +[//]: # (ROS_API_NODE_END) + +[//]: # (ROS_API_NODE_START) +[//]: # (ROS_API_NODE_COMPATIBLE_1_0) +[//]: # (ROS_API_NODE_COMPATIBLE_1_2) +[//]: # (ROS_API_NODE_NAME_START) + +### controller_node + +[//]: # (ROS_API_NODE_NAME_END) +[//]: # (ROS_API_NODE_DESCRIPTION_START) + +This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. + +[//]: # (ROS_API_NODE_DESCRIPTION_END) + +#### Publishers + +[//]: # (ROS_API_NODE_PUBLISHERS_START) + +- `/panther/lights/driver/front_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. +- `/panther/lights/driver/rear_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. + +[//]: # (ROS_API_NODE_PUBLISHERS_END) + +#### Service Servers + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) + +- `/panther/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) + +#### Parameters + +[//]: # (ROS_API_NODE_PARAMETERS_START) + +- `~controller_frequency` [*float*, default: **50.0**]: frequency [Hz] at which the lights controller node will process animations. +- `~led_config_file` [*string*, default: **$(find panther_lights)/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. +- `~user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. + [//]: # (ROS_API_NODE_PARAMETERS_END) [//]: # (ROS_API_NODE_END) [//]: # (ROS_API_PACKAGE_END) + +## LED configuration + +Basic led configuration is loaded from [`led_config.yaml`](config/led_config.yaml) file. It includes definition of robot panels, virtual segments and default animations. + +### Panels + +The `panels` section of the YAML file lists all the physical LED panels on the robot. Each panel has two attributes: + +- `channel` [*int*, default: **None**] the identifier for the LED panel. It is used to differentiate between multiple panels. +- `number_of_leds`: defines the total number of LEDs present on the panel. + +### Segments + +The `segments` section is used to create virtual segments on the robot by dividing the LED panels into different parts. This allows for more precise control over which LEDs are lit up for different effects or indicators. Each segment has three attributes: + +- `name`: the identifier for the segment, such as "front" or "rear". It is used to differentiate between multiple segments. +- `channel`: This specifies which LED panel the segment belongs to. It have to match one of the channels defined in the `panels` section. +- `led_range`: This defines the range of LEDs within the panel that the segment covers. The range is specified as a start-end pair (e.g. 0-45). The range can be specified in reverse order (e.g. 45-0), which may be useful for wiring or orientation reasons. + +### Segments map + +The `segments_map` section allows creating named groups of segments on which animations can be displayed. Each entry under `segments_map` consists of a key representing the group name and a list of segments included in the group. Segment names have to match one of the segments defined in the `segments` section. By default you can use provided mapping: + +- `all` [*list*, default: **None**]: Grouping both `front` and `rear` segments together. +- `front` [*list*, default: **None**]: Containing only the `front` segment. +- `rear` [*list*, default: **None**]: Containing only the `rear` segment. + +### Animations + +The `led_animations` section contains list with definitions for various animations that can be displayed on the LED segments. Supported keys are: + +- `animations` [*list*, default: **None**]: definition of animation for each Bumper Lights. Supported keys are: + - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `panther_lights::ImageAnimation`, `panther_lights::ChargingAnimation`. + - `segments` [*string*, default **None**]: Indicates which segment mapping this particular animation applies to (e.g., all, front, rear). + - `animation` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section. +- `id` [*int*, default: **None**]: unique ID of an animation. +- `name` [*string*, default: **ANIMATION_**]: name of an animation. If not provided will default to **ANIMATION_**, where `` is equal to `id` parameter of the given animation. +- `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: + - **1** interrupts and removes animation with priorities **2** and **3**. + - **2** interrupts animations with priority **3**. + - **3** adds animation to the end of the queue. +- `timeout` [*float*, default: **120.0**]: time in **[s]**, after which animation will be removed from the queue. + +Default animations can be found in the table below: + +| ID | NAME | PRIORITY | DESCRIPTION | +| :-: | ----------------- | :------: | ----------------------------------------------------------- | +| 0 | E_STOP | 3 | red expanding from the center to the edges | +| 1 | READY | 3 | green expanding from center to the edges | +| 2 | ERROR | 1 | red, whole bumper blinking twice | +| 3 | MANUAL_ACTION | 3 | blue expanding from the center to the edges | +| 4 | AUTONOMOUS_ACTION | 3 | orange expanding from center to the edges | +| 5 | GOAL_ACHIEVED | 2 | purple, whole bumper blinking three times | +| 6 | LOW_BATTERY | 2 | two orange stripes moving towards the center, repeats twice | +| 7 | CRITICAL_BATTERY | 2 | two red stripes moving towards the center, repeats twice | +| 9 | CHARGING_BATTERY | 3 | whole bumper blinks with a duty cycle proportional to the Battery percentage. Short blinking means low Battery, and no blinking means full Battery. The color changes from red to green | + +### Animation Types + +#### Animation + +Basic animation definition. Keys are inherited from the basic **Animation** class by all animations. Supported keys are: + +- `brightness` [*float*, default: **1.0**]: animation brightness relative to APA102C driver `global_brightness`. The range between **[0.0, 1.0]**. +- `duration` [*float*, default: **None**]: duration of the animation. +- `repeat` [*int*, default: **1**]: number of times the animation will be repeated. + +> [!NOTE] +> Overall display duration of an animation is a product of a single image duration and repeat count. The result of `duration` x `repeat` can't exceed 10 **[s]**. If animation fails to fulfill the requirement it will result in an error. + +#### ImageAnimation + +Animation of type `panther_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys: + +- `color` [*int*, default: **None**]: The image is turned into grayscale, and then the color is applied with brightness from the gray image. Values have to be in HEX format. This parameter is not required. +- `image` [*string*, default: **None**]: path to an image file. Only global paths are valid. Allows using `$(find ros_package)` syntax. + +#### ChargingAnimation + +Animation of type `panther_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged). + +### Defining Animations + +Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations. + +Create a YAML file with an animation description list. Example file: + +```yaml +# my_awesome_user_animations.yaml +user_animations: + # animation with default image and custom color + - id: 21 + name: ANIMATION_1 + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_red.png + duration: 2 + repeat: 2 + color: 0xffff00 + + # animation with custom image + - id: 22 + name: ANIMATION_2 + priority: 3 + animation: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: /animations/custom_image.png + duration: 3 + repeat: 1 + + # animation with a custom image from custom ROS package + - id: 23 + name: ANIMATION_3 + priority: 3 + animation: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find my_custom_animation_package)/animations/custom_image.png + duration: 3 + repeat: 1 + + # different animations for Front and Rear Bumper Light + - id: 24 + name: ANIMATION_4 + priority: 3 + animation: + - type: panther_lights::ImageAnimation + segments: front + animation: + image: $(find panther_lights)/animations/triangle01_blue.png + duration: 2 + repeat: 2 + - type: panther_lights::ImageAnimation + segments: rear + animation: + image: $(find panther_lights)/animations/triangle01_red.png + duration: 3 + repeat: 1 +``` + +> [!NOTE] +> ID numbers from 0 to 19 are reserved for system animations. + +> [!NOTE] +> Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**. + +Remember to modify launch command to use user animations: + +``` bash +ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml +``` + +Test new animations: + +```bash +ros2 service call /lights/controller/set/animation panther_msgs/srv/SetLEDAnimation "{animation: {id: 0, param: ''}, repeating: true}"" +``` + +--- +## Defining a Custom Animation Type + +It is possible to define your own animation type with expected, new behavior. For more information, see: [**Animation API**](./LIGHTS_API.md). diff --git a/panther_lights/animations/battery_critical.png b/panther_lights/animations/battery_critical.png new file mode 100644 index 000000000..d431f77e6 Binary files /dev/null and b/panther_lights/animations/battery_critical.png differ diff --git a/panther_lights/animations/battery_low.png b/panther_lights/animations/battery_low.png new file mode 100644 index 000000000..bfdc182f7 Binary files /dev/null and b/panther_lights/animations/battery_low.png differ diff --git a/panther_lights/animations/strip01_purple.png b/panther_lights/animations/strip01_purple.png new file mode 100644 index 000000000..bf4df0a71 Binary files /dev/null and b/panther_lights/animations/strip01_purple.png differ diff --git a/panther_lights/animations/strip01_red.png b/panther_lights/animations/strip01_red.png new file mode 100644 index 000000000..5886cfe10 Binary files /dev/null and b/panther_lights/animations/strip01_red.png differ diff --git a/panther_lights/animations/triangle01_blue.png b/panther_lights/animations/triangle01_blue.png new file mode 100644 index 000000000..a57821945 Binary files /dev/null and b/panther_lights/animations/triangle01_blue.png differ diff --git a/panther_lights/animations/triangle01_cyan.png b/panther_lights/animations/triangle01_cyan.png new file mode 100644 index 000000000..fdfc508bc Binary files /dev/null and b/panther_lights/animations/triangle01_cyan.png differ diff --git a/panther_lights/animations/triangle01_green.png b/panther_lights/animations/triangle01_green.png new file mode 100644 index 000000000..553f2ceb2 Binary files /dev/null and b/panther_lights/animations/triangle01_green.png differ diff --git a/panther_lights/animations/triangle01_orange.png b/panther_lights/animations/triangle01_orange.png new file mode 100644 index 000000000..5b5238538 Binary files /dev/null and b/panther_lights/animations/triangle01_orange.png differ diff --git a/panther_lights/animations/triangle01_purple.png b/panther_lights/animations/triangle01_purple.png new file mode 100644 index 000000000..54829b091 Binary files /dev/null and b/panther_lights/animations/triangle01_purple.png differ diff --git a/panther_lights/animations/triangle01_red.png b/panther_lights/animations/triangle01_red.png new file mode 100644 index 000000000..3a1ba0789 Binary files /dev/null and b/panther_lights/animations/triangle01_red.png differ diff --git a/panther_lights/animations/triangle01_yellow.png b/panther_lights/animations/triangle01_yellow.png new file mode 100644 index 000000000..5c478584f Binary files /dev/null and b/panther_lights/animations/triangle01_yellow.png differ diff --git a/panther_lights/config/led_config.yaml b/panther_lights/config/led_config.yaml new file mode 100644 index 000000000..4860ad504 --- /dev/null +++ b/panther_lights/config/led_config.yaml @@ -0,0 +1,116 @@ +panels: + - channel: 1 + number_of_leds: 46 + - channel: 2 + number_of_leds: 46 + +segments: + - name: front + channel: 1 + led_range: 0-45 + - name: rear + channel: 2 + led_range: 45-0 + +segments_map: + all: [front, rear] + front: [front] + rear: [rear] + +led_animations: + - id: 0 + name: E_STOP + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_red.png + duration: 2 + + - id: 1 + name: READY + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_green.png + duration: 2 + + - id: 2 + name: ERROR + priority: 1 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_red.png + duration: 3 + repeat: 2 + + - id: 3 + name: MANUAL_ACTION + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_blue.png + duration: 3 + + - id: 4 + name: AUTONOMOUS_ACTION + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_orange.png + duration: 3 + + - id: 5 + name: GOAL_ACHIEVED + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_purple.png + duration: 3 + repeat: 2 + + - id: 6 + name: LOW_BATTERY + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/battery_low.png + duration: 2 + repeat: 2 + + - id: 7 + name: CRITICAL_BATTERY + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/battery_critical.png + duration: 2 + repeat: 2 + + # The BATTERY_STATE animation is no longer supported. + # Animation ID: 8 was intentionally omitted to keep numbering compatibility with ROS 1. + + - id: 9 + name: CHARGING_BATTERY + priority: 3 + animations: + - type: panther_lights::ChargingAnimation + segments: all + animation: + duration: 3 + repeat: 2 diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/panther_lights/include/panther_lights/animation/animation.hpp new file mode 100644 index 000000000..e81eaca00 --- /dev/null +++ b/panther_lights/include/panther_lights/animation/animation.hpp @@ -0,0 +1,192 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_ANIMATION_HPP_ +#define PANTHER_LIGHTS_ANIMATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace panther_lights +{ + +class Animation +{ +public: + virtual ~Animation() {} + + /** + * @brief Initialize and verify if animation was correctly defined + * + * @param animation_description YAML description of animation + * @param num_led number of LEDs + * @param controller_frequency frequency at which animation will be updated + * + * @exception std::out_of_range or std::runtime_error if animation + * parameters defined in description are missing or are incorrect + */ + virtual void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) + { + Reset(); + num_led_ = num_led; + frame_ = std::vector(num_led_ * kRGBAColorLen, 0); + + auto duration = panther_utils::GetYAMLKeyValue(animation_description, "duration"); + if ((duration - std::numeric_limits::epsilon()) <= 0.0) { + throw std::out_of_range("Duration has to be positive"); + } + + loops_ = panther_utils::GetYAMLKeyValue(animation_description, "repeat", 1); + + if (duration * loops_ > 10.0) { + throw std::runtime_error("Animation display duration (duration * repeat) exceeds 10 seconds"); + } + + if (animation_description["brightness"]) { + auto brightness = animation_description["brightness"].as(); + if (brightness < 0.0 || brightness > 1.0) { + throw std::out_of_range("Brightness has to be in range <0,1>"); + } + brightness_ = static_cast(round(brightness * 255)); + } + + anim_len_ = int(round(duration * controller_frequency)); + full_anim_len_ = anim_len_ * loops_; + + if (anim_len_ < 1) { + throw std::runtime_error( + "Animation duration is too short to display with the current frequency"); + } + } + + /** + * @brief Update animation frame + * + * @exception std::runtime_error if UpdateFrame() method returns frame with invalid size + */ + void Update() + { + if (current_cycle_ >= loops_) { + std::fill(frame_.begin(), frame_.end(), 0); + return; + } + + auto frame = UpdateFrame(); + + if (frame.size() != num_led_ * kRGBAColorLen) { + throw std::runtime_error( + "Invalid frame size. Check animation UpdateFrame() method implementation"); + } + + anim_iteration_++; + progress_ = float(anim_iteration_ + anim_len_ * current_cycle_) / full_anim_len_; + + if (anim_iteration_ >= anim_len_) { + anim_iteration_ = 0; + current_cycle_++; + } + + if (current_cycle_ >= loops_) { + finished_ = true; + } + + frame_ = frame; + } + + /** + * @brief Reset animation, this will cause animation + * to start from the beginning if Update() method is invoked + */ + void Reset() + { + anim_iteration_ = 0; + current_cycle_ = 0; + finished_ = false; + progress_ = 0.0; + std::fill(frame_.begin(), frame_.end(), 0); + } + + /** + * @brief Return animation frame + * + * @param invert_frame_order if true will return frame with inverted RGBA values order (last 4 + * values will become first etc.) + * + * @return the newest animation frame, if animation is finished will return vector filled with 0 + */ + std::vector GetFrame(const bool invert_frame_order = false) + { + return invert_frame_order ? InvertRGBAFrame(frame_) : frame_; + } + + virtual void SetParam(const std::string & /*param*/){}; + + bool IsFinished() const { return finished_; } + std::size_t GetNumberOfLeds() const { return num_led_; } + std::uint8_t GetBrightness() const { return brightness_; } + float GetProgress() const { return progress_; } + + static constexpr std::size_t kRGBAColorLen = 4; + +protected: + Animation() {} + + /** + * @brief Abstract method that has to be implemented inside child class + * it should return RGBA animation frame with size equal to num_led_ * kRGBAColorLen + */ + virtual std::vector UpdateFrame() = 0; + + std::vector InvertRGBAFrame(const std::vector & frame) const + { + std::vector inverted_frame(frame.size()); + for (std::size_t i = 0; i < frame.size(); i += kRGBAColorLen) { + std::copy(frame.end() - i - kRGBAColorLen, frame.end() - i, inverted_frame.begin() + i); + } + return inverted_frame; + } + + std::size_t GetAnimationLength() const { return anim_len_; } + std::size_t GetAnimationIteration() const { return anim_iteration_; } + +private: + std::size_t num_led_; + std::size_t anim_len_; + std::size_t full_anim_len_; + + bool finished_ = false; + float progress_ = 0.0; + std::size_t loops_; + std::size_t current_cycle_ = 0; + std::size_t anim_iteration_ = 0; + std::uint8_t brightness_ = 255; + + std::string param_; + std::vector frame_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/charging_animation.hpp b/panther_lights/include/panther_lights/animation/charging_animation.hpp new file mode 100644 index 000000000..8b29b2651 --- /dev/null +++ b/panther_lights/include/panther_lights/animation/charging_animation.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +#define PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ + +#include +#include +#include +#include + +#include + +#include + +namespace panther_lights +{ + +class ChargingAnimation : public Animation +{ +public: + ChargingAnimation() {} + ~ChargingAnimation() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override; + + void SetParam(const std::string & param) override; + +protected: + std::vector UpdateFrame() override; + + std::array HSVtoRGB(const float h, const float s, const float v) const; + std::vector CreateRGBAFrame( + const std::array & color, const float brightness) const; + +private: + static constexpr float kFadeFactor = 0.15; + static constexpr float kHMin = 0.0; + static constexpr float kHMax = 120.0; + + std::size_t fade_duration_; + std::size_t fill_start_; + std::size_t fill_end_; + std::array color_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/image_animation.hpp b/panther_lights/include/panther_lights/animation/image_animation.hpp new file mode 100644 index 000000000..008a6d03a --- /dev/null +++ b/panther_lights/include/panther_lights/animation/image_animation.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ +#define PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace gil = boost::gil; + +namespace panther_lights +{ + +class ImageAnimation : public Animation +{ +public: + ImageAnimation() {} + ~ImageAnimation() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override; + +protected: + std::vector UpdateFrame() override; + + /** + * @brief Process raw image path including extracting ros package shared directory path specified + * with '$(find ros_package)` syntax + * + * @param image_path raw path to an image, it should be a global path or should contain '$(find + * ros_package)` syntax + * + * @return global path to an image file + * @exception std::runtime_error if provided image_path is invalid or file does not exists + */ + std::filesystem::path ParseImagePath(const std::string & image_path) const; + + gil::rgba8_image_t RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) const; + + /** + * @brief This method converts RGB image to gray, normalizes gray image brightness and then + * applies provided color + * + * @param image RGB image that will be converted + * @param color 24-bit RGB color + */ + void RGBAImageConvertColor(gil::rgba8_image_t & image, const std::uint32_t color) const; + + gil::gray_alpha8_image_t RGBAImageConvertToGrey(const gil::rgba8_image_t & image) const; + + void GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) const; + +private: + gil::rgba8_image_t image_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index edbfb1e07..36a8386da 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -62,7 +62,7 @@ class APA102 * @brief Creates buffer with BGR format with structure appropriate for * the SPI transfer based on a given RGBA frame * - * @returns buffer vector. + * @return buffer vector. * * @exception std::runtime_error if frame has incorrect number of bytes */ diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp new file mode 100644 index 000000000..25a51dd36 --- /dev/null +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -0,0 +1,169 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ +#define PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ + +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include +#include +#include + +namespace panther_lights +{ + +using ImageMsg = sensor_msgs::msg::Image; +using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; + +class ControllerNode : public rclcpp::Node +{ +public: + ControllerNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~ControllerNode() {} + +protected: + /** + * @brief Initializes LED panel based on YAML description. This LED panel is a representation of + * the real panel of the robot + * + * @param panels_description YAML description of the panel, must contain 'channel' and + * 'number_of_leds' keys + * + * @exception std::runtime_error if initialization of the LED panel fails + */ + void InitializeLEDPanels(const YAML::Node & panels_description); + + /** + * @brief Initializes LED segment based on YAML description. This LED segment is a representation + * of the abstract segment located on the panel of the robot + * + * @param segments_description YAML description of the segment, must contain 'name', 'channel' and + * 'led_range' keys + * @param controller_freq Frequency at which animations will be processed + * + * @exception std::runtime_error if initialization of the LED segment fails + */ + void InitializeLEDSegments(const YAML::Node & segments_description, const float controller_freq); + + /** + * @brief Initializes LED segments map based on YAML description. This assigns list with segments + * names to abstract names that can be used with animations to specify on which segments animation + * should be displayed + * + * @param segments_map_description YAML description of the segments map + */ + void InitializeLEDSegmentsMap(const YAML::Node & segments_map_description); + + /** + * @brief Adds animations to an unordered map with animations + * + * @param animations_description YAML description with list of animations + * + * @exception std::runtime_error if fails to load an animation or animation with given ID already + * exists in the map + */ + void LoadDefaultAnimations(const YAML::Node & animations_description); + + /** + * @brief Adds animations to an unordered map with animations + * + * @param user_led_animations_file path to YAML file with user animations description + */ + void LoadUserAnimations(const std::string & user_led_animations_file); + + /** + * @brief Adds animation to an unordered map with animations + * + * @param animations_description YAML description of the animation + * + * @exception std::runtime_error if fails to load an animation or animation with given ID already + * exists in the map + */ + void LoadAnimation(const YAML::Node & animation_description); + + /** + * @brief Sets animation with requested ID + * + * @param animation_id Animation ID + * + * @exception std::runtime_error animation with give ID does not exists or (to be updated) + */ + void SetAnimation(const std::size_t animation_id, const bool repeating); + + /** + * @brief Updates all segments animations, converts then into panel frames and publishes panel + * frames on respective topics + */ + void UpdateAndPublishAnimation(); + + /** + * @brief Add animation to LED animations queue + * + * @param animation_id ID of the animations + * @param repeating Whether animations should repeat + * @param param Optional animation parameter + * + * @exception std::runtime_error if no animation with given ID exists + */ + void AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param); + + /** + * @brief Add animations to LED segments based on LED animation + * + * @param led_animation LED animation + * + * @exception std::runtime_error animation has invalid segment name or it fails to load + */ + void SetLEDAnimation(const std::shared_ptr & led_animation); + + std::shared_ptr current_animation_; + std::shared_ptr animations_queue_; + +private: + void PublishPanelFrame(const std::size_t channel); + void SetLEDAnimationCB( + const SetLEDAnimationSrv::Request::SharedPtr & request, + SetLEDAnimationSrv::Response::SharedPtr response); + void ControllerTimerCB(); + + std::unordered_map> led_panels_; + std::unordered_map::SharedPtr> panel_publishers_; + std::unordered_map> segments_; + std::unordered_map> segments_map_; + std::unordered_map animations_descriptions_; + std::shared_ptr segment_converter_; + + rclcpp::Service::SharedPtr set_led_animation_server_; + rclcpp::TimerBase::SharedPtr controller_timer_; + + bool animation_finished_ = true; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ diff --git a/panther_lights/include/panther_lights/dummy_scheduler_node.hpp b/panther_lights/include/panther_lights/dummy_scheduler_node.hpp index 2a420da96..644755f9f 100644 --- a/panther_lights/include/panther_lights/dummy_scheduler_node.hpp +++ b/panther_lights/include/panther_lights/dummy_scheduler_node.hpp @@ -25,19 +25,14 @@ #include #include +#include + namespace panther_lights { using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; - -struct RGBAColor -{ - std::uint8_t r; - std::uint8_t g; - std::uint8_t b; - std::uint8_t a; -}; +using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; class SchedulerNode : public rclcpp::Node { @@ -45,27 +40,20 @@ class SchedulerNode : public rclcpp::Node SchedulerNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - void Initialize(); - private: void ControllerTimerCB(); - void PublishColor(const RGBAColor & color); - - static constexpr RGBAColor kColorRed = {255, 0, 0, 255}; - static constexpr RGBAColor kColorGreen = {0, 255, 0, 255}; - static constexpr RGBAColor kColorOrange = {255, 140, 0, 255}; + void CallSetLEDAnimationSrv(const std::uint16_t animation_id); bool e_stop_state_ = true; int num_led_; float battery_percentage_; + std::uint16_t current_anim_id_; rclcpp::TimerBase::SharedPtr controller_timer_; rclcpp::Subscription::SharedPtr e_stop_sub_; rclcpp::Subscription::SharedPtr battery_state_sub_; - std::shared_ptr it_; - std::shared_ptr rear_light_pub_; - std::shared_ptr front_light_pub_; + rclcpp::Client::SharedPtr set_animation_client_; }; } // namespace panther_lights diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp new file mode 100644 index 000000000..8b9d8e36f --- /dev/null +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -0,0 +1,194 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ +#define PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +/** + * @brief Structure describing basic animation, including its type, description and list of segments + * it will be assigned to + */ +struct AnimationDescription +{ + std::string type; + std::vector segments; + YAML::Node animation; +}; + +/** + * @brief Structure describing a complete LED animation, containing its ID, priority, name, timeout + * and, a list of animations that will be displayed on LED segments + */ +struct LEDAnimationDescription +{ + std::uint8_t id; + std::uint8_t priority; + std::string name; + float timeout; + std::vector animations; +}; + +/** + * @brief Class representing animation that can be displayed on robot segments + */ +class LEDAnimation +{ +public: + /** + * @brief Initializes LED animation + * + * @param led_animation_description YAML description of the LED animation + * @param segments This parameter is used to create map of segments used by this LED animation + * @param init_time Time of creation of the LED animation + */ + LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time); + + ~LEDAnimation() {} + + /** + * @brief Indicates if LED animation is finished + * + * @return True if all animations on all segments are finished, false otherwise + */ + bool IsFinished(); + + /** + * @brief Reset all animations on all LED segments + * + * @param time This time is used to set new animation initialization time + */ + void Reset(const rclcpp::Time & time); + + std::string GetName() const { return led_animation_description_.name; } + std::uint8_t GetPriority() const { return led_animation_description_.priority; } + std::vector GetAnimations() const + { + return led_animation_description_.animations; + } + rclcpp::Time GetInitTime() const { return init_time_; } + float GetTimeout() const { return led_animation_description_.timeout; } + + /** + * @brief Get LED animation progress + * + * @return The smallest progress of all animations on all segments + */ + float GetProgress() const; + + bool IsRepeating() const { return repeating_; } + std::string GetParam() const { return param_; } + + void SetRepeating(const bool value) { repeating_ = value; } + void SetParam(const std::string & param) { param_ = param; } + + static constexpr std::uint8_t kDefaultPriority = 3; + static constexpr float kDefaultTimeout = 120.0f; + static constexpr std::array kValidPriorities = {1, 2, 3}; + +private: + const LEDAnimationDescription led_animation_description_; + rclcpp::Time init_time_; + + bool repeating_; + std::string param_; + std::vector> animation_segments_; +}; + +/** + * @brief Class used to manage queue of LED animations + */ +class LEDAnimationsQueue +{ +public: + /** + * @brief Initializes LED animations queue + * + * @param max_queue_size Max size of the queue + */ + LEDAnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} + + /** + * @brief Add animation to the queue and sort animations according to their priority and time of + * initialization + * + * @param animation LED animation to add to queue + * @param time Time of initialization of the animation + * + * @exception std::runtime_error if queue has number of elements equal to max_queue_size + */ + void Put(const std::shared_ptr & animation, const rclcpp::Time & time); + + /** + * @brief Get and remove first LED animation from the queue + * + * @return First LED animation from the queue + * + * @exception std::runtime_error if queue is empty + */ + std::shared_ptr Get(); + + /** + * @brief Remove all animations with priority equal or lower to specified one + * + * @param priority Animation with this priority or lower will be removed from the queue + */ + void Clear(const std::size_t priority = 2); + + /** + * @brief Removes animations that has reached theirs timeout from the queue + * + * @param time Time used to check if animation has timed out + */ + void Validate(const rclcpp::Time & time); + + /** + * @brief Return priority of the first animation in the queue + * + * @return Priority of the first animation or default animation priority if queue is empty + */ + std::size_t GetFirstAnimationPriority() const; + + void Remove(const std::shared_ptr & animation); + bool HasAnimation(const std::shared_ptr & animation) const; + + bool Empty() const { return queue_.empty(); } + +private: + std::deque> queue_; + const std::size_t max_queue_size_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ diff --git a/panther_lights/include/panther_lights/led_panel.hpp b/panther_lights/include/panther_lights/led_panel.hpp new file mode 100644 index 000000000..02ab5f159 --- /dev/null +++ b/panther_lights/include/panther_lights/led_panel.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_PANEL_HPP_ +#define PANTHER_LIGHTS_LED_PANEL_HPP_ + +#include +#include + +namespace panther_lights +{ + +/** + * @brief Class that represents LED panel of the robot + */ +class LEDPanel +{ +public: + LEDPanel(const std::size_t num_led); + + ~LEDPanel() = default; + + /** + * @brief Updates LED panel frame + * + * @param iterator_first position at which values will be inserted + * @param values vector with values that will be inserted into the frame + * + * @exception std::runtime_error if values vector is empty or can't be fit into the farme + */ + void UpdateFrame(const std::size_t iterator_first, const std::vector & values); + + std::vector GetFrame() const { return frame_; } + std::size_t GetNumberOfLeds() const { return num_led_; } + +private: + const std::size_t num_led_; + std::vector frame_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_PANEL_HPP_ diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp new file mode 100644 index 000000000..2bda77dac --- /dev/null +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -0,0 +1,137 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_SEGMENT_HPP_ +#define PANTHER_LIGHTS_LED_SEGMENT_HPP_ + +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +/** + * @brief Class that represents virtual LED segment of the robot + */ +class LEDSegment +{ +public: + /** + * @brief Parses basic parameters of the LED segment + * + * @param segment_description YAML description of the segment. Must contain given keys: + * - led_range (string) - two numbers with hyphen in between, eg.: '0-45', + * - channel (int) - id of physical LED channel to which segment is assigned. + * @param controller_frequency frequency at which animation will be updated. + * + * @exception std::runtime_error or std::invalid_argument if missing required description key or + * key couldn't be parsed + */ + LEDSegment(const YAML::Node & segment_description, const float controller_frequency); + + ~LEDSegment(); + + /** + * @brief Overwrite current animation + * + * @param animation_description YAML description of the animation. Must contain 'type' key - + * pluginlib animation type + * @param repeating if true, will set the default animation for the panel + * + * @exception std::runtime_error if 'type' key is missing, given pluginlib fails to load or + * animation fails to initialize + */ + void SetAnimation( + const std::string & type, const YAML::Node & animation_description, const bool repeating, + const std::string & param = ""); + + /** + * @brief Update animation frame + * + * @param param optional parameter to pass to animation when updating + * + * @exception std::runtime_error if fails to update animation + */ + void UpdateAnimation(); + + /** + * @brief Check if animation is finished. This does not return state of the default animation + * + * @return True if animation is finished, false otherwise + */ + bool IsAnimationFinished() const { return animation_finished_; } + + /** + * @brief Get current animation frame + * + * @return Current animation frame or default animation frame if it was defined and the main + * animation is finished + * @exception std::runtime_error if segment animation is not defined + */ + std::vector GetAnimationFrame() const; + + /** + * @brief Get current animation progress + * + * @return Current animation progress + * + * @exception std::runtime_error if segment animation is not defined + */ + float GetAnimationProgress() const; + + /** + * @brief Reset current animation + * + * @exception std::runtime_error if segment animation is not defined + */ + void ResetAnimation(); + + /** + * @brief Get current animation brightness + * + * @exception std::runtime_error if segment animation is not defined + */ + std::uint8_t GetAnimationBrightness() const; + + std::size_t GetFirstLEDPosition() const; + + std::size_t GetChannel() const { return channel_; } + + bool HasAnimation() const { return animation_ || default_animation_; } + +protected: + std::shared_ptr animation_; + std::shared_ptr default_animation_; + +private: + const float controller_frequency_; + bool invert_led_order_ = false; + bool animation_finished_ = true; + std::size_t channel_; + std::size_t first_led_iterator_; + std::size_t last_led_iterator_; + std::size_t num_led_; + + std::shared_ptr> animation_loader_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_SEGMENT_HPP_ diff --git a/panther_lights/include/panther_lights/segment_converter.hpp b/panther_lights/include/panther_lights/segment_converter.hpp new file mode 100644 index 000000000..214bc1d32 --- /dev/null +++ b/panther_lights/include/panther_lights/segment_converter.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ +#define PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ + +#include +#include +#include + +#include +#include + +namespace panther_lights +{ + +class SegmentConverter +{ +public: + SegmentConverter() = default; + ~SegmentConverter() = default; + + void Convert( + const std::unordered_map> & segments, + const std::unordered_map> & panels); +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index 988a9cb58..d32dd2219 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -16,11 +16,25 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import Shutdown +from launch.actions import DeclareLaunchArgument, Shutdown +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): + led_config_file = LaunchConfiguration("led_config_file") + declare_led_config_file_arg = DeclareLaunchArgument( + "led_config_file", + 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", + ) + lights_driver_node = Node( package="panther_lights", executable="driver_node", @@ -28,6 +42,17 @@ def generate_launch_description(): on_exit=Shutdown(), ) + lights_controller_node = Node( + package="panther_lights", + executable="controller_node", + name="lights_controller_node", + parameters=[ + {"led_config_file": led_config_file}, + {"user_led_animations_file": user_led_animations_file}, + ], + on_exit=Shutdown(), + ) + dummy_scheduler_node = Node( package="panther_lights", executable="dummy_scheduler_node", @@ -35,7 +60,10 @@ def generate_launch_description(): ) actions = [ + declare_led_config_file_arg, + declare_user_led_animations_file_arg, lights_driver_node, + lights_controller_node, dummy_scheduler_node, ] diff --git a/panther_lights/package.xml b/panther_lights/package.xml index dda5ee790..265713204 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -18,8 +18,11 @@ diagnostic_updater image_transport + libpng-dev panther_gpiod panther_msgs + panther_utils + pluginlib rclcpp sensor_msgs diff --git a/panther_lights/plugins.xml b/panther_lights/plugins.xml new file mode 100644 index 000000000..11f0ed750 --- /dev/null +++ b/panther_lights/plugins.xml @@ -0,0 +1,8 @@ + + + Animation processed from an image + + + Charging animation representing percentage of battery + + diff --git a/panther_lights/src/animation/charging_animation.cpp b/panther_lights/src/animation/charging_animation.cpp new file mode 100644 index 000000000..356836f46 --- /dev/null +++ b/panther_lights/src/animation/charging_animation.cpp @@ -0,0 +1,160 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace panther_lights +{ + +void ChargingAnimation::Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) +{ + Animation::Initialize(animation_description, num_led, controller_frequency); + + fade_duration_ = int(std::round(this->GetAnimationLength() * kFadeFactor)); +} + +void ChargingAnimation::SetParam(const std::string & param) +{ + float battery_percent; + try { + battery_percent = std::clamp(std::stof(param), 0.0f, 1.0f); + } catch (const std::invalid_argument & /*e*/) { + throw std::runtime_error("Can not cast param to float!"); + } + + const auto anim_len = this->GetAnimationLength(); + std::size_t on_duration = static_cast(std::round(float(anim_len) * battery_percent)); + + on_duration = on_duration < 2 * fade_duration_ ? 2 * fade_duration_ : on_duration; + + if (on_duration >= anim_len) { + fade_duration_ = 0; + } + + fill_start_ = (anim_len - on_duration) / 2; + fill_end_ = (anim_len + on_duration) / 2; + const float hue = (kHMax - kHMin) * battery_percent + kHMin; + color_ = HSVtoRGB(hue / 360.0, 1.0, 1.0); +} + +std::vector ChargingAnimation::UpdateFrame() +{ + auto anim_iteration = this->GetAnimationIteration(); + auto empty_frame = std::vector(this->GetNumberOfLeds() * 4, 0); + for (std::size_t i = 3; i < empty_frame.size(); i += 4) { + empty_frame[i] = 255; + } + + if (anim_iteration < fill_start_) { + return empty_frame; + } + + if (anim_iteration < fill_start_ + fade_duration_) { + auto brightness = std::sin(M_PI_2 * (float(anim_iteration - fill_start_) / fade_duration_)); + return CreateRGBAFrame(color_, brightness); + } + + if (anim_iteration <= fill_end_ - fade_duration_) { + return CreateRGBAFrame(color_, 1.0); + } + + if (anim_iteration < fill_end_) { + auto brightness = std::sin( + M_PI / (2.0 * fade_duration_) * + ((float(anim_iteration) - float(fill_end_)) + 2.0 * fade_duration_)); + return CreateRGBAFrame(color_, brightness); + } + + return empty_frame; +} + +std::array ChargingAnimation::HSVtoRGB( + const float h, const float s, const float v) const +{ + if (std::fabs(s) < std::numeric_limits::epsilon()) { + return { + {static_cast(v * 255), static_cast(v * 255), + static_cast(v * 255)}}; + } + + const int i = static_cast(std::floor(h * 6.0f)); + const float f = (h * 6.0f) - i; + const float p = v * (1.0f - s); + const float q = v * (1.0f - s * f); + const float t = v * (1.0f - s * (1.0f - f)); + + switch (i % 6) { + case 0: + return { + {static_cast(v * 255), static_cast(t * 255), + static_cast(p * 255)}}; + case 1: + return { + {static_cast(q * 255), static_cast(v * 255), + static_cast(p * 255)}}; + case 2: + return { + {static_cast(p * 255), static_cast(v * 255), + static_cast(t * 255)}}; + case 3: + return { + {static_cast(p * 255), static_cast(q * 255), + static_cast(v * 255)}}; + case 4: + return { + {static_cast(t * 255), static_cast(p * 255), + static_cast(v * 255)}}; + default: + return { + {static_cast(v * 255), static_cast(p * 255), + static_cast(q * 255)}}; + } +} + +std::vector ChargingAnimation::CreateRGBAFrame( + const std::array & color, const float brightness) const +{ + const std::array rgba_color = { + static_cast(color[0] * brightness), + static_cast(color[1] * brightness), + static_cast(color[2] * brightness), + 255, + }; + std::vector frame(this->GetNumberOfLeds() * rgba_color.size()); + + for (std::size_t i = 0; i < frame.size(); i += rgba_color.size()) { + std::copy(rgba_color.begin(), rgba_color.end(), frame.begin() + i); + } + + return frame; +} + +} // namespace panther_lights + +#include + +PLUGINLIB_EXPORT_CLASS(panther_lights::ChargingAnimation, panther_lights::Animation) diff --git a/panther_lights/src/animation/image_animation.cpp b/panther_lights/src/animation/image_animation.cpp new file mode 100644 index 000000000..b8463dd79 --- /dev/null +++ b/panther_lights/src/animation/image_animation.cpp @@ -0,0 +1,163 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace panther_lights +{ + +void ImageAnimation::Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) +{ + Animation::Initialize(animation_description, num_led, controller_frequency); + + const auto image_path = + ParseImagePath(panther_utils::GetYAMLKeyValue(animation_description, "image")); + gil::rgba8_image_t base_image; + gil::read_and_convert_image(std::string(image_path), base_image, gil::png_tag()); + image_ = RGBAImageResize(base_image, this->GetNumberOfLeds(), this->GetAnimationLength()); + + if (animation_description["color"]) { + RGBAImageConvertColor(image_, animation_description["color"].as()); + } +} + +std::vector ImageAnimation::UpdateFrame() +{ + std::vector frame; + for (std::size_t i = 0; i < this->GetNumberOfLeds(); i++) { + auto pixel = gil::const_view(image_)(i, this->GetAnimationIteration()); + frame.push_back(pixel[0]); + frame.push_back(pixel[1]); + frame.push_back(pixel[2]); + frame.push_back(pixel[3]); + } + + return frame; +} + +std::filesystem::path ImageAnimation::ParseImagePath(const std::string & image_path) const +{ + std::filesystem::path global_img_path; + + if (!std::filesystem::path(image_path).is_absolute()) { + if (image_path[0] != '$') { + throw std::runtime_error("Invalid image path"); + } + + std::smatch match; + if (!std::regex_search(image_path, match, std::regex("^\\$\\(find .*\\)"))) { + throw std::runtime_error("Can't process substitution expression"); + } + + const std::string ros_pkg_expr = match[0]; + const std::string ros_pkg = std::regex_replace( + ros_pkg_expr, std::regex("^\\$\\(find \\s*|\\)$"), ""); + const std::string img_relative_path = image_path.substr(ros_pkg_expr.length()); + + try { + global_img_path = ament_index_cpp::get_package_share_directory(ros_pkg) + img_relative_path; + } catch (const ament_index_cpp::PackageNotFoundError & e) { + throw std::runtime_error("Can't find ROS package: " + ros_pkg); + } + } else { + global_img_path = image_path; + } + + if (!std::filesystem::exists(global_img_path)) { + throw std::runtime_error("File doesn't exists: " + std::string(global_img_path)); + } + + return global_img_path; +} + +gil::rgba8_image_t ImageAnimation::RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) const +{ + gil::rgba8_image_t resized_image(width, height); + gil::resize_view(gil::const_view(image), view(resized_image), gil::bilinear_sampler()); + + return resized_image; +} + +void ImageAnimation::RGBAImageConvertColor( + gil::rgba8_image_t & image, const std::uint32_t color) const +{ + auto grey_image = RGBAImageConvertToGrey(image); + GreyImageNormalizeBrightness(grey_image); + + // extract RGB values from hex + auto r = (std::uint32_t(color) >> 16) & (0xFF); + auto g = (std::uint32_t(color) >> 8) & (0xFF); + auto b = (std::uint32_t(color)) & (0xFF); + + gil::transform_pixels( + gil::const_view(grey_image), gil::view(image), + [r, g, b](const gil::gray_alpha8_pixel_t & pixel) { + return gil::rgba8_pixel_t( + static_cast(pixel[0] * r / 255), + static_cast(pixel[0] * g / 255), + static_cast(pixel[0] * b / 255), pixel[1]); + }); +} + +gil::gray_alpha8_image_t ImageAnimation::RGBAImageConvertToGrey( + const gil::rgba8_image_t & image) const +{ + gil::gray_alpha8_image_t grey_image(image.dimensions()); + gil::transform_pixels( + gil::const_view(image), gil::view(grey_image), [](const gil::rgba8_pixel_t & pixel) { + return gil::gray_alpha8_pixel_t( + static_cast(0.299 * pixel[0] + 0.587 * pixel[1] + 0.114 * pixel[2]), + pixel[3]); + }); + return grey_image; +} + +void ImageAnimation::GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) const +{ + std::uint8_t max_value = *std::max_element( + gil::nth_channel_view(gil::const_view(image), 0).begin(), + gil::nth_channel_view(gil::const_view(image), 0).end()); + gil::transform_pixels( + gil::const_view(image), gil::view(image), [max_value](const gil::gray_alpha8_pixel_t & pixel) { + return gil::gray_alpha8_pixel_t( + static_cast(float(pixel[0]) / float(max_value) * 255), pixel[1]); + }); +} + +} // namespace panther_lights + +#include + +PLUGINLIB_EXPORT_CLASS(panther_lights::ImageAnimation, panther_lights::Animation) diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp new file mode 100644 index 000000000..ff3035409 --- /dev/null +++ b/panther_lights/src/controller_node.cpp @@ -0,0 +1,363 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace panther_lights +{ + +ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + using namespace std::placeholders; + + this->declare_parameter("led_config_file"); + this->declare_parameter("user_led_animations_file", ""); + this->declare_parameter("controller_freq", 50.0); + + const auto led_config_file = this->get_parameter("led_config_file").as_string(); + const auto user_led_animations_file = this->get_parameter("user_led_animations_file").as_string(); + const float controller_freq = this->get_parameter("controller_freq").as_double(); + + YAML::Node led_config_desc = YAML::LoadFile(led_config_file); + + InitializeLEDPanels(led_config_desc["panels"]); + InitializeLEDSegments(led_config_desc["segments"], controller_freq); + InitializeLEDSegmentsMap(led_config_desc["segments_map"]); + LoadDefaultAnimations(led_config_desc["led_animations"]); + + if (user_led_animations_file != "") { + LoadUserAnimations(user_led_animations_file); + } + + segment_converter_ = std::make_shared(); + + animations_queue_ = std::make_shared(10); + + set_led_animation_server_ = this->create_service( + "lights/controller/set/animation", std::bind(&ControllerNode::SetLEDAnimationCB, this, _1, _2)); + + controller_timer_ = this->create_wall_timer( + std::chrono::microseconds(static_cast(1e6 / controller_freq)), + std::bind(&ControllerNode::ControllerTimerCB, this)); + + RCLCPP_INFO(this->get_logger(), "Node started"); +} + +void ControllerNode::InitializeLEDPanels(const YAML::Node & panels_description) +{ + RCLCPP_INFO(this->get_logger(), "Initializing LED panels"); + + for (auto & panel : panels_description.as>()) { + const auto channel = panther_utils::GetYAMLKeyValue(panel, "channel"); + const auto number_of_leds = panther_utils::GetYAMLKeyValue( + panel, "number_of_leds"); + + const auto result = led_panels_.emplace(channel, std::make_unique(number_of_leds)); + if (!result.second) { + throw std::runtime_error( + "Multiple panels with channel nr '" + std::to_string(channel) + "' found"); + } + + const auto pub_result = panel_publishers_.emplace( + channel, this->create_publisher( + "lights/driver/channel_" + std::to_string(channel) + "_frame", 10)); + if (!pub_result.second) { + throw std::runtime_error( + "Multiple panel publishers for channel nr '" + std::to_string(channel) + "' found"); + } + + RCLCPP_INFO(this->get_logger(), "Successfully initialized panel with channel nr %li", channel); + } +} + +void ControllerNode::InitializeLEDSegments( + const YAML::Node & segments_description, const float controller_freq) +{ + RCLCPP_INFO(this->get_logger(), "Initializing LED segments"); + for (auto & segment : segments_description.as>()) { + const auto segment_name = panther_utils::GetYAMLKeyValue(segment, "name"); + + try { + const auto result = segments_.emplace( + segment_name, std::make_shared(segment, controller_freq)); + if (!result.second) { + throw std::runtime_error("Multiple segments with given name found"); + } + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to initialize '" + segment_name + "' segment: " + std::string(e.what())); + } catch (const std::invalid_argument & e) { + throw std::runtime_error( + "Failed to initialize '" + segment_name + "' segment: " + std::string(e.what())); + } + + RCLCPP_INFO(this->get_logger(), "Successfully initialized '%s' segment", segment_name.c_str()); + } +} + +void ControllerNode::InitializeLEDSegmentsMap(const YAML::Node & segments_map_description) +{ + for (const auto & key : segments_map_description) { + const auto name = key.first.as(); + const auto value = key.second.as>(); + segments_map_.emplace(name, value); + } +} + +void ControllerNode::LoadDefaultAnimations(const YAML::Node & animations_description) +{ + RCLCPP_INFO(this->get_logger(), "Loading users LED animations"); + + for (auto & animation_description : animations_description.as>()) { + LoadAnimation(animation_description); + } + + RCLCPP_INFO(this->get_logger(), "Animations successfully loaded"); +} + +void ControllerNode::LoadUserAnimations(const std::string & user_led_animations_file) +{ + RCLCPP_INFO(this->get_logger(), "Loading users LED animations"); + + try { + YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_file); + auto user_animations = panther_utils::GetYAMLKeyValue>( + user_led_animations, "user_animations"); + + for (auto & animation_description : user_animations) { + try { + auto id = panther_utils::GetYAMLKeyValue(animation_description, "id"); + if (id < 20) { + throw std::runtime_error("Animation ID must be greater than 19"); + } + + auto priority = panther_utils::GetYAMLKeyValue( + animation_description, "priority", LEDAnimation::kDefaultPriority); + if (priority == 1) { + throw std::runtime_error("User animation can not have priority 1"); + } + + LoadAnimation(animation_description); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + this->get_logger(), "Skipping user animation that failed to load: %s", e.what()); + } + } + } catch (const std::exception & e) { + RCLCPP_WARN(this->get_logger(), "Failed to load user animations: %s", e.what()); + } + + RCLCPP_INFO(this->get_logger(), "User animations successfully loaded"); +} + +void ControllerNode::LoadAnimation(const YAML::Node & animation_description) +{ + LEDAnimationDescription led_animation_desc; + + try { + led_animation_desc.id = panther_utils::GetYAMLKeyValue( + animation_description, "id"); + led_animation_desc.name = panther_utils::GetYAMLKeyValue( + animation_description, "name", "ANIMATION_" + std::to_string(led_animation_desc.id)); + led_animation_desc.priority = panther_utils::GetYAMLKeyValue( + animation_description, "priority", LEDAnimation::kDefaultPriority); + led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( + animation_description, "timeout", LEDAnimation::kDefaultTimeout); + + if ( + std::find( + LEDAnimation::kValidPriorities.begin(), LEDAnimation::kValidPriorities.end(), + led_animation_desc.priority) == LEDAnimation::kValidPriorities.end()) { + throw std::runtime_error("Invalid LED animation priority"); + } + + auto animations = panther_utils::GetYAMLKeyValue>( + animation_description, "animations"); + for (auto & animation : animations) { + AnimationDescription animation_desc; + animation_desc.type = panther_utils::GetYAMLKeyValue(animation, "type"); + animation_desc.animation = panther_utils::GetYAMLKeyValue(animation, "animation"); + + auto segments_group = panther_utils::GetYAMLKeyValue(animation, "segments"); + animation_desc.segments = segments_map_.at(segments_group); + + led_animation_desc.animations.push_back(animation_desc); + } + + const auto result = animations_descriptions_.emplace(led_animation_desc.id, led_animation_desc); + if (!result.second) { + throw std::runtime_error("Animation with given ID already exists"); + } + + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to load '" + led_animation_desc.name + "' animation: " + std::string(e.what())); + } +} + +void ControllerNode::SetLEDAnimationCB( + const SetLEDAnimationSrv::Request::SharedPtr & request, + SetLEDAnimationSrv::Response::SharedPtr response) +{ + try { + AddAnimationToQueue(request->animation.id, request->repeating, request->animation.param); + response->success = true; + } catch (const std::exception & e) { + response->success = false; + response->message = e.what(); + } +} + +void ControllerNode::PublishPanelFrame(const std::size_t channel) +{ + auto panel = led_panels_.at(channel); + const auto number_of_leds = panel->GetNumberOfLeds(); + + ImageMsg::UniquePtr image(new ImageMsg); + image->header.frame_id = "lights_channel_" + std::to_string(channel); + image->header.stamp = this->get_clock()->now(); + image->encoding = "rgba8"; + image->height = 1; + image->width = number_of_leds; + image->step = number_of_leds * 4; + image->data = panel->GetFrame(); + + panel_publishers_.at(channel)->publish(std::move(image)); +} + +void ControllerNode::ControllerTimerCB() +{ + if (animation_finished_) { + animations_queue_->Validate(this->get_clock()->now()); + + if (!animations_queue_->Empty()) { + try { + SetLEDAnimation(animations_queue_->Get()); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to Set LED animation: %s", e.what()); + } + } + } + + if (!current_animation_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for animation"); + return; + } + + if (current_animation_->GetPriority() > animations_queue_->GetFirstAnimationPriority()) { + if (current_animation_->GetProgress() < 0.65f) { + current_animation_->Reset(this->get_clock()->now()); + animations_queue_->Put(current_animation_, this->get_clock()->now()); + } + animation_finished_ = true; + return; + } + + UpdateAndPublishAnimation(); + + animation_finished_ = current_animation_->IsFinished(); +} + +void ControllerNode::UpdateAndPublishAnimation() +{ + std::vector> segments_vec; + + for (auto & [segment_name, segment] : segments_) { + try { + if (segment->HasAnimation()) { + segment->UpdateAnimation(); + } + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + this->get_logger(), "Failed to update animation on %s segment: %s", segment_name.c_str(), + e.what()); + } + } + + try { + segment_converter_->Convert(segments_, led_panels_); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), e.what()); + return; + } + + for (auto & panel : led_panels_) { + PublishPanelFrame(panel.first); + } +} + +void ControllerNode::AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param) +{ + if (animations_descriptions_.find(animation_id) == animations_descriptions_.end()) { + throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); + } + + auto animation_description = animations_descriptions_.at(animation_id); + auto animation = std::make_shared( + animation_description, segments_, this->get_clock()->now()); + animation->SetRepeating(repeating); + animation->SetParam(param); + animations_queue_->Put(animation, this->get_clock()->now()); +} + +void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_animation) +{ + const auto animations = led_animation->GetAnimations(); + for (auto & animation : animations) { + for (auto & segment : animation.segments) { + if (segments_.find(segment) == segments_.end()) { + throw std::runtime_error("No segment with name: " + segment); + } + + try { + segments_.at(segment)->SetAnimation( + animation.type, animation.animation, led_animation->IsRepeating(), + led_animation->GetParam()); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to set '" + led_animation->GetName() + "' animation: " + std::string(e.what())); + } + } + } + + current_animation_.reset(); + current_animation_ = std::move(led_animation); +} + +} // namespace panther_lights diff --git a/panther_lights/src/controller_node_main.cpp b/panther_lights/src/controller_node_main.cpp new file mode 100644 index 000000000..94d61ffdc --- /dev/null +++ b/panther_lights/src/controller_node_main.cpp @@ -0,0 +1,37 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto lights_controller_node = + std::make_shared("lights_controller_node"); + + try { + rclcpp::spin(lights_controller_node); + } catch (const std::runtime_error & err) { + std::cerr << "[lights_controller_node] Caught exception: " << err.what() << std::endl; + } + + std::cout << "[lights_controller_node] Shutting down" << std::endl; + rclcpp::shutdown(); + return 0; +} diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 14f2e4bbc..b12f1855e 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -68,6 +68,7 @@ void DriverNode::Initialize() std::vector gpio_info_storage = {panther_gpiod::GPIOInfo{ panther_gpiod::GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}}; gpio_driver_ = std::make_unique(gpio_info_storage); + gpio_driver_->GPIOMonitorEnable(); front_panel_ts_ = this->get_clock()->now(); rear_panel_ts_ = this->get_clock()->now(); @@ -76,13 +77,13 @@ void DriverNode::Initialize() rear_panel_.SetGlobalBrightness(global_brightness); front_light_sub_ = std::make_shared( - it_->subscribe("lights/driver/front_panel_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { + it_->subscribe("lights/driver/channel_1_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, front_panel_, front_panel_ts_, "front"); front_panel_ts_ = msg->header.stamp; })); rear_light_sub_ = std::make_shared( - it_->subscribe("lights/driver/rear_panel_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { + it_->subscribe("lights/driver/channel_2_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, rear_panel_, rear_panel_ts_, "rear"); rear_panel_ts_ = msg->header.stamp; })); @@ -101,37 +102,39 @@ void DriverNode::OnShutdown() // Give back control over LEDs SetPowerPin(false); + + gpio_driver_.reset(); } void DriverNode::FrameCB( const ImageMsg::ConstSharedPtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, const std::string & panel_name) { - std::string meessage; + std::string message; if ( (this->get_clock()->now() - rclcpp::Time(msg->header.stamp)) > rclcpp::Duration::from_seconds(frame_timeout_)) { - meessage = "Timeout exceeded, ignoring frame"; + message = "Timeout exceeded, ignoring frame"; } else if (rclcpp::Time(msg->header.stamp) < last_time) { - meessage = "Dropping message from past"; + message = "Dropping message from past"; } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { - meessage = "Incorrect image encoding ('" + msg->encoding + "')"; + message = "Incorrect image encoding ('" + msg->encoding + "')"; } else if (msg->height != 1) { - meessage = "Incorrect image height " + std::to_string(msg->height); + message = "Incorrect image height " + std::to_string(msg->height); } else if (msg->width != (std::uint32_t)num_led_) { - meessage = "Incorrect image width " + std::to_string(msg->width); + message = "Incorrect image width " + std::to_string(msg->width); } - if (!meessage.empty()) { + if (!message.empty()) { if (panel_name == "front") { RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "%s on front panel!", meessage.c_str()); + this->get_logger(), *this->get_clock(), 5000, "%s on front panel!", message.c_str()); } else if (panel_name == "rear") { RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "%s on rear panel!", meessage.c_str()); + this->get_logger(), *this->get_clock(), 5000, "%s on rear panel!", message.c_str()); } - auto warn_msg = meessage + " on " + panel_name + " panel!"; + auto warn_msg = message + " on " + panel_name + " panel!"; diagnostic_updater_.broadcast(diagnostic_msgs::msg::DiagnosticStatus::WARN, warn_msg); return; diff --git a/panther_lights/src/dummy_scheduler_node.cpp b/panther_lights/src/dummy_scheduler_node.cpp index 6b86c8e0d..5fac6bf81 100644 --- a/panther_lights/src/dummy_scheduler_node.cpp +++ b/panther_lights/src/dummy_scheduler_node.cpp @@ -23,6 +23,8 @@ #include #include +#include + namespace panther_lights { @@ -35,12 +37,8 @@ SchedulerNode::SchedulerNode(const std::string & node_name, const rclcpp::NodeOp controller_timer_ = this->create_wall_timer( std::chrono::milliseconds(50), std::bind(&SchedulerNode::ControllerTimerCB, this)); - RCLCPP_INFO(this->get_logger(), "Node started"); -} - -void SchedulerNode::Initialize() -{ - it_ = std::make_shared(this->shared_from_this()); + set_animation_client_ = + this->create_client("lights/controller/set/animation"); e_stop_sub_ = this->create_subscription( "hardware/e_stop", 5, [&](const BoolMsg::SharedPtr msg) { e_stop_state_ = msg->data; }); @@ -48,50 +46,37 @@ void SchedulerNode::Initialize() "battery", 5, [&](const BatteryStateMsg::SharedPtr msg) { battery_percentage_ = msg->percentage; }); - front_light_pub_ = std::make_shared( - it_->advertise("lights/driver/front_panel_frame", 5)); - rear_light_pub_ = std::make_shared( - it_->advertise("lights/driver/rear_panel_frame", 5)); - - RCLCPP_INFO(this->get_logger(), "Controller initialised"); + RCLCPP_INFO(this->get_logger(), "Node started"); } void SchedulerNode::ControllerTimerCB() { - if (!it_) { - Initialize(); - } - if (battery_percentage_ < 0.4) { - PublishColor(kColorOrange); + CallSetLEDAnimationSrv(6); } else if (!e_stop_state_) { - PublishColor(kColorGreen); + CallSetLEDAnimationSrv(1); } else { - PublishColor(kColorRed); + CallSetLEDAnimationSrv(0); } } -void SchedulerNode::PublishColor(const RGBAColor & color) +void SchedulerNode::CallSetLEDAnimationSrv(const std::uint16_t animation_id) { - sensor_msgs::msg::Image image_msg; - image_msg.header.stamp = this->get_clock()->now(); - image_msg.encoding = sensor_msgs::image_encodings::RGBA8; - image_msg.height = 1; - image_msg.width = num_led_; - image_msg.step = image_msg.width * 4; // 4 for RGBA channels - - for (int i = 0; i < num_led_; i++) { - image_msg.data.push_back(color.r); - image_msg.data.push_back(color.g); - image_msg.data.push_back(color.b); - image_msg.data.push_back(color.a); + if (current_anim_id_ == animation_id) { + return; } - image_msg.header.frame_id = "front_light_link"; - front_light_pub_->publish(image_msg); + RCLCPP_INFO(this->get_logger(), "Calling LED srv"); + if (!set_animation_client_->wait_for_service(std::chrono::milliseconds(1000))) { + RCLCPP_INFO(this->get_logger(), "Service not available"); + return; + } + auto req = std::make_shared(); + req->animation.id = animation_id; + req->repeating = true; + set_animation_client_->async_send_request(req); - image_msg.header.frame_id = "rear_light_link"; - rear_light_pub_->publish(image_msg); + current_anim_id_ = animation_id; } } // namespace panther_lights diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp new file mode 100644 index 000000000..65c4f5e4e --- /dev/null +++ b/panther_lights/src/led_animations_queue.cpp @@ -0,0 +1,152 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace panther_lights +{ + +LEDAnimation::LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time) +: led_animation_description_(led_animation_description), + init_time_(init_time), + repeating_(false), + param_("") +{ + for (const auto & animation : led_animation_description_.animations) { + for (const auto & segment : animation.segments) { + if (segments.find(segment) == segments.end()) { + throw std::runtime_error("No segment with name: " + segment); + } + animation_segments_.push_back(segments.at(segment)); + } + } +} + +bool LEDAnimation::IsFinished() +{ + return std::all_of( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { return segment->IsAnimationFinished(); }); +} + +float LEDAnimation::GetProgress() const +{ + float progress = 1.0f; + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [&progress](const std::shared_ptr & segment) { + auto anim_progress = segment->GetAnimationProgress(); + progress = anim_progress < progress ? anim_progress : progress; + }); + + return progress; +} + +void LEDAnimation::Reset(const rclcpp::Time & time) +{ + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { segment->ResetAnimation(); }); + init_time_ = time; +} + +void LEDAnimationsQueue::Put( + const std::shared_ptr & animation, const rclcpp::Time & time) +{ + if (animation->GetPriority() == 1) { + Clear(); + } + Validate(time); + + if (queue_.size() == max_queue_size_) { + throw std::runtime_error("Animation queue overloaded"); + } + + queue_.push_back(animation); + std::sort( + queue_.begin(), queue_.end(), + [](const std::shared_ptr & a, const std::shared_ptr & b) { + return a->GetPriority() < b->GetPriority() || + (a->GetPriority() == b->GetPriority() && a->GetInitTime() < b->GetInitTime()); + }); +} + +std::shared_ptr LEDAnimationsQueue::Get() +{ + if (!queue_.empty()) { + const auto animation = queue_.front(); + queue_.pop_front(); + return animation; + } + throw std::runtime_error("Queue empty"); +} + +void LEDAnimationsQueue::Clear(const std::size_t priority) +{ + const auto new_end = std::remove_if( + queue_.begin(), queue_.end(), [priority](const std::shared_ptr & animation) { + return animation->GetPriority() >= priority; + }); + queue_.erase(new_end, queue_.end()); +} + +void LEDAnimationsQueue::Validate(const rclcpp::Time & time) +{ + for (auto it = queue_.begin(); it != queue_.end();) { + if ((time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { + RCLCPP_WARN( + rclcpp::get_logger("controller_node"), + "Warning: Timeout for animation: %s. Removing from the queue", + it->get()->GetName().c_str()); + it = queue_.erase(it); + } else { + ++it; + } + } +} + +std::size_t LEDAnimationsQueue::GetFirstAnimationPriority() const +{ + if (!Empty()) { + return queue_.front()->GetPriority(); + } + return LEDAnimation::kDefaultPriority; +} + +void LEDAnimationsQueue::Remove(const std::shared_ptr & animation) +{ + auto it = std::find(queue_.begin(), queue_.end(), animation); + if (it != queue_.end()) { + queue_.erase(it); + } +} + +bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr & animation) const +{ + return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); +} + +} // namespace panther_lights diff --git a/panther_lights/src/led_panel.cpp b/panther_lights/src/led_panel.cpp new file mode 100644 index 000000000..8d56362c3 --- /dev/null +++ b/panther_lights/src/led_panel.cpp @@ -0,0 +1,46 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace panther_lights +{ + +LEDPanel::LEDPanel(const std::size_t num_led) : num_led_(num_led) +{ + frame_ = std::vector(num_led_ * 4, 0); +} + +void LEDPanel::UpdateFrame( + const std::size_t iterator_first, const std::vector & values) +{ + if (values.empty()) { + throw std::runtime_error("Values vector can't be empty"); + } + if (values.size() > frame_.size()) { + throw std::runtime_error("Values size is greater than frame size"); + } + if (values.size() + iterator_first > frame_.size()) { + throw std::runtime_error("Values can't be fit into the frame at given position"); + } + + std::copy(values.begin(), values.end(), frame_.begin() + iterator_first); +} + +} // namespace panther_lights diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp new file mode 100644 index 000000000..6827afd38 --- /dev/null +++ b/panther_lights/src/led_segment.cpp @@ -0,0 +1,170 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include + +#include +#include + +namespace panther_lights +{ + +LEDSegment::LEDSegment(const YAML::Node & segment_description, const float controller_frequency) +: controller_frequency_(controller_frequency) +{ + channel_ = panther_utils::GetYAMLKeyValue(segment_description, "channel"); + const auto led_range = panther_utils::GetYAMLKeyValue( + segment_description, "led_range"); + + const std::size_t split_char = led_range.find('-'); + + if (split_char == std::string::npos) { + throw std::invalid_argument("No '-' character found in the led_range expression"); + } + + try { + first_led_iterator_ = std::stoi(led_range.substr(0, split_char)); + last_led_iterator_ = std::stoi(led_range.substr(split_char + 1)); + + if (first_led_iterator_ > last_led_iterator_) { + invert_led_order_ = true; + } + } catch (const std::invalid_argument & e) { + throw std::invalid_argument("Error converting string to integer"); + } + + num_led_ = std::abs(int(last_led_iterator_ - first_led_iterator_)) + 1; + + animation_loader_ = std::make_shared>( + "panther_lights", "panther_lights::Animation"); +} + +LEDSegment::~LEDSegment() +{ + // make sure that animations are destroyed before pluginlib loader + animation_.reset(); + default_animation_.reset(); + animation_loader_.reset(); +} + +void LEDSegment::SetAnimation( + const std::string & type, const YAML::Node & animation_description, const bool repeating, + const std::string & param) +{ + std::shared_ptr animation; + + try { + animation = animation_loader_->createSharedInstance(type); + } catch (pluginlib::PluginlibException & e) { + throw std::runtime_error("The plugin failed to load. Error: " + std::string(e.what())); + } + + try { + animation->Initialize(animation_description, num_led_, controller_frequency_); + animation->SetParam(param); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); + } catch (const std::out_of_range & e) { + throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); + } + + animation_ = std::move(animation); + animation_finished_ = false; + + if (repeating) { + default_animation_ = animation_; + animation_finished_ = true; + } + if (default_animation_) { + default_animation_->Reset(); + } +} + +void LEDSegment::UpdateAnimation() +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + if (animation_->IsFinished()) { + animation_finished_ = true; + } + + std::shared_ptr animation_to_update = + animation_finished_ && default_animation_ ? default_animation_ : animation_; + + if (animation_finished_ && default_animation_ && default_animation_->IsFinished()) { + default_animation_->Reset(); + } + + try { + animation_to_update->Update(); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Failed to update animation: " + std::string(e.what())); + } +} + +std::vector LEDSegment::GetAnimationFrame() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + if (default_animation_ && animation_finished_) { + return default_animation_->GetFrame(invert_led_order_); + } + + return animation_->GetFrame(invert_led_order_); +} + +float LEDSegment::GetAnimationProgress() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + return animation_->GetProgress(); +} + +void LEDSegment::ResetAnimation() +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + animation_->Reset(); + animation_finished_ = false; +} + +std::uint8_t LEDSegment::GetAnimationBrightness() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + return animation_->GetBrightness(); +} + +std::size_t LEDSegment::GetFirstLEDPosition() const +{ + return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; +} + +} // namespace panther_lights diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp new file mode 100644 index 000000000..c6594f89d --- /dev/null +++ b/panther_lights/src/segment_converter.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace panther_lights +{ + +void SegmentConverter::Convert( + const std::unordered_map> & segments, + const std::unordered_map> & panels) +{ + for (const auto & [segment_name, segment] : segments) { + if (!segment->HasAnimation()) { + continue; + } + + try { + auto panel = panels.at(segment->GetChannel()); + + auto frame = segment->GetAnimationFrame(); + for (std::size_t i = 3; i < frame.size(); i += 4) { + frame[i] = segment->GetAnimationBrightness(); + } + + panel->UpdateFrame(segment->GetFirstLEDPosition(), frame); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to convert '" + segment_name + + "' segment animation to panel frame. Error: " + std::string(e.what())); + } + } +} + +} // namespace panther_lights diff --git a/panther_lights/test/test_animation.cpp b/panther_lights/test/test_animation.cpp new file mode 100644 index 000000000..9f69a281b --- /dev/null +++ b/panther_lights/test/test_animation.cpp @@ -0,0 +1,213 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include + +#include + +class AnimationWrapper : public panther_lights::Animation +{ +public: + AnimationWrapper() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override + { + Animation::Initialize(animation_description, num_led, controller_frequency); + frame_size_ = this->GetNumberOfLeds() * 4; + } + + std::vector UpdateFrame() override + { + return std::vector(frame_size_, 147); + } + + std::vector InvertRGBAFrame(const std::vector & frame) const + { + return Animation::InvertRGBAFrame(frame); + } + + std::size_t GetAnimationLength() const { return Animation::GetAnimationLength(); } + std::size_t GetAnimationIteration() const { return Animation::GetAnimationIteration(); } + void SetFrameSize(const std::size_t frame_size) { frame_size_ = frame_size; } + +private: + std::size_t frame_size_; +}; + +class TestAnimation : public testing::Test +{ +public: + TestAnimation() { animation_ = std::make_unique(); } + ~TestAnimation() {} + +protected: + std::unique_ptr animation_; +}; + +TEST_F(TestAnimation, Initialize) +{ + YAML::Node animation_description; + + // missing duration + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + // invalid duration + animation_description["duration"] = "-1.0"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + animation_description["duration"] = "word"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + // invalid animation length + animation_description["duration"] = "0.1"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 1.0), std::runtime_error); + + animation_description["duration"] = "2.0"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); + + // invalid repeat + animation_description["repeat"] = "-2"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + animation_description["repeat"] = "1.1"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + // exceeded anim display duration + animation_description["repeat"] = "6"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + animation_description["repeat"] = "2"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); + + // invalid brightness + animation_description["brightness"] = "-0.5"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + animation_description["brightness"] = "1.2"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + + animation_description["brightness"] = "0.5"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); +} + +TEST_F(TestAnimation, CheckInitialValues) +{ + const std::size_t num_led = 10; + const float controller_frequency = 10.0; + YAML::Node animation_description = YAML::Load("{duration: 2.0, repeat: 2}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, controller_frequency)); + + EXPECT_EQ(num_led, animation_->GetNumberOfLeds()); + EXPECT_EQ(std::size_t(20), animation_->GetAnimationLength()); + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + EXPECT_FLOAT_EQ(0.0, animation_->GetProgress()); +} + +TEST_F(TestAnimation, Reset) +{ + ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); + + // update animation + ASSERT_NO_THROW(animation_->Update()); + EXPECT_NE(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_NE(0.0F, animation_->GetProgress()); + + // reset animation + animation_->Reset(); + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FLOAT_EQ(0.0, animation_->GetProgress()); + EXPECT_FALSE(animation_->IsFinished()); +} + +TEST_F(TestAnimation, UpdateWithInvalidFrameSize) +{ + ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); + EXPECT_NO_THROW(animation_->Update()); + + animation_->SetFrameSize(11); + EXPECT_THROW(animation_->Update(), std::runtime_error); +} + +TEST_F(TestAnimation, Update) +{ + const std::size_t num_led = 10; + const float controller_frequency = 10.0; + YAML::Node animation_description = YAML::Load("{duration: 2.0, repeat: 2}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, controller_frequency)); + + // check random progress + for (std::size_t i = 0; i < 5; i++) { + ASSERT_NO_THROW(animation_->Update()); + } + EXPECT_EQ(std::size_t(5), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + float expected_progress = 5.0 / (20 * 2); + EXPECT_FLOAT_EQ(expected_progress, animation_->GetProgress()); + + animation_->Reset(); + + // reach end of first loop of animation + for (std::size_t i = 0; i < 20; i++) { + ASSERT_NO_THROW(animation_->Update()); + } + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + expected_progress = 20.0 / (20 * 2); + EXPECT_FLOAT_EQ(expected_progress, animation_->GetProgress()); + + // reach animation end + for (std::size_t i = 0; i < 20; i++) { + ASSERT_NO_THROW(animation_->Update()); + } + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_TRUE(animation_->IsFinished()); + EXPECT_FLOAT_EQ(1.0, animation_->GetProgress()); + + // after reaching animation end Update() method when invoked should return frame filled with 0 + animation_->Update(); + auto frame = animation_->GetFrame(); + EXPECT_EQ(num_led * 4, frame.size()); + for (std::size_t i = 0; i < num_led * 3; i++) { + EXPECT_EQ(0, frame[i]); + } +} + +TEST_F(TestAnimation, InvertRGBAFrame) +{ + std::vector test_frame = {0, 10, 20, 255, 30, 40, 50, 255, + 60, 70, 80, 255, 100, 110, 120, 255, + 130, 140, 150, 255, 160, 170, 180, 255}; + std::vector expected_frame = {160, 170, 180, 255, 130, 140, 150, 255, + 100, 110, 120, 255, 60, 70, 80, 255, + 30, 40, 50, 255, 0, 10, 20, 255}; + + auto inverted_frame = animation_->InvertRGBAFrame(test_frame); + for (std::size_t i = 0; i < inverted_frame.size(); i++) { + EXPECT_EQ(expected_frame[i], inverted_frame[i]); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_charging_animation.cpp b/panther_lights/test/test_charging_animation.cpp new file mode 100644 index 000000000..b25123d99 --- /dev/null +++ b/panther_lights/test/test_charging_animation.cpp @@ -0,0 +1,206 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include + +class ChargingAnimationWrapper : public panther_lights::ChargingAnimation +{ +public: + ChargingAnimationWrapper() {} + + std::vector UpdateFrame() { return ChargingAnimation::UpdateFrame(); } + + std::array HSVtoRGB(const float h, const float s, const float v) const + { + return ChargingAnimation::HSVtoRGB(h, s, v); + } + + std::vector CreateRGBAFrame( + const std::array color, const float brightness) const + { + return ChargingAnimation::CreateRGBAFrame(color, brightness); + } + + std::size_t GetAnimationLength() const { return ChargingAnimation::GetAnimationLength(); } + std::size_t GetAnimationIteration() const { return ChargingAnimation::GetAnimationIteration(); } +}; + +class TestChargingAnimation : public testing::Test +{ +public: + TestChargingAnimation(); + ~TestChargingAnimation(); + +protected: + void TestRGBColor( + const std::array & color, std::uint8_t r, std::uint8_t g, + std::uint8_t b) const; + void TestRGBAFrame( + const std::vector & frame, std::uint8_t r, std::uint8_t g, std::uint8_t b, + std::uint8_t a); + + std::unique_ptr animation_; +}; + +TestChargingAnimation::TestChargingAnimation() +{ + animation_ = std::make_unique(); +} + +TestChargingAnimation::~TestChargingAnimation() {} + +void TestChargingAnimation::TestRGBColor( + const std::array & color, std::uint8_t r, std::uint8_t g, std::uint8_t b) const +{ + EXPECT_EQ(r, color[0]); + EXPECT_EQ(g, color[1]); + EXPECT_EQ(b, color[2]); +} + +void TestChargingAnimation::TestRGBAFrame( + const std::vector & frame, std::uint8_t r, std::uint8_t g, std::uint8_t b, + std::uint8_t a) +{ + for (std::size_t i = 0; i < frame.size(); i += 4) { + EXPECT_EQ(r, frame[i]); + EXPECT_EQ(g, frame[i + 1]); + EXPECT_EQ(b, frame[i + 2]); + EXPECT_EQ(a, frame[i + 3]); + } +} + +TEST_F(TestChargingAnimation, HSVtoRGB) +{ + // saturation equal 0 with max value should return white + auto color = animation_->HSVtoRGB(0.0, 0.0, 1.0); + TestRGBColor(color, 255, 255, 255); + + // red + color = animation_->HSVtoRGB(0.0, 1.0, 1.0); + TestRGBColor(color, 255, 0, 0); + + // green + color = animation_->HSVtoRGB(120.0 / 360.0, 1.0, 1.0); + TestRGBColor(color, 0, 255, 0); + + // blue + color = animation_->HSVtoRGB(240.0 / 360.0, 1.0, 1.0); + TestRGBColor(color, 0, 0, 255); + + // test colors for each of 6 hue segments + color = animation_->HSVtoRGB(51.0f / 360.0f, 0.8f, 0.3f); + TestRGBColor(color, 76, 67, 15); + + color = animation_->HSVtoRGB(77.0f / 360.0f, 0.5f, 0.7f); + TestRGBColor(color, 153, 178, 89); + + color = animation_->HSVtoRGB(150.0f / 360.0f, 0.2f, 0.8f); + TestRGBColor(color, 163, 204, 183); + + color = animation_->HSVtoRGB(222.0f / 360.0f, 0.8f, 0.5f); + TestRGBColor(color, 25, 56, 127); + + color = animation_->HSVtoRGB(291.0f / 360.0f, 0.7f, 0.4f); + TestRGBColor(color, 91, 30, 102); + + color = animation_->HSVtoRGB(345.0f / 360.0f, 0.1f, 0.9f); + TestRGBColor(color, 229, 206, 212); +} + +TEST_F(TestChargingAnimation, CreateRGBAFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + std::array color{70, 120, 91}; + auto frame = animation_->CreateRGBAFrame(color, 1.0); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 70, 120, 91, 255); + + // reduce brightness + frame = animation_->CreateRGBAFrame(color, 0.5); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 35, 60, 45, 255); +} + +TEST_F(TestChargingAnimation, SetParam) +{ + EXPECT_THROW(animation_->SetParam("not_a_number"), std::runtime_error); + EXPECT_NO_THROW(animation_->SetParam("0.7")); +} + +TEST_F(TestChargingAnimation, UpdateFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + // UpdateFrame depends on parent class variables which can be only updated using Update method. + // For full battery whole animation should be a solid green color + animation_->SetParam("1.0"); + for (std::uint8_t i = 0; i < animation_->GetAnimationLength(); i++) { + animation_->Update(); + auto frame = animation_->GetFrame(); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 255, 0, 255); + } + + animation_->Reset(); + animation_->SetParam("0.5"); + std::vector frame; + + // the beginning of the animation should be dark + animation_->Update(); + frame = animation_->GetFrame(); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 0, 0, 255); + + // reach middle of the animation + while (animation_->GetAnimationIteration() < animation_->GetAnimationLength() / 2) { + animation_->Update(); + frame = animation_->GetFrame(); + } + + // the middle of animation for param 0.5 should be yellow + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 255, 255, 0, 255); + + // reach end of the animation + while (!animation_->IsFinished()) { + animation_->Update(); + frame = animation_->GetFrame(); + } + + // the end of the animation should be dark + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 0, 0, 255); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_controller_node.cpp b/panther_lights/test/test_controller_node.cpp new file mode 100644 index 000000000..b56ad08dd --- /dev/null +++ b/panther_lights/test/test_controller_node.cpp @@ -0,0 +1,324 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +class ControllerNodeWrapper : public panther_lights::ControllerNode +{ +public: + ControllerNodeWrapper(const std::string & node_name, const rclcpp::NodeOptions & options) + : ControllerNode(node_name, options) + { + } + + void InitializeLEDPanels(const YAML::Node & panels_description) + { + return ControllerNode::InitializeLEDPanels(panels_description); + } + + void InitializeLEDSegments(const YAML::Node & segments_description, const float controller_freq) + { + return ControllerNode::InitializeLEDSegments(segments_description, controller_freq); + } + + void LoadAnimation(const YAML::Node & animation_description) + { + return ControllerNode::LoadAnimation(animation_description); + } + + void AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param = "") + { + return ControllerNode::AddAnimationToQueue(animation_id, repeating, param); + } + + std::shared_ptr GetQueue() { return this->animations_queue_; } + + std::shared_ptr GetCurrentAnimation() + { + return this->current_animation_; + } +}; + +class TestControllerNode : public testing::Test +{ +public: + TestControllerNode(); + ~TestControllerNode(); + +protected: + void CreateLEDConfig(const std::filesystem::path file_path); + void CallSetLEDAnimationSrv( + const std::size_t animation_id, const bool repeating, const std::string & param = ""); + + static constexpr std::size_t kTestNumberOfLeds = 10; + static constexpr std::size_t kTestChannel = 1; + static constexpr char kTestSegmentName[] = "test"; + static constexpr char kTestSegmentLedRange[] = "0-9"; + + std::filesystem::path led_config_file_; + std::shared_ptr controller_node_; + rclcpp::Client::SharedPtr set_led_anim_client_; +}; + +TestControllerNode::TestControllerNode() +{ + led_config_file_ = testing::TempDir() + "/led_config.yaml"; + + CreateLEDConfig(led_config_file_); + + std::vector params; + params.push_back(rclcpp::Parameter("led_config_file", led_config_file_)); + + rclcpp::NodeOptions options; + options.parameter_overrides(params); + + controller_node_ = std::make_shared("controller_node", options); + + set_led_anim_client_ = controller_node_->create_client( + "lights/controller/set/animation"); +} + +TestControllerNode::~TestControllerNode() { std::filesystem::remove(led_config_file_); } + +void TestControllerNode::CreateLEDConfig(const std::filesystem::path file_path) +{ + YAML::Node panel; + panel["channel"] = kTestChannel; + panel["number_of_leds"] = kTestNumberOfLeds; + + YAML::Node segment; + segment["name"] = kTestSegmentName; + segment["channel"] = kTestChannel; + segment["led_range"] = kTestSegmentLedRange; + + YAML::Node segments_map; + segments_map["test"] = std::vector(1, kTestSegmentName); + + YAML::Node animation; + animation["image"] = "$(find panther_lights)/animations/triangle01_red.png"; + animation["duration"] = 2; + + YAML::Node animation_desc; + animation_desc["type"] = "panther_lights::ImageAnimation"; + animation_desc["segments"] = "test"; + animation_desc["animation"] = animation; + + YAML::Node led_animation_0; + led_animation_0["id"] = 0; + led_animation_0["animations"] = std::vector(1, animation_desc); + YAML::Node led_animation_1; + led_animation_1["id"] = 1; + led_animation_1["animations"] = std::vector(1, animation_desc); + led_animation_1["priority"] = 2; + + std::vector led_animations; + led_animations.push_back(led_animation_0); + led_animations.push_back(led_animation_1); + + YAML::Node led_config; + led_config["panels"] = std::vector(1, panel); + led_config["segments"] = std::vector(1, segment); + led_config["segments_map"] = segments_map; + led_config["led_animations"] = led_animations; + + YAML::Emitter out; + out << led_config; + + std::ofstream fout(file_path); + if (fout.is_open()) { + fout << out.c_str(); + fout.close(); + } else { + throw std::runtime_error("Failed to create file: " + std::string(file_path)); + } +} + +void TestControllerNode::CallSetLEDAnimationSrv( + const std::size_t animation_id, const bool repeating, const std::string & param) +{ + auto request = std::make_shared(); + request->animation.id = animation_id; + request->animation.param = param; + request->repeating = repeating; + + auto result = set_led_anim_client_->async_send_request(request); + + ASSERT_TRUE( + rclcpp::spin_until_future_complete(controller_node_, result) == + rclcpp::FutureReturnCode::SUCCESS); + EXPECT_TRUE(result.get()->success); +} + +TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) +{ + YAML::Node panel_1_desc; + panel_1_desc["channel"] = kTestChannel; + panel_1_desc["number_of_leds"] = kTestNumberOfLeds; + + YAML::Node panel_2_desc; + panel_2_desc["channel"] = kTestChannel; + panel_2_desc["number_of_leds"] = kTestNumberOfLeds; + + std::vector panels; + panels.push_back(panel_1_desc); + panels.push_back(panel_2_desc); + + YAML::Node panels_desc; + panels_desc["panels"] = panels; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->InitializeLEDPanels(panels_desc["panels"]); }, + "Multiple panels with channel nr")); +} + +TEST_F(TestControllerNode, InitializeLEDSegmentsThrowRepeatingName) +{ + YAML::Node segment_1_desc; + segment_1_desc["name"] = kTestSegmentName; + segment_1_desc["channel"] = kTestChannel; + segment_1_desc["led_range"] = kTestSegmentLedRange; + + YAML::Node segment_2_desc; + segment_2_desc["name"] = kTestSegmentName; + segment_2_desc["channel"] = kTestChannel; + segment_2_desc["led_range"] = kTestSegmentLedRange; + + std::vector segments; + segments.push_back(segment_1_desc); + segments.push_back(segment_2_desc); + + YAML::Node segments_desc; + segments_desc["segments"] = segments; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->InitializeLEDSegments(segments_desc["segments"], 50.0); }, + "Multiple segments with given name found")); +} + +TEST_F(TestControllerNode, LoadAnimationInvalidPriority) +{ + YAML::Node led_animation_desc; + led_animation_desc["id"] = 11; + led_animation_desc["priority"] = 0; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Invalid LED animation priority")); + + led_animation_desc["priority"] = 4; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Invalid LED animation priority")); +} + +TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) +{ + YAML::Node led_animation_desc; + led_animation_desc["id"] = 11; + led_animation_desc["animations"] = std::vector(); + + ASSERT_NO_THROW(controller_node_->LoadAnimation(led_animation_desc)); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Animation with given ID already exists")); +} + +TEST_F(TestControllerNode, AddAnimationToQueueThrowBadAnimationID) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); +} + +TEST_F(TestControllerNode, AddAnimationToQueue) +{ + auto queue = controller_node_->GetQueue(); + EXPECT_TRUE(queue->Empty()); + EXPECT_NO_THROW(controller_node_->AddAnimationToQueue(0, false)); + EXPECT_FALSE(queue->Empty()); +} + +TEST_F(TestControllerNode, CallSelLEDAnimationService) +{ + this->CallSetLEDAnimationSrv(0, false); + + // spin to invoke timer + auto anim = controller_node_->GetCurrentAnimation(); + while (!anim) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + anim = controller_node_->GetCurrentAnimation(); + } + + EXPECT_STREQ("ANIMATION_0", anim->GetName().c_str()); + + // add another animation to queue + auto queue = controller_node_->GetQueue(); + EXPECT_TRUE(queue->Empty()); + + this->CallSetLEDAnimationSrv(0, false); + + EXPECT_FALSE(queue->Empty()); +} + +TEST_F(TestControllerNode, CallSelLEDAnimationServicePriorityInterrupt) +{ + this->CallSetLEDAnimationSrv(0, false); + + // spin to invoke timer + auto anim = controller_node_->GetCurrentAnimation(); + while (!anim) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + anim = controller_node_->GetCurrentAnimation(); + } + + // add animation with higher priority spin to give time for controller to overwrite current + // animation then check if animation has changed + this->CallSetLEDAnimationSrv(1, false); + for (int i = 0; i < 10; i++) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + anim = controller_node_->GetCurrentAnimation(); + EXPECT_STREQ("ANIMATION_1", anim->GetName().c_str()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} diff --git a/panther_lights/test/test_image_animation.cpp b/panther_lights/test/test_image_animation.cpp new file mode 100644 index 000000000..6936d0668 --- /dev/null +++ b/panther_lights/test/test_image_animation.cpp @@ -0,0 +1,223 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace gil = boost::gil; + +class ImageAnimationWrapper : public panther_lights::ImageAnimation +{ +public: + ImageAnimationWrapper() {} + + std::filesystem::path ParseImagePath(const std::string & image_path) const + { + return ImageAnimation::ParseImagePath(image_path); + } + + gil::rgba8_image_t RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) + { + return ImageAnimation::RGBAImageResize(image, width, height); + } + + void RGBAImageConvertColor(gil::rgba8_image_t & image, const std::uint32_t color) + { + return ImageAnimation::RGBAImageConvertColor(image, color); + } + + gil::gray_alpha8_image_t RGBAImageConvertToGrey(gil::rgba8_image_t & image) + { + return ImageAnimation::RGBAImageConvertToGrey(image); + } + + void GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) + { + return ImageAnimation::GreyImageNormalizeBrightness(image); + } + + std::vector UpdateFrame() { return ImageAnimation::UpdateFrame(); } +}; + +class TestImageAnimation : public testing::Test +{ +public: + TestImageAnimation(); + ~TestImageAnimation(); + +protected: + std::string test_image_path = "/tmp/test_image.png"; + std::unique_ptr animation_; +}; + +TestImageAnimation::TestImageAnimation() +{ + gil::rgba8_image_t image(100, 100); + gil::write_view(test_image_path, gil::const_view(image), gil::png_tag()); + animation_ = std::make_unique(); +} + +TestImageAnimation::~TestImageAnimation() { std::filesystem::remove(test_image_path); } + +TEST_F(TestImageAnimation, ParseImagePath) +{ + std::string image_path = "not/a/global/path"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + image_path = "/path/to/no/existing/file"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // should return the same path if path is global and file exists + EXPECT_EQ(this->test_image_path, animation_->ParseImagePath(this->test_image_path)); + + // test ROS package path + image_path = "$(find invalid_ros_package)/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // invalid substitution + image_path = "$(fin panther_lights)/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + image_path = "$(find panther_lights/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // following ones may not work if ROS package is not build or sourced + image_path = "$(find panther_lights)/animations/strip01_red.png"; + EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); + + // multiple spaces after find syntax + image_path = "$(find panther_lights)/animations/strip01_red.png"; + EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); +} + +TEST_F(TestImageAnimation, RGBAImageResize) +{ + gil::rgba8_image_t input_image(100, 100); + auto output_image = animation_->RGBAImageResize(input_image, 75, 112); + EXPECT_EQ(75, output_image.width()); + EXPECT_EQ(112, output_image.height()); +} + +TEST_F(TestImageAnimation, RGBAImageConvertColor) +{ + gil::rgba8_image_t white_image(5, 5); + gil::fill_pixels(gil::view(white_image), gil::rgba8_pixel_t(255, 255, 255, 255)); + + // copy white image + auto blue_image = white_image; + animation_->RGBAImageConvertColor(blue_image, 0x0000FF); + // expect r,g pixels to be 0, b to be 255 + gil::for_each_pixel(gil::view(blue_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(0, pixel[0]); + EXPECT_EQ(0, pixel[1]); + EXPECT_EQ(255, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); + + // copy white image + auto green_image = white_image; + animation_->RGBAImageConvertColor(green_image, 0x00FF00); + // expect r,b pixels to be 0, g to be 255 + gil::for_each_pixel(gil::view(green_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(0, pixel[0]); + EXPECT_EQ(255, pixel[1]); + EXPECT_EQ(0, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); + + // copy white image + auto red_image = white_image; + animation_->RGBAImageConvertColor(red_image, 0xFF0000); + // expect g,b pixels to be 0, r to be 255 + gil::for_each_pixel(gil::view(red_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(255, pixel[0]); + EXPECT_EQ(0, pixel[1]); + EXPECT_EQ(0, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); +} + +TEST_F(TestImageAnimation, RGBAImageConvertToGrey) +{ + gil::rgba8_image_t rgb_image(5, 5); + gil::fill_pixels(gil::view(rgb_image), gil::rgba8_pixel_t(30, 100, 200, 255)); + + const int expected_grey_value = 0.299 * 30 + 0.587 * 100 + 0.114 * 200; + auto grey_image = animation_->RGBAImageConvertToGrey(rgb_image); + gil::for_each_pixel( + gil::view(grey_image), [expected_grey_value](const gil::gray_alpha8_pixel_t & pixel) { + EXPECT_EQ(expected_grey_value, pixel[0]); + }); +} + +TEST_F(TestImageAnimation, GreyImageNormalizeBrightness) +{ + gil::gray_alpha8_image_t grey_image(5, 5); + gil::fill_pixels(gil::view(grey_image), gil::gray_alpha8_pixel_t(204)); + // make the first row a bit darker + for (int i = 0; i < grey_image.width(); i++) { + gil::at_c<0>(*gil::view(grey_image).at(i)) = 102; + } + + animation_->GreyImageNormalizeBrightness(grey_image); + // check the first row + for (int i = 0; i < grey_image.width(); i++) { + EXPECT_EQ(int(102.0 / 204 * 255), gil::at_c<0>(*gil::view(grey_image).at(i))); + } + // check the rest of the image + for (int i = grey_image.width(); i < grey_image.width() * grey_image.height(); i++) { + EXPECT_EQ(255, gil::at_c<0>(*gil::view(grey_image).at(i))); + } +} + +TEST_F(TestImageAnimation, Initialize) +{ + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + // missing image in description + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + animation_description["image"] = this->test_image_path; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); +} + +TEST_F(TestImageAnimation, UpdateFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = + YAML::Load("{duration: 2.0, image: " + this->test_image_path + "}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + auto frame = animation_->UpdateFrame(); + EXPECT_EQ(num_led * 4, frame.size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp new file mode 100644 index 000000000..79273d5b9 --- /dev/null +++ b/panther_lights/test/test_led_animation.cpp @@ -0,0 +1,170 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +class TestLEDAnimation : public testing::Test +{ +public: + TestLEDAnimation(); + ~TestLEDAnimation() {} + + void SetSegmentAnimations(); + +protected: + static constexpr char kTestSegmentName1[] = "segment_1"; + static constexpr char kTestSegmentName2[] = "segment_2"; + + std::shared_ptr led_anim_; + std::unordered_map> segments_; +}; + +TestLEDAnimation::TestLEDAnimation() +{ + auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); + auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); + segments_.emplace( + kTestSegmentName1, std::make_shared(segment_1_desc, 50.0)); + segments_.emplace( + kTestSegmentName2, std::make_shared(segment_2_desc, 50.0)); + + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {kTestSegmentName1, kTestSegmentName2}; + anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.animation = + YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.id = 0; + led_anim_desc.name = "TEST"; + led_anim_desc.priority = 1; + led_anim_desc.timeout = 10.0; + led_anim_desc.animations = {anim_desc}; + + led_anim_ = std::make_shared( + led_anim_desc, segments_, rclcpp::Time(0)); +} + +void TestLEDAnimation::SetSegmentAnimations() +{ + const auto animations = led_anim_->GetAnimations(); + for (auto & animation : animations) { + for (auto & segment : animation.segments) { + segments_.at(segment)->SetAnimation( + animation.type, animation.animation, led_anim_->IsRepeating(), led_anim_->GetParam()); + } + } +} + +TEST(TestLEDAnimationInitialization, InvalidSegmentName) +{ + std::unordered_map> segments; + + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {"invalid_segment"}; + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.animations = {anim_desc}; + + EXPECT_THROW( + std::make_shared(led_anim_desc, segments, rclcpp::Time(0)), + std::runtime_error); +} + +TEST_F(TestLEDAnimation, IsFinished) +{ + SetSegmentAnimations(); + + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); + } + + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); + } + + EXPECT_TRUE(led_anim_->IsFinished()); +} + +TEST_F(TestLEDAnimation, GetProgress) +{ + SetSegmentAnimations(); + + EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); + + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); + } + + EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); + } + + EXPECT_FLOAT_EQ(1.0, led_anim_->GetProgress()); +} + +TEST_F(TestLEDAnimation, Reset) +{ + SetSegmentAnimations(); + + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); + } + + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); + } + + EXPECT_TRUE(led_anim_->GetInitTime() == rclcpp::Time(0)); + EXPECT_TRUE(led_anim_->IsFinished()); + EXPECT_TRUE(segments_.at(kTestSegmentName1)->IsAnimationFinished()); + EXPECT_TRUE(segments_.at(kTestSegmentName2)->IsAnimationFinished()); + EXPECT_FLOAT_EQ(1.0, segments_.at(kTestSegmentName1)->GetAnimationProgress()); + EXPECT_FLOAT_EQ(1.0, segments_.at(kTestSegmentName2)->GetAnimationProgress()); + + auto reset_time = rclcpp::Time(1); + led_anim_->Reset(reset_time); + + EXPECT_TRUE(led_anim_->GetInitTime() == reset_time); + EXPECT_FALSE(led_anim_->IsFinished()); + EXPECT_FALSE(segments_.at(kTestSegmentName1)->IsAnimationFinished()); + EXPECT_FALSE(segments_.at(kTestSegmentName2)->IsAnimationFinished()); + EXPECT_FLOAT_EQ(0.0, segments_.at(kTestSegmentName1)->GetAnimationProgress()); + EXPECT_FLOAT_EQ(0.0, segments_.at(kTestSegmentName2)->GetAnimationProgress()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_led_animations_queue.cpp b/panther_lights/test/test_led_animations_queue.cpp new file mode 100644 index 000000000..c673bc742 --- /dev/null +++ b/panther_lights/test/test_led_animations_queue.cpp @@ -0,0 +1,250 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +class TestLEDAnimationsQueue : public testing::Test +{ +public: + TestLEDAnimationsQueue(); + ~TestLEDAnimationsQueue() {} + + panther_lights::LEDAnimation CreateLEDAnimation( + const std::string & name, const std::uint8_t priority, + const rclcpp::Time & init_time = rclcpp::Time(0)); + +protected: + std::shared_ptr led_anim_queue_; + std::unordered_map> segments_; + + const std::size_t max_queue_size_ = 5; +}; + +TestLEDAnimationsQueue::TestLEDAnimationsQueue() +{ + auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); + auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); + segments_.emplace( + "segment_1", std::make_shared(segment_1_desc, 50.0)); + segments_.emplace( + "segment_2", std::make_shared(segment_2_desc, 50.0)); + + led_anim_queue_ = std::make_shared(5); +} + +panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation( + const std::string & name, const std::uint8_t priority, const rclcpp::Time & init_time) +{ + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {"segment_1", "segment_2"}; + anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.animation = + YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.id = 0; + led_anim_desc.name = name; + led_anim_desc.priority = priority; + led_anim_desc.timeout = 10.0; + led_anim_desc.animations = {anim_desc}; + + return panther_lights::LEDAnimation(led_anim_desc, segments_, init_time); +} + +TEST_F(TestLEDAnimationsQueue, Put) +{ + auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + + EXPECT_FALSE(led_anim_queue_->Empty()); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim)); +} + +TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) +{ + auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + for (std::size_t i = 0; i < max_queue_size_; i++) { + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + } + + EXPECT_THROW(led_anim_queue_->Put(led_anim, rclcpp::Time(0)), std::runtime_error); +} + +TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) +{ + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST", 3)); + + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_1); + // animation with priority 1 should remove animations with lower priorities + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, PutSortByPriority) +{ + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST", 3)); + + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_3); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_3); +} + +TEST_F(TestLEDAnimationsQueue, PutSortByTime) +{ + auto led_anim_t0 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + auto led_anim_t1 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(1))); + auto led_anim_t2 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(2))); + auto led_anim_t3 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(3))); + + led_anim_queue_->Put(led_anim_t3, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t1, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t2, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t0, rclcpp::Time(4)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t0); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t1); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t3); +} + +TEST_F(TestLEDAnimationsQueue, GetQueueEmpty) +{ + EXPECT_THROW(led_anim_queue_->Get(), std::runtime_error); +} + +TEST_F(TestLEDAnimationsQueue, Clear) +{ + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + led_anim_queue_->Clear(3); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_3)); + + led_anim_queue_->Clear(); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + + led_anim_queue_->Clear(1); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) +{ + auto led_anim = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + led_anim_queue_->Validate(rclcpp::Time(0)); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim)); + + // exceed timeout + led_anim_queue_->Validate(rclcpp::Time(12, 0)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) +{ + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 1); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 2); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 3); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->Empty()); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 3); +} + +TEST_F(TestLEDAnimationsQueue, Remove) +{ + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + led_anim_queue_->Remove(led_anim_pr_2); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + led_anim_queue_->Remove(led_anim_pr_3); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_3)); + led_anim_queue_->Remove(led_anim_pr_1); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_led_panel.cpp b/panther_lights/test/test_led_panel.cpp new file mode 100644 index 000000000..51269a1b0 --- /dev/null +++ b/panther_lights/test/test_led_panel.cpp @@ -0,0 +1,131 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +class TestLEDPanel : public testing::Test +{ +public: + TestLEDPanel() { led_panel_ = std::make_unique(num_led_); } + ~TestLEDPanel() {} + +protected: + void UpdateAndTestFrame( + const std::size_t iterator_first, const std::vector & test_frame); + std::unique_ptr led_panel_; + + const std::size_t num_led_ = 46; + const std::size_t frame_size_ = num_led_ * 4; +}; + +void TestLEDPanel::UpdateAndTestFrame( + const std::size_t iterator_first, const std::vector & test_frame) +{ + // reset frame + led_panel_->UpdateFrame(0, std::vector(num_led_ * 4, 0)); + + // update frame + led_panel_->UpdateFrame(iterator_first, test_frame); + auto frame = led_panel_->GetFrame(); + + for (std::size_t i = 0; i < frame.size(); i++) { + if (i < iterator_first) { + EXPECT_EQ(frame.at(i), 0); + } else if (i < iterator_first + test_frame.size()) { + EXPECT_EQ(frame.at(i), test_frame.at(i - iterator_first)); + } else { + EXPECT_EQ(frame.at(i), 0); + } + } +} + +TEST(TestLEDPanelInitialization, FrameSize) +{ + const std::size_t num_led = 22; + auto led_panel = panther_lights::LEDPanel(num_led); + EXPECT_EQ(num_led * 4, led_panel.GetFrame().size()); +} + +TEST_F(TestLEDPanel, UpdateFrameFullFrame) +{ + const auto test_frame = std::vector(num_led_ * 4, 64); + UpdateAndTestFrame(0, test_frame); +} + +TEST_F(TestLEDPanel, UpdateFrameShortFrame) +{ + const std::size_t test_frame_size = std::size_t(frame_size_ / 2); + const auto test_frame = std::vector(test_frame_size, 77); + UpdateAndTestFrame(0, test_frame); + + const std::size_t fit_frame_in_the_middle_index = frame_size_ / 4; + UpdateAndTestFrame(fit_frame_in_the_middle_index, test_frame); + + const std::size_t fit_frame_at_the_end_index = frame_size_ - test_frame_size; + UpdateAndTestFrame(fit_frame_at_the_end_index, test_frame); +} + +TEST_F(TestLEDPanel, UpdateFrameOneElementFrame) +{ + auto frame_with_one_element = std::vector(1, 88); + UpdateAndTestFrame(frame_size_ - 1, frame_with_one_element); +} + +TEST_F(TestLEDPanel, UpdateFrameEmptyFrame) +{ + std::vector test_frame; + EXPECT_THROW(led_panel_->UpdateFrame(0, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameInvalidValuesSize) +{ + const auto test_frame = std::vector(num_led_ * 4 + 1, 0); + EXPECT_THROW(led_panel_->UpdateFrame(0, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameValuesOutOfRange) +{ + const auto test_frame = std::vector(num_led_ * 4, 0); + EXPECT_THROW(led_panel_->UpdateFrame(7, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameLastFrameValueOutOfRange) +{ + const auto test_frame_size = 10; + const auto test_frame = std::vector(test_frame_size, 0); + EXPECT_THROW( + led_panel_->UpdateFrame(frame_size_ - (test_frame_size - 1), test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameIteratorOutOfRange) +{ + const auto test_frame = std::vector(10, 0); + EXPECT_THROW(led_panel_->UpdateFrame(frame_size_, test_frame), std::runtime_error); + + const auto frame_with_one_element = std::vector(1, 0); + EXPECT_THROW(led_panel_->UpdateFrame(frame_size_, frame_with_one_element), std::runtime_error); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_led_segment.cpp b/panther_lights/test/test_led_segment.cpp new file mode 100644 index 000000000..0afe24370 --- /dev/null +++ b/panther_lights/test/test_led_segment.cpp @@ -0,0 +1,256 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +class LEDSegmentWrapper : public panther_lights::LEDSegment +{ +public: + LEDSegmentWrapper(const YAML::Node & segment_description, const float controller_frequency) + : LEDSegment(segment_description, controller_frequency) + { + } + + std::shared_ptr GetAnimation() const { return animation_; } + std::shared_ptr GetDefaultAnimation() const + { + return default_animation_; + } +}; + +class TestLEDSegment : public testing::Test +{ +public: + TestLEDSegment(); + ~TestLEDSegment() {} + +protected: + std::shared_ptr led_segment_; + + const float controller_freq = 50.0; + const std::size_t segment_led_num_ = 10; +}; + +TestLEDSegment::TestLEDSegment() +{ + const auto segment_desc = + YAML::Load("{led_range: 0-" + std::to_string(segment_led_num_ - 1) + ", channel: 1}"); + led_segment_ = std::make_shared(segment_desc, controller_freq); +} + +YAML::Node CreateSegmentDescription(const std::string & led_range, const std::string & channel) +{ + return YAML::Load("{led_range: " + led_range + ", channel: " + channel + "}"); +} + +TEST(TestLEDSegmentInitialization, DescriptionMissingRequiredKey) +{ + auto segment_desc = YAML::Load(""); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Missing 'channel' in description")); + + segment_desc = YAML::Load("channel: 0"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Missing 'led_range' in description")); +} + +TEST(TestLEDSegmentInitialization, InvalidChannelExpression) +{ + auto segment_desc = CreateSegmentDescription("0-10", "s1"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Failed to convert 'channel' key")); + + segment_desc["channel"] = "-1"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Failed to convert 'channel' key")); +} + +TEST(TestLEDSegmentInitialization, InvalidLedRangeExpression) +{ + auto segment_desc = CreateSegmentDescription("010", "1"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "No '-' character found in the led_range expression")); + + segment_desc["led_range"] = "s0-10"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Error converting string to integer")); + + segment_desc["led_range"] = "0-p10"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Error converting string to integer")); +} + +TEST(TestLEDSegmentInitialization, ValidDescription) +{ + const auto segment_desc = CreateSegmentDescription("0-10", "1"); + EXPECT_NO_THROW(panther_lights::LEDSegment(segment_desc, 10.0)); +} + +TEST(TestLEDSegmentInitialization, FirstLedPosition) +{ + auto segment_desc = CreateSegmentDescription("0-10", "1"); + std::shared_ptr led_segment; + + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "5-11"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "10-10"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(10 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "13-5"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "17-0"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); +} + +TEST_F(TestLEDSegment, GetAnimationFrameNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationFrame(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, GetAnimationProgressNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationProgress(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, ResetAnimationNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->ResetAnimation(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, GetAnimationBrightnessNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationBrightness(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, SetAnimationInvalidType) +{ + const YAML::Node animation_desc; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { + led_segment_->SetAnimation("panther_lights::WrongAnimationType}", animation_desc, false); + }, + "The plugin failed to load. Error: ")); +} + +TEST_F(TestLEDSegment, SetAnimationFailAnimationInitialization) +{ + const auto animation_desc = YAML::Load("{type: panther_lights::ImageAnimation}"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->SetAnimation("panther_lights::ImageAnimation", animation_desc, false); }, + "Failed to initialize animation: ")); +} + +TEST_F(TestLEDSegment, SetAnimation) +{ + // test each known animtion type + const auto image_anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + const auto charging_anim_desc = YAML::Load("{duration: 2}"); + + EXPECT_NO_THROW( + led_segment_->SetAnimation("panther_lights::ImageAnimation", image_anim_desc, false)); + + EXPECT_NO_THROW(led_segment_->SetAnimation( + "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); +} + +TEST_F(TestLEDSegment, SetAnimationRepeating) +{ + const auto anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() == nullptr); + + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + + EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() != nullptr); + EXPECT_TRUE(led_segment_->IsAnimationFinished()); +} + +TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->UpdateAnimation(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, UpdateAnimation) +{ + const auto anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + EXPECT_NO_THROW(led_segment_->UpdateAnimation()); + EXPECT_EQ(segment_led_num_ * 4, led_segment_->GetAnimationFrame().size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive) +{ + const auto anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + + auto default_anim = led_segment_->GetDefaultAnimation(); + while (!default_anim->IsFinished()) { + ASSERT_NO_THROW(led_segment_->UpdateAnimation()); + } + + // add new animation, and check if default animation was reset + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_FALSE(default_anim->IsFinished()); +} diff --git a/panther_lights/test/test_segment_converter.cpp b/panther_lights/test/test_segment_converter.cpp new file mode 100644 index 000000000..516d7eab2 --- /dev/null +++ b/panther_lights/test/test_segment_converter.cpp @@ -0,0 +1,187 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +class TestSegmentConverter : public testing::Test +{ +public: + TestSegmentConverter() + { + segment_converter_ = std::make_unique(); + + // create 2 basic panels with different number of leds + led_panels_.insert({1, std::make_unique(panel_1_num_led_)}); + led_panels_.insert({2, std::make_unique(panel_2_num_led_)}); + } + ~TestSegmentConverter() {} + +protected: + YAML::Node CreateSegmentDescription( + const std::size_t first_led, const std::size_t last_led, const std::size_t channel) const; + YAML::Node CreateImageAnimationDescription(); + + std::size_t panel_1_num_led_ = 20; + std::size_t panel_2_num_led_ = 30; + + std::unique_ptr segment_converter_; + std::unordered_map> segments_; + std::unordered_map> led_panels_; +}; + +YAML::Node TestSegmentConverter::CreateSegmentDescription( + const std::size_t first_led, const std::size_t last_led, const std::size_t channel) const +{ + YAML::Node desc; + desc["led_range"] = std::to_string(first_led) + "-" + std::to_string(last_led); + desc["channel"] = channel; + return desc; +} + +YAML::Node TestSegmentConverter::CreateImageAnimationDescription() +{ + return YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); +} + +TEST_F(TestSegmentConverter, ConvertInvalidChannel) +{ + segments_.emplace( + "name", + std::make_shared(CreateSegmentDescription(0, 10, 123), 50.0)); + const auto anim_desc = CreateImageAnimationDescription(); + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::out_of_range); +} + +TEST_F(TestSegmentConverter, ConvertInvalidLedRange) +{ + segments_.emplace( + "name", std::make_shared( + CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 1, 1), 50.0)); + const auto anim_desc = CreateImageAnimationDescription(); + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::runtime_error); +} + +TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) +{ + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); + segments_.emplace( + "name_2", std::make_shared( + CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); + + const auto anim_desc = CreateImageAnimationDescription(); + + for (auto & segment : segments_) { + ASSERT_NO_THROW( + segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW(segment.second->UpdateAnimation()); + } + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + +TEST_F(TestSegmentConverter, ConvertMultipleSegments) +{ + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0)); + segments_.emplace( + "name_2", + std::make_shared( + CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0)); + + segments_.emplace( + "name_3", std::make_shared( + CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0)); + segments_.emplace( + "name_4", + std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0)); + segments_.emplace( + "name_5", std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0)); + + const auto anim_desc = CreateImageAnimationDescription(); + for (auto & segment : segments_) { + ASSERT_NO_THROW( + segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW(segment.second->UpdateAnimation()); + } + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + +TEST_F(TestSegmentConverter, ConvertBrightnessOverride) +{ + const std::size_t channel = 1; + const float float_brightness = 0.2f; + const std::uint8_t expected_brightness = static_cast(round(float_brightness * 255)); + auto anim_desc = CreateImageAnimationDescription(); + anim_desc["brightness"] = float_brightness; + + segments_.emplace( + "name", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, channel), 50.0)); + + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + segment_converter_->Convert(segments_, led_panels_); + ASSERT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); + + const auto frame = led_panels_.at(channel)->GetFrame(); + + for (std::size_t i = 3; i < frame.size(); i += 4) { + EXPECT_EQ(expected_brightness, frame[i]); + } +} + +TEST_F(TestSegmentConverter, ConvertNoThrowIfAnimationNotSet) +{ + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); + segments_.emplace( + "name_2", std::make_shared( + CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index 176233959..2197c69e0 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(diagnostic_updater REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(realtime_tools REQUIRED) +find_package(yaml-cpp REQUIRED) include_directories(${GTEST_INCLUDE_DIRS}) @@ -55,6 +56,13 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_ros_test_utils rclcpp std_msgs) + + ament_add_gtest(${PROJECT_NAME}_test_yaml_utils test/test_yaml_utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_yaml_utils + PUBLIC $ + $) + target_link_libraries(${PROJECT_NAME}_test_yaml_utils yaml-cpp) endif() ament_export_dependencies(realtime_tools) diff --git a/panther_utils/include/panther_utils/test/ros_test_utils.hpp b/panther_utils/include/panther_utils/test/ros_test_utils.hpp index 2e4ffd7b3..868bd1ddc 100644 --- a/panther_utils/include/panther_utils/test/ros_test_utils.hpp +++ b/panther_utils/include/panther_utils/test/ros_test_utils.hpp @@ -38,7 +38,7 @@ namespace panther_utils::test_utils template bool WaitForMsg( const std::shared_ptr & node, std::shared_ptr & msg, - const std::chrono::nanoseconds & timeout) + const std::chrono::milliseconds & timeout) { msg = nullptr; rclcpp::Time start_time = node->now(); diff --git a/panther_utils/include/panther_utils/test/test_utils.hpp b/panther_utils/include/panther_utils/test/test_utils.hpp index 14bf8db59..7674735b9 100644 --- a/panther_utils/include/panther_utils/test/test_utils.hpp +++ b/panther_utils/include/panther_utils/test/test_utils.hpp @@ -45,25 +45,28 @@ bool CheckNaNVector(const std::vector & vector) } /** - * @brief Tests if a method throws an exception of a given type and the error message contains the + * @brief Check if a method throws an exception of a given type and the error message contains the * provided message * * @param func The method that will be tested * @param error_msg The error message that has to be contained within the thrown message + * + * @return True if method throws an exception of a given type and the error message contains the + * provided message, false otherwise */ template -void ExpectThrowWithDescription(const Func & func, const std::string & error_msg) +bool IsMessageThrown(const Func & func, const std::string & error_msg) { - EXPECT_THROW( - { - try { - func(); - } catch (const ExceptionType & e) { - EXPECT_TRUE(std::string(e.what()).find(error_msg) != std::string::npos); - throw; - } - }, - ExceptionType); + try { + func(); + } catch (const ExceptionType & e) { + if (std::string(e.what()).find(error_msg) != std::string::npos) { + return true; + } + } catch (...) { + } + + return false; } } // namespace panther_utils::test_utils diff --git a/panther_utils/include/panther_utils/yaml_utils.hpp b/panther_utils/include/panther_utils/yaml_utils.hpp new file mode 100644 index 000000000..7e9f73a5e --- /dev/null +++ b/panther_utils/include/panther_utils/yaml_utils.hpp @@ -0,0 +1,74 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_UTILS_YAML_UTILS_HPP_ +#define PANTHER_UTILS_YAML_UTILS_HPP_ + +#include +#include + +#include + +namespace panther_utils +{ + +/** + * @brief Parse YAML key value from description + * + * @param description YAML description + * @param key Key name + * + * @exception std::runtime_error if key does'n exists or fails to convert key value to given type + */ +template +T GetYAMLKeyValue(const YAML::Node & description, const std::string & key) +{ + if (!description[key]) { + throw std::runtime_error("Missing '" + static_cast(key) + "' in description"); + } + + try { + return description[key].as(); + } catch (const YAML::BadConversion & e) { + throw std::runtime_error("Failed to convert '" + static_cast(key) + "' key"); + } +} + +/** + * @brief Parse YAML key value from description + * + * @param description YAML description + * @param key Key name + * @param default_value Value that will be returned if key doesn't exists + * + * @exception std::runtime_error if fails to convert key value to given type + */ +template +T GetYAMLKeyValue(const YAML::Node & description, const std::string & key, const T default_value) +{ + try { + return GetYAMLKeyValue(description, key); + } catch (const std::runtime_error & e) { + if ( + std::string(e.what()).find( + "Missing '" + static_cast(key) + "' in description") != std::string::npos) { + return default_value; + } + throw; + } +} + +} // namespace panther_utils + +#endif // PANTHER_UTILS_YAML_UTILS_HPP_ diff --git a/panther_utils/package.xml b/panther_utils/package.xml index 9a520caaa..60c240977 100644 --- a/panther_utils/package.xml +++ b/panther_utils/package.xml @@ -19,6 +19,7 @@ rclcpp realtime_tools std_msgs + yaml-cpp ament_cmake_gtest ament_lint_auto diff --git a/panther_utils/test/test_test_utils.cpp b/panther_utils/test/test_test_utils.cpp index 6346e071a..b8b0a7a58 100644 --- a/panther_utils/test/test_test_utils.cpp +++ b/panther_utils/test/test_test_utils.cpp @@ -37,22 +37,49 @@ TEST(TestTestUtils, CheckNanVector) EXPECT_THROW(TestCheckNaNVector(), std::runtime_error); } -TEST(TestTestUtils, ExpectThrowWithDescription) +TEST(TestTestUtils, IsMessageThrownTrue) { - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::out_of_range("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::out_of_range("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::invalid_argument("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::invalid_argument("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "Example"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "exception")); +} + +TEST(TestTestUtils, IsMessageThrownDifferentException) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::out_of_range("Example exception"); }, "Example exception")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::invalid_argument("Example exception"); }, "Example exception")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception")); +} + +TEST(TestTestUtils, IsMessageThrownDifferentMessage) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Different exception message")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception ")); +} + +TEST(TestTestUtils, IsMessageThrownNoThrow) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { return; }, "Example exception")); } int main(int argc, char ** argv) diff --git a/panther_utils/test/test_yaml_utils.cpp b/panther_utils/test/test_yaml_utils.cpp new file mode 100644 index 000000000..cb3374236 --- /dev/null +++ b/panther_utils/test/test_yaml_utils.cpp @@ -0,0 +1,96 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +TEST(TestGetYAMLKeyValue, MissingKey) +{ + YAML::Node desc; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [desc]() { panther_utils::GetYAMLKeyValue(desc, "key_name"); }, + "Missing 'key_name' in description")); +} + +TEST(TestGetYAMLKeyValue, ConversionFailure) +{ + YAML::Node desc; + + desc["float_key"] = 1.5; + desc["string_key"] = "string"; + desc["int_vector_key"] = "[1 2 3]"; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "float_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "string_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "int_vector_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue>(desc, "string_key"); }, + "Failed to convert")); +} + +TEST(TestGetYAMLKeyValue, GetKey) +{ + YAML::Node desc; + + desc["int_key"] = 2; + desc["float_key"] = 1.5; + desc["string_key"] = "string"; + + const auto int_value = panther_utils::GetYAMLKeyValue(desc, "int_key"); + EXPECT_EQ(2, int_value); + + const auto float_value = panther_utils::GetYAMLKeyValue(desc, "float_key"); + EXPECT_EQ(1.5, float_value); + + const auto str_value = panther_utils::GetYAMLKeyValue(desc, "string_key"); + EXPECT_EQ("string", str_value); +} + +TEST(TestGetYAMLKeyValue, GetVectorKey) +{ + YAML::Node desc; + + desc["int_vector_key"] = std::vector(5, 147); + + const auto value_vector = panther_utils::GetYAMLKeyValue>( + desc, "int_vector_key"); + for (auto value : value_vector) { + EXPECT_EQ(147, value); + } +} + +TEST(TestGetYAMLKeyValue, GetKeyDefaultValue) +{ + YAML::Node desc; + const int default_value = 54; + + const auto value = panther_utils::GetYAMLKeyValue(desc, "key_name", default_value); + EXPECT_EQ(default_value, value); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}