From ce92e45ff402c53f6d307fd82551d38d7a301a1c Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 3 Sep 2024 10:37:48 +0000 Subject: [PATCH 01/18] Implement APA102Interface remove spatial namespace --- .../include/panther_lights/apa102.hpp | 25 +++++++++---- .../panther_lights/lights_driver_node.hpp | 20 ++++++----- panther_lights/src/apa102.cpp | 4 +-- panther_lights/src/lights_driver_node.cpp | 36 +++++++++---------- panther_lights/test/test_apa102.cpp | 6 ++-- .../test/test_lights_driver_node.cpp | 4 +-- 6 files changed, 56 insertions(+), 39 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 323a4b0d2..12c644f2c 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -16,19 +16,32 @@ #define PANTHER_LIGHTS_APA102_HPP_ #include +#include #include #include -namespace panther_lights::apa102 +namespace panther_lights { +class APA102Interface +{ +public: + virtual ~APA102Interface() = default; + + virtual void SetGlobalBrightness(const std::uint8_t brightness) = 0; + virtual void SetGlobalBrightness(const float brightness) = 0; + virtual void SetPanel(const std::vector & frame) const = 0; + + using SharedPtr = std::shared_ptr; +}; + /** * @brief Class representing an APA102 LED panel. * * This class provides methods to control the APA102 LED panel, including setting the global * brightness, setting the LED panel based on a given frame. */ -class APA102 +class APA102 : public APA102Interface { public: APA102( @@ -42,7 +55,7 @@ class APA102 * * @exception std::out_of_range if brightness value is out of defined range */ - void SetGlobalBrightness(const std::uint8_t brightness); + void SetGlobalBrightness(const std::uint8_t brightness) override; /** * @brief Set APA102 LED global brightness @@ -51,7 +64,7 @@ class APA102 * * @exception std::out_of_range if brightness value is out of defined range */ - void SetGlobalBrightness(const float brightness); + void SetGlobalBrightness(const float brightness) override; /** * @brief Set APA102 LED panel based on given frame @@ -61,7 +74,7 @@ class APA102 * @exception std::ios_base::failure if failed to send data over SPI * or std::runtime_error if frame is invalid */ - void SetPanel(const std::vector & frame) const; + void SetPanel(const std::vector & frame) const override; protected: /** @@ -96,6 +109,6 @@ class APA102 const std::uint32_t speed_; }; -} // namespace panther_lights::apa102 +} // namespace panther_lights #endif // PANTHER_LIGHTS_APA102_HPP_ diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index 3a650f874..17299b10f 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -43,14 +43,18 @@ class LightsDriverNode : public rclcpp::Node public: LightsDriverNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + LightsDriverNode( + APA102Interface::SharedPtr channel_1, APA102Interface::SharedPtr channel_2, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + protected: int num_led_; double frame_timeout_; bool led_control_granted_; bool led_control_pending_; - rclcpp::Time chanel_1_ts_; - rclcpp::Time chanel_2_ts_; + rclcpp::Time channel_1_ts_; + rclcpp::Time channel_2_ts_; private: void OnShutdown(); @@ -86,8 +90,8 @@ class LightsDriverNode : public rclcpp::Node * logging. Valid names are: 'channel_1', 'channel_2'. */ void FrameCB( - const ImageMsg::UniquePtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, - const std::string & panel_name); + const ImageMsg::UniquePtr & msg, const APA102Interface::SharedPtr & panel, + const rclcpp::Time & last_time, const std::string & panel_name); void SetBrightnessCB( const SetLEDBrightnessSrv::Request::SharedPtr & request, @@ -112,8 +116,8 @@ class LightsDriverNode : public rclcpp::Node unsigned initialization_attempt_; rclcpp::Time led_control_call_time_; - apa102::APA102 chanel_1_; - apa102::APA102 chanel_2_; + APA102Interface::SharedPtr channel_1_; + APA102Interface::SharedPtr channel_2_; rclcpp::TimerBase::SharedPtr initialization_timer_; @@ -122,8 +126,8 @@ class LightsDriverNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_callback_group_; - rclcpp::Subscription::SharedPtr chanel_1_sub_; - rclcpp::Subscription::SharedPtr chanel_2_sub_; + rclcpp::Subscription::SharedPtr channel_1_sub_; + rclcpp::Subscription::SharedPtr channel_2_sub_; diagnostic_updater::Updater diagnostic_updater_; }; diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index ab527f3bc..77eb77ff0 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -27,7 +27,7 @@ #include #include -namespace panther_lights::apa102 +namespace panther_lights { APA102::APA102(const std::string & device, const std::uint32_t speed, const bool cs_high) @@ -126,4 +126,4 @@ void APA102::SPISendBuffer(const std::vector & buffer) const } } -} // namespace panther_lights::apa102 +} // namespace panther_lights diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index d746c6e18..eefd66381 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -44,8 +44,8 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) led_control_granted_(false), led_control_pending_(false), initialization_attempt_(0), - chanel_1_("/dev/spiled-channel1"), - chanel_2_("/dev/spiled-channel2"), + channel_1_(std::make_shared("/dev/spiled-channel1")), + channel_2_(std::make_shared("/dev/spiled-channel2")), diagnostic_updater_(this) { RCLCPP_INFO(this->get_logger(), "Constructing node."); @@ -60,8 +60,8 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) num_led_ = this->get_parameter("num_led").as_int(); const float global_brightness = this->get_parameter("global_brightness").as_double(); - chanel_1_.SetGlobalBrightness(global_brightness); - chanel_2_.SetGlobalBrightness(global_brightness); + channel_1_->SetGlobalBrightness(global_brightness); + channel_2_->SetGlobalBrightness(global_brightness); client_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -76,18 +76,18 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) initialization_timer_ = this->create_wall_timer( std::chrono::milliseconds(100), std::bind(&LightsDriverNode::InitializationTimerCB, this)); - chanel_1_ts_ = this->get_clock()->now(); - chanel_1_sub_ = this->create_subscription( + channel_1_ts_ = this->get_clock()->now(); + channel_1_sub_ = this->create_subscription( "lights/channel_1_frame", 5, [&](const ImageMsg::UniquePtr & msg) { - FrameCB(msg, chanel_1_, chanel_1_ts_, "channel_1"); - chanel_1_ts_ = msg->header.stamp; + FrameCB(msg, channel_1_, channel_1_ts_, "channel_1"); + channel_1_ts_ = msg->header.stamp; }); - chanel_2_ts_ = this->get_clock()->now(); - chanel_2_sub_ = this->create_subscription( + channel_2_ts_ = this->get_clock()->now(); + channel_2_sub_ = this->create_subscription( "lights/channel_2_frame", 5, [&](const ImageMsg::UniquePtr & msg) { - FrameCB(msg, chanel_2_, chanel_2_ts_, "channel_2"); - chanel_2_ts_ = msg->header.stamp; + FrameCB(msg, channel_2_, channel_2_ts_, "channel_2"); + channel_2_ts_ = msg->header.stamp; }); diagnostic_updater_.setHardwareID("Bumper Lights"); @@ -133,8 +133,8 @@ void LightsDriverNode::InitializationTimerCB() void LightsDriverNode::ClearLEDs() { - chanel_1_.SetPanel(std::vector(num_led_ * 4, 0)); - chanel_2_.SetPanel(std::vector(num_led_ * 4, 0)); + channel_1_->SetPanel(std::vector(num_led_ * 4, 0)); + channel_2_->SetPanel(std::vector(num_led_ * 4, 0)); } void LightsDriverNode::ToggleLEDControl(const bool enable) @@ -191,7 +191,7 @@ void LightsDriverNode::ToggleLEDControlCB( } void LightsDriverNode::FrameCB( - const ImageMsg::UniquePtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, + const ImageMsg::UniquePtr & msg, const APA102::SharedPtr & panel, const rclcpp::Time & last_time, const std::string & panel_name) { if (!led_control_granted_) { @@ -222,7 +222,7 @@ void LightsDriverNode::FrameCB( return; } - panel.SetPanel(msg->data); + panel->SetPanel(msg->data); } void LightsDriverNode::SetBrightnessCB( @@ -231,8 +231,8 @@ void LightsDriverNode::SetBrightnessCB( const float brightness = req->data; try { - chanel_1_.SetGlobalBrightness(brightness); - chanel_2_.SetGlobalBrightness(brightness); + channel_1_->SetGlobalBrightness(brightness); + channel_2_->SetGlobalBrightness(brightness); } catch (const std::out_of_range & e) { res->success = false; res->message = "Failed to set brightness: " + std::string(e.what()); diff --git a/panther_lights/test/test_apa102.cpp b/panther_lights/test/test_apa102.cpp index f38320f95..d0f1e37d4 100644 --- a/panther_lights/test/test_apa102.cpp +++ b/panther_lights/test/test_apa102.cpp @@ -16,7 +16,7 @@ #include "panther_lights/apa102.hpp" -class APA102Wrapper : public panther_lights::apa102::APA102 +class APA102Wrapper : public panther_lights::APA102 { public: APA102Wrapper(const std::string & device) : APA102(device) {} @@ -40,8 +40,8 @@ class TestAPA102 : public testing::Test TEST_F(TestAPA102, PortsAvailable) { - EXPECT_NO_THROW({ panther_lights::apa102::APA102 chanel_1_("/dev/spidev0.0"); }); - EXPECT_NO_THROW({ panther_lights::apa102::APA102 chanel_2_("/dev/spidev0.1"); }); + EXPECT_NO_THROW({ panther_lights::APA102 channel_1_("/dev/spidev0.0"); }); + EXPECT_NO_THROW({ panther_lights::APA102 channel_2_("/dev/spidev0.1"); }); } TEST_F(TestAPA102, SetGlobalBrightnessFloat) diff --git a/panther_lights/test/test_lights_driver_node.cpp b/panther_lights/test/test_lights_driver_node.cpp index 244269609..bcc7a07b3 100644 --- a/panther_lights/test/test_lights_driver_node.cpp +++ b/panther_lights/test/test_lights_driver_node.cpp @@ -42,8 +42,8 @@ class DriverNodeWrapper : public panther_lights::LightsDriverNode int getNumLeds() const { return num_led_; } double getTimeout() const { return frame_timeout_; } bool isInitialised() const { return led_control_granted_; } - rclcpp::Time setChanel1TS(const rclcpp::Time & ts) { return chanel_1_ts_ = ts; } - rclcpp::Time setChanel2TS(const rclcpp::Time & ts) { return chanel_2_ts_ = ts; } + rclcpp::Time setChanel1TS(const rclcpp::Time & ts) { return channel_1_ts_ = ts; } + rclcpp::Time setChanel2TS(const rclcpp::Time & ts) { return channel_2_ts_ = ts; } }; class TestDriverNode : public testing::Test From aab6fa344e069989ee030b2aac895c5045d21f36 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 4 Sep 2024 13:12:15 +0000 Subject: [PATCH 02/18] Test apa102 --- .../include/panther_lights/apa102.hpp | 62 ++++++- panther_lights/src/apa102.cpp | 46 ++--- panther_lights/src/lights_driver_node.cpp | 4 +- panther_lights/test/test_apa102.cpp | 175 ++++++++++++++---- 4 files changed, 218 insertions(+), 69 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 12c644f2c..801a0610c 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -15,6 +15,11 @@ #ifndef PANTHER_LIGHTS_APA102_HPP_ #define PANTHER_LIGHTS_APA102_HPP_ +#include +#include +#include +#include + #include #include #include @@ -23,6 +28,50 @@ namespace panther_lights { +class SPIDeviceInterface +{ +public: + virtual ~SPIDeviceInterface() = default; + + /** + * @brief Open SPI device + * + * @param device Name of the device + */ + virtual int Open(const std::string & device) = 0; + + /** + * @brief Perform an I/O control operation on the device + * + * @param fd File descriptor + * @param request Request code + * @param arg Argument + */ + virtual int IOControl(int fd, unsigned long request, const void * arg) = 0; + + /** + * @brief Close the device + * + * @param fd File descriptor + */ + virtual int Close(int fd) = 0; + + using SharedPtr = std::shared_ptr; +}; + +class SPIDevice : public SPIDeviceInterface +{ +public: + int Open(const std::string & device) override { return ::open(device.c_str(), O_WRONLY); } + + int IOControl(int fd, unsigned long request, const void * arg) override + { + return ::ioctl(fd, request, arg); + } + + int Close(int fd) override { return ::close(fd); } +}; + class APA102Interface { public: @@ -40,12 +89,18 @@ class APA102Interface * * This class provides methods to control the APA102 LED panel, including setting the global * brightness, setting the LED panel based on a given frame. + * + * @param spi_device SPI Device object + * @param device_name name of the device + * @param speed Speed of the SPI communication + * @param cs_high Chip select high flag */ class APA102 : public APA102Interface { public: APA102( - const std::string & device, const std::uint32_t speed = 800000, const bool cs_high = false); + SPIDeviceInterface::SharedPtr spi_device, const std::string & device_name, + const std::uint32_t speed = 800000, const bool cs_high = false); ~APA102(); /** @@ -104,9 +159,10 @@ class APA102 : public APA102Interface static constexpr std::uint16_t kCorrGreen = 200; static constexpr std::uint16_t kCorrBlue = 62; - const int fd_; - const std::string device_; + SPIDeviceInterface::SharedPtr spi_device_; + const std::string device_name_; const std::uint32_t speed_; + const int file_descriptor_; }; } // namespace panther_lights diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 77eb77ff0..8a2a70aa6 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -14,11 +14,6 @@ #include "panther_lights/apa102.hpp" -#include -#include -#include -#include - #include #include #include @@ -30,36 +25,41 @@ namespace panther_lights { -APA102::APA102(const std::string & device, const std::uint32_t speed, const bool cs_high) -: fd_(open(device.c_str(), O_WRONLY)), device_(device), speed_(speed) +APA102::APA102( + SPIDeviceInterface::SharedPtr spi_device, const std::string & device_name, + const std::uint32_t speed, const bool cs_high) +: spi_device_(spi_device), + device_name_(device_name), + speed_(speed), + file_descriptor_(spi_device->Open(device_name)) { - if (fd_ < 0) { - throw std::ios_base::failure("Failed to open " + device_ + "."); + if (file_descriptor_ < 0) { + throw std::ios_base::failure("Failed to open " + device_name_ + "."); } static std::uint8_t mode = cs_high ? SPI_MODE_3 : SPI_MODE_3 | SPI_CS_HIGH; - if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) { - close(fd_); - throw std::ios_base::failure("Failed to set mode for " + device_ + "."); + if (spi_device_->IOControl(file_descriptor_, SPI_IOC_WR_MODE32, &mode) == -1) { + spi_device_->Close(file_descriptor_); + throw std::ios_base::failure("Failed to set mode for " + device_name_ + "."); } - if (ioctl(fd_, SPI_IOC_WR_BITS_PER_WORD, &kBits) == -1) { - close(fd_); - throw std::ios_base::failure("Can't set bits per word for " + device_ + "."); + if (spi_device_->IOControl(file_descriptor_, SPI_IOC_WR_BITS_PER_WORD, &kBits) == -1) { + spi_device_->Close(file_descriptor_); + throw std::ios_base::failure("Can't set bits per word for " + device_name_ + "."); } - if (ioctl(fd_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) { - close(fd_); - throw std::ios_base::failure("Can't set speed for " + device_ + "."); + if (spi_device_->IOControl(file_descriptor_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) { + spi_device_->Close(file_descriptor_); + throw std::ios_base::failure("Can't set speed for " + device_name_ + "."); } } -APA102::~APA102() { close(fd_); } +APA102::~APA102() { spi_device_->Close(file_descriptor_); } void APA102::SetGlobalBrightness(const float brightness) { if (brightness < 0.0f || brightness > 1.0f) { - throw std::out_of_range("Brightness out of range <0.0,1.0>."); + throw std::out_of_range("Brightness out of range [0.0, 1.0]."); } SetGlobalBrightness(std::uint8_t(ceil(brightness * 31.0f))); } @@ -67,7 +67,7 @@ void APA102::SetGlobalBrightness(const float brightness) void APA102::SetGlobalBrightness(const std::uint8_t brightness) { if (brightness > 31) { - throw std::out_of_range("Brightness out of range <0,31>."); + throw std::out_of_range("Brightness out of range [0, 31]."); } global_brightness_ = std::uint16_t(brightness); } @@ -119,10 +119,10 @@ void APA102::SPISendBuffer(const std::vector & buffer) const tr.delay_usecs = 0; tr.bits_per_word = kBits; - const int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr); + const int ret = spi_device_->IOControl(file_descriptor_, SPI_IOC_MESSAGE(1), &tr); if (ret < 1) { - throw std::ios_base::failure("Failed to send data over SPI " + device_ + "."); + throw std::ios_base::failure("Failed to send data over SPI " + device_name_ + "."); } } diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index eefd66381..966a0730a 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -44,8 +44,8 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) led_control_granted_(false), led_control_pending_(false), initialization_attempt_(0), - channel_1_(std::make_shared("/dev/spiled-channel1")), - channel_2_(std::make_shared("/dev/spiled-channel2")), + channel_1_(std::make_shared(std::make_shared(), "/dev/spiled-channel1")), + channel_2_(std::make_shared(std::make_shared(), "/dev/spiled-channel2")), diagnostic_updater_(this) { RCLCPP_INFO(this->get_logger(), "Constructing node."); diff --git a/panther_lights/test/test_apa102.cpp b/panther_lights/test/test_apa102.cpp index d0f1e37d4..a0307d14e 100644 --- a/panther_lights/test/test_apa102.cpp +++ b/panther_lights/test/test_apa102.cpp @@ -12,14 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gtest/gtest.h" +#include +#include #include "panther_lights/apa102.hpp" +static constexpr char kMockDeviceName[] = "/dev/mocked_device"; +static constexpr int kStartFrame = 0x00; +static constexpr int kEndFrame = 0xFF; + +class MockSPIDevice : public panther_lights::SPIDeviceInterface +{ +public: + MOCK_METHOD(int, Open, (const std::string & device), (override)); + MOCK_METHOD(int, IOControl, (int fd, unsigned long request, const void * arg), (override)); + MOCK_METHOD(int, Close, (int fd), (override)); + + // Nice mock suppresses warnings about uninteresting calls + using NiceMock = testing::NiceMock; +}; + class APA102Wrapper : public panther_lights::APA102 { public: - APA102Wrapper(const std::string & device) : APA102(device) {} + APA102Wrapper(std::shared_ptr spi_device, const std::string & device_name) + : APA102(spi_device, device_name) + { + } std::vector RGBAFrameToBGRBuffer(const std::vector & frame) const { @@ -31,52 +50,121 @@ class APA102Wrapper : public panther_lights::APA102 class TestAPA102 : public testing::Test { protected: - TestAPA102() { apa102_ = std::make_unique("/dev/spidev0.0"); } - + TestAPA102(); ~TestAPA102() { apa102_.reset(); } + std::shared_ptr spi_device_; std::unique_ptr apa102_; }; -TEST_F(TestAPA102, PortsAvailable) +TestAPA102::TestAPA102() +{ + spi_device_ = std::make_shared(); + + apa102_ = std::make_unique(spi_device_, kMockDeviceName); +} + +TEST_F(TestAPA102, TestInitialization) { EXPECT_TRUE(spi_device_ != nullptr); } + +TEST(TestInitialization, InvalidDevices) +{ + auto spi_device = std::make_shared(); + + // Return -1 to simulate failed device opening + ON_CALL(*spi_device, Open(kMockDeviceName)).WillByDefault(testing::Return(-1)); + + EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); + , std::ios_base::failure); +} + +TEST(TestInitialization, SetModeFailure) +{ + auto spi_device = std::make_shared(); + + // Return -1 to simulate failed setting mode + ON_CALL(*spi_device, IOControl(testing::_, SPI_IOC_WR_MODE32, testing::_)) + .WillByDefault(testing::Return(-1)); + EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); + + EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); + , std::ios_base::failure); +} + +TEST(TestInitialization, SetBitsFailure) { - EXPECT_NO_THROW({ panther_lights::APA102 channel_1_("/dev/spidev0.0"); }); - EXPECT_NO_THROW({ panther_lights::APA102 channel_2_("/dev/spidev0.1"); }); + auto spi_device = std::make_shared(); + + // Return -1 to simulate failed setting bits + ON_CALL(*spi_device, IOControl(testing::_, SPI_IOC_WR_BITS_PER_WORD, testing::_)) + .WillByDefault(testing::Return(-1)); + EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); + + EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); + , std::ios_base::failure); +} + +TEST(TestInitialization, SetSpeedFailure) +{ + auto spi_device = std::make_shared(); + + // Return -1 to simulate failed setting speed + ON_CALL(*spi_device, IOControl(testing::_, SPI_IOC_WR_MAX_SPEED_HZ, testing::_)) + .WillByDefault(testing::Return(-1)); + EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); + + EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); + , std::ios_base::failure); +} + +TEST_F(TestAPA102, SetGlobalBrightnessRatioNegative) +{ + const float brightness_ratio = -1.0; + EXPECT_THROW(apa102_->SetGlobalBrightness(brightness_ratio), std::out_of_range); } -TEST_F(TestAPA102, SetGlobalBrightnessFloat) +TEST_F(TestAPA102, SetGlobalBrightnessRatioTooHigh) { - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(static_cast(0))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 0); + const float brightness_ratio = 1.1; + EXPECT_THROW(apa102_->SetGlobalBrightness(brightness_ratio), std::out_of_range); +} - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(static_cast(0.001))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 1); +TEST_F(TestAPA102, SetGlobalBrightnessRatioValid) +{ + const float brightness_ratio = 0.5; - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(static_cast(0.5))); + ASSERT_NO_THROW(apa102_->SetGlobalBrightness(brightness_ratio)); EXPECT_EQ(apa102_->GetGlobalBrightness(), 16); +} + +TEST_F(TestAPA102, SetGlobalBrightnessNegative) +{ + // Wrap around to 255 + const std::uint8_t brightness = -1; - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(static_cast(0.999))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 31); + EXPECT_THROW(apa102_->SetGlobalBrightness(brightness), std::out_of_range); +} - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(static_cast(1.0))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 31); +TEST_F(TestAPA102, SetGlobalBrightnessTooHigh) +{ + const std::uint8_t brightness = 32; - EXPECT_THROW(apa102_->SetGlobalBrightness(static_cast(-1.0)), std::out_of_range); - EXPECT_THROW(apa102_->SetGlobalBrightness(static_cast(1.1)), std::out_of_range); + EXPECT_THROW(apa102_->SetGlobalBrightness(brightness), std::out_of_range); } -TEST_F(TestAPA102, SetGlobalBrightnessUint8) +TEST_F(TestAPA102, SetGlobalBrightnessValid) { - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(std::uint8_t(0))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 0); + const std::uint8_t brightness = 16; - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(std::uint8_t(16))); + ASSERT_NO_THROW(apa102_->SetGlobalBrightness(brightness)); EXPECT_EQ(apa102_->GetGlobalBrightness(), 16); +} - EXPECT_NO_THROW(apa102_->SetGlobalBrightness(std::uint8_t(31))); - EXPECT_EQ(apa102_->GetGlobalBrightness(), 31); +TEST_F(TestAPA102, RGBAFrameToBGRBufferInvalidFrame) +{ + std::vector frame = { + 255, 128, 64}; // Valid frame requires 4 values to match RGBA format - EXPECT_THROW(apa102_->SetGlobalBrightness(std::uint8_t(32)), std::out_of_range); + EXPECT_THROW(apa102_->RGBAFrameToBGRBuffer(frame), std::runtime_error); } TEST_F(TestAPA102, RGBAFrameToBGRBuffer) @@ -86,23 +174,28 @@ TEST_F(TestAPA102, RGBAFrameToBGRBuffer) apa102_->SetGlobalBrightness(std::uint8_t(16)); auto buffer = apa102_->RGBAFrameToBGRBuffer(frame); - EXPECT_EQ(buffer.size(), static_cast(12)); - EXPECT_EQ(buffer[0], 0x00); // Init frame - EXPECT_EQ(buffer[1], 0x00); // Init frame - EXPECT_EQ(buffer[2], 0x00); // Init frame - EXPECT_EQ(buffer[3], 0x00); // Init frame - EXPECT_EQ(buffer[4], 0xEC); // brightness value based on the frame - EXPECT_EQ(buffer[5], 0x0F); // B component after color correction - EXPECT_EQ(buffer[6], 0x64); // G component after color correction - EXPECT_EQ(buffer[7], 0xFF); // R component after color correction - EXPECT_EQ(buffer[8], 0xFF); // End frame - EXPECT_EQ(buffer[9], 0xFF); // End frame - EXPECT_EQ(buffer[10], 0xFF); // End frame - EXPECT_EQ(buffer[11], 0xFF); // End frame + ASSERT_EQ(buffer.size(), static_cast(12)); + + // Verify start frame + for (int i = 0; i < 4; i++) { + EXPECT_EQ(buffer[i], kStartFrame); + } + // Verify end frame + for (int i = 8; i < 12; i++) { + EXPECT_EQ(buffer[i], kEndFrame); + } + + // Verify RGBA frame + EXPECT_EQ(buffer[4], 0xEC); // brightness value based on the frame + EXPECT_EQ(buffer[5], 0x0F); // B component after color correction + EXPECT_EQ(buffer[6], 0x64); // G component after color correction + EXPECT_EQ(buffer[7], 0xFF); // R component after color correction } int main(int argc, char ** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + + auto result = RUN_ALL_TESTS(); + return result; } From fb716137df85afba54a9c13e60a8494fb90eda33 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Thu, 5 Sep 2024 12:15:02 +0000 Subject: [PATCH 03/18] Implement tests for lights driver --- .../panther_lights/lights_driver_node.hpp | 48 +-- panther_lights/src/lights_driver_node.cpp | 4 +- .../test/test_lights_driver_node.cpp | 374 +++++++++++------- 3 files changed, 261 insertions(+), 165 deletions(-) diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index 17299b10f..094dc091a 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -48,31 +48,11 @@ class LightsDriverNode : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: - int num_led_; - double frame_timeout_; - bool led_control_granted_; - bool led_control_pending_; - - rclcpp::Time channel_1_ts_; - rclcpp::Time channel_2_ts_; - -private: - void OnShutdown(); - - void InitializationTimerCB(); - /** * @brief Clears all LEDs on both channels. */ void ClearLEDs(); - /** - * @brief Toggles LED control ON or OFF. - * - * @param enable True to enable LED control, false to disable. - */ - void ToggleLEDControl(const bool enable); - /** * @brief Callback to execute when service invoked to toggle LED control returns response. * @@ -93,10 +73,6 @@ class LightsDriverNode : public rclcpp::Node const ImageMsg::UniquePtr & msg, const APA102Interface::SharedPtr & panel, const rclcpp::Time & last_time, const std::string & panel_name); - void SetBrightnessCB( - const SetLEDBrightnessSrv::Request::SharedPtr & request, - SetLEDBrightnessSrv::Response::SharedPtr response); - /** * @brief Logs a warning message to the panel throttle log. Since this is throttle warning, we * need to add panel name condition to log from both panels. @@ -107,6 +83,30 @@ class LightsDriverNode : public rclcpp::Node */ void PanelThrottleWarnLog(const std::string panel_name, const std::string message); + int num_led_; + double frame_timeout_; + bool led_control_granted_; + bool led_control_pending_; + + rclcpp::Time channel_1_ts_; + rclcpp::Time channel_2_ts_; + +private: + void OnShutdown(); + + void InitializationTimerCB(); + + /** + * @brief Toggles LED control ON or OFF. + * + * @param enable True to enable LED control, false to disable. + */ + void ToggleLEDControl(const bool enable); + + void SetBrightnessCB( + const SetLEDBrightnessSrv::Request::SharedPtr & request, + SetLEDBrightnessSrv::Response::SharedPtr response); + void DiagnoseLights(diagnostic_updater::DiagnosticStatusWrapper & status); static constexpr unsigned kMaxInitializationAttempts = 3; diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 966a0730a..e014695ae 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -191,8 +191,8 @@ void LightsDriverNode::ToggleLEDControlCB( } void LightsDriverNode::FrameCB( - const ImageMsg::UniquePtr & msg, const APA102::SharedPtr & panel, const rclcpp::Time & last_time, - const std::string & panel_name) + const ImageMsg::UniquePtr & msg, const APA102Interface::SharedPtr & panel, + const rclcpp::Time & last_time, const std::string & panel_name) { if (!led_control_granted_) { PanelThrottleWarnLog( diff --git a/panther_lights/test/test_lights_driver_node.cpp b/panther_lights/test/test_lights_driver_node.cpp index bcc7a07b3..3602a3919 100644 --- a/panther_lights/test/test_lights_driver_node.cpp +++ b/panther_lights/test/test_lights_driver_node.cpp @@ -14,191 +14,287 @@ #include -#include "gtest/gtest.h" +#include +#include -#include "rclcpp/rclcpp.hpp" +#include -#include "sensor_msgs/image_encodings.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "std_srvs/srv/set_bool.hpp" +#include +#include +#include -#include "panther_lights/lights_driver_node.hpp" #include "panther_msgs/srv/set_led_brightness.hpp" + +#include "panther_lights/apa102.hpp" +#include "panther_lights/lights_driver_node.hpp" #include "panther_utils/test/ros_test_utils.hpp" using ImageMsg = sensor_msgs::msg::Image; using SetBoolSrv = std_srvs::srv::SetBool; using SetLEDBrightnessSrv = panther_msgs::srv::SetLEDBrightness; +class MockAPA102 : public panther_lights::APA102Interface +{ +public: + MOCK_METHOD(void, SetGlobalBrightness, (const std::uint8_t brightness), (override)); + MOCK_METHOD(void, SetGlobalBrightness, (const float brightness), (override)); + MOCK_METHOD(void, SetPanel, (const std::vector & frame), (const, override)); + + using NiceMock = testing::NiceMock; +}; + +// LightsDriverNode constructor implemented for testing purposes +panther_lights::LightsDriverNode::LightsDriverNode( + APA102Interface::SharedPtr channel_1, APA102Interface::SharedPtr channel_2, + const rclcpp::NodeOptions & options) +: Node("lights_driver", options), + led_control_granted_(false), + led_control_pending_(false), + initialization_attempt_(0), + channel_1_(channel_1), + channel_2_(channel_2), + diagnostic_updater_(this) +{ + num_led_ = 46; + frame_timeout_ = 0.1; +}; + class DriverNodeWrapper : public panther_lights::LightsDriverNode { public: DriverNodeWrapper( + std::shared_ptr channel_1, std::shared_ptr channel_2, const rclcpp::NodeOptions & options = rclcpp::NodeOptions().use_intra_process_comms(true)) - : LightsDriverNode(options) + : LightsDriverNode(channel_1, channel_2, options) { } - int getNumLeds() const { return num_led_; } - double getTimeout() const { return frame_timeout_; } - bool isInitialised() const { return led_control_granted_; } - rclcpp::Time setChanel1TS(const rclcpp::Time & ts) { return channel_1_ts_ = ts; } - rclcpp::Time setChanel2TS(const rclcpp::Time & ts) { return channel_2_ts_ = ts; } -}; + void ClearLEDs() { return LightsDriverNode::ClearLEDs(); } -class TestDriverNode : public testing::Test -{ -public: - TestDriverNode() + void ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future) { - service_node_ = std::make_shared("dummy_service_node"); - server_callback_group_ = - service_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - led_control_enable_service_ = service_node_->create_service( - "hardware/led_control_enable", - [](const SetBoolSrv::Request::SharedPtr & req, SetBoolSrv::Response::SharedPtr res) { - res->success = req->data; - }, - - rmw_qos_profile_services_default, server_callback_group_); - - lights_driver_node_ = std::make_shared(); - channel_1_pub_ = lights_driver_node_->create_publisher("lights/channel_1_frame", 5); - channel_2_pub_ = lights_driver_node_->create_publisher("lights/channel_2_frame", 5); - set_brightness_client_ = - lights_driver_node_->create_client("lights/set_brightness"); + return LightsDriverNode::ToggleLEDControlCB(future); } - ~TestDriverNode() {} - -protected: - ImageMsg::UniquePtr CreateImageMsg() + void FrameCB( + const ImageMsg::UniquePtr & msg, const panther_lights::APA102Interface::SharedPtr & panel, + const rclcpp::Time & last_time, const std::string & panel_name) { - ImageMsg::UniquePtr msg(new ImageMsg); - - // Filling msg with dummy data - msg->header.stamp = lights_driver_node_->now(); - msg->header.frame_id = "image_frame"; - msg->height = 1; - msg->width = lights_driver_node_->getNumLeds(); - msg->encoding = sensor_msgs::image_encodings::RGBA8; - msg->is_bigendian = false; - msg->step = msg->width * 4; - msg->data = std::vector(msg->step * msg->height, 0); - - return msg; + return LightsDriverNode::FrameCB(msg, panel, last_time, panel_name); } - std::shared_ptr lights_driver_node_; - rclcpp::Node::SharedPtr service_node_; - rclcpp::Publisher::SharedPtr channel_1_pub_; - rclcpp::Publisher::SharedPtr channel_2_pub_; - rclcpp::Client::SharedPtr set_brightness_client_; + int GetNumLed() const { return num_led_; } + double GetTimeout() const { return frame_timeout_; } + bool GetLedControlGranted() const { return led_control_granted_; } + bool GetLedControlPending() const { return led_control_pending_; } - rclcpp::CallbackGroup::SharedPtr server_callback_group_; - rclcpp::Service::SharedPtr led_control_enable_service_; + rclcpp::Time SetChannel1Ts(const rclcpp::Time & ts) { return channel_1_ts_ = ts; } + rclcpp::Time SetChannel2Ts(const rclcpp::Time & ts) { return channel_2_ts_ = ts; } }; -TEST_F(TestDriverNode, ServiceTestSuccess) +class TestLightsDriverNode : public testing::Test +{ +protected: + TestLightsDriverNode(); + ~TestLightsDriverNode() {}; + + ImageMsg::UniquePtr CreateValidImageMsg(); + std::shared_future> + CreateSetBoolSrvFuture(bool request_data, bool response_success); + + std::shared_ptr channel_1_; + std::shared_ptr channel_2_; + std::unique_ptr lights_driver_node_; +}; + +TestLightsDriverNode::TestLightsDriverNode() +{ + channel_1_ = std::make_shared(); + channel_2_ = std::make_shared(); + + lights_driver_node_ = std::make_unique(channel_1_, channel_2_); +} + +ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg() { - ASSERT_TRUE(set_brightness_client_->wait_for_service(std::chrono::seconds(1))); - auto request = std::make_shared(); - request->data = 0.5; - auto future = set_brightness_client_->async_send_request(request); - ASSERT_TRUE( - panther_utils::test_utils::WaitForFuture(lights_driver_node_, future, std::chrono::seconds(1))); - auto response = future.get(); - EXPECT_TRUE(response->success); + ImageMsg::UniquePtr msg(new ImageMsg); + + // Filling the message with valid data + msg->header.stamp = lights_driver_node_->now(); + msg->header.frame_id = "image_frame"; + msg->height = 1; + msg->width = lights_driver_node_->GetNumLed(); + msg->encoding = sensor_msgs::image_encodings::RGBA8; + msg->is_bigendian = false; + msg->step = msg->width * 4; + msg->data = std::vector(msg->step * msg->height, 255); + + return msg; } -TEST_F(TestDriverNode, ServiceTestFail) +std::shared_future> +TestLightsDriverNode::CreateSetBoolSrvFuture(bool request_data, bool response_success) { - ASSERT_TRUE(set_brightness_client_->wait_for_service(std::chrono::seconds(1))); - auto request = std::make_shared(); - request->data = 2; - auto future = set_brightness_client_->async_send_request(request); - ASSERT_TRUE( - panther_utils::test_utils::WaitForFuture(lights_driver_node_, future, std::chrono::seconds(1))); - auto response = future.get(); - EXPECT_FALSE(response->success); + auto request = std::make_shared(); + request->data = request_data; + + auto response = std::make_shared(); + response->success = response_success; + + std::promise> promise; + promise.set_value(std::make_pair(request, response)); + + return promise.get_future(); } -// TODO check and fix following tests. -// Following test pass but I'm not sure if they actually test what they indicate they do. -// This is because the initialization has changed and it works completely different now. -// Now way to check if publishing / initialization was successful because SPI control is -// controlled in different package. This has to be refactored but needs more thinking it through. -TEST_F(TestDriverNode, PublishTimeoutFail) +TEST_F(TestLightsDriverNode, TestInitialization) { - auto msg = CreateImageMsg(); - msg->header.stamp.sec = lights_driver_node_->get_clock()->now().seconds() - - lights_driver_node_->getTimeout() - 1; - channel_1_pub_->publish(std::move(msg)); - rclcpp::spin_some(lights_driver_node_); - rclcpp::spin_some(service_node_); - EXPECT_FALSE(lights_driver_node_->isInitialised()); + auto channel_1 = std::make_shared(); + auto channel_2 = std::make_shared(); + auto lights_driver_node = std::make_shared(channel_1, channel_2); + + EXPECT_TRUE(lights_driver_node != nullptr); } -TEST_F(TestDriverNode, PublishOldMsgFail) +TEST_F(TestLightsDriverNode, ClearLEDs) { - auto msg = CreateImageMsg(); - lights_driver_node_->setChanel1TS(msg->header.stamp); - msg->header.stamp.nanosec--; - channel_1_pub_->publish(std::move(msg)); - rclcpp::spin_some(lights_driver_node_); - rclcpp::spin_some(service_node_); - EXPECT_FALSE(lights_driver_node_->isInitialised()); + auto num_led = lights_driver_node_->GetNumLed(); + std::vector zero_frame(num_led * 4, 0); + + EXPECT_CALL(*channel_1_, SetPanel(zero_frame)).Times(1); + EXPECT_CALL(*channel_2_, SetPanel(zero_frame)).Times(1); + + lights_driver_node_->ClearLEDs(); } -TEST_F(TestDriverNode, PublishEncodingFail) +// ### ToggleLEDControlCB tests ### + +TEST_F(TestLightsDriverNode, ToggleLEDControlCBFailure) { - auto msg = CreateImageMsg(); + auto future = CreateSetBoolSrvFuture(true, false); + + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_FALSE(lights_driver_node_->GetLedControlPending()); + EXPECT_FALSE(lights_driver_node_->GetLedControlGranted()); +} + +TEST_F(TestLightsDriverNode, ToggleLEDControlCBEnabled) +{ + auto future = CreateSetBoolSrvFuture(true, true); + + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_FALSE(lights_driver_node_->GetLedControlPending()); + EXPECT_TRUE(lights_driver_node_->GetLedControlGranted()); +} + +TEST_F(TestLightsDriverNode, ToggleLEDControlCBDisabled) +{ + auto future = CreateSetBoolSrvFuture(false, true); + + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_FALSE(lights_driver_node_->GetLedControlPending()); + EXPECT_FALSE(lights_driver_node_->GetLedControlGranted()); +} + +// ### FrameCB tests ### + +TEST_F(TestLightsDriverNode, FrameCBSuccessNoControl) +{ + auto msg = CreateValidImageMsg(); + + EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); +} + +TEST_F(TestLightsDriverNode, FrameCBSuccess) +{ + auto msg = CreateValidImageMsg(); + + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(1); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); +} + +TEST_F(TestLightsDriverNode, FrameCBTimeout) +{ + auto msg = CreateValidImageMsg(); + auto timeout = lights_driver_node_->GetTimeout(); + + // Set timestamp to exceed timeout + msg->header.stamp.sec = msg->header.stamp.sec - timeout - 1; + + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(msg->data)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); +} + +TEST_F(TestLightsDriverNode, FrameCBPast) +{ + auto msg = CreateValidImageMsg(); + + // Set last_time to be younger than msg timestamp + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(msg->data)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, lights_driver_node_->now(), "channel_1"); +} + +TEST_F(TestLightsDriverNode, FrameCBEncoding) +{ + auto msg = CreateValidImageMsg(); + + // Set incorrect encoding msg->encoding = sensor_msgs::image_encodings::RGB8; - channel_1_pub_->publish(std::move(msg)); - rclcpp::spin_some(lights_driver_node_); - rclcpp::spin_some(service_node_); - EXPECT_FALSE(lights_driver_node_->isInitialised()); + + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(msg->data)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); } -TEST_F(TestDriverNode, PublishHeightFail) +TEST_F(TestLightsDriverNode, FrameCBHeight) { - auto msg = CreateImageMsg(); + auto msg = CreateValidImageMsg(); + + // Set incorrect height msg->height = 2; - channel_1_pub_->publish(std::move(msg)); - rclcpp::spin_some(lights_driver_node_); - rclcpp::spin_some(service_node_); - EXPECT_FALSE(lights_driver_node_->isInitialised()); -} - -TEST_F(TestDriverNode, PublishWidthFail) -{ - auto msg = CreateImageMsg(); - msg->width = lights_driver_node_->getNumLeds() + 1; - channel_1_pub_->publish(std::move(msg)); - rclcpp::spin_some(lights_driver_node_); - rclcpp::spin_some(service_node_); - EXPECT_FALSE(lights_driver_node_->isInitialised()); -} - -// // TODO: For some reason this function breaks other test that's why PublishSuccess is last one. -// // Update: now it also fails during destruction because SPI can not be accessed. Commented out -// // for now. -// TEST_F(TestDriverNode, PublishSuccess) -// { -// auto msg_1 = CreateImageMsg(); -// auto msg_2 = CreateImageMsg(); - -// channel_1_pub_->publish(std::move(msg_1)); -// channel_2_pub_->publish(std::move(msg_2)); -// rclcpp::spin_some(lights_driver_node_); -// rclcpp::spin_some(service_node_); -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// rclcpp::spin_some(lights_driver_node_); -// rclcpp::spin_some(service_node_); -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); - -// EXPECT_TRUE(lights_driver_node_->isInitialised()); -// } + + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(msg->data)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); +} + +TEST_F(TestLightsDriverNode, FrameCBWidth) +{ + auto msg = CreateValidImageMsg(); + + // Set incorrect width + msg->width = lights_driver_node_->GetNumLed() + 1; + + auto future = CreateSetBoolSrvFuture(true, true); + lights_driver_node_->ToggleLEDControlCB(std::move(future)); + + EXPECT_CALL(*channel_1_, SetPanel(msg->data)).Times(0); + + lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); +} int main(int argc, char ** argv) { From 7f2dc9e83bbca8439167396a03d3468d1939b328 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Thu, 5 Sep 2024 12:40:31 +0000 Subject: [PATCH 04/18] Make lights controller tests lightweight --- panther_lights/CMakeLists.txt | 39 ++++---- panther_lights/package.xml | 4 + .../test/test_lights_controller_node.cpp | 97 +++---------------- 3 files changed, 39 insertions(+), 101 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index b75a2e640..a86796f7d 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -89,6 +89,8 @@ install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_animation test/animation/test_animation.cpp) @@ -183,6 +185,25 @@ if(BUILD_TESTING) panther_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp) + ament_add_gmock(${PROJECT_NAME}_test_apa102 test/test_apa102.cpp + src/apa102.cpp) + target_include_directories( + ${PROJECT_NAME}_test_apa102 + PUBLIC $ + $) + + ament_add_gmock( + ${PROJECT_NAME}_test_lights_driver_node test/test_lights_driver_node.cpp + src/apa102.cpp src/lights_driver_node.cpp) + target_include_directories( + ${PROJECT_NAME}_test_lights_driver_node + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_lights_driver_node + panther_utils) + target_link_libraries(${PROJECT_NAME}_test_lights_driver_node + lights_driver_node_component) + ament_add_gtest( ${PROJECT_NAME}_test_lights_controller_node test/test_lights_controller_node.cpp @@ -200,24 +221,6 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_lights_controller_node lights_controller_node_component yaml-cpp) - ament_add_gtest(${PROJECT_NAME}_test_apa102 test/test_apa102.cpp - src/apa102.cpp) - target_include_directories( - ${PROJECT_NAME}_test_apa102 - PUBLIC $ - $) - - ament_add_gtest( - ${PROJECT_NAME}_test_lights_driver_node test/test_lights_driver_node.cpp - src/apa102.cpp src/lights_driver_node.cpp) - target_include_directories( - ${PROJECT_NAME}_test_lights_driver_node - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_lights_driver_node - panther_utils) - target_link_libraries(${PROJECT_NAME}_test_lights_driver_node - lights_driver_node_component) endif() ament_export_include_directories(include) diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 756757a69..a42cf4302 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -27,6 +27,10 @@ sensor_msgs std_srvs + ament_cmake_gtest + google-mock + ros_testing + ament_cmake diff --git a/panther_lights/test/test_lights_controller_node.cpp b/panther_lights/test/test_lights_controller_node.cpp index 61fd74d2f..7a27f3a40 100644 --- a/panther_lights/test/test_lights_controller_node.cpp +++ b/panther_lights/test/test_lights_controller_node.cpp @@ -63,16 +63,14 @@ class ControllerNodeWrapper : public panther_lights::LightsControllerNode } }; -class TestControllerNode : public testing::Test +class TestLightsControllerNode : public testing::Test { public: - TestControllerNode(); - ~TestControllerNode(); + TestLightsControllerNode(); + ~TestLightsControllerNode(); 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; @@ -81,10 +79,9 @@ class TestControllerNode : public testing::Test std::filesystem::path led_config_file_; std::shared_ptr lights_controller_node_; - rclcpp::Client::SharedPtr set_led_anim_client_; }; -TestControllerNode::TestControllerNode() +TestLightsControllerNode::TestLightsControllerNode() { led_config_file_ = testing::TempDir() + "/led_config.yaml"; @@ -97,14 +94,11 @@ TestControllerNode::TestControllerNode() options.parameter_overrides(params); lights_controller_node_ = std::make_shared(options); - - set_led_anim_client_ = lights_controller_node_->create_client( - "lights/set_animation"); } -TestControllerNode::~TestControllerNode() { std::filesystem::remove(led_config_file_); } +TestLightsControllerNode::~TestLightsControllerNode() { std::filesystem::remove(led_config_file_); } -void TestControllerNode::CreateLEDConfig(const std::filesystem::path file_path) +void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_path) { YAML::Node panel; panel["channel"] = kTestChannel; @@ -157,23 +151,7 @@ void TestControllerNode::CreateLEDConfig(const std::filesystem::path 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(lights_controller_node_, result) == - rclcpp::FutureReturnCode::SUCCESS); - EXPECT_TRUE(result.get()->success); -} - -TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) +TEST_F(TestLightsControllerNode, InitializeLEDPanelsThrowRepeatingChannel) { YAML::Node panel_1_desc; panel_1_desc["channel"] = kTestChannel; @@ -195,7 +173,7 @@ TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) "Multiple panels with channel nr")); } -TEST_F(TestControllerNode, InitializeLEDSegmentsThrowRepeatingName) +TEST_F(TestLightsControllerNode, InitializeLEDSegmentsThrowRepeatingName) { YAML::Node segment_1_desc; segment_1_desc["name"] = kTestSegmentName; @@ -219,7 +197,7 @@ TEST_F(TestControllerNode, InitializeLEDSegmentsThrowRepeatingName) "Multiple segments with given name found")); } -TEST_F(TestControllerNode, LoadAnimationInvalidPriority) +TEST_F(TestLightsControllerNode, LoadAnimationInvalidPriority) { YAML::Node led_animation_desc; led_animation_desc["id"] = 11; @@ -236,7 +214,7 @@ TEST_F(TestControllerNode, LoadAnimationInvalidPriority) "Invalid LED animation priority")); } -TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) +TEST_F(TestLightsControllerNode, LoadAnimationThrowRepeatingID) { YAML::Node led_animation_desc; led_animation_desc["id"] = 11; @@ -249,13 +227,13 @@ TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) "Animation with given ID already exists")); } -TEST_F(TestControllerNode, AddAnimationToQueueThrowBadAnimationID) +TEST_F(TestLightsControllerNode, AddAnimationToQueueThrowBadAnimationID) { EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); } -TEST_F(TestControllerNode, AddAnimationToQueue) +TEST_F(TestLightsControllerNode, AddAnimationToQueue) { auto queue = lights_controller_node_->GetQueue(); EXPECT_TRUE(queue->Empty()); @@ -263,60 +241,13 @@ TEST_F(TestControllerNode, AddAnimationToQueue) EXPECT_FALSE(queue->Empty()); } -TEST_F(TestControllerNode, CallSelLEDAnimationService) -{ - this->CallSetLEDAnimationSrv(0, false); - - // spin to invoke timer - auto anim = lights_controller_node_->GetCurrentAnimation(); - while (!anim) { - rclcpp::spin_some(lights_controller_node_->get_node_base_interface()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - anim = lights_controller_node_->GetCurrentAnimation(); - } - - EXPECT_STREQ("ANIMATION_0", anim->GetName().c_str()); - - // add another animation to queue - auto queue = lights_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 = lights_controller_node_->GetCurrentAnimation(); - while (!anim) { - rclcpp::spin_some(lights_controller_node_->get_node_base_interface()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - anim = lights_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(lights_controller_node_->get_node_base_interface()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - anim = lights_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(); + auto result = RUN_ALL_TESTS(); rclcpp::shutdown(); - return run_tests; + return result; } From dd9575cf4ffda095c374b8f7cca79b923384a189 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 6 Sep 2024 09:40:22 +0000 Subject: [PATCH 05/18] Change License in system monitor integration test --- .../test/integration/system_monitor_node.test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_diagnostics/test/integration/system_monitor_node.test.py b/panther_diagnostics/test/integration/system_monitor_node.test.py index 6307e9724..dc895713f 100644 --- a/panther_diagnostics/test/integration/system_monitor_node.test.py +++ b/panther_diagnostics/test/integration/system_monitor_node.test.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 TIER IV, Inc. +# 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. From 728c1dda15da89f8d28ebf851ff0a56a3751ea24 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Mon, 9 Sep 2024 13:34:19 +0000 Subject: [PATCH 06/18] Implement panther_lights integration tests --- panther_lights/CMakeLists.txt | 34 +++-- .../test/integration/panther_lights.test.py | 143 ++++++++++++++++++ .../{ => unit}/animation/test_animation.cpp | 0 .../animation/test_charging_animation.cpp | 0 .../animation/test_image_animation.cpp | 0 .../led_components/test_led_animation.cpp | 0 .../test_led_animations_queue.cpp | 0 .../led_components/test_led_panel.cpp | 0 .../led_components/test_led_segment.cpp | 0 .../led_components/test_segment_converter.cpp | 0 .../test/{ => unit}/test_apa102.cpp | 0 .../test_lights_controller_node.cpp | 0 .../{ => unit}/test_lights_driver_node.cpp | 0 13 files changed, 165 insertions(+), 12 deletions(-) create mode 100644 panther_lights/test/integration/panther_lights.test.py rename panther_lights/test/{ => unit}/animation/test_animation.cpp (100%) rename panther_lights/test/{ => unit}/animation/test_charging_animation.cpp (100%) rename panther_lights/test/{ => unit}/animation/test_image_animation.cpp (100%) rename panther_lights/test/{ => unit}/led_components/test_led_animation.cpp (100%) rename panther_lights/test/{ => unit}/led_components/test_led_animations_queue.cpp (100%) rename panther_lights/test/{ => unit}/led_components/test_led_panel.cpp (100%) rename panther_lights/test/{ => unit}/led_components/test_led_segment.cpp (100%) rename panther_lights/test/{ => unit}/led_components/test_segment_converter.cpp (100%) rename panther_lights/test/{ => unit}/test_apa102.cpp (100%) rename panther_lights/test/{ => unit}/test_lights_controller_node.cpp (100%) rename panther_lights/test/{ => unit}/test_lights_driver_node.cpp (100%) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index a86796f7d..070bd8c17 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -92,8 +92,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros_testing REQUIRED) + # Unit tests ament_add_gtest(${PROJECT_NAME}_test_animation - test/animation/test_animation.cpp) + test/unit/animation/test_animation.cpp) target_include_directories( ${PROJECT_NAME}_test_animation PUBLIC $ @@ -103,7 +104,7 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_charging_animation - test/animation/test_charging_animation.cpp + test/unit/animation/test_charging_animation.cpp src/animation/charging_animation.cpp) target_include_directories( ${PROJECT_NAME}_test_charging_animation @@ -115,7 +116,8 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_image_animation - test/animation/test_image_animation.cpp src/animation/image_animation.cpp) + test/unit/animation/test_image_animation.cpp + src/animation/image_animation.cpp) target_include_directories( ${PROJECT_NAME}_test_image_animation PUBLIC $ @@ -125,7 +127,7 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp) ament_add_gtest( - ${PROJECT_NAME}_test_led_panel test/led_components/test_led_panel.cpp + ${PROJECT_NAME}_test_led_panel test/unit/led_components/test_led_panel.cpp src/led_components/led_panel.cpp) target_include_directories( ${PROJECT_NAME}_test_led_panel @@ -133,7 +135,8 @@ if(BUILD_TESTING) $) ament_add_gtest( - ${PROJECT_NAME}_test_led_segment test/led_components/test_led_segment.cpp + ${PROJECT_NAME}_test_led_segment + test/unit/led_components/test_led_segment.cpp src/led_components/led_segment.cpp) target_include_directories( ${PROJECT_NAME}_test_led_segment @@ -145,7 +148,7 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_segment_converter - test/led_components/test_segment_converter.cpp + test/unit/led_components/test_segment_converter.cpp src/led_components/segment_converter.cpp src/led_components/led_panel.cpp src/led_components/led_segment.cpp) @@ -159,7 +162,7 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_led_animation - test/led_components/test_led_animation.cpp + test/unit/led_components/test_led_animation.cpp src/led_components/led_panel.cpp src/led_components/led_segment.cpp src/led_components/led_animations_queue.cpp) @@ -173,7 +176,7 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_led_animations_queue - test/led_components/test_led_animations_queue.cpp + test/unit/led_components/test_led_animations_queue.cpp src/led_components/led_panel.cpp src/led_components/led_segment.cpp src/led_components/led_animations_queue.cpp) @@ -185,7 +188,7 @@ if(BUILD_TESTING) panther_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp) - ament_add_gmock(${PROJECT_NAME}_test_apa102 test/test_apa102.cpp + ament_add_gmock(${PROJECT_NAME}_test_apa102 test/unit/test_apa102.cpp src/apa102.cpp) target_include_directories( ${PROJECT_NAME}_test_apa102 @@ -193,8 +196,9 @@ if(BUILD_TESTING) $) ament_add_gmock( - ${PROJECT_NAME}_test_lights_driver_node test/test_lights_driver_node.cpp - src/apa102.cpp src/lights_driver_node.cpp) + ${PROJECT_NAME}_test_lights_driver_node + test/unit/test_lights_driver_node.cpp src/apa102.cpp + src/lights_driver_node.cpp) target_include_directories( ${PROJECT_NAME}_test_lights_driver_node PUBLIC $ @@ -206,7 +210,7 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_lights_controller_node - test/test_lights_controller_node.cpp + test/unit/test_lights_controller_node.cpp src/lights_controller_node.cpp src/led_components/led_segment.cpp src/led_components/led_panel.cpp @@ -221,6 +225,12 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_lights_controller_node lights_controller_node_component yaml-cpp) + # Integration tests + option(TEST_INTEGRATION "Run integration tests" OFF) + if(TEST_INTEGRATION) + add_ros_test(test/integration/panther_lights.test.py) + endif() + endif() ament_export_include_directories(include) diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py new file mode 100644 index 000000000..89491041f --- /dev/null +++ b/panther_lights/test/integration/panther_lights.test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +# 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. + + +import unittest + +import launch +import launch_testing +import rclpy +import rclpy.qos +from diagnostic_msgs.msg import DiagnosticArray +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_testing_ros import WaitForTopics +from panther_msgs.msg import LEDAnimation +from panther_msgs.srv import SetLEDAnimation +from sensor_msgs.msg import Image +from std_srvs.srv import SetBool + + +def generate_test_description(): + + led_config_file = ( + PathJoinSubstitution([FindPackageShare("panther_lights"), "config", "led_config.yaml"]), + ) + + lights_controller_node = Node( + package="panther_lights", + executable="lights_controller_node", + parameters=[{"led_config_file": led_config_file}], + ) + + lights_driver_node = Node( + package="panther_lights", + executable="lights_driver_node", + ) + + # Start test after 1s + delay_timer = launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ) + + actions = [lights_controller_node, lights_driver_node, delay_timer] + + context = {} + + return ( + LaunchDescription(actions), + context, + ) + + +class TestNode(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.test_node = rclpy.create_node("test_node") + self.led_control_requested = None + + self.led_control_enable_srv = self.test_node.create_service( + srv_type=SetBool, + srv_name="hardware/led_control_enable", + callback=self.led_control_enable_cb, + qos_profile=rclpy.qos.qos_profile_services_default, + ) + + self.set_led_animation_client = self.test_node.create_client( + srv_type=SetLEDAnimation, + srv_name="lights/set_animation", + qos_profile=rclpy.qos.qos_profile_services_default, + ) + + def tearDown(self): + self.test_node.destroy_node() + + def led_control_enable_cb(self, request, response): + self.led_control_requested = request.data + response.success = True + response.message = "LED control enabled" + return response + + def test_initialization(self, proc_output): + rclpy.spin_until_future_complete(self.test_node, rclpy.task.Future(), timeout_sec=2.0) + + self.assertTrue(self.led_control_requested) + + ## Controller initialization + proc_output.assertWaitFor("[lights_controller]: Loaded default animations.") + proc_output.assertWaitFor("[lights_controller]: Initialized successfully.") + ## Driver initialization + proc_output.assertWaitFor("[lights_driver]: Node constructed successfully.") + proc_output.assertWaitFor("[lights_driver]: LED control granted.") + + def request_error_animation(self): + request = SetLEDAnimation.Request() + request.animation = LEDAnimation(id=LEDAnimation.ERROR) + request.repeating = False + + self.set_led_animation_client.wait_for_service(timeout_sec=1.0) + self.set_led_animation_client.call_async(request) + + def test_msg_publishers(self): + self.request_error_animation() + + topic_list = [ + ("lights/channel_1_frame", Image), + ("lights/channel_2_frame", Image), + ("diagnostics", DiagnosticArray), + ] + + with WaitForTopics(topic_list, timeout=5.0) as wait_for_topics: + received_topics_str = ", ".join(wait_for_topics.topics_received()) + print("Received messages from the following topics: [" + received_topics_str + "]") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/panther_lights/test/animation/test_animation.cpp b/panther_lights/test/unit/animation/test_animation.cpp similarity index 100% rename from panther_lights/test/animation/test_animation.cpp rename to panther_lights/test/unit/animation/test_animation.cpp diff --git a/panther_lights/test/animation/test_charging_animation.cpp b/panther_lights/test/unit/animation/test_charging_animation.cpp similarity index 100% rename from panther_lights/test/animation/test_charging_animation.cpp rename to panther_lights/test/unit/animation/test_charging_animation.cpp diff --git a/panther_lights/test/animation/test_image_animation.cpp b/panther_lights/test/unit/animation/test_image_animation.cpp similarity index 100% rename from panther_lights/test/animation/test_image_animation.cpp rename to panther_lights/test/unit/animation/test_image_animation.cpp diff --git a/panther_lights/test/led_components/test_led_animation.cpp b/panther_lights/test/unit/led_components/test_led_animation.cpp similarity index 100% rename from panther_lights/test/led_components/test_led_animation.cpp rename to panther_lights/test/unit/led_components/test_led_animation.cpp diff --git a/panther_lights/test/led_components/test_led_animations_queue.cpp b/panther_lights/test/unit/led_components/test_led_animations_queue.cpp similarity index 100% rename from panther_lights/test/led_components/test_led_animations_queue.cpp rename to panther_lights/test/unit/led_components/test_led_animations_queue.cpp diff --git a/panther_lights/test/led_components/test_led_panel.cpp b/panther_lights/test/unit/led_components/test_led_panel.cpp similarity index 100% rename from panther_lights/test/led_components/test_led_panel.cpp rename to panther_lights/test/unit/led_components/test_led_panel.cpp diff --git a/panther_lights/test/led_components/test_led_segment.cpp b/panther_lights/test/unit/led_components/test_led_segment.cpp similarity index 100% rename from panther_lights/test/led_components/test_led_segment.cpp rename to panther_lights/test/unit/led_components/test_led_segment.cpp diff --git a/panther_lights/test/led_components/test_segment_converter.cpp b/panther_lights/test/unit/led_components/test_segment_converter.cpp similarity index 100% rename from panther_lights/test/led_components/test_segment_converter.cpp rename to panther_lights/test/unit/led_components/test_segment_converter.cpp diff --git a/panther_lights/test/test_apa102.cpp b/panther_lights/test/unit/test_apa102.cpp similarity index 100% rename from panther_lights/test/test_apa102.cpp rename to panther_lights/test/unit/test_apa102.cpp diff --git a/panther_lights/test/test_lights_controller_node.cpp b/panther_lights/test/unit/test_lights_controller_node.cpp similarity index 100% rename from panther_lights/test/test_lights_controller_node.cpp rename to panther_lights/test/unit/test_lights_controller_node.cpp diff --git a/panther_lights/test/test_lights_driver_node.cpp b/panther_lights/test/unit/test_lights_driver_node.cpp similarity index 100% rename from panther_lights/test/test_lights_driver_node.cpp rename to panther_lights/test/unit/test_lights_driver_node.cpp From 4d16480f22bc5a26b91fea3953f4582f5303b259 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Mon, 9 Sep 2024 13:43:59 +0000 Subject: [PATCH 07/18] Fix comments --- panther_lights/test/integration/panther_lights.test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py index 89491041f..5575e45f6 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/panther_lights/test/integration/panther_lights.test.py @@ -106,10 +106,10 @@ def test_initialization(self, proc_output): self.assertTrue(self.led_control_requested) - ## Controller initialization + # Controller initialization proc_output.assertWaitFor("[lights_controller]: Loaded default animations.") proc_output.assertWaitFor("[lights_controller]: Initialized successfully.") - ## Driver initialization + # Driver initialization proc_output.assertWaitFor("[lights_driver]: Node constructed successfully.") proc_output.assertWaitFor("[lights_driver]: LED control granted.") From 5eda7056a3b987469fe7fbf562e3caedc0ee8697 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 10 Sep 2024 10:05:16 +0000 Subject: [PATCH 08/18] Address review suggestions --- panther_lights/test/test_apa102.cpp | 14 ++++---------- panther_lights/test/test_lights_driver_node.cpp | 2 ++ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/panther_lights/test/test_apa102.cpp b/panther_lights/test/test_apa102.cpp index a0307d14e..2f5bbd2f8 100644 --- a/panther_lights/test/test_apa102.cpp +++ b/panther_lights/test/test_apa102.cpp @@ -64,8 +64,6 @@ TestAPA102::TestAPA102() apa102_ = std::make_unique(spi_device_, kMockDeviceName); } -TEST_F(TestAPA102, TestInitialization) { EXPECT_TRUE(spi_device_ != nullptr); } - TEST(TestInitialization, InvalidDevices) { auto spi_device = std::make_shared(); @@ -73,8 +71,7 @@ TEST(TestInitialization, InvalidDevices) // Return -1 to simulate failed device opening ON_CALL(*spi_device, Open(kMockDeviceName)).WillByDefault(testing::Return(-1)); - EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); - , std::ios_base::failure); + EXPECT_THROW(APA102Wrapper(spi_device, kMockDeviceName), std::ios_base::failure); } TEST(TestInitialization, SetModeFailure) @@ -86,8 +83,7 @@ TEST(TestInitialization, SetModeFailure) .WillByDefault(testing::Return(-1)); EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); - EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); - , std::ios_base::failure); + EXPECT_THROW(APA102Wrapper(spi_device, kMockDeviceName), std::ios_base::failure); } TEST(TestInitialization, SetBitsFailure) @@ -99,8 +95,7 @@ TEST(TestInitialization, SetBitsFailure) .WillByDefault(testing::Return(-1)); EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); - EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); - , std::ios_base::failure); + EXPECT_THROW(APA102Wrapper(spi_device, kMockDeviceName), std::ios_base::failure); } TEST(TestInitialization, SetSpeedFailure) @@ -112,8 +107,7 @@ TEST(TestInitialization, SetSpeedFailure) .WillByDefault(testing::Return(-1)); EXPECT_CALL(*spi_device, Close(testing::_)).Times(1); - EXPECT_THROW(std::make_unique(spi_device, kMockDeviceName); - , std::ios_base::failure); + EXPECT_THROW(APA102Wrapper(spi_device, kMockDeviceName), std::ios_base::failure); } TEST_F(TestAPA102, SetGlobalBrightnessRatioNegative) diff --git a/panther_lights/test/test_lights_driver_node.cpp b/panther_lights/test/test_lights_driver_node.cpp index 3602a3919..9a20b0f5e 100644 --- a/panther_lights/test/test_lights_driver_node.cpp +++ b/panther_lights/test/test_lights_driver_node.cpp @@ -218,8 +218,10 @@ TEST_F(TestLightsDriverNode, FrameCBSuccess) lights_driver_node_->ToggleLEDControlCB(std::move(future)); EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(1); + EXPECT_CALL(*channel_2_, SetPanel(testing::_)).Times(1); lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); + lights_driver_node_->FrameCB(msg, channel_2_, msg->header.stamp, "channel_2"); } TEST_F(TestLightsDriverNode, FrameCBTimeout) From 52d9e8dda5d79e974651ce984f363c1f8236e787 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 11 Sep 2024 09:53:38 +0000 Subject: [PATCH 09/18] Add panther_codebase_version to docker image build payload --- .github/workflows/release-candidate.yaml | 1 + .github/workflows/release-project.yaml | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/release-candidate.yaml b/.github/workflows/release-candidate.yaml index 7f97919e3..c1504ad15 100644 --- a/.github/workflows/release-candidate.yaml +++ b/.github/workflows/release-candidate.yaml @@ -79,6 +79,7 @@ jobs: wait_interval: 10 client_payload: | { + "panther_codebase_version": "${{ env.RC_BRANCH_NAME }}", "build_type": "development", "target_distro": "humble" } diff --git a/.github/workflows/release-project.yaml b/.github/workflows/release-project.yaml index 7ece39639..47800b4f5 100644 --- a/.github/workflows/release-project.yaml +++ b/.github/workflows/release-project.yaml @@ -93,6 +93,7 @@ jobs: wait_interval: 10 client_payload: | { + "panther_codebase_version": "${{ github.event.inputs.version }}", "build_type": "development", "target_distro": "humble" } From 6a9e94582bedae26790c110eb10c4262ed9ada59 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Thu, 12 Sep 2024 06:36:33 +0000 Subject: [PATCH 10/18] Rename test fixture --- panther_lights/test/integration/panther_lights.test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py index 5575e45f6..98d8e2b72 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/panther_lights/test/integration/panther_lights.test.py @@ -65,7 +65,7 @@ def generate_test_description(): ) -class TestNode(unittest.TestCase): +class TestNodesIntegration(unittest.TestCase): @classmethod def setUpClass(cls): From 3f9d608291d16b6ff27a55b2355d687ef90a6e54 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 13 Sep 2024 06:12:42 +0000 Subject: [PATCH 11/18] Add underscore prefix to private members --- .../test/integration/panther_lights.test.py | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py index 98d8e2b72..a9700abd0 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/panther_lights/test/integration/panther_lights.test.py @@ -76,35 +76,35 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - self.test_node = rclpy.create_node("test_node") - self.led_control_requested = None + self._test_node = rclpy.create_node("test_node") + self._led_control_requested = None - self.led_control_enable_srv = self.test_node.create_service( + self._led_control_enable_srv = self._test_node.create_service( srv_type=SetBool, srv_name="hardware/led_control_enable", - callback=self.led_control_enable_cb, + callback=self._led_control_enable_cb, qos_profile=rclpy.qos.qos_profile_services_default, ) - self.set_led_animation_client = self.test_node.create_client( + self._set_led_animation_client = self._test_node.create_client( srv_type=SetLEDAnimation, srv_name="lights/set_animation", qos_profile=rclpy.qos.qos_profile_services_default, ) def tearDown(self): - self.test_node.destroy_node() + self._test_node.destroy_node() - def led_control_enable_cb(self, request, response): - self.led_control_requested = request.data + def _led_control_enable_cb(self, request, response): + self._led_control_requested = request.data response.success = True response.message = "LED control enabled" return response def test_initialization(self, proc_output): - rclpy.spin_until_future_complete(self.test_node, rclpy.task.Future(), timeout_sec=2.0) + rclpy.spin_until_future_complete(self._test_node, rclpy.task.Future(), timeout_sec=2.0) - self.assertTrue(self.led_control_requested) + self.assertTrue(self._led_control_requested) # Controller initialization proc_output.assertWaitFor("[lights_controller]: Loaded default animations.") @@ -113,16 +113,16 @@ def test_initialization(self, proc_output): proc_output.assertWaitFor("[lights_driver]: Node constructed successfully.") proc_output.assertWaitFor("[lights_driver]: LED control granted.") - def request_error_animation(self): + def _request_error_animation(self): request = SetLEDAnimation.Request() request.animation = LEDAnimation(id=LEDAnimation.ERROR) request.repeating = False - self.set_led_animation_client.wait_for_service(timeout_sec=1.0) - self.set_led_animation_client.call_async(request) + self._set_led_animation_client.wait_for_service(timeout_sec=1.0) + self._set_led_animation_client.call_async(request) def test_msg_publishers(self): - self.request_error_animation() + self._request_error_animation() topic_list = [ ("lights/channel_1_frame", Image), From 9eb271c8dbd6eacd1018fcd78df717aab25ec9cd Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 13 Sep 2024 07:29:31 +0000 Subject: [PATCH 12/18] Add subscription verification --- .../test/integration/panther_lights.test.py | 7 ++ .../panther_utils/integration_test_utils.py | 79 +++++++++++++++++++ 2 files changed, 86 insertions(+) create mode 100644 panther_utils/panther_utils/integration_test_utils.py diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py index a9700abd0..046bab913 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/panther_lights/test/integration/panther_lights.test.py @@ -19,6 +19,7 @@ import launch import launch_testing +import panther_utils.integration_test_utils as test_utils import rclpy import rclpy.qos from diagnostic_msgs.msg import DiagnosticArray @@ -134,6 +135,12 @@ def test_msg_publishers(self): received_topics_str = ", ".join(wait_for_topics.topics_received()) print("Received messages from the following topics: [" + received_topics_str + "]") + def test_msg_subscribers(self): + node_info = test_utils.get_node_info("/lights_driver") + + self.assertIn("/lights/channel_1_frame", node_info.subscribers) + self.assertIn("/lights/channel_2_frame", node_info.subscribers) + @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): diff --git a/panther_utils/panther_utils/integration_test_utils.py b/panther_utils/panther_utils/integration_test_utils.py new file mode 100644 index 000000000..47dd47391 --- /dev/null +++ b/panther_utils/panther_utils/integration_test_utils.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# 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. + +import subprocess +from typing import List + + +class ROSNodeInfo: + """ + Class representing the ROS node info. + """ + + def __init__(self): + self.subscribers: List[str] = [] + self.publishers: List[str] = [] + self.service_servers: List[str] = [] + self.service_clients: List[str] = [] + self.action_servers: List[str] = [] + self.action_clients: List[str] = [] + + +def get_node_info(node_name: str) -> ROSNodeInfo: + """ + Executes the command 'ros2 node info ' and returns the ROSNodeInfo object. + + Args: + node_name (str): The name of the ROS 2 node to get information about. + + Returns: + ROSNodeInfo: An object representing a complete node info. + """ + node_info = ROSNodeInfo() + + section_map = { + "Subscribers:": node_info.subscribers, + "Publishers:": node_info.publishers, + "Service Servers:": node_info.service_servers, + "Service Clients:": node_info.service_clients, + "Action Servers:": node_info.action_servers, + "Action Clients:": node_info.action_clients, + } + + try: + # Execute the `ros2 node info` command + result = subprocess.run( + ["ros2", "node", "info", node_name], capture_output=True, text=True, check=True + ) + + # Parse the output + lines = result.stdout.splitlines() + current_section = None + + for line in lines: + line = line.strip() + if line in section_map: + current_section = section_map[line] + elif line and current_section is not None: + current_section.append(line.split(":")[0].strip()) + else: + current_section = None + + except subprocess.CalledProcessError as e: + print(f"Error executing command: {e}") + print(f"stderr: {e.stderr}") + + return node_info From ffc4fe6ce423ecaee1ab557203b69dc506c808bc Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 13 Sep 2024 07:59:20 +0000 Subject: [PATCH 13/18] Avoid pre-commit complains --- panther_lights/test/integration/panther_lights.test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_lights/test/integration/panther_lights.test.py b/panther_lights/test/integration/panther_lights.test.py index 046bab913..fafd6bf19 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/panther_lights/test/integration/panther_lights.test.py @@ -138,8 +138,8 @@ def test_msg_publishers(self): def test_msg_subscribers(self): node_info = test_utils.get_node_info("/lights_driver") - self.assertIn("/lights/channel_1_frame", node_info.subscribers) - self.assertIn("/lights/channel_2_frame", node_info.subscribers) + self.assertTrue("/lights/channel_1_frame" in node_info.subscribers) + self.assertTrue("/lights/channel_2_frame" in node_info.subscribers) @launch_testing.post_shutdown_test() From 962719bb6eb7c50de8978a3b3267cf7102164c37 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 13 Sep 2024 08:12:15 +0000 Subject: [PATCH 14/18] Add error handling in get_node_info() method --- panther_utils/panther_utils/integration_test_utils.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/panther_utils/panther_utils/integration_test_utils.py b/panther_utils/panther_utils/integration_test_utils.py index 47dd47391..621f906c1 100644 --- a/panther_utils/panther_utils/integration_test_utils.py +++ b/panther_utils/panther_utils/integration_test_utils.py @@ -41,6 +41,9 @@ def get_node_info(node_name: str) -> ROSNodeInfo: Returns: ROSNodeInfo: An object representing a complete node info. + + Raises: + RuntimeError: If the command execution fails. """ node_info = ROSNodeInfo() @@ -73,7 +76,6 @@ def get_node_info(node_name: str) -> ROSNodeInfo: current_section = None except subprocess.CalledProcessError as e: - print(f"Error executing command: {e}") - print(f"stderr: {e.stderr}") + raise RuntimeError(f"Error executing command: {e}. stderr: {e.stderr}") from e return node_info From 05324f5ae1c3e419814a9c974f3ddf0bc7975e08 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:45:28 +0200 Subject: [PATCH 15/18] Fix safety_manager path (#411) Signed-off-by: Jakub Delicat Co-authored-by: rafal-gorecki --- panther_manager/src/safety_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp index d204690ce..68ea0c191 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/panther_manager/src/safety_manager_node.cpp @@ -62,7 +62,7 @@ SafetyManagerNode::SafetyManagerNode( const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string(); const std::map shutdown_initial_blackboard = { - {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path.c_str()}, + {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path}, }; shutdown_tree_manager_ = std::make_unique( "Shutdown", shutdown_initial_blackboard, 7777); From 3dced4ddd8cbb40055454585a9c24bee95db7547 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Tue, 19 Nov 2024 11:20:50 +0100 Subject: [PATCH 16/18] Ros2 fix led bug (#441) * add pin validation wait time * add comment explaining usage of pin validation wait time --- .../panther_system/gpio/gpio_driver.hpp | 6 +++++- .../src/panther_system/gpio/gpio_controller.cpp | 4 +++- .../src/panther_system/gpio/gpio_driver.cpp | 12 ++++++++++-- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp index d71d81c94..9c45fa28b 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp @@ -189,13 +189,17 @@ class GPIODriver * * @param pin GPIOPin to set the value for. * @param value The boolean value to set for the pin. + * @param pin_validation_wait_time The time duration to wait for the pin value to change before + * checking if change was successful. * * @throws std::invalid_argument if trying to set the value for an INPUT pin. * @throws std::runtime_error if changing the GPIO state fails. * * @return true if the pin value is successfully set, false otherwise. */ - bool SetPinValue(const GPIOPin pin, const bool value); + bool SetPinValue( + const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)); private: std::unique_ptr CreateLineRequest(gpiod::chip & chip); diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp index fe13fcd84..fdb6ba2ae 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp +++ b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp @@ -177,7 +177,9 @@ bool GPIOControllerPTH12X::ChargerEnable(const bool enable) bool GPIOControllerPTH12X::LEDControlEnable(const bool enable) { - return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); + // pin_validation_wait_time=10ms used due to slow pin state transition + // on pin loaded by high 100nF capacity in SBC Overlay v1.4 + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable, std::chrono::milliseconds(10)); } std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp b/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp index 09b8c0fdb..48126889c 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp +++ b/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp @@ -164,7 +164,8 @@ bool GPIODriver::IsPinActive(const GPIOPin pin) return pin_info.value == gpiod::line::value::ACTIVE; } -bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) +bool GPIODriver::SetPinValue( + const GPIOPin pin, const bool value, const std::chrono::milliseconds & pin_validation_wait_time) { GPIOInfo & gpio_info = GetGPIOInfoRef(pin); @@ -174,10 +175,17 @@ bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) gpiod::line::value gpio_value = value ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE; - std::lock_guard lock(gpio_info_storage_mutex_); + std::unique_lock lock(gpio_info_storage_mutex_); + try { line_request_->set_value(gpio_info.offset, gpio_value); + if (pin_validation_wait_time.count() > 0) { + lock.unlock(); + std::this_thread::sleep_for(pin_validation_wait_time); + lock.lock(); + } + if (line_request_->get_value(gpio_info.offset) != gpio_value) { throw std::runtime_error("Failed to change GPIO state."); } From 11bf007ffb55876bb323b7c1bb5d360dadeec5d8 Mon Sep 17 00:00:00 2001 From: action-bot Date: Mon, 2 Dec 2024 12:10:38 +0000 Subject: [PATCH 17/18] Update changelog --- panther/CHANGELOG.rst | 5 +++++ panther_battery/CHANGELOG.rst | 5 +++++ panther_bringup/CHANGELOG.rst | 5 +++++ panther_controller/CHANGELOG.rst | 5 +++++ panther_description/CHANGELOG.rst | 5 +++++ panther_diagnostics/CHANGELOG.rst | 7 +++++++ panther_gazebo/CHANGELOG.rst | 5 +++++ panther_hardware_interfaces/CHANGELOG.rst | 6 ++++++ panther_lights/CHANGELOG.rst | 19 +++++++++++++++++++ panther_localization/CHANGELOG.rst | 5 +++++ panther_manager/CHANGELOG.rst | 6 ++++++ panther_utils/CHANGELOG.rst | 8 ++++++++ 12 files changed, 81 insertions(+) diff --git a/panther/CHANGELOG.rst b/panther/CHANGELOG.rst index f889df6f9..874db2d6d 100644 --- a/panther/CHANGELOG.rst +++ b/panther/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_battery/CHANGELOG.rst b/panther_battery/CHANGELOG.rst index de4e0a10e..462f96b17 100644 --- a/panther_battery/CHANGELOG.rst +++ b/panther_battery/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_battery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * LEDStrip plugin to Gazebo (`#391 `_) diff --git a/panther_bringup/CHANGELOG.rst b/panther_bringup/CHANGELOG.rst index da3524f36..f42400fac 100644 --- a/panther_bringup/CHANGELOG.rst +++ b/panther_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_controller/CHANGELOG.rst b/panther_controller/CHANGELOG.rst index 8ab5b60ee..b312ade78 100644 --- a/panther_controller/CHANGELOG.rst +++ b/panther_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge pull request `#403 `_ from husarion/ros2-control-ns-fix diff --git a/panther_description/CHANGELOG.rst b/panther_description/CHANGELOG.rst index 07a9fce1e..78eb0eec5 100644 --- a/panther_description/CHANGELOG.rst +++ b/panther_description/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * LEDStrip plugin to Gazebo (`#391 `_) diff --git a/panther_diagnostics/CHANGELOG.rst b/panther_diagnostics/CHANGELOG.rst index 4553018b8..fa6ef7060 100644 --- a/panther_diagnostics/CHANGELOG.rst +++ b/panther_diagnostics/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package panther_diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests +* Change License in system monitor integration test +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: Dawid Kmak, pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_gazebo/CHANGELOG.rst b/panther_gazebo/CHANGELOG.rst index a11cc3b1b..56d61e110 100644 --- a/panther_gazebo/CHANGELOG.rst +++ b/panther_gazebo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * LEDStrip plugin to Gazebo (`#391 `_) diff --git a/panther_hardware_interfaces/CHANGELOG.rst b/panther_hardware_interfaces/CHANGELOG.rst index 039200a56..70a524797 100644 --- a/panther_hardware_interfaces/CHANGELOG.rst +++ b/panther_hardware_interfaces/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package panther_hardware_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Ros2 fix led bug (`#441 `_) +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: Dawid Kmak, pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_lights/CHANGELOG.rst b/panther_lights/CHANGELOG.rst index 5edb9e2aa..c45ac17e3 100644 --- a/panther_lights/CHANGELOG.rst +++ b/panther_lights/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package panther_lights ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests +* Avoid pre-commit complains +* Add subscription verification +* Add underscore prefix to private members +* Rename test fixture +* Merge branch 'ros2-devel' into ros2-lights-integration-tests +* Merge pull request `#407 `_ from husarion/ros2-lights-tests +* Address review suggestions +* Fix comments +* Implement panther_lights integration tests +* Merge branch 'ros2-devel' into ros2-lights-tests +* Make lights controller tests lightweight +* Implement tests for lights driver +* Test apa102 +* Implement APA102Interface remove spatial namespace +* Contributors: Dawid Kmak, pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_localization/CHANGELOG.rst b/panther_localization/CHANGELOG.rst index d33e3ed72..66e55182b 100644 --- a/panther_localization/CHANGELOG.rst +++ b/panther_localization/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package panther_localization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_manager/CHANGELOG.rst b/panther_manager/CHANGELOG.rst index 610316f20..c5bd33fc8 100644 --- a/panther_manager/CHANGELOG.rst +++ b/panther_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package panther_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix safety_manager path (`#411 `_) +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: Jakub Delicat, pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor diff --git a/panther_utils/CHANGELOG.rst b/panther_utils/CHANGELOG.rst index 7d3e06c3b..9209076c7 100644 --- a/panther_utils/CHANGELOG.rst +++ b/panther_utils/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package panther_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests +* Add error handling in get_node_info() method +* Add subscription verification +* Merge branch 'ros2-devel' into ros2-lights-tests +* Contributors: Dawid Kmak, pawelirh + 2.1.1 (2024-09-05) ------------------ * Merge branch 'ros2-devel' into ros2-ns-refactor From 45d1cb7374e076d767462e0cf0415ff0c35c37be Mon Sep 17 00:00:00 2001 From: action-bot Date: Mon, 2 Dec 2024 12:10:39 +0000 Subject: [PATCH 18/18] 2.1.2 --- panther/CHANGELOG.rst | 4 ++-- panther/package.xml | 2 +- panther_battery/CHANGELOG.rst | 4 ++-- panther_battery/package.xml | 2 +- panther_bringup/CHANGELOG.rst | 4 ++-- panther_bringup/package.xml | 2 +- panther_controller/CHANGELOG.rst | 4 ++-- panther_controller/package.xml | 2 +- panther_description/CHANGELOG.rst | 4 ++-- panther_description/package.xml | 2 +- panther_diagnostics/CHANGELOG.rst | 4 ++-- panther_diagnostics/package.xml | 2 +- panther_gazebo/CHANGELOG.rst | 4 ++-- panther_gazebo/package.xml | 2 +- panther_hardware_interfaces/CHANGELOG.rst | 4 ++-- panther_hardware_interfaces/package.xml | 2 +- panther_lights/CHANGELOG.rst | 4 ++-- panther_lights/package.xml | 2 +- panther_localization/CHANGELOG.rst | 4 ++-- panther_localization/package.xml | 2 +- panther_manager/CHANGELOG.rst | 4 ++-- panther_manager/package.xml | 2 +- panther_utils/CHANGELOG.rst | 4 ++-- panther_utils/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/panther/CHANGELOG.rst b/panther/CHANGELOG.rst index 874db2d6d..d24fd5c6a 100644 --- a/panther/CHANGELOG.rst +++ b/panther/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther/package.xml b/panther/package.xml index f849f5671..788b9385d 100644 --- a/panther/package.xml +++ b/panther/package.xml @@ -2,7 +2,7 @@ panther - 2.1.1 + 2.1.2 Meta package that contains all packages of Panther Husarion Apache License 2.0 diff --git a/panther_battery/CHANGELOG.rst b/panther_battery/CHANGELOG.rst index 462f96b17..25ebdace9 100644 --- a/panther_battery/CHANGELOG.rst +++ b/panther_battery/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_battery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_battery/package.xml b/panther_battery/package.xml index 8e67a1b9b..53e7fab86 100644 --- a/panther_battery/package.xml +++ b/panther_battery/package.xml @@ -2,7 +2,7 @@ panther_battery - 2.1.1 + 2.1.2 Nodes monitoring the battery state of Husarion Panhter robot Husarion Apache License 2.0 diff --git a/panther_bringup/CHANGELOG.rst b/panther_bringup/CHANGELOG.rst index f42400fac..cbf7d0d18 100644 --- a/panther_bringup/CHANGELOG.rst +++ b/panther_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index a5fe44b42..ebd80a4e9 100644 --- a/panther_bringup/package.xml +++ b/panther_bringup/package.xml @@ -2,7 +2,7 @@ panther_bringup - 2.1.1 + 2.1.2 Default launch files and configuration used to start Husarion Panther robot Husarion Apache License 2.0 diff --git a/panther_controller/CHANGELOG.rst b/panther_controller/CHANGELOG.rst index b312ade78..ca6d9524b 100644 --- a/panther_controller/CHANGELOG.rst +++ b/panther_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_controller/package.xml b/panther_controller/package.xml index 067db90ce..5a72a3256 100644 --- a/panther_controller/package.xml +++ b/panther_controller/package.xml @@ -2,7 +2,7 @@ panther_controller - 2.1.1 + 2.1.2 ros2 controllers configuration for Panther Husarion Apache License 2.0 diff --git a/panther_description/CHANGELOG.rst b/panther_description/CHANGELOG.rst index 78eb0eec5..6bb731c50 100644 --- a/panther_description/CHANGELOG.rst +++ b/panther_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_description/package.xml b/panther_description/package.xml index 8ebdd01da..9b30cc772 100644 --- a/panther_description/package.xml +++ b/panther_description/package.xml @@ -2,7 +2,7 @@ panther_description - 2.1.1 + 2.1.2 The panther_description package Husarion Apache License 2.0 diff --git a/panther_diagnostics/CHANGELOG.rst b/panther_diagnostics/CHANGELOG.rst index fa6ef7060..bb34cde93 100644 --- a/panther_diagnostics/CHANGELOG.rst +++ b/panther_diagnostics/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests * Change License in system monitor integration test * Merge branch 'ros2-devel' into ros2-lights-tests diff --git a/panther_diagnostics/package.xml b/panther_diagnostics/package.xml index 9f827b6d8..f51b357ee 100644 --- a/panther_diagnostics/package.xml +++ b/panther_diagnostics/package.xml @@ -2,7 +2,7 @@ panther_diagnostics - 2.1.1 + 2.1.2 Package for diagnosting usage of OS on the Panther Robot Husarion Apache License 2.0 diff --git a/panther_gazebo/CHANGELOG.rst b/panther_gazebo/CHANGELOG.rst index 56d61e110..1156d408f 100644 --- a/panther_gazebo/CHANGELOG.rst +++ b/panther_gazebo/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index ef2d3d6b0..7be9ad3bc 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -2,7 +2,7 @@ panther_gazebo - 2.1.1 + 2.1.2 The panther_description package Husarion Apache License 2.0 diff --git a/panther_hardware_interfaces/CHANGELOG.rst b/panther_hardware_interfaces/CHANGELOG.rst index 70a524797..29c324a76 100644 --- a/panther_hardware_interfaces/CHANGELOG.rst +++ b/panther_hardware_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_hardware_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Ros2 fix led bug (`#441 `_) * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: Dawid Kmak, pawelirh diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml index 8e497ce30..b60217495 100644 --- a/panther_hardware_interfaces/package.xml +++ b/panther_hardware_interfaces/package.xml @@ -2,7 +2,7 @@ panther_hardware_interfaces - 2.1.1 + 2.1.2 Hardware controller for Panther Husarion Apache License 2.0 diff --git a/panther_lights/CHANGELOG.rst b/panther_lights/CHANGELOG.rst index c45ac17e3..5ddb68220 100644 --- a/panther_lights/CHANGELOG.rst +++ b/panther_lights/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_lights ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests * Avoid pre-commit complains * Add subscription verification diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 2dfb2248c..c7a55e77e 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -2,7 +2,7 @@ panther_lights - 2.1.1 + 2.1.2 Package used to control the Husarion Panther Bumper Lights Husarion Apache License 2.0 diff --git a/panther_localization/CHANGELOG.rst b/panther_localization/CHANGELOG.rst index 66e55182b..cf9bbec86 100644 --- a/panther_localization/CHANGELOG.rst +++ b/panther_localization/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_localization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: pawelirh diff --git a/panther_localization/package.xml b/panther_localization/package.xml index 7cef452ad..10d1fe44f 100644 --- a/panther_localization/package.xml +++ b/panther_localization/package.xml @@ -2,7 +2,7 @@ panther_localization - 2.1.1 + 2.1.2 robot localization configuration for Panther Husarion Apache License 2.0 diff --git a/panther_manager/CHANGELOG.rst b/panther_manager/CHANGELOG.rst index c5bd33fc8..9d06f8c88 100644 --- a/panther_manager/CHANGELOG.rst +++ b/panther_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Fix safety_manager path (`#411 `_) * Merge branch 'ros2-devel' into ros2-lights-tests * Contributors: Jakub Delicat, pawelirh diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 8310427d5..5acc77e63 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -3,7 +3,7 @@ panther_manager - 2.1.1 + 2.1.2 Set of nodes used for high level management of Husarion Panther robot Husarion Apache License 2.0 diff --git a/panther_utils/CHANGELOG.rst b/panther_utils/CHANGELOG.rst index 9209076c7..1be203b8c 100644 --- a/panther_utils/CHANGELOG.rst +++ b/panther_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package panther_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.2 (2024-12-02) +------------------ * Merge pull request `#408 `_ from husarion/ros2-lights-integration-tests * Add error handling in get_node_info() method * Add subscription verification diff --git a/panther_utils/package.xml b/panther_utils/package.xml index aad717ec0..53fd30f8a 100644 --- a/panther_utils/package.xml +++ b/panther_utils/package.xml @@ -2,7 +2,7 @@ panther_utils - 2.1.1 + 2.1.2 Package for commonly used functions, classes, and configurations Husarion Apache License 2.0