ROS 2 lights converter

panther_gpiod/test/test_gpio_driver.cpp

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

panther_lights/CMakeLists.txt

@@ -10,6 +10,7 @@ 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)
@@ -37,8 +38,10 @@ ament_target_dependencies(
   rclcpp
   sensor_msgs)
 
-add_executable(controller_node src/controller_node_main.cpp
-                               src/controller_node.cpp)
+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)
 
 ament_target_dependencies(controller_node rclcpp pluginlib sensor_msgs)
 target_link_libraries(controller_node yaml-cpp)
@@ -92,6 +95,33 @@ if(BUILD_TESTING)
   ament_target_dependencies(${PROJECT_NAME}_test_charging_animation
                             ament_index_cpp pluginlib)
   target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp)
+
+  ament_add_gtest(${PROJECT_NAME}_test_led_panel test/test_led_panel.cpp
+                  src/led_panel.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_panel
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+
+  ament_add_gtest(${PROJECT_NAME}_test_led_segment test/test_led_segment.cpp
+                  src/led_segment.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_led_segment
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_led_segment pluginlib
+                            panther_utils)
+  target_link_libraries(${PROJECT_NAME}_test_led_segment yaml-cpp)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_segment_converter test/test_segment_converter.cpp
+    src/segment_converter.cpp src/led_panel.cpp src/led_segment.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_segment_converter
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib)
+  target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp)
 endif()
 
 ament_package()

panther_lights/include/panther_lights/animation/animation.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 Husarion sp. z o.o.
+// 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.
@@ -15,6 +15,7 @@
 #ifndef PANTHER_LIGHTS_ANIMATION_HPP_
 #define PANTHER_LIGHTS_ANIMATION_HPP_
 
+#include <algorithm>
 #include <cmath>
 #include <cstdint>
 #include <limits>
@@ -48,6 +49,7 @@ class Animation
   {
     Reset();
     num_led_ = num_led;
+    frame_ = std::vector<std::uint8_t>(num_led_ * kRGBAColorLen, 0);
 
     if (!animation_description["duration"]) {
       throw std::runtime_error("Missing 'duration' in animation description");
@@ -84,17 +86,16 @@ class Animation
   }
 
   /**
-   * @brief Update and return animation frame
+   * @brief Update animation frame
    *
-   * @returns the newest animation frame, if animation is finished will return vector filled with 0
    * @exception std::runtime_error if UpdateFrame() method returns frame with invalid size
    */
-  std::vector<std::uint8_t> Call()
+  void Update()
   {
     if (current_cycle_ < loops_) {
       auto frame = UpdateFrame();
 
-      if (frame.size() != num_led_ * 4) {
+      if (frame.size() != num_led_ * kRGBAColorLen) {
         throw std::runtime_error(
           "Invalid frame size. Check animation UpdateFrame() method implementation");
       }
@@ -111,10 +112,11 @@ class Animation
         finished_ = true;
       }
 
-      return frame;
+      frame_ = frame;
+      return;
     }
 
-    return std::vector<std::uint8_t>(num_led_ * 4, 0);
+    std::fill(frame_.begin(), frame_.end(), 0);
   }
 
   /**
@@ -129,22 +131,46 @@ class Animation
     progress_ = 0.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<std::uint8_t> 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_; }
 
-  virtual void SetParam(const std::string & /*param*/){};
+  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_ * 4
+   * it should return RGBA animation frame with size equal to num_led_ * kRGBAColorLen
    */
   virtual std::vector<std::uint8_t> UpdateFrame() = 0;
 
+  std::vector<std::uint8_t> InvertRGBAFrame(const std::vector<std::uint8_t> & frame) const
+  {
+    std::vector<std::uint8_t> 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_; }
 
@@ -161,6 +187,7 @@ class Animation
   std::uint8_t brightness_ = 255;
 
   std::string param_;
+  std::vector<std::uint8_t> frame_;
 };
 
 }  // namespace panther_lights

panther_lights/include/panther_lights/animation/charging_animation.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 Husarion sp. z o.o.
+// 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.

panther_lights/include/panther_lights/animation/image_animation.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 Husarion sp. z o.o.
+// 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.
@@ -52,7 +52,7 @@ class ImageAnimation : public Animation
    * @param image_path raw path to an image, it should be a global path or should contain '$(find
    * ros_package)` syntax
    *
-   * @returns global path to an image file
+   * @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;

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
    */

panther_lights/include/panther_lights/controller_node.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 Husarion sp. z o.o.
+// 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.

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 <cstdint>
+#include <vector>
+
+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<std::uint8_t> & values);
+
+  std::vector<std::uint8_t> GetFrame() const { return frame_; }
+  std::size_t GetNumberOfLeds() const { return num_led_; }
+
+private:
+  const std::size_t num_led_;
+  std::vector<std::uint8_t> frame_;
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_LED_PANEL_HPP_

panther_lights/include/panther_lights/led_segment.hpp

@@ -0,0 +1,90 @@
+// 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 <cstdint>
+#include <vector>
+
+#include <yaml-cpp/yaml.h>
+
+#include <pluginlib/class_loader.hpp>
+
+#include <panther_lights/animation/animation.hpp>
+
+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 phisical 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
+   *
+   * @exception std::runtime_error if 'type' key is missing, given pluginlib fails to load or
+   * animation fails to initialize
+   */
+  void SetAnimation(const YAML::Node & animation_description);
+
+  /**
+   * @brief Update animation frame
+   *
+   * @exception std::runtime_error if fails to update animation
+   */
+  void UpdateAnimation(const std::string & param = "");
+
+  std::size_t GetFirstLEDPosition() const;
+  std::size_t GetChannel() const { return channel_; }
+  std::vector<std::uint8_t> GetAnimationFrame() const
+  {
+    return animation_->GetFrame(invert_led_order_);
+  }
+
+private:
+  const float controller_frequency_;
+  bool invert_led_order_ = false;
+  std::size_t channel_;
+  std::size_t first_led_iterator_;
+  std::size_t last_led_iterator_;
+  std::size_t num_led_;
+
+  std::shared_ptr<panther_lights::Animation> animation_;
+  std::shared_ptr<pluginlib::ClassLoader<panther_lights::Animation>> animation_loader_;
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_LED_SEGMENT_HPP_