From 1e4f9c6baef7058959788c60ff1976779fef1b9c Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 3 Apr 2023 14:13:38 +0000 Subject: [PATCH 01/31] Use CPP for APA102 --- panther_lights/CMakeLists.txt | 26 ++- .../include/panther_lights/apa102.hpp | 31 +++ .../panther_lights/lights_driver_node.hpp | 83 ++++++++ panther_lights/package.xml | 5 +- panther_lights/src/apa102.cpp | 102 ++++++++++ panther_lights/src/driver_node.py | 179 ------------------ panther_lights/src/lights_driver_node.cpp | 107 +++++++++++ panther_lights/src/main.cpp | 29 +++ 8 files changed, 380 insertions(+), 182 deletions(-) create mode 100644 panther_lights/include/panther_lights/apa102.hpp create mode 100644 panther_lights/include/panther_lights/lights_driver_node.hpp create mode 100644 panther_lights/src/apa102.cpp delete mode 100755 panther_lights/src/driver_node.py create mode 100644 panther_lights/src/lights_driver_node.cpp create mode 100644 panther_lights/src/main.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 2cea09a0d..c94cbead2 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -1,15 +1,39 @@ cmake_minimum_required(VERSION 3.0.2) project(panther_lights) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(catkin REQUIRED COMPONENTS panther_msgs + image_transport + roscpp rospy sensor_msgs std_msgs std_srvs ) -catkin_package() +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS panther_msgs roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(driver_node + src/main.cpp + src/lights_driver_node.cpp + src/apa102.cpp +) + +add_dependencies(driver_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(driver_node + ${catkin_LIBRARIES} +) install(DIRECTORY config diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp new file mode 100644 index 000000000..5b6864e67 --- /dev/null +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -0,0 +1,31 @@ +#ifndef PANTHER_LIGHTS_APA_102_HPP_ +#define PANTHER_LIGHTS_APA_102_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace apa_102 +{ + + class APA102 + { + public: + APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); + ~APA102(); + void set_panel(const std::vector& frame) const; + + private: + const std::string device_; + const int fd_; + const std::uint8_t bits_ = 8; + const std::uint32_t speed_; + }; + +} // namespace apa_102 + +#endif // PANTHER_LIGHTS_APA_102_HPP_ \ No newline at end of file diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp new file mode 100644 index 000000000..8fcbcd3cf --- /dev/null +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -0,0 +1,83 @@ +#ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_ +#define PANTHER_LIGHTS_DRIVER_NODE_HPP_ + +#include "panther_lights/apa102.hpp" + +#include +#include +#include + +#include +#include + +namespace gpio +{ + class OutputPin + { + public: + OutputPin(const int pin_number) + { + std::ofstream export_file; + export_file.open("/sys/class/gpio/export", std::ios::out); + export_file << std::to_string(pin_number); + export_file.close(); + + std::ofstream out_set_file; + out_set_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/direction", std::ios::out); + out_set_file << "out"; + out_set_file.close(); + + pin_.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/value", std::ios::out); + } + + ~OutputPin() + { + pin_.close(); + } + + void set_val(const bool val) + { + pin_ << std::to_string(int(val)); + } + private: + std::ofstream pin_; + }; +} + +namespace panther_lights_driver +{ + + class LightsDriverNode + { + public: + LightsDriverNode( + std::shared_ptr private_nh, + std::shared_ptr nh, + std::shared_ptr it); + ~LightsDriverNode(); + + private: + const apa_102::APA102 front_panel_; + const apa_102::APA102 rear_panel_; + gpio::OutputPin power_pin_; + std::string node_name_; + double frame_timeout_; + + ros::Time front_panel_ts_; + ros::Time rear_panel_ts_; + + int num_led_; + void frame_cb(const sensor_msgs::ImageConstPtr& msg, + const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) const; + void set_brightness_cb(); + + std::shared_ptr ph_; + std::shared_ptr nh_; + std::shared_ptr it_; + image_transport::Subscriber rear_light_sub_; + image_transport::Subscriber front_light_sub_; + }; + +} // panther_lights_driver_node + +#endif // PANTHER_LIGHTS_DRIVER_NODE_HPP_ \ No newline at end of file diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 3f0a2652b..bff3065c4 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -22,9 +22,10 @@ std_msgs std_srvs + image_transport + roscpp + - python-rpi.gpio-pip - python3-apa102-pi-pip python3-pil diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp new file mode 100644 index 000000000..aae704ea2 --- /dev/null +++ b/panther_lights/src/apa102.cpp @@ -0,0 +1,102 @@ +#include "panther_lights/apa102.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace apa_102 +{ + APA102::APA102(const std::string device, const std::uint32_t speed, const bool cs_high) + : device_(device) + , speed_(speed) + , fd_(open(device_.c_str(), O_RDWR)) + { + if (fd_ < 0) + { + throw std::ios_base::failure(std::string("Failed to open ") + device_); + } + + static std::uint8_t mode = 0x00; + if (cs_high) + { + mode |= SPI_CS_HIGH; + } + if (ioctl(fd_, SPI_IOC_WR_MODE, &mode) == -1) + { + close(fd_); + throw std::ios_base::failure(std::string("Failed to set mode for ") + device_); + } + + if (ioctl(fd_, SPI_IOC_WR_BITS_PER_WORD, &bits_) == -1) + { + close(fd_); + throw std::ios_base::failure(std::string("Can't set bits per word for ") + device_); + } + + if (ioctl(fd_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) + { + close(fd_); + throw std::ios_base::failure(std::string("Can't set speed for ") + device_); + } + } + + APA102::~APA102() + { + close(fd_); + } + + void APA102::set_panel(const std::vector& frame) const + { + if (frame.size() % 4 != 0) + { + throw std::runtime_error("Incorrect number of bytes to transfer to LEDs"); + } + // init buffer with start and end frames + std::size_t buffer_size = (4 * sizeof(std::uint8_t)) + frame.size() + (4 * sizeof(std::uint8_t)); + std::uint8_t* buffer = new std::uint8_t[buffer_size]; + + // init start and end frames + for (std::size_t i = 0; i < 3; i++) + { + buffer[i] = 0xFF; + buffer[i + frame.size()] = 0x00; + } + + // copy frame from vector to sending buffer + for (std::size_t i = 0; i < frame.size() / 4; i++) + { + // padding + std::size_t pad = i * 4; + buffer[pad] = 0xE0 || (frame[pad] >> 2); + buffer[pad + 1] = frame[pad + 3]; + buffer[pad + 2] = frame[pad + 2]; + buffer[pad + 3] = frame[pad + 1]; + } + + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long long) buffer, + .rx_buf = 0, + .len = (unsigned int) buffer_size, + .speed_hz = speed_, + .delay_usecs = 0, + .bits_per_word = 8, + }; + + int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr); + delete buffer; + + if (ret < 1) + { + throw std::ios_base::failure(std::string("Failed to send data over SPI ") + device_); + } + } +} \ No newline at end of file diff --git a/panther_lights/src/driver_node.py b/panther_lights/src/driver_node.py deleted file mode 100755 index 5169ff5e4..000000000 --- a/panther_lights/src/driver_node.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python3 - -from dataclasses import dataclass -from threading import Lock - -from apa102_pi.driver import apa102 -import RPi.GPIO as GPIO - -import rospy - -from sensor_msgs.msg import Image - -from panther_msgs.srv import SetLEDBrightness, SetLEDBrightnessRequest, SetLEDBrightnessResponse - - -@dataclass -class LEDConstants: - LED_SWITCH_FRONT_STATE: bool = GPIO.HIGH - LED_SWITCH_REAR_STATE: bool = GPIO.LOW - LED_POWER_ON_STATE: bool = GPIO.LOW # active LED with low state - LED_SWITCH_PIN: int = 20 - LED_POWER_PIN: int = 26 - LED_MAX_BRIGHTNESS: int = 31 - PANEL_FRONT: int = 0 - PANEL_REAR: int = 1 - - -class LightsDriverNode: - def __init__(self, name: str) -> None: - rospy.init_node(name, anonymous=False) - - global_brightness = rospy.get_param('~global_brightness', 1.0) - self._frame_timeout = rospy.get_param('~frame_timeout', 0.1) - self._num_led = rospy.get_param('~num_led', 46) # has to match controller num_led - - try: - apa_driver_brightness = self._percent_to_apa_driver_brightness(global_brightness) - except ValueError as err: - rospy.logwarn(f'[{rospy.get_name()}] Invalid brightness: {err}. Using default.') - apa_driver_brightness = LEDConstants.LED_MAX_BRIGHTNESS - - self._lock = Lock() - self._pixels = apa102.APA102( - num_led=self._num_led, - order='rgb', - mosi=10, - sclk=11, - global_brightness=apa_driver_brightness, - ) - self._front_active = True - self._rear_active = True - self._last_time_stamp_front = rospy.Time.now() - self._last_time_stamp_rear = rospy.Time.now() - color_correction_rgb = [255, 200, 62] - # rgb values normalized to avoid additional division - self._color_correction = [value / 255 for value in color_correction_rgb] - - # setup and activate panels - self._setup_panels() - - # clear panels - self._clear_panel(LEDConstants.PANEL_FRONT) - self._clear_panel(LEDConstants.PANEL_REAR) - - # ------------------------------- - # Subscribers - # ------------------------------- - - self._front_frame_sub = rospy.Subscriber( - 'lights/driver/front_panel_frame', Image, self._front_frame_cb, queue_size=3 - ) - self._rear_frame_sub = rospy.Subscriber( - 'lights/driver/rear_panel_frame', Image, self._rear_frame_cb, queue_size=3 - ) - - # ------------------------------- - # Service servers - # ------------------------------- - - self._set_brightness_server = rospy.Service( - 'lights/driver/set/brightness', SetLEDBrightness, self._set_brightness_cb - ) - - rospy.loginfo(f'[{rospy.get_name()}] Node started') - - def _percent_to_apa_driver_brightness(self, brightness_percent: float) -> int: - if 0.0 <= brightness_percent <= 1.0: - brightness = int(brightness_percent * LEDConstants.LED_MAX_BRIGHTNESS) - # set minimal brightness for small values - if brightness == 0 and brightness_percent > 0.0: - brightness = 1 - return brightness - raise ValueError('Brightness out of range <0,1>') - - def _setup_panels(self) -> None: - GPIO.setmode(GPIO.BCM) - GPIO.setup(LEDConstants.LED_SWITCH_PIN, GPIO.OUT) - GPIO.setup(LEDConstants.LED_POWER_PIN, GPIO.OUT) - GPIO.output( - (LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_POWER_PIN), - ( - LEDConstants.LED_SWITCH_FRONT_STATE, - LEDConstants.LED_POWER_ON_STATE, - ), - ) - - def _front_frame_cb(self, image: Image) -> None: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Front frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_front: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for front panel.') - return - self._last_time_stamp_front = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_FRONT, rgb_frame, brightness) - - def _rear_frame_cb(self, image: Image) -> None: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Rear frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_rear: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for rear panel.') - return - self._last_time_stamp_rear = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_REAR, rgb_frame, brightness) - - def _decode_img_msg(self, image: Image) -> tuple: - rgb_frame = [[image.data[i + j] for j in range(3)] for i in range(0, len(image.data), 4)] - brightness = image.data[3] - return rgb_frame, brightness - - def _set_panel_frame(self, panel_num: int, panel_frame: list, brightness: int = 255) -> None: - with self._lock: - # select panel - if panel_num == LEDConstants.PANEL_FRONT and self._front_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_FRONT_STATE) - elif panel_num == LEDConstants.PANEL_REAR and self._rear_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_REAR_STATE) - else: - raise ValueError('panther lights have only two panels') - - for i, pixel in enumerate(panel_frame): - r = int(pixel[0] * self._color_correction[0]) - g = int(pixel[1] * self._color_correction[1]) - b = int(pixel[2] * self._color_correction[2]) - pixel_hex = (r << 16) + (g << 8) + b - self._pixels.set_pixel_rgb(i, pixel_hex, int(brightness / 255.0 * 100.0)) - self._pixels.show() - - def _set_brightness_cb(self, req: SetLEDBrightnessRequest) -> SetLEDBrightnessResponse: - try: - brightness = self._percent_to_apa_driver_brightness(req.data) - self._pixels.global_brightness = brightness - except ValueError as err: - return SetLEDBrightnessResponse(False, f'{err}') - - return SetLEDBrightnessResponse(True, f'Changed brightness to {req.data}') - - def _clear_panel(self, panel_num: int) -> None: - self._set_panel_frame(panel_num, [[0, 0, 0]] * self._num_led) - - def __del__(self) -> None: - self._clear_panel(LEDConstants.PANEL_FRONT) - self._clear_panel(LEDConstants.PANEL_REAR) - GPIO.output(LEDConstants.LED_POWER_PIN, not LEDConstants.LED_POWER_ON_STATE) - - -def main(): - lights_driver_node = LightsDriverNode('lights_driver_node') - rospy.spin() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp new file mode 100644 index 000000000..7f39c64d4 --- /dev/null +++ b/panther_lights/src/lights_driver_node.cpp @@ -0,0 +1,107 @@ +#include "panther_lights/lights_driver_node.hpp" +#include "panther_lights/apa102.hpp" + +#include +#include + +#include +#include + +namespace panther_lights_driver +{ + LightsDriverNode::LightsDriverNode( + std::shared_ptr private_nh, + std::shared_ptr nh, + std::shared_ptr it + ) + : ph_(std::move(private_nh)) + , nh_(std::move(nh)) + , it_(std::move(it)) + , front_panel_("/dev/spidev0.0") + , rear_panel_("/dev/spidev0.1") + , power_pin_(26) + { + node_name_ = ros::this_node::getName(); + + frame_timeout_ = ph_->param("frame_timeout", 0.1); + num_led_ = ph_->param("num_led", 46); + + front_panel_ts_ = ros::Time::now(); + rear_panel_ts_ = ros::Time::now(); + + // take control over LEDs + power_pin_.set_val(true); + // clear LEDs + front_panel_.set_panel(std::vector(num_led_ * 4, 0)); + rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); + + front_light_sub_ = it_->subscribe("lights/driver/front_panel_frame", 5, + [this](const sensor_msgs::ImageConstPtr& msg) { + frame_cb(msg, front_panel_, front_panel_ts_, "front"); + front_panel_ts_ = msg->header.stamp; + }); + + rear_light_sub_ = it_->subscribe("lights/driver/rear_panel_frame", 5, + [this](const sensor_msgs::ImageConstPtr& msg) { + frame_cb(msg, rear_panel_, rear_panel_ts_, "rear"); + rear_panel_ts_ = msg->header.stamp; + }); + + + ROS_INFO("[%s] Node started", node_name_.c_str()); + } + + LightsDriverNode::~LightsDriverNode() + { + // clear LEDs + front_panel_.set_panel(std::vector(num_led_ * 4, 0)); + rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); + // give back control over LEDs + power_pin_.set_val(false); + } + + void LightsDriverNode::frame_cb(const sensor_msgs::ImageConstPtr& msg, + const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) const + { + if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) + { + ROS_WARN("[%s] Timeout exceeded, ignoring frame on %s panel!", + node_name_.c_str(), panel_name.c_str()); + return; + } + else if (msg->header.stamp < last_time) + { + ROS_WARN("[%s] Dropping message from past on panel %s panel!", + node_name_.c_str(), panel_name.c_str()); + return; + } + else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) + { + ROS_WARN("[%s] Incorrect image encoding ('%s') on panel %s panel!", + node_name_.c_str(), msg->encoding.c_str(), panel_name.c_str()); + return; + } + else if (msg->height != 1) + { + ROS_WARN("[%s] Incorrect image height '%u' on panel '%s'!, expected '1'", + node_name_.c_str(), msg->height, panel_name.c_str()); + return; + } + else if (msg->width != num_led_) + { + ROS_WARN("[%s] Incorrect image height '%u' on panel '%s'!, expected '%u'", + node_name_.c_str(), msg->width, panel_name.c_str(), num_led_); + return; + } + else if ((ros::Time::now() - msg->header.stamp).toSec() > 5.0) + { + ROS_WARN("[%s] Timeout. Dropping frame on panel %s panel!", + node_name_.c_str(), panel_name.c_str()); + return; + } + else + { + panel.set_panel(msg->data); + } + } +} \ No newline at end of file diff --git a/panther_lights/src/main.cpp b/panther_lights/src/main.cpp new file mode 100644 index 000000000..323e8f3c3 --- /dev/null +++ b/panther_lights/src/main.cpp @@ -0,0 +1,29 @@ +#include "panther_lights/lights_driver_node.hpp" + +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "lights_driver_node"); + + auto private_nh = std::make_shared("~"); + auto nh = std::make_shared(); + auto it = std::make_shared(*nh); + + try + { + panther_lights_driver::LightsDriverNode lights_driver_node(private_nh, nh, it); + ros::spin(); + } + + catch (const std::exception& e) { + std::cerr << "[" << ros::this_node::getName() << "]" + << " Caught exception: " << e.what() << std::endl; + } + + std::cout << "[" << ros::this_node::getName() << "]" + << " Shutting down" << std::endl; + ros::shutdown(); + return 0; +} \ No newline at end of file From 71346018973a40ac4d97716eff3530cc1db59bb9 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Tue, 4 Apr 2023 09:39:39 +0000 Subject: [PATCH 02/31] Add color correction --- panther_lights/include/panther_lights/apa102.hpp | 4 ++++ .../panther_lights/lights_driver_node.hpp | 16 ++++++++++++---- panther_lights/src/apa102.cpp | 7 ++++--- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 5b6864e67..b99f66609 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -24,6 +24,10 @@ namespace apa_102 const int fd_; const std::uint8_t bits_ = 8; const std::uint32_t speed_; + // color correction constants + const uint16_t corr_red = 255.0; + const uint16_t corr_green = 200.0; + const uint16_t corr_blue = 62.0; }; } // namespace apa_102 diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index 8fcbcd3cf..b8191dafc 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -17,15 +17,23 @@ namespace gpio public: OutputPin(const int pin_number) { + // Export pin std::ofstream export_file; export_file.open("/sys/class/gpio/export", std::ios::out); export_file << std::to_string(pin_number); export_file.close(); - std::ofstream out_set_file; - out_set_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/direction", std::ios::out); - out_set_file << "out"; - out_set_file.close(); + // Set direction to output + std::ofstream direction_file; + direction_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/direction", std::ios::out); + direction_file << "out"; + direction_file.close(); + + // Invert logic + std::ofstream active_low_file; + active_low_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/active_low", std::ios::out); + active_low_file << "1"; + active_low_file.close(); pin_.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/value", std::ios::out); } diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index aae704ea2..235e51061 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -77,9 +77,10 @@ namespace apa_102 // padding std::size_t pad = i * 4; buffer[pad] = 0xE0 || (frame[pad] >> 2); - buffer[pad + 1] = frame[pad + 3]; - buffer[pad + 2] = frame[pad + 2]; - buffer[pad + 3] = frame[pad + 1]; + // convert rgb to bgr with collor correction + buffer[pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 3]) * corr_blue) >> 8); + buffer[pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_green) >> 8); + buffer[pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_red) >> 8); } struct spi_ioc_transfer tr = { From 2dd1c0e6f0d394c8da25b0613620b958527c4726 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Apr 2023 18:14:37 +0000 Subject: [PATCH 03/31] Utilise gpiod --- panther_lights/CMakeLists.txt | 1 + .../panther_lights/lights_driver_node.hpp | 46 ++----------------- panther_lights/package.xml | 2 + panther_lights/src/lights_driver_node.cpp | 18 ++++++-- 4 files changed, 21 insertions(+), 46 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index c94cbead2..000048812 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -32,6 +32,7 @@ add_executable(driver_node add_dependencies(driver_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(driver_node + gpiodcxx ${catkin_LIBRARIES} ) diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index b8191dafc..9107cb2d6 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -7,51 +7,11 @@ #include #include +#include + #include #include -namespace gpio -{ - class OutputPin - { - public: - OutputPin(const int pin_number) - { - // Export pin - std::ofstream export_file; - export_file.open("/sys/class/gpio/export", std::ios::out); - export_file << std::to_string(pin_number); - export_file.close(); - - // Set direction to output - std::ofstream direction_file; - direction_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/direction", std::ios::out); - direction_file << "out"; - direction_file.close(); - - // Invert logic - std::ofstream active_low_file; - active_low_file.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/active_low", std::ios::out); - active_low_file << "1"; - active_low_file.close(); - - pin_.open("/sys/class/gpio/gpio" + std::to_string(pin_number) + "/value", std::ios::out); - } - - ~OutputPin() - { - pin_.close(); - } - - void set_val(const bool val) - { - pin_ << std::to_string(int(val)); - } - private: - std::ofstream pin_; - }; -} - namespace panther_lights_driver { @@ -67,7 +27,7 @@ namespace panther_lights_driver private: const apa_102::APA102 front_panel_; const apa_102::APA102 rear_panel_; - gpio::OutputPin power_pin_; + gpiod::line power_pin_; std::string node_name_; double frame_timeout_; diff --git a/panther_lights/package.xml b/panther_lights/package.xml index bff3065c4..90b20896b 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -22,6 +22,8 @@ std_msgs std_srvs + libgpiod-dev + image_transport roscpp diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 7f39c64d4..58a6a8001 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace panther_lights_driver { LightsDriverNode::LightsDriverNode( @@ -19,18 +21,27 @@ namespace panther_lights_driver , it_(std::move(it)) , front_panel_("/dev/spidev0.0") , rear_panel_("/dev/spidev0.1") - , power_pin_(26) { node_name_ = ros::this_node::getName(); frame_timeout_ = ph_->param("frame_timeout", 0.1); num_led_ = ph_->param("num_led", 46); + ::gpiod::chip chip("gpiochip0"); + power_pin_ = chip.find_line("LED_SBC_SEL"); + + const gpiod::line_request lr = { + node_name_, + gpiod::line_request::DIRECTION_OUTPUT, + gpiod::line_request::FLAG_ACTIVE_LOW + }; + power_pin_.request(lr, 1); + front_panel_ts_ = ros::Time::now(); rear_panel_ts_ = ros::Time::now(); // take control over LEDs - power_pin_.set_val(true); + power_pin_.set_value(1); // clear LEDs front_panel_.set_panel(std::vector(num_led_ * 4, 0)); rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); @@ -57,7 +68,8 @@ namespace panther_lights_driver front_panel_.set_panel(std::vector(num_led_ * 4, 0)); rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); // give back control over LEDs - power_pin_.set_val(false); + power_pin_.set_value(0); + power_pin_.release(); } void LightsDriverNode::frame_cb(const sensor_msgs::ImageConstPtr& msg, From 8e5956d18c71f2eb132b13b4c02f7280f37a6a1c Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Apr 2023 18:15:40 +0000 Subject: [PATCH 04/31] Remove :: --- panther_lights/src/lights_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 58a6a8001..31cb178bf 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -27,7 +27,7 @@ namespace panther_lights_driver frame_timeout_ = ph_->param("frame_timeout", 0.1); num_led_ = ph_->param("num_led", 46); - ::gpiod::chip chip("gpiochip0"); + gpiod::chip chip("gpiochip0"); power_pin_ = chip.find_line("LED_SBC_SEL"); const gpiod::line_request lr = { From ca2abc86f8387248b33f4b754a4ba21769e84392 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Apr 2023 20:44:31 +0000 Subject: [PATCH 05/31] Fix lights after tests --- .../panther_lights/lights_driver_node.hpp | 4 ++++ panther_lights/src/apa102.cpp | 17 ++++++++------- panther_lights/src/lights_driver_node.cpp | 21 ++++++++++++------- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index 9107cb2d6..cf26d0dcf 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -12,6 +12,10 @@ #include #include +#define ROS_WARN_THROTTLE_PANELS(panel, p1, p2, ...) \ +if (panel == p1) ROS_WARN_THROTTLE(__VA_ARGS__); \ +else if (panel == p2) ROS_WARN_THROTTLE(__VA_ARGS__) + namespace panther_lights_driver { diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 235e51061..fb759f898 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -65,10 +65,10 @@ namespace apa_102 std::uint8_t* buffer = new std::uint8_t[buffer_size]; // init start and end frames - for (std::size_t i = 0; i < 3; i++) + for (std::size_t i = 0; i < 4; i++) { - buffer[i] = 0xFF; - buffer[i + frame.size()] = 0x00; + buffer[i] = 0x00; + buffer[buffer_size - i - 1] = 0xFF; } // copy frame from vector to sending buffer @@ -76,11 +76,12 @@ namespace apa_102 { // padding std::size_t pad = i * 4; - buffer[pad] = 0xE0 || (frame[pad] >> 2); + // header with brightness + buffer[4 + pad] = 0xE0 | (frame[pad + 3] >> 3); // convert rgb to bgr with collor correction - buffer[pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 3]) * corr_blue) >> 8); - buffer[pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_green) >> 8); - buffer[pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_red) >> 8); + buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 3]) * corr_blue) >> 8); + buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_green) >> 8); + buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_red) >> 8); } struct spi_ioc_transfer tr = { @@ -93,7 +94,7 @@ namespace apa_102 }; int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr); - delete buffer; + delete[] buffer; if (ret < 1) { diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 31cb178bf..74eef6efd 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -25,15 +25,15 @@ namespace panther_lights_driver node_name_ = ros::this_node::getName(); frame_timeout_ = ph_->param("frame_timeout", 0.1); - num_led_ = ph_->param("num_led", 46); + num_led_ = ph_->param("num_led", 47); - gpiod::chip chip("gpiochip0"); + const gpiod::chip chip("gpiochip0"); power_pin_ = chip.find_line("LED_SBC_SEL"); const gpiod::line_request lr = { node_name_, gpiod::line_request::DIRECTION_OUTPUT, - gpiod::line_request::FLAG_ACTIVE_LOW + 0 }; power_pin_.request(lr, 1); @@ -77,31 +77,36 @@ namespace panther_lights_driver { if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) { - ROS_WARN("[%s] Timeout exceeded, ignoring frame on %s panel!", + ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, + "[%s] Timeout exceeded, ignoring frame on %s panel!", node_name_.c_str(), panel_name.c_str()); return; } else if (msg->header.stamp < last_time) { - ROS_WARN("[%s] Dropping message from past on panel %s panel!", + ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, + "[%s] Dropping message from past on panel %s panel!", node_name_.c_str(), panel_name.c_str()); return; } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { - ROS_WARN("[%s] Incorrect image encoding ('%s') on panel %s panel!", + ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, + "[%s] Incorrect image encoding ('%s') on panel %s panel!", node_name_.c_str(), msg->encoding.c_str(), panel_name.c_str()); return; } else if (msg->height != 1) { - ROS_WARN("[%s] Incorrect image height '%u' on panel '%s'!, expected '1'", + ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, + "[%s] Incorrect image height %u on %s panel!, expected 1", node_name_.c_str(), msg->height, panel_name.c_str()); return; } else if (msg->width != num_led_) { - ROS_WARN("[%s] Incorrect image height '%u' on panel '%s'!, expected '%u'", + ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, + "[%s] Incorrect image height %u on %s panel!, expected %u", node_name_.c_str(), msg->width, panel_name.c_str(), num_led_); return; } From aa86625ef380625787dfd72fa068a9227a4a689a Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Apr 2023 21:16:56 +0000 Subject: [PATCH 06/31] Fix default pin number --- panther_lights/src/lights_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 74eef6efd..217330d30 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -25,7 +25,7 @@ namespace panther_lights_driver node_name_ = ros::this_node::getName(); frame_timeout_ = ph_->param("frame_timeout", 0.1); - num_led_ = ph_->param("num_led", 47); + num_led_ = ph_->param("num_led", 46); const gpiod::chip chip("gpiochip0"); power_pin_ = chip.find_line("LED_SBC_SEL"); From 666834c470cd5bf1fd97defd2251662365395ef2 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Thu, 6 Apr 2023 23:45:22 +0000 Subject: [PATCH 07/31] Add set brightness service --- .../include/panther_lights/apa102.hpp | 10 ++- .../panther_lights/lights_driver_node.hpp | 13 ++-- panther_lights/src/apa102.cpp | 24 +++++-- panther_lights/src/lights_driver_node.cpp | 63 ++++++++++++++----- 4 files changed, 84 insertions(+), 26 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index b99f66609..43f20f984 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -17,6 +17,8 @@ namespace apa_102 public: APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); ~APA102(); + void set_global_brightness(const std::uint8_t brightness); + void set_global_brightness(const double brightness); void set_panel(const std::vector& frame) const; private: @@ -24,10 +26,12 @@ namespace apa_102 const int fd_; const std::uint8_t bits_ = 8; const std::uint32_t speed_; + std::uint16_t global_brightness_; + // color correction constants - const uint16_t corr_red = 255.0; - const uint16_t corr_green = 200.0; - const uint16_t corr_blue = 62.0; + const std::uint16_t corr_red = 255.0; + const std::uint16_t corr_green = 200.0; + const std::uint16_t corr_blue = 62.0; }; } // namespace apa_102 diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index cf26d0dcf..2d3e56f13 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -12,6 +12,8 @@ #include #include +#include "panther_msgs/SetLEDBrightness.h" + #define ROS_WARN_THROTTLE_PANELS(panel, p1, p2, ...) \ if (panel == p1) ROS_WARN_THROTTLE(__VA_ARGS__); \ else if (panel == p2) ROS_WARN_THROTTLE(__VA_ARGS__) @@ -29,25 +31,28 @@ namespace panther_lights_driver ~LightsDriverNode(); private: - const apa_102::APA102 front_panel_; - const apa_102::APA102 rear_panel_; + apa_102::APA102 front_panel_; + apa_102::APA102 rear_panel_; gpiod::line power_pin_; std::string node_name_; double frame_timeout_; ros::Time front_panel_ts_; ros::Time rear_panel_ts_; + bool panels_initialised_{false}; int num_led_; void frame_cb(const sensor_msgs::ImageConstPtr& msg, - const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) const; - void set_brightness_cb(); + const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name); + bool set_brightness_cb(panther_msgs::SetLEDBrightness::Request& request, + panther_msgs::SetLEDBrightness::Response& response); std::shared_ptr ph_; std::shared_ptr nh_; std::shared_ptr it_; image_transport::Subscriber rear_light_sub_; image_transport::Subscriber front_light_sub_; + ros::ServiceServer set_brightness_server_; }; } // panther_lights_driver_node diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index fb759f898..da8653371 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -54,6 +55,18 @@ namespace apa_102 close(fd_); } + void APA102::set_global_brightness(const double brightness) + { + std::uint8_t val = brightness > 0.0f ? ceil(brightness * 31.0f) : 0; + set_global_brightness(val); + } + + void APA102::set_global_brightness(const std::uint8_t brightness) + { + // clamp values to be at max 31 + global_brightness_ = std::uint16_t(brightness) & 0x1F; + } + void APA102::set_panel(const std::vector& frame) const { if (frame.size() % 4 != 0) @@ -77,17 +90,18 @@ namespace apa_102 // padding std::size_t pad = i * 4; // header with brightness - buffer[4 + pad] = 0xE0 | (frame[pad + 3] >> 3); + std::uint8_t brightness = (std::uint16_t(frame[pad + 3]) * global_brightness_) / 255; + buffer[4 + pad] = 0xE0 | brightness; // convert rgb to bgr with collor correction - buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 3]) * corr_blue) >> 8); - buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_green) >> 8); - buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_red) >> 8); + buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue) / 255); + buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green) / 255); + buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red) / 255); } struct spi_ioc_transfer tr = { .tx_buf = (unsigned long long) buffer, .rx_buf = 0, - .len = (unsigned int) buffer_size, + .len = (unsigned int)buffer_size, .speed_hz = speed_, .delay_usecs = 0, .bits_per_word = 8, diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 217330d30..fbea63129 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -4,6 +4,8 @@ #include #include +#include "panther_msgs/SetLEDBrightness.h" + #include #include @@ -24,6 +26,7 @@ namespace panther_lights_driver { node_name_ = ros::this_node::getName(); + const double global_brightness = ph_->param("global_brightness", 1.0); frame_timeout_ = ph_->param("frame_timeout", 0.1); num_led_ = ph_->param("num_led", 46); @@ -35,16 +38,13 @@ namespace panther_lights_driver gpiod::line_request::DIRECTION_OUTPUT, 0 }; - power_pin_.request(lr, 1); + power_pin_.request(lr, 0); front_panel_ts_ = ros::Time::now(); rear_panel_ts_ = ros::Time::now(); - // take control over LEDs - power_pin_.set_value(1); - // clear LEDs - front_panel_.set_panel(std::vector(num_led_ * 4, 0)); - rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); + front_panel_.set_global_brightness(global_brightness); + rear_panel_.set_global_brightness(global_brightness); front_light_sub_ = it_->subscribe("lights/driver/front_panel_frame", 5, [this](const sensor_msgs::ImageConstPtr& msg) { @@ -58,8 +58,19 @@ namespace panther_lights_driver rear_panel_ts_ = msg->header.stamp; }); + set_brightness_server_ = nh_->advertiseService("lights/driver/set/brightness", &LightsDriverNode::set_brightness_cb, this); + + ROS_INFO("[%s] Node started spinning.", node_name_.c_str()); + + ros::Rate rate(30); + while (ros::ok() && !panels_initialised_) + { + ros::spinOnce(); + rate.sleep(); + ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); + } - ROS_INFO("[%s] Node started", node_name_.c_str()); + ROS_INFO("[%s] Lights are now being displayed.", node_name_.c_str()); } LightsDriverNode::~LightsDriverNode() @@ -72,8 +83,28 @@ namespace panther_lights_driver power_pin_.release(); } + bool LightsDriverNode::set_brightness_cb(panther_msgs::SetLEDBrightness::Request& request, + panther_msgs::SetLEDBrightness::Response& response) + { + float brightness = request.data; + if (brightness < 0.0f || brightness > 1.0f) + { + response.success = 0; + response.message = "Brightness out of range <0,1>"; + return true; + } + front_panel_.set_global_brightness(brightness); + rear_panel_.set_global_brightness(brightness); + auto str_bright = std::to_string(brightness); + // round string to two decimal places + str_bright = str_bright.substr(0, str_bright.find(".") + 3); + response.success = 1; + response.message = "Changed brightness to " + str_bright; + return true; + } + void LightsDriverNode::frame_cb(const sensor_msgs::ImageConstPtr& msg, - const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) const + const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) { if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) { @@ -85,28 +116,28 @@ namespace panther_lights_driver else if (msg->header.stamp < last_time) { ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Dropping message from past on panel %s panel!", + "[%s] Dropping message from past on panel %s panel!", node_name_.c_str(), panel_name.c_str()); return; } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image encoding ('%s') on panel %s panel!", + "[%s] Incorrect image encoding ('%s') on panel %s panel!", node_name_.c_str(), msg->encoding.c_str(), panel_name.c_str()); return; } else if (msg->height != 1) { ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image height %u on %s panel!, expected 1", + "[%s] Incorrect image height %u on %s panel!, expected 1", node_name_.c_str(), msg->height, panel_name.c_str()); return; } else if (msg->width != num_led_) { ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image height %u on %s panel!, expected %u", + "[%s] Incorrect image height %u on %s panel!, expected %u", node_name_.c_str(), msg->width, panel_name.c_str(), num_led_); return; } @@ -116,9 +147,13 @@ namespace panther_lights_driver node_name_.c_str(), panel_name.c_str()); return; } - else + + if (!panels_initialised_) { - panel.set_panel(msg->data); + panels_initialised_ = true; + // take control over LEDs + power_pin_.set_value(1); } + panel.set_panel(msg->data); } } \ No newline at end of file From 639ea528c06e6d3014271417119aeb52443bda32 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 7 Apr 2023 12:34:32 +0000 Subject: [PATCH 08/31] Invert LED_SBC_SEL pin --- panther_lights/src/lights_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index fbea63129..ea59f1519 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -36,7 +36,7 @@ namespace panther_lights_driver const gpiod::line_request lr = { node_name_, gpiod::line_request::DIRECTION_OUTPUT, - 0 + gpiod::line_request::FLAG_ACTIVE_LOW }; power_pin_.request(lr, 0); From c96a5a5d8a2bc9fca9428d4a764a3feae20d6779 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Tue, 11 Apr 2023 09:19:57 +0000 Subject: [PATCH 09/31] Change SPI mode --- panther_lights/src/apa102.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index da8653371..b012bdd57 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -29,9 +29,9 @@ namespace apa_102 static std::uint8_t mode = 0x00; if (cs_high) { - mode |= SPI_CS_HIGH; + mode |= SPI_CS_HIGH | SPI_MODE_0; } - if (ioctl(fd_, SPI_IOC_WR_MODE, &mode) == -1) + if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) { close(fd_); throw std::ios_base::failure(std::string("Failed to set mode for ") + device_); From 7353325a66d73ddf4fe6399f9c11f79d6493979d Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 12 Apr 2023 11:00:12 +0200 Subject: [PATCH 10/31] Change clock polarity --- panther_lights/src/apa102.cpp | 4 ++-- panther_lights/src/lights_driver_node.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index b012bdd57..b612a85a4 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -26,10 +26,10 @@ namespace apa_102 throw std::ios_base::failure(std::string("Failed to open ") + device_); } - static std::uint8_t mode = 0x00; + static std::uint8_t mode = SPI_MODE_3; if (cs_high) { - mode |= SPI_CS_HIGH | SPI_MODE_0; + mode |= SPI_CS_HIGH; } if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) { diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index ea59f1519..857d80a5c 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -60,7 +60,7 @@ namespace panther_lights_driver set_brightness_server_ = nh_->advertiseService("lights/driver/set/brightness", &LightsDriverNode::set_brightness_cb, this); - ROS_INFO("[%s] Node started spinning.", node_name_.c_str()); + ROS_INFO("[%s] Node started spinning", node_name_.c_str()); ros::Rate rate(30); while (ros::ok() && !panels_initialised_) @@ -70,7 +70,7 @@ namespace panther_lights_driver ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); } - ROS_INFO("[%s] Lights are now being displayed.", node_name_.c_str()); + ROS_INFO("[%s] Lights are now being displayed", node_name_.c_str()); } LightsDriverNode::~LightsDriverNode() From f9ef2c82229dfe7fd4c7cd644441f76f8a233d4b Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Tue, 18 Apr 2023 14:04:05 +0000 Subject: [PATCH 11/31] Reduce number of ROS lgger instances --- panther_lights/src/lights_driver_node.cpp | 57 ++++++++++++----------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 857d80a5c..32354a6f0 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -106,54 +106,55 @@ namespace panther_lights_driver void LightsDriverNode::frame_cb(const sensor_msgs::ImageConstPtr& msg, const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) { + std::string meessage; if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) { - ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Timeout exceeded, ignoring frame on %s panel!", - node_name_.c_str(), panel_name.c_str()); - return; + meessage = "Timeout exceeded, ignoring frame"; } else if (msg->header.stamp < last_time) { - ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Dropping message from past on panel %s panel!", - node_name_.c_str(), panel_name.c_str()); - return; + meessage = "Dropping message from past"; } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { - ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image encoding ('%s') on panel %s panel!", - node_name_.c_str(), msg->encoding.c_str(), panel_name.c_str()); - return; + meessage = "Incorrect image encoding ('" + msg->encoding + "')"; } else if (msg->height != 1) { - ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image height %u on %s panel!, expected 1", - node_name_.c_str(), msg->height, panel_name.c_str()); - return; + meessage = "Incorrect image height " + std::to_str(msg->height); } else if (msg->width != num_led_) { - ROS_WARN_THROTTLE_PANELS(panel_name, "front", "rear", 5.0, - "[%s] Incorrect image height %u on %s panel!, expected %u", - node_name_.c_str(), msg->width, panel_name.c_str(), num_led_); - return; + meessage = "Incorrect image width " + std::to_str(msg->width); } else if ((ros::Time::now() - msg->header.stamp).toSec() > 5.0) { - ROS_WARN("[%s] Timeout. Dropping frame on panel %s panel!", - node_name_.c_str(), panel_name.c_str()); - return; + meessage = "Timeout. Dropping frame"; } - if (!panels_initialised_) + if (!meessage.empty()) { - panels_initialised_ = true; - // take control over LEDs - power_pin_.set_value(1); + if (panel_name == "front") + { + ROS_WARN_THROTTLE(5.0, "[%s] %s on front panel!", node_name_.c_str(), meessage.c_str()); + + } + else if (panel_name == "rear") + { + ROS_WARN_THROTTLE(5.0, "[%s] %s on front rear!", node_name_.c_str(), meessage.c_str()); + } } - panel.set_panel(msg->data); + else + { + if (!panels_initialised_) + { + panels_initialised_ = true; + // take control over LEDs + power_pin_.set_value(1); + } + panel.set_panel(msg->data); + + } + } } \ No newline at end of file From 83fe3b822c9533048ba0fd781cd1d243a7daa548 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Tue, 18 Apr 2023 16:25:21 +0000 Subject: [PATCH 12/31] Celan up code, update to libgpiod 2.0 --- panther_lights/CMakeLists.txt | 25 +- .../include/panther_lights/apa102.hpp | 48 ++- .../panther_lights/lights_driver_node.hpp | 87 +++--- panther_lights/src/apa102.cpp | 178 ++++++------ panther_lights/src/lights_driver_node.cpp | 273 +++++++++--------- panther_lights/src/main.cpp | 14 +- 6 files changed, 304 insertions(+), 321 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 000048812..f04461572 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -1,9 +1,25 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.16.3) project(panther_lights) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +find_library(LIBGPIOD libgpiod) +if(NOT LIBGPIOD) + message(WARNING "libgpiod not found. Building from source") + include(ExternalProject) + + ExternalProject_Add(libgpiod + GIT_REPOSITORY git://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git + GIT_TAG v2.0.1 + PREFIX ${CATKIN_DEVEL_PREFIX} + CONFIGURE_COMMAND /autogen.sh --prefix= --enable-tools=no --enable-bindings-cxx + BUILD_COMMAND make -j ${N} + INSTALL_COMMAND make install INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/lib/libgpiod + BUILD_IN_SOURCE 1 + ) +endif() + find_package(catkin REQUIRED COMPONENTS panther_msgs image_transport @@ -16,6 +32,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include + LIBRARIES libgpiod CATKIN_DEPENDS panther_msgs roscpp ) @@ -30,9 +47,9 @@ add_executable(driver_node src/apa102.cpp ) -add_dependencies(driver_node ${catkin_EXPORTED_TARGETS}) +add_dependencies(driver_node libgpiod ${catkin_EXPORTED_TARGETS}) target_link_libraries(driver_node - gpiodcxx + ${CATKIN_DEVEL_PREFIX}/lib/libgpiodcxx.so ${catkin_LIBRARIES} ) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 43f20f984..3e96cd8c6 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -1,10 +1,6 @@ #ifndef PANTHER_LIGHTS_APA_102_HPP_ #define PANTHER_LIGHTS_APA_102_HPP_ -#include -#include - -#include #include #include #include @@ -12,28 +8,28 @@ namespace apa_102 { - class APA102 - { - public: - APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); - ~APA102(); - void set_global_brightness(const std::uint8_t brightness); - void set_global_brightness(const double brightness); - void set_panel(const std::vector& frame) const; +class APA102 +{ +public: + APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); + ~APA102(); + void set_global_brightness(const std::uint8_t brightness); + void set_global_brightness(const double brightness); + void set_panel(const std::vector & frame) const; + +private: + const int fd_; + const std::string device_; + const std::uint8_t bits_ = 8; + const std::uint32_t speed_; + std::uint16_t global_brightness_; - private: - const std::string device_; - const int fd_; - const std::uint8_t bits_ = 8; - const std::uint32_t speed_; - std::uint16_t global_brightness_; - - // color correction constants - const std::uint16_t corr_red = 255.0; - const std::uint16_t corr_green = 200.0; - const std::uint16_t corr_blue = 62.0; - }; + // color correction constants + const std::uint16_t corr_red = 255.0; + const std::uint16_t corr_green = 200.0; + const std::uint16_t corr_blue = 62.0; +}; -} // namespace apa_102 +} // namespace apa_102 -#endif // PANTHER_LIGHTS_APA_102_HPP_ \ No newline at end of file +#endif // PANTHER_LIGHTS_APA_102_HPP_ \ No newline at end of file diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/lights_driver_node.hpp index 2d3e56f13..e60118cf7 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/lights_driver_node.hpp @@ -1,60 +1,57 @@ #ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_ #define PANTHER_LIGHTS_DRIVER_NODE_HPP_ -#include "panther_lights/apa102.hpp" +#include +#include #include #include -#include #include - #include -#include -#include "panther_msgs/SetLEDBrightness.h" +#include -#define ROS_WARN_THROTTLE_PANELS(panel, p1, p2, ...) \ -if (panel == p1) ROS_WARN_THROTTLE(__VA_ARGS__); \ -else if (panel == p2) ROS_WARN_THROTTLE(__VA_ARGS__) +#include namespace panther_lights_driver { - class LightsDriverNode - { - public: - LightsDriverNode( - std::shared_ptr private_nh, - std::shared_ptr nh, - std::shared_ptr it); - ~LightsDriverNode(); - - private: - apa_102::APA102 front_panel_; - apa_102::APA102 rear_panel_; - gpiod::line power_pin_; - std::string node_name_; - double frame_timeout_; - - ros::Time front_panel_ts_; - ros::Time rear_panel_ts_; - bool panels_initialised_{false}; - - int num_led_; - void frame_cb(const sensor_msgs::ImageConstPtr& msg, - const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name); - bool set_brightness_cb(panther_msgs::SetLEDBrightness::Request& request, - panther_msgs::SetLEDBrightness::Response& response); - - std::shared_ptr ph_; - std::shared_ptr nh_; - std::shared_ptr it_; - image_transport::Subscriber rear_light_sub_; - image_transport::Subscriber front_light_sub_; - ros::ServiceServer set_brightness_server_; - }; - -} // panther_lights_driver_node - -#endif // PANTHER_LIGHTS_DRIVER_NODE_HPP_ \ No newline at end of file +class LightsDriverNode +{ +public: + LightsDriverNode( + const std::shared_ptr private_nh, std::shared_ptr nh, + const std::shared_ptr it); + ~LightsDriverNode(); + +private: + int num_led_; + double frame_timeout_; + bool panels_initialised_ = false; + std::string node_name_; + + apa_102::APA102 front_panel_; + apa_102::APA102 rear_panel_; + + ros::Time front_panel_ts_; + ros::Time rear_panel_ts_; + std::shared_ptr ph_; + std::shared_ptr nh_; + std::shared_ptr it_; + ros::ServiceServer set_brightness_server_; + image_transport::Subscriber rear_light_sub_; + image_transport::Subscriber front_light_sub_; + + void frame_cb( + const sensor_msgs::ImageConstPtr & msg, const apa_102::APA102 & panel, + const ros::Time & last_time, const std::string panel_name); + void set_pin_value(const gpiod::line::value value) const; + bool set_brightness_cb( + panther_msgs::SetLEDBrightness::Request & request, + panther_msgs::SetLEDBrightness::Response & response); +}; + +} // namespace panther_lights_driver + +#endif // PANTHER_LIGHTS_DRIVER_NODE_HPP_ \ No newline at end of file diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index b612a85a4..7da94b01c 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -1,118 +1,102 @@ -#include "panther_lights/apa102.hpp" +#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include #include -#include #include +#include +#include + +#include +#include +#include +#include +#include +#include namespace apa_102 { - APA102::APA102(const std::string device, const std::uint32_t speed, const bool cs_high) - : device_(device) - , speed_(speed) - , fd_(open(device_.c_str(), O_RDWR)) - { - if (fd_ < 0) - { - throw std::ios_base::failure(std::string("Failed to open ") + device_); - } - - static std::uint8_t mode = SPI_MODE_3; - if (cs_high) - { - mode |= SPI_CS_HIGH; - } - if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) - { - close(fd_); - throw std::ios_base::failure(std::string("Failed to set mode for ") + device_); - } - - if (ioctl(fd_, SPI_IOC_WR_BITS_PER_WORD, &bits_) == -1) - { - close(fd_); - throw std::ios_base::failure(std::string("Can't set bits per word for ") + device_); - } - - if (ioctl(fd_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) - { - close(fd_); - throw std::ios_base::failure(std::string("Can't set speed for ") + device_); - } +APA102::APA102(const std::string device, const std::uint32_t speed, const bool cs_high) +: device_(device), speed_(speed), fd_(open(device_.c_str(), O_RDWR)) +{ + if (fd_ < 0) { + throw std::ios_base::failure(std::string("Failed to open ") + device_); } - APA102::~APA102() - { + static std::uint8_t mode = SPI_MODE_3; + if (cs_high) { + mode |= SPI_CS_HIGH; + } + if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) { close(fd_); + throw std::ios_base::failure(std::string("Failed to set mode for ") + device_); } - void APA102::set_global_brightness(const double brightness) - { - std::uint8_t val = brightness > 0.0f ? ceil(brightness * 31.0f) : 0; - set_global_brightness(val); + if (ioctl(fd_, SPI_IOC_WR_BITS_PER_WORD, &bits_) == -1) { + close(fd_); + throw std::ios_base::failure(std::string("Can't set bits per word for ") + device_); } - void APA102::set_global_brightness(const std::uint8_t brightness) - { - // clamp values to be at max 31 - global_brightness_ = std::uint16_t(brightness) & 0x1F; + if (ioctl(fd_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) { + close(fd_); + throw std::ios_base::failure(std::string("Can't set speed for ") + device_); } +} + +APA102::~APA102() { close(fd_); } + +void APA102::set_global_brightness(const double brightness) +{ + std::uint8_t val = brightness > 0.0f ? ceil(brightness * 31.0f) : 0; + set_global_brightness(val); +} - void APA102::set_panel(const std::vector& frame) const - { - if (frame.size() % 4 != 0) - { - throw std::runtime_error("Incorrect number of bytes to transfer to LEDs"); - } - // init buffer with start and end frames - std::size_t buffer_size = (4 * sizeof(std::uint8_t)) + frame.size() + (4 * sizeof(std::uint8_t)); - std::uint8_t* buffer = new std::uint8_t[buffer_size]; +void APA102::set_global_brightness(const std::uint8_t brightness) +{ + // clamp values to be at max 31 + global_brightness_ = std::uint16_t(brightness) & 0x1F; +} - // init start and end frames - for (std::size_t i = 0; i < 4; i++) - { - buffer[i] = 0x00; - buffer[buffer_size - i - 1] = 0xFF; - } +void APA102::set_panel(const std::vector & frame) const +{ + if (frame.size() % 4 != 0) { + throw std::runtime_error("Incorrect number of bytes to transfer to LEDs"); + } + // init buffer with start and end frames + std::size_t buffer_size = (4 * sizeof(std::uint8_t)) + frame.size() + (4 * sizeof(std::uint8_t)); + std::uint8_t * buffer = new std::uint8_t[buffer_size]; - // copy frame from vector to sending buffer - for (std::size_t i = 0; i < frame.size() / 4; i++) - { - // padding - std::size_t pad = i * 4; - // header with brightness - std::uint8_t brightness = (std::uint16_t(frame[pad + 3]) * global_brightness_) / 255; - buffer[4 + pad] = 0xE0 | brightness; - // convert rgb to bgr with collor correction - buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue) / 255); - buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green) / 255); - buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red) / 255); - } + // init start and end frames + for (std::size_t i = 0; i < 4; i++) { + buffer[i] = 0x00; + buffer[buffer_size - i - 1] = 0xFF; + } + + // copy frame from vector to sending buffer + for (std::size_t i = 0; i < frame.size() / 4; i++) { + // padding + std::size_t pad = i * 4; + // header with brightness + std::uint8_t brightness = (std::uint16_t(frame[pad + 3]) * global_brightness_) / 255; + buffer[4 + pad] = 0xE0 | brightness; + // convert rgb to bgr with collor correction + buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue) / 255); + buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green) / 255); + buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red) / 255); + } - struct spi_ioc_transfer tr = { - .tx_buf = (unsigned long long) buffer, - .rx_buf = 0, - .len = (unsigned int)buffer_size, - .speed_hz = speed_, - .delay_usecs = 0, - .bits_per_word = 8, - }; + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long long)buffer, + .rx_buf = 0, + .len = (unsigned int)buffer_size, + .speed_hz = speed_, + .delay_usecs = 0, + .bits_per_word = 8, + }; - int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr); - delete[] buffer; + int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr); + delete[] buffer; - if (ret < 1) - { - throw std::ios_base::failure(std::string("Failed to send data over SPI ") + device_); - } + if (ret < 1) { + throw std::ios_base::failure(std::string("Failed to send data over SPI ") + device_); } -} \ No newline at end of file +} +} // namespace apa_102 \ No newline at end of file diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 32354a6f0..19065db4f 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -1,160 +1,149 @@ -#include "panther_lights/lights_driver_node.hpp" -#include "panther_lights/apa102.hpp" +#include -#include -#include - -#include "panther_msgs/SetLEDBrightness.h" - -#include +#include #include #include +#include + +#include +#include + +#include + +#include namespace panther_lights_driver { - LightsDriverNode::LightsDriverNode( - std::shared_ptr private_nh, - std::shared_ptr nh, - std::shared_ptr it - ) - : ph_(std::move(private_nh)) - , nh_(std::move(nh)) - , it_(std::move(it)) - , front_panel_("/dev/spidev0.0") - , rear_panel_("/dev/spidev0.1") - { - node_name_ = ros::this_node::getName(); - - const double global_brightness = ph_->param("global_brightness", 1.0); - frame_timeout_ = ph_->param("frame_timeout", 0.1); - num_led_ = ph_->param("num_led", 46); - - const gpiod::chip chip("gpiochip0"); - power_pin_ = chip.find_line("LED_SBC_SEL"); - - const gpiod::line_request lr = { - node_name_, - gpiod::line_request::DIRECTION_OUTPUT, - gpiod::line_request::FLAG_ACTIVE_LOW - }; - power_pin_.request(lr, 0); - - front_panel_ts_ = ros::Time::now(); - rear_panel_ts_ = ros::Time::now(); - - front_panel_.set_global_brightness(global_brightness); - rear_panel_.set_global_brightness(global_brightness); - - front_light_sub_ = it_->subscribe("lights/driver/front_panel_frame", 5, - [this](const sensor_msgs::ImageConstPtr& msg) { - frame_cb(msg, front_panel_, front_panel_ts_, "front"); - front_panel_ts_ = msg->header.stamp; - }); - - rear_light_sub_ = it_->subscribe("lights/driver/rear_panel_frame", 5, - [this](const sensor_msgs::ImageConstPtr& msg) { - frame_cb(msg, rear_panel_, rear_panel_ts_, "rear"); - rear_panel_ts_ = msg->header.stamp; - }); - - set_brightness_server_ = nh_->advertiseService("lights/driver/set/brightness", &LightsDriverNode::set_brightness_cb, this); - - ROS_INFO("[%s] Node started spinning", node_name_.c_str()); - - ros::Rate rate(30); - while (ros::ok() && !panels_initialised_) - { - ros::spinOnce(); - rate.sleep(); - ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); - } +LightsDriverNode::LightsDriverNode( + const std::shared_ptr private_nh, std::shared_ptr nh, + const std::shared_ptr it) +: ph_(std::move(private_nh)), + nh_(std::move(nh)), + it_(std::move(it)), + front_panel_("/dev/spidev0.0"), + rear_panel_("/dev/spidev0.1") +{ + node_name_ = ros::this_node::getName(); - ROS_INFO("[%s] Lights are now being displayed", node_name_.c_str()); - } + const double global_brightness = ph_->param("global_brightness", 1.0); + frame_timeout_ = ph_->param("frame_timeout", 0.1); + num_led_ = ph_->param("num_led", 46); + + set_pin_value(gpiod::line::value::INACTIVE); + + front_panel_ts_ = ros::Time::now(); + rear_panel_ts_ = ros::Time::now(); + + front_panel_.set_global_brightness(global_brightness); + rear_panel_.set_global_brightness(global_brightness); + + front_light_sub_ = it_->subscribe( + "lights/driver/front_panel_frame", 5, [this](const sensor_msgs::ImageConstPtr & msg) { + frame_cb(msg, front_panel_, front_panel_ts_, "front"); + front_panel_ts_ = msg->header.stamp; + }); + + rear_light_sub_ = it_->subscribe( + "lights/driver/rear_panel_frame", 5, [this](const sensor_msgs::ImageConstPtr & msg) { + frame_cb(msg, rear_panel_, rear_panel_ts_, "rear"); + rear_panel_ts_ = msg->header.stamp; + }); + + set_brightness_server_ = nh_->advertiseService( + "lights/driver/set/brightness", &LightsDriverNode::set_brightness_cb, this); - LightsDriverNode::~LightsDriverNode() - { - // clear LEDs - front_panel_.set_panel(std::vector(num_led_ * 4, 0)); - rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); - // give back control over LEDs - power_pin_.set_value(0); - power_pin_.release(); + ROS_INFO("[%s] Node started spinning", node_name_.c_str()); + + while (ros::ok() && !panels_initialised_) { + ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); + ros::Duration(1.0 / 30.0).sleep(); + ros::spinOnce(); } - bool LightsDriverNode::set_brightness_cb(panther_msgs::SetLEDBrightness::Request& request, - panther_msgs::SetLEDBrightness::Response& response) - { - float brightness = request.data; - if (brightness < 0.0f || brightness > 1.0f) - { - response.success = 0; - response.message = "Brightness out of range <0,1>"; - return true; - } - front_panel_.set_global_brightness(brightness); - rear_panel_.set_global_brightness(brightness); - auto str_bright = std::to_string(brightness); - // round string to two decimal places - str_bright = str_bright.substr(0, str_bright.find(".") + 3); - response.success = 1; - response.message = "Changed brightness to " + str_bright; + ROS_INFO("[%s] Lights are now being displayed", node_name_.c_str()); +} + +LightsDriverNode::~LightsDriverNode() +{ + // clear LEDs + front_panel_.set_panel(std::vector(num_led_ * 4, 0)); + rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); + + // give back control over LEDs + set_pin_value(gpiod::line::value::INACTIVE); +} + +bool LightsDriverNode::set_brightness_cb( + panther_msgs::SetLEDBrightness::Request & request, + panther_msgs::SetLEDBrightness::Response & response) +{ + float brightness = request.data; + if (brightness < 0.0f || brightness > 1.0f) { + response.success = 0; + response.message = "Brightness out of range <0,1>"; return true; } + front_panel_.set_global_brightness(brightness); + rear_panel_.set_global_brightness(brightness); + auto str_bright = std::to_string(brightness); + + // round string to two decimal places + str_bright = str_bright.substr(0, str_bright.find(".") + 3); + response.success = 1; + response.message = "Changed brightness to " + str_bright; + return true; +} + +void LightsDriverNode::frame_cb( + const sensor_msgs::ImageConstPtr & msg, const apa_102::APA102 & panel, + const ros::Time & last_time, const std::string panel_name) +{ + std::string meessage; + if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) { + meessage = "Timeout exceeded, ignoring frame"; + } else if (msg->header.stamp < last_time) { + meessage = "Dropping message from past"; + } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { + meessage = "Incorrect image encoding ('" + msg->encoding + "')"; + } else if (msg->height != 1) { + meessage = "Incorrect image height " + std::to_string(msg->height); + } else if (msg->width != num_led_) { + meessage = "Incorrect image width " + std::to_string(msg->width); + } else if ((ros::Time::now() - msg->header.stamp).toSec() > 5.0) { + meessage = "Timeout. Dropping frame"; + } - void LightsDriverNode::frame_cb(const sensor_msgs::ImageConstPtr& msg, - const apa_102::APA102& panel, const ros::Time& last_time, const std::string panel_name) - { - std::string meessage; - if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) - { - meessage = "Timeout exceeded, ignoring frame"; - } - else if (msg->header.stamp < last_time) - { - meessage = "Dropping message from past"; - } - else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) - { - meessage = "Incorrect image encoding ('" + msg->encoding + "')"; - } - else if (msg->height != 1) - { - meessage = "Incorrect image height " + std::to_str(msg->height); - } - else if (msg->width != num_led_) - { - meessage = "Incorrect image width " + std::to_str(msg->width); - } - else if ((ros::Time::now() - msg->header.stamp).toSec() > 5.0) - { - meessage = "Timeout. Dropping frame"; - } - - if (!meessage.empty()) - { - if (panel_name == "front") - { - ROS_WARN_THROTTLE(5.0, "[%s] %s on front panel!", node_name_.c_str(), meessage.c_str()); - - } - else if (panel_name == "rear") - { - ROS_WARN_THROTTLE(5.0, "[%s] %s on front rear!", node_name_.c_str(), meessage.c_str()); - } + if (!meessage.empty()) { + if (panel_name == "front") { + ROS_WARN_THROTTLE(5.0, "[%s] %s on front panel!", node_name_.c_str(), meessage.c_str()); + } else if (panel_name == "rear") { + ROS_WARN_THROTTLE(5.0, "[%s] %s on front rear!", node_name_.c_str(), meessage.c_str()); } - else - { - if (!panels_initialised_) - { - panels_initialised_ = true; - // take control over LEDs - power_pin_.set_value(1); - } - panel.set_panel(msg->data); + } else { + if (!panels_initialised_) { + panels_initialised_ = true; + // take control over LEDs + set_pin_value(gpiod::line::value::ACTIVE); } - + panel.set_panel(msg->data); } -} \ No newline at end of file +} + +void LightsDriverNode::set_pin_value(const gpiod::line::value value) const +{ + auto chip = gpiod::chip(std::filesystem::path{"/dev/gpiochip0"}); + auto power_pin_offset = gpiod::line::offset(chip.get_line_offset_from_name("LED_SBC_SEL")); + + auto settings = gpiod::line_settings(); + settings.set_direction(gpiod::line::direction::OUTPUT); + settings.set_active_low(true); + settings.set_output_value(value); + + auto rb = chip.prepare_request(); + rb.set_consumer(node_name_); + rb.add_line_settings(power_pin_offset, settings); + rb.do_request(); +} +} // namespace panther_lights_driver \ No newline at end of file diff --git a/panther_lights/src/main.cpp b/panther_lights/src/main.cpp index 323e8f3c3..a6f674a89 100644 --- a/panther_lights/src/main.cpp +++ b/panther_lights/src/main.cpp @@ -1,9 +1,10 @@ -#include "panther_lights/lights_driver_node.hpp" +#include #include + #include -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "lights_driver_node"); @@ -11,19 +12,18 @@ int main(int argc, char** argv) auto nh = std::make_shared(); auto it = std::make_shared(*nh); - try - { + try { panther_lights_driver::LightsDriverNode lights_driver_node(private_nh, nh, it); ros::spin(); } - catch (const std::exception& e) { + catch (const std::exception & e) { std::cerr << "[" << ros::this_node::getName() << "]" - << " Caught exception: " << e.what() << std::endl; + << " Caught exception: " << e.what() << std::endl; } std::cout << "[" << ros::this_node::getName() << "]" - << " Shutting down" << std::endl; + << " Shutting down" << std::endl; ros::shutdown(); return 0; } \ No newline at end of file From 0493f376650be783857b1894000d1c5913279b45 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:46:15 +0200 Subject: [PATCH 13/31] Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/include/panther_lights/apa102.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 3e96cd8c6..be86e8ea3 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -13,6 +13,7 @@ class APA102 public: APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); ~APA102(); + void set_global_brightness(const std::uint8_t brightness); void set_global_brightness(const double brightness); void set_panel(const std::vector & frame) const; From 809ddc3a1f6043308fdf125b118bb932c89572c7 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:46:36 +0200 Subject: [PATCH 14/31] Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/lights_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 19065db4f..4ff1721ec 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -39,7 +39,7 @@ LightsDriverNode::LightsDriverNode( rear_panel_.set_global_brightness(global_brightness); front_light_sub_ = it_->subscribe( - "lights/driver/front_panel_frame", 5, [this](const sensor_msgs::ImageConstPtr & msg) { + "lights/driver/front_panel_frame", 5, [&](const sensor_msgs::ImageConstPtr & msg) { frame_cb(msg, front_panel_, front_panel_ts_, "front"); front_panel_ts_ = msg->header.stamp; }); From be28a6907c67b9295eb48148f749f6928a661516 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:49:55 +0200 Subject: [PATCH 15/31] Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/lights_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/lights_driver_node.cpp index 4ff1721ec..296450e33 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/lights_driver_node.cpp @@ -80,7 +80,7 @@ bool LightsDriverNode::set_brightness_cb( { float brightness = request.data; if (brightness < 0.0f || brightness > 1.0f) { - response.success = 0; + response.success = false; response.message = "Brightness out of range <0,1>"; return true; } From e853280ff6e8d28723358fabe4d36766714c0b5f Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:50:21 +0200 Subject: [PATCH 16/31] Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/include/panther_lights/apa102.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index be86e8ea3..2fee54198 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -26,9 +26,9 @@ class APA102 std::uint16_t global_brightness_; // color correction constants - const std::uint16_t corr_red = 255.0; - const std::uint16_t corr_green = 200.0; - const std::uint16_t corr_blue = 62.0; + const std::uint16_t corr_red_ = 255.0; + const std::uint16_t corr_green_ = 200.0; + const std::uint16_t corr_blue_ = 62.0; }; } // namespace apa_102 From 54947be5cdfeded13aff66e4ad3d968acd195c15 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 21 Apr 2023 12:51:29 +0000 Subject: [PATCH 17/31] Review changes --- panther_lights/CMakeLists.txt | 14 +++++----- panther_lights/README.md | 2 +- .../include/panther_lights/apa102.hpp | 6 ++--- ...lights_driver_node.hpp => driver_node.hpp} | 12 ++++----- panther_lights/package.xml | 7 ++--- panther_lights/src/apa102.cpp | 6 ++--- ...lights_driver_node.cpp => driver_node.cpp} | 26 +++++++++---------- panther_lights/src/main.cpp | 6 ++--- 8 files changed, 38 insertions(+), 41 deletions(-) rename panther_lights/include/panther_lights/{lights_driver_node.hpp => driver_node.hpp} (85%) rename panther_lights/src/{lights_driver_node.cpp => driver_node.cpp} (86%) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index f04461572..5f7509146 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -21,13 +21,13 @@ if(NOT LIBGPIOD) endif() find_package(catkin REQUIRED COMPONENTS - panther_msgs - image_transport - roscpp - rospy - sensor_msgs - std_msgs - std_srvs +image_transport +panther_msgs +roscpp +rospy +sensor_msgs +std_msgs +std_srvs ) catkin_package( diff --git a/panther_lights/README.md b/panther_lights/README.md index a287b6128..da6f8a4ec 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -33,7 +33,7 @@ This node is responsible for processing animations and publishing frames to be d - `~test` [*bool*, default: **false**]: enables testing mode, enabling extra functionalities. - `~user_animations` [*list*, default: **None**]: optional list of animations defined by the user. -### driver_node.py +### driver_node This node is responsible for displaying frames on the Husarion Panther robot LED panels. diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 3e96cd8c6..c5e96dae4 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -1,5 +1,5 @@ -#ifndef PANTHER_LIGHTS_APA_102_HPP_ -#define PANTHER_LIGHTS_APA_102_HPP_ +#ifndef PANTHER_LIGHTS_APA102_HPP_ +#define PANTHER_LIGHTS_APA102_HPP_ #include #include @@ -32,4 +32,4 @@ class APA102 } // namespace apa_102 -#endif // PANTHER_LIGHTS_APA_102_HPP_ \ No newline at end of file +#endif // PANTHER_LIGHTS_APA102_HPP_ \ No newline at end of file diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp similarity index 85% rename from panther_lights/include/panther_lights/lights_driver_node.hpp rename to panther_lights/include/panther_lights/driver_node.hpp index e60118cf7..277218926 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -14,16 +14,16 @@ #include -namespace panther_lights_driver +namespace panther_lights { -class LightsDriverNode +class DriverNode { public: - LightsDriverNode( - const std::shared_ptr private_nh, std::shared_ptr nh, + DriverNode( + const std::shared_ptr ph, std::shared_ptr nh, const std::shared_ptr it); - ~LightsDriverNode(); + ~DriverNode(); private: int num_led_; @@ -52,6 +52,6 @@ class LightsDriverNode panther_msgs::SetLEDBrightness::Response & response); }; -} // namespace panther_lights_driver +} // namespace panther_lights #endif // PANTHER_LIGHTS_DRIVER_NODE_HPP_ \ No newline at end of file diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 90b20896b..c43e336a6 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -16,17 +16,14 @@ catkin + image_transport panther_msgs + roscpp rospy sensor_msgs std_msgs std_srvs - libgpiod-dev - - image_transport - roscpp - python3-pil diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 7da94b01c..92f52ac7f 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -78,9 +78,9 @@ void APA102::set_panel(const std::vector & frame) const std::uint8_t brightness = (std::uint16_t(frame[pad + 3]) * global_brightness_) / 255; buffer[4 + pad] = 0xE0 | brightness; // convert rgb to bgr with collor correction - buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue) / 255); - buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green) / 255); - buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red) / 255); + buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue_) / 255); + buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green_) / 255); + buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red_) / 255); } struct spi_ioc_transfer tr = { diff --git a/panther_lights/src/lights_driver_node.cpp b/panther_lights/src/driver_node.cpp similarity index 86% rename from panther_lights/src/lights_driver_node.cpp rename to panther_lights/src/driver_node.cpp index 19065db4f..8a0d4660c 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -13,12 +13,12 @@ #include -namespace panther_lights_driver +namespace panther_lights { -LightsDriverNode::LightsDriverNode( - const std::shared_ptr private_nh, std::shared_ptr nh, +DriverNode::DriverNode( + const std::shared_ptr ph, std::shared_ptr nh, const std::shared_ptr it) -: ph_(std::move(private_nh)), +: ph_(std::move(ph)), nh_(std::move(nh)), it_(std::move(it)), front_panel_("/dev/spidev0.0"), @@ -51,9 +51,8 @@ LightsDriverNode::LightsDriverNode( }); set_brightness_server_ = nh_->advertiseService( - "lights/driver/set/brightness", &LightsDriverNode::set_brightness_cb, this); + "lights/driver/set/brightness", &DriverNode::set_brightness_cb, this); - ROS_INFO("[%s] Node started spinning", node_name_.c_str()); while (ros::ok() && !panels_initialised_) { ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); @@ -61,10 +60,11 @@ LightsDriverNode::LightsDriverNode( ros::spinOnce(); } - ROS_INFO("[%s] Lights are now being displayed", node_name_.c_str()); + ROS_INFO("[%s] LED panels initialised", node_name_.c_str()); + ROS_INFO("[%s] Node started", node_name_.c_str()); } -LightsDriverNode::~LightsDriverNode() +DriverNode::~DriverNode() { // clear LEDs front_panel_.set_panel(std::vector(num_led_ * 4, 0)); @@ -74,7 +74,7 @@ LightsDriverNode::~LightsDriverNode() set_pin_value(gpiod::line::value::INACTIVE); } -bool LightsDriverNode::set_brightness_cb( +bool DriverNode::set_brightness_cb( panther_msgs::SetLEDBrightness::Request & request, panther_msgs::SetLEDBrightness::Response & response) { @@ -95,7 +95,7 @@ bool LightsDriverNode::set_brightness_cb( return true; } -void LightsDriverNode::frame_cb( +void DriverNode::frame_cb( const sensor_msgs::ImageConstPtr & msg, const apa_102::APA102 & panel, const ros::Time & last_time, const std::string panel_name) { @@ -131,7 +131,7 @@ void LightsDriverNode::frame_cb( } } -void LightsDriverNode::set_pin_value(const gpiod::line::value value) const +void DriverNode::set_pin_value(const gpiod::line::value value) const { auto chip = gpiod::chip(std::filesystem::path{"/dev/gpiochip0"}); auto power_pin_offset = gpiod::line::offset(chip.get_line_offset_from_name("LED_SBC_SEL")); @@ -146,4 +146,4 @@ void LightsDriverNode::set_pin_value(const gpiod::line::value value) const rb.add_line_settings(power_pin_offset, settings); rb.do_request(); } -} // namespace panther_lights_driver \ No newline at end of file +} // namespace panther_lights \ No newline at end of file diff --git a/panther_lights/src/main.cpp b/panther_lights/src/main.cpp index a6f674a89..12174f0f4 100644 --- a/panther_lights/src/main.cpp +++ b/panther_lights/src/main.cpp @@ -1,4 +1,4 @@ -#include +#include #include @@ -8,12 +8,12 @@ int main(int argc, char ** argv) { ros::init(argc, argv, "lights_driver_node"); - auto private_nh = std::make_shared("~"); + auto ph = std::make_shared("~"); auto nh = std::make_shared(); auto it = std::make_shared(*nh); try { - panther_lights_driver::LightsDriverNode lights_driver_node(private_nh, nh, it); + panther_lights::DriverNode driver_node(ph, nh, it); ros::spin(); } From 4175b7a29b329d4e2d52b9a559304056486a0101 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 21 Apr 2023 15:03:18 +0200 Subject: [PATCH 18/31] Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/apa102.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 92f52ac7f..62fbd95e0 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -21,10 +21,7 @@ APA102::APA102(const std::string device, const std::uint32_t speed, const bool c throw std::ios_base::failure(std::string("Failed to open ") + device_); } - static std::uint8_t mode = SPI_MODE_3; - if (cs_high) { - mode |= SPI_CS_HIGH; - } + 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(std::string("Failed to set mode for ") + device_); From c212ed2a5ab5c1b114e73717d810e9f19513107b Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 21 Apr 2023 13:15:06 +0000 Subject: [PATCH 19/31] Review cleanup --- panther_lights/CMakeLists.txt | 2 +- .../include/panther_lights/driver_node.hpp | 5 ++-- panther_lights/src/driver_node.cpp | 24 +++++++++---------- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 5f7509146..a98d1c9a3 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -43,7 +43,7 @@ include_directories( add_executable(driver_node src/main.cpp - src/lights_driver_node.cpp + src/driver_node.cpp src/apa102.cpp ) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index 277218926..17f1eca58 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -44,12 +44,11 @@ class DriverNode image_transport::Subscriber front_light_sub_; void frame_cb( - const sensor_msgs::ImageConstPtr & msg, const apa_102::APA102 & panel, + const sensor_msgs::Image::ConstPtr & msg, const apa_102::APA102 & panel, const ros::Time & last_time, const std::string panel_name); void set_pin_value(const gpiod::line::value value) const; bool set_brightness_cb( - panther_msgs::SetLEDBrightness::Request & request, - panther_msgs::SetLEDBrightness::Response & response); + panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res); }; } // namespace panther_lights diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 7c5411eb9..57f60a650 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -39,20 +39,19 @@ DriverNode::DriverNode( rear_panel_.set_global_brightness(global_brightness); front_light_sub_ = it_->subscribe( - "lights/driver/front_panel_frame", 5, [&](const sensor_msgs::ImageConstPtr & msg) { + "lights/driver/front_panel_frame", 5, [&](const sensor_msgs::Image::ConstPtr & msg) { frame_cb(msg, front_panel_, front_panel_ts_, "front"); front_panel_ts_ = msg->header.stamp; }); rear_light_sub_ = it_->subscribe( - "lights/driver/rear_panel_frame", 5, [this](const sensor_msgs::ImageConstPtr & msg) { + "lights/driver/rear_panel_frame", 5, [this](const sensor_msgs::Image::ConstPtr & msg) { frame_cb(msg, rear_panel_, rear_panel_ts_, "rear"); rear_panel_ts_ = msg->header.stamp; }); - set_brightness_server_ = nh_->advertiseService( - "lights/driver/set/brightness", &DriverNode::set_brightness_cb, this); - + set_brightness_server_ = + nh_->advertiseService("lights/driver/set/brightness", &DriverNode::set_brightness_cb, this); while (ros::ok() && !panels_initialised_) { ROS_INFO_THROTTLE(5.0, "[%s] Waiting for animation to arrive...", node_name_.c_str()); @@ -75,13 +74,12 @@ DriverNode::~DriverNode() } bool DriverNode::set_brightness_cb( - panther_msgs::SetLEDBrightness::Request & request, - panther_msgs::SetLEDBrightness::Response & response) + panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res) { - float brightness = request.data; + float brightness = req.data; if (brightness < 0.0f || brightness > 1.0f) { - response.success = false; - response.message = "Brightness out of range <0,1>"; + res.success = false; + res.message = "Brightness out of range <0,1>"; return true; } front_panel_.set_global_brightness(brightness); @@ -90,13 +88,13 @@ bool DriverNode::set_brightness_cb( // round string to two decimal places str_bright = str_bright.substr(0, str_bright.find(".") + 3); - response.success = 1; - response.message = "Changed brightness to " + str_bright; + res.success = true; + res.message = "Changed brightness to " + str_bright; return true; } void DriverNode::frame_cb( - const sensor_msgs::ImageConstPtr & msg, const apa_102::APA102 & panel, + const sensor_msgs::Image::ConstPtr & msg, const apa_102::APA102 & panel, const ros::Time & last_time, const std::string panel_name) { std::string meessage; From 786a637cf820fe45537fd7f66a3176f9d6c05ad8 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 21 Apr 2023 13:16:14 +0000 Subject: [PATCH 20/31] Add build dependencies --- panther_power_control/package.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/panther_power_control/package.xml b/panther_power_control/package.xml index bee51b7db..27be3cb46 100644 --- a/panther_power_control/package.xml +++ b/panther_power_control/package.xml @@ -17,6 +17,11 @@ catkin + libtool + autoconf + + + geometry_msgs panther_msgs rospy From 2a192bba582ea9fb452ccdd7f6d4fe466ab2273c Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 21 Apr 2023 13:32:25 +0000 Subject: [PATCH 21/31] FIx launchfile --- panther_lights/launch/lights.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/launch/lights.launch b/panther_lights/launch/lights.launch index b859f2634..3d6418303 100644 --- a/panther_lights/launch/lights.launch +++ b/panther_lights/launch/lights.launch @@ -3,7 +3,7 @@ - Date: Mon, 8 May 2023 10:26:14 +0200 Subject: [PATCH 22/31] Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index a98d1c9a3..608ca9481 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -21,13 +21,13 @@ if(NOT LIBGPIOD) endif() find_package(catkin REQUIRED COMPONENTS -image_transport -panther_msgs -roscpp -rospy -sensor_msgs -std_msgs -std_srvs + image_transport + panther_msgs + roscpp + rospy + sensor_msgs + std_msgs + std_srvs ) catkin_package( From 590745376dce8303d0861eca5fade29c9d193004 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 8 May 2023 10:26:20 +0200 Subject: [PATCH 23/31] Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 608ca9481..aa0c99031 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -5,6 +5,7 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_library(LIBGPIOD libgpiod) + if(NOT LIBGPIOD) message(WARNING "libgpiod not found. Building from source") include(ExternalProject) From 0ed663db515b90e8f0dfc3f26fe0080a40bbad23 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 8 May 2023 10:28:12 +0200 Subject: [PATCH 24/31] Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/apa102.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 62fbd95e0..14f4403b3 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -1,9 +1,9 @@ #include #include -#include #include #include +#include #include #include From 2468f11f9b4ced52430546e21d9d104372e9d3d9 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 8 May 2023 10:28:32 +0200 Subject: [PATCH 25/31] Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/driver_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 57f60a650..a263abdd6 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -15,6 +15,7 @@ namespace panther_lights { + DriverNode::DriverNode( const std::shared_ptr ph, std::shared_ptr nh, const std::shared_ptr it) From 685508f18640ae428014c843ea61ee06658f1484 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 8 May 2023 10:28:44 +0200 Subject: [PATCH 26/31] Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_lights/src/driver_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index a263abdd6..ddd84f6ec 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -145,4 +145,5 @@ void DriverNode::set_pin_value(const gpiod::line::value value) const rb.add_line_settings(power_pin_offset, settings); rb.do_request(); } + } // namespace panther_lights \ No newline at end of file From 0d19458f809b0a506ff37fa33645fbd18d482568 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 8 May 2023 08:30:30 +0000 Subject: [PATCH 27/31] Review changes --- panther_lights/include/panther_lights/apa102.hpp | 4 ++-- panther_lights/include/panther_lights/driver_node.hpp | 6 +++--- panther_lights/package.xml | 4 ++++ panther_lights/src/apa102.cpp | 4 ++-- panther_lights/src/driver_node.cpp | 2 +- panther_power_control/package.xml | 5 ----- 6 files changed, 12 insertions(+), 13 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 7c21dec71..85caa3feb 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -5,7 +5,7 @@ #include #include -namespace apa_102 +namespace panther_lights { class APA102 @@ -31,6 +31,6 @@ class APA102 const std::uint16_t corr_blue_ = 62.0; }; -} // namespace apa_102 +} // namespace panther_lights #endif // PANTHER_LIGHTS_APA102_HPP_ \ No newline at end of file diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index 17f1eca58..a4886f4dd 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -31,8 +31,8 @@ class DriverNode bool panels_initialised_ = false; std::string node_name_; - apa_102::APA102 front_panel_; - apa_102::APA102 rear_panel_; + APA102 front_panel_; + APA102 rear_panel_; ros::Time front_panel_ts_; ros::Time rear_panel_ts_; @@ -44,7 +44,7 @@ class DriverNode image_transport::Subscriber front_light_sub_; void frame_cb( - const sensor_msgs::Image::ConstPtr & msg, const apa_102::APA102 & panel, + const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, const std::string panel_name); void set_pin_value(const gpiod::line::value value) const; bool set_brightness_cb( diff --git a/panther_lights/package.xml b/panther_lights/package.xml index c43e336a6..308504378 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -16,6 +16,10 @@ catkin + libtool + autoconf + autoconf-archive + image_transport panther_msgs roscpp diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 62fbd95e0..5376f5306 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -12,7 +12,7 @@ #include #include -namespace apa_102 +namespace panther_lights { APA102::APA102(const std::string device, const std::uint32_t speed, const bool cs_high) : device_(device), speed_(speed), fd_(open(device_.c_str(), O_RDWR)) @@ -96,4 +96,4 @@ void APA102::set_panel(const std::vector & frame) const throw std::ios_base::failure(std::string("Failed to send data over SPI ") + device_); } } -} // namespace apa_102 \ No newline at end of file +} // namespace panther_lights \ No newline at end of file diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 57f60a650..d98026c19 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -94,7 +94,7 @@ bool DriverNode::set_brightness_cb( } void DriverNode::frame_cb( - const sensor_msgs::Image::ConstPtr & msg, const apa_102::APA102 & panel, + const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, const std::string panel_name) { std::string meessage; diff --git a/panther_power_control/package.xml b/panther_power_control/package.xml index 27be3cb46..bee51b7db 100644 --- a/panther_power_control/package.xml +++ b/panther_power_control/package.xml @@ -17,11 +17,6 @@ catkin - libtool - autoconf - - - geometry_msgs panther_msgs rospy From fee608f94d972cc4f5ef3263cabfb0ad741267e3 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 8 May 2023 09:50:55 +0000 Subject: [PATCH 28/31] Fix inlude order --- panther_lights/include/panther_lights/driver_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index a4886f4dd..ab644f0c2 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -1,8 +1,6 @@ #ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_ #define PANTHER_LIGHTS_DRIVER_NODE_HPP_ -#include - #include #include #include @@ -14,6 +12,8 @@ #include +#include + namespace panther_lights { From 711d85c148f9488ae4747cdc1a4494cee18ecea6 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 8 May 2023 13:21:33 +0000 Subject: [PATCH 29/31] Fix references and APA102 constructor --- panther_lights/include/panther_lights/apa102.hpp | 9 +++++---- panther_lights/include/panther_lights/driver_node.hpp | 8 ++++---- panther_lights/src/apa102.cpp | 4 ++-- panther_lights/src/driver_node.cpp | 8 ++++---- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index 85caa3feb..9b8d94075 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -11,7 +11,8 @@ namespace panther_lights class APA102 { public: - APA102(const std::string device, const std::uint32_t speed = 800000, const bool cs_high = false); + APA102( + const std::string & device, const std::uint32_t speed = 800000, const bool cs_high = false); ~APA102(); void set_global_brightness(const std::uint8_t brightness); @@ -26,9 +27,9 @@ class APA102 std::uint16_t global_brightness_; // color correction constants - const std::uint16_t corr_red_ = 255.0; - const std::uint16_t corr_green_ = 200.0; - const std::uint16_t corr_blue_ = 62.0; + const std::uint16_t corr_red_ = 255; + const std::uint16_t corr_green_ = 200; + const std::uint16_t corr_blue_ = 62; }; } // namespace panther_lights diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index ab644f0c2..19793387b 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -21,8 +21,8 @@ class DriverNode { public: DriverNode( - const std::shared_ptr ph, std::shared_ptr nh, - const std::shared_ptr it); + const std::shared_ptr & ph, std::shared_ptr & nh, + const std::shared_ptr & it); ~DriverNode(); private: @@ -44,8 +44,8 @@ class DriverNode image_transport::Subscriber front_light_sub_; void frame_cb( - const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, - const ros::Time & last_time, const std::string panel_name); + const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, + const std::string & panel_name); void set_pin_value(const gpiod::line::value value) const; bool set_brightness_cb( panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res); diff --git a/panther_lights/src/apa102.cpp b/panther_lights/src/apa102.cpp index 0afd7aa50..f845f2c49 100644 --- a/panther_lights/src/apa102.cpp +++ b/panther_lights/src/apa102.cpp @@ -14,8 +14,8 @@ namespace panther_lights { -APA102::APA102(const std::string device, const std::uint32_t speed, const bool cs_high) -: device_(device), speed_(speed), fd_(open(device_.c_str(), O_RDWR)) +APA102::APA102(const std::string & device, const std::uint32_t speed, const bool cs_high) +: device_(device), speed_(speed), fd_(open(device.c_str(), O_WRONLY)) { if (fd_ < 0) { throw std::ios_base::failure(std::string("Failed to open ") + device_); diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 88cce4a47..2639305d3 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -17,8 +17,8 @@ namespace panther_lights { DriverNode::DriverNode( - const std::shared_ptr ph, std::shared_ptr nh, - const std::shared_ptr it) + const std::shared_ptr & ph, std::shared_ptr & nh, + const std::shared_ptr & it) : ph_(std::move(ph)), nh_(std::move(nh)), it_(std::move(it)), @@ -95,8 +95,8 @@ bool DriverNode::set_brightness_cb( } void DriverNode::frame_cb( - const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, - const ros::Time & last_time, const std::string panel_name) + const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, + const std::string & panel_name) { std::string meessage; if ((ros::Time::now() - msg->header.stamp).toSec() > frame_timeout_) { From febc8b09e9d4887260e79e6cec5a4038604cd228 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Tue, 9 May 2023 19:00:54 +0200 Subject: [PATCH 30/31] Add build dependencies for libgpiod v2 --- panther_lights/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 308504378..7de02cac6 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -16,9 +16,11 @@ catkin - libtool autoconf autoconf-archive + libtool + m4 + pkg-config image_transport panther_msgs From 4ca10ea4189675248c3df5abaaba2dbe16dede8c Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 10 May 2023 09:14:47 +0000 Subject: [PATCH 31/31] Bring back libgpiod v1 --- panther_lights/CMakeLists.txt | 22 ++------------- .../include/panther_lights/driver_node.hpp | 2 +- panther_lights/package.xml | 7 +---- panther_lights/src/driver_node.cpp | 28 ++++++------------- 4 files changed, 13 insertions(+), 46 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index aa0c99031..b7cddbb74 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -4,23 +4,6 @@ project(panther_lights) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -find_library(LIBGPIOD libgpiod) - -if(NOT LIBGPIOD) - message(WARNING "libgpiod not found. Building from source") - include(ExternalProject) - - ExternalProject_Add(libgpiod - GIT_REPOSITORY git://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git - GIT_TAG v2.0.1 - PREFIX ${CATKIN_DEVEL_PREFIX} - CONFIGURE_COMMAND /autogen.sh --prefix= --enable-tools=no --enable-bindings-cxx - BUILD_COMMAND make -j ${N} - INSTALL_COMMAND make install INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/lib/libgpiod - BUILD_IN_SOURCE 1 - ) -endif() - find_package(catkin REQUIRED COMPONENTS image_transport panther_msgs @@ -33,7 +16,6 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include - LIBRARIES libgpiod CATKIN_DEPENDS panther_msgs roscpp ) @@ -48,9 +30,9 @@ add_executable(driver_node src/apa102.cpp ) -add_dependencies(driver_node libgpiod ${catkin_EXPORTED_TARGETS}) +add_dependencies(driver_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(driver_node - ${CATKIN_DEVEL_PREFIX}/lib/libgpiodcxx.so + gpiodcxx ${catkin_LIBRARIES} ) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index 19793387b..bc9544324 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -29,6 +29,7 @@ class DriverNode int num_led_; double frame_timeout_; bool panels_initialised_ = false; + gpiod::line power_pin_; std::string node_name_; APA102 front_panel_; @@ -46,7 +47,6 @@ class DriverNode void frame_cb( const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, const std::string & panel_name); - void set_pin_value(const gpiod::line::value value) const; bool set_brightness_cb( panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res); }; diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 7de02cac6..d783945e1 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -16,13 +16,8 @@ catkin - autoconf - autoconf-archive - libtool - m4 - pkg-config - image_transport + libgpiod-dev panther_msgs roscpp rospy diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 2639305d3..f89b8c7b8 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -31,7 +31,12 @@ DriverNode::DriverNode( frame_timeout_ = ph_->param("frame_timeout", 0.1); num_led_ = ph_->param("num_led", 46); - set_pin_value(gpiod::line::value::INACTIVE); + const gpiod::chip chip("gpiochip0"); + power_pin_ = chip.find_line("LED_SBC_SEL"); + + const gpiod::line_request lr = { + node_name_, gpiod::line_request::DIRECTION_OUTPUT, gpiod::line_request::FLAG_ACTIVE_LOW}; + power_pin_.request(lr, 0); front_panel_ts_ = ros::Time::now(); rear_panel_ts_ = ros::Time::now(); @@ -71,7 +76,8 @@ DriverNode::~DriverNode() rear_panel_.set_panel(std::vector(num_led_ * 4, 0)); // give back control over LEDs - set_pin_value(gpiod::line::value::INACTIVE); + power_pin_.set_value(0); + power_pin_.release(); } bool DriverNode::set_brightness_cb( @@ -124,26 +130,10 @@ void DriverNode::frame_cb( panels_initialised_ = true; // take control over LEDs - set_pin_value(gpiod::line::value::ACTIVE); + power_pin_.set_value(1); } panel.set_panel(msg->data); } } -void DriverNode::set_pin_value(const gpiod::line::value value) const -{ - auto chip = gpiod::chip(std::filesystem::path{"/dev/gpiochip0"}); - auto power_pin_offset = gpiod::line::offset(chip.get_line_offset_from_name("LED_SBC_SEL")); - - auto settings = gpiod::line_settings(); - settings.set_direction(gpiod::line::direction::OUTPUT); - settings.set_active_low(true); - settings.set_output_value(value); - - auto rb = chip.prepare_request(); - rb.set_consumer(node_name_); - rb.add_line_settings(power_pin_offset, settings); - rb.do_request(); -} - } // namespace panther_lights \ No newline at end of file