diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 2cea09a0d..b7cddbb74 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -1,15 +1,40 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.16.3) project(panther_lights) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(catkin REQUIRED COMPONENTS + image_transport panther_msgs + 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/driver_node.cpp + src/apa102.cpp +) + +add_dependencies(driver_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(driver_node + gpiodcxx + ${catkin_LIBRARIES} +) install(DIRECTORY config 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 new file mode 100644 index 000000000..9b8d94075 --- /dev/null +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -0,0 +1,37 @@ +#ifndef PANTHER_LIGHTS_APA102_HPP_ +#define PANTHER_LIGHTS_APA102_HPP_ + +#include +#include +#include + +namespace panther_lights +{ + +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_; + + // color correction constants + const std::uint16_t corr_red_ = 255; + const std::uint16_t corr_green_ = 200; + const std::uint16_t corr_blue_ = 62; +}; + +} // 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 new file mode 100644 index 000000000..bc9544324 --- /dev/null +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -0,0 +1,56 @@ +#ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_ +#define PANTHER_LIGHTS_DRIVER_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +class DriverNode +{ +public: + DriverNode( + const std::shared_ptr & ph, std::shared_ptr & nh, + const std::shared_ptr & it); + ~DriverNode(); + +private: + int num_led_; + double frame_timeout_; + bool panels_initialised_ = false; + gpiod::line power_pin_; + std::string node_name_; + + APA102 front_panel_; + 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::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time, + const std::string & panel_name); + bool set_brightness_cb( + panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res); +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_DRIVER_NODE_HPP_ \ No newline at end of file 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 @@ - catkin + image_transport + libgpiod-dev panther_msgs + roscpp rospy sensor_msgs std_msgs std_srvs - 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..f845f2c49 --- /dev/null +++ b/panther_lights/src/apa102.cpp @@ -0,0 +1,99 @@ +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +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_WRONLY)) +{ + if (fd_ < 0) { + throw std::ios_base::failure(std::string("Failed to open ") + device_); + } + + 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_); + } + + 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_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) { + 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 < 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, + }; + + 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_); + } +} +} // 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 new file mode 100644 index 000000000..f89b8c7b8 --- /dev/null +++ b/panther_lights/src/driver_node.cpp @@ -0,0 +1,139 @@ +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +namespace panther_lights +{ + +DriverNode::DriverNode( + 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)), + 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, [&](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::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); + + 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(); + } + + ROS_INFO("[%s] LED panels initialised", node_name_.c_str()); + ROS_INFO("[%s] Node started", node_name_.c_str()); +} + +DriverNode::~DriverNode() +{ + // 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(); +} + +bool DriverNode::set_brightness_cb( + panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res) +{ + float brightness = req.data; + if (brightness < 0.0f || brightness > 1.0f) { + res.success = false; + res.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); + res.success = true; + res.message = "Changed brightness to " + str_bright; + return true; +} + +void DriverNode::frame_cb( + 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_) { + 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"; + } + + 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); + } +} + +} // namespace panther_lights \ 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/main.cpp b/panther_lights/src/main.cpp new file mode 100644 index 000000000..12174f0f4 --- /dev/null +++ b/panther_lights/src/main.cpp @@ -0,0 +1,29 @@ +#include + +#include + +#include + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "lights_driver_node"); + + auto ph = std::make_shared("~"); + auto nh = std::make_shared(); + auto it = std::make_shared(*nh); + + try { + panther_lights::DriverNode driver_node(ph, 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