ROS 2 panther lights

panther_battery/package.xml

@@ -12,6 +12,7 @@
   <url type="bugtracker">https://github.com/husarion/panther_ros/issues</url>
 
   <author email="dawid.kmak@husarion.com">Dawid Kmak</author>
+  <author email="krzysztof.wojciechowski@husarion.com">Krzysztof Wojciechowski</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

panther_lights/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.10.2)
+project(panther_lights)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(panther_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# install libgpiod
+include(ExternalProject)
+
+file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build/include)
+
+ExternalProject_Add(
+  libgpiod-build
+  GIT_REPOSITORY git://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git
+  GIT_TAG v2.0.2
+  # EXCLUDE_FROM_ALL TRUE
+  PREFIX ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build
+  CONFIGURE_COMMAND <SOURCE_DIR>/autogen.sh --prefix=<INSTALL_DIR>
+                    --enable-tools=no --enable-bindings-cxx
+  BUILD_COMMAND make -j ${N}
+  INSTALL_COMMAND make install
+                  INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build
+  BUILD_IN_SOURCE 1)
+
+add_library(libgpiod STATIC IMPORTED GLOBAL)
+add_dependencies(libgpiod libgpiod-build)
+
+set_target_properties(
+  libgpiod
+  PROPERTIES IMPORTED_LOCATION
+             ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build/lib/libgpiodcxx.so)
+set_target_properties(
+  libgpiod PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
+                      ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build/include)
+
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build/lib
+        DESTINATION ${CMAKE_INSTALL_PREFIX})
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/libgpiod-build/include
+        DESTINATION ${CMAKE_INSTALL_PREFIX})
+
+include_directories(include ${panther_utils_INCLUDE_DIRS})
+
+add_executable(driver_node src/driver_node_main.cpp src/driver_node.cpp
+                           src/apa102.cpp)
+
+target_link_libraries(driver_node libgpiod)
+
+ament_target_dependencies(driver_node image_transport panther_msgs rclcpp
+                          sensor_msgs)
+
+add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp
+                                    src/dummy_scheduler_node.cpp)
+
+ament_target_dependencies(dummy_scheduler_node image_transport rclcpp
+                          sensor_msgs)
+
+install(TARGETS driver_node dummy_scheduler_node
+        DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+# if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line
+# skips the linter which checks for copyrights # comment the line when a
+# copyright and license is added to all source files
+# set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only
+# works in a git repo) # comment the line when this package is in a git repo and
+# when # a copyright and license is added to all source files
+# set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies()
+# endif()
+
+ament_package()

panther_lights/README.md

@@ -0,0 +1,56 @@
+[//]: # (ROS_API_PACKAGE_START)
+[//]: # (ROS_API_PACKAGE_NAME_START)
+
+# panther_lights
+
+[//]: # (ROS_API_PACKAGE_NAME_END)
+[//]: # (ROS_API_PACKAGE_DESCRIPTION_START)
+
+Package used to control the Husarion Panther Bumper Lights.
+
+[//]: # (ROS_API_PACKAGE_DESCRIPTION_END)
+
+## ROS Nodes
+
+[//]: # (ROS_API_NODE_START)
+[//]: # (ROS_API_NODE_COMPATIBLE_1_0)
+[//]: # (ROS_API_NODE_COMPATIBLE_1_2)
+[//]: # (ROS_API_NODE_NAME_START)
+
+### driver_node
+
+[//]: # (ROS_API_NODE_NAME_END)
+[//]: # (ROS_API_NODE_DESCRIPTION_START)
+
+This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
+
+[//]: # (ROS_API_NODE_DESCRIPTION_END)
+
+#### Subscribers
+
+[//]: # (ROS_API_NODE_SUBSCRIBERS_START)
+
+- `/panther/lights/driver/front_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights.
+- `/panther/lights/driver/rear_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights.
+
+[//]: # (ROS_API_NODE_SUBSCRIBERS_END)
+
+#### Service Servers
+
+[//]: # (ROS_API_NODE_SERVICE_SERVERS_START)
+
+- `/panther/lights/driver/set/brightness` [*panther_msgs/SetLEDBrightness*]: allows setting global LED brightness, value ranges from **0.0** to **1.0**.
+
+[//]: # (ROS_API_NODE_SERVICE_SERVERS_END)
+
+#### Parameters
+
+[//]: # (ROS_API_NODE_PARAMETERS_START)
+
+- `~frame_timeout` [*float*, default: **0.1**]: time in **[s]** after which an incoming frame will be considered too old.
+- `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**.
+- `~num_led` [*int*, default: **46**]: number of LEDs in a single bumper.
+
+[//]: # (ROS_API_NODE_PARAMETERS_END)
+[//]: # (ROS_API_NODE_END)
+[//]: # (ROS_API_PACKAGE_END)

panther_lights/include/panther_lights/apa102.hpp

@@ -0,0 +1,65 @@
+// Copyright 2023 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PANTHER_LIGHTS_APA102_HPP_
+#define PANTHER_LIGHTS_APA102_HPP_
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace panther_lights::apa102
+{
+
+class APA102
+{
+public:
+  APA102(
+    const std::string & device, const std::uint32_t speed = 800000, const bool cs_high = false);
+  ~APA102();
+
+  void SetGlobalBrightness(const std::uint8_t brightness);
+  void SetGlobalBrightness(const double brightness);
+  void SetPanel(const std::vector<std::uint8_t> & frame) const;
+
+protected:
+  /**
+   * @brief Modifies provided buffer to BGR format with structure appropriate for
+   * the SPI transfer based on a given RGBA frame
+   *
+   * @returns buffer length.
+   *
+   * @exception std::runtime_error if frame has incorrect number of bytes
+   */
+  std::size_t RGBAFrameToBGRBuffer(
+    const std::vector<std::uint8_t> & frame, std::uint8_t *& buffer) const;
+
+  std::uint16_t global_brightness_;
+
+private:
+  static constexpr std::uint8_t kBits = 8;
+
+  // Color correction constants
+  static constexpr std::uint16_t kCorrRed = 255;
+  static constexpr std::uint16_t kCorrGreen = 200;
+  static constexpr std::uint16_t kCorrBlue = 62;
+
+  const int fd_;
+  const std::string device_;
+  const std::uint32_t speed_;
+};
+
+}  // namespace panther_lights::apa102
+
+#endif  // PANTHER_LIGHTS_APA102_HPP_

panther_lights/include/panther_lights/driver_node.hpp

@@ -0,0 +1,71 @@
+// Copyright 2023 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_
+#define PANTHER_LIGHTS_DRIVER_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include <gpiod.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+#include <panther_msgs/srv/set_led_brightness.hpp>
+
+#include <panther_lights/apa102.hpp>
+
+namespace panther_lights
+{
+
+using ImageMsg = sensor_msgs::msg::Image;
+using SetLEDBrightnessSrv = panther_msgs::srv::SetLEDBrightness;
+
+class DriverNode : public rclcpp::Node
+{
+public:
+  DriverNode(
+    const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  void Initialize();
+
+private:
+  void OnShutdown();
+  void FrameCB(
+    const ImageMsg::ConstSharedPtr & msg, const apa102::APA102 & panel,
+    const rclcpp::Time & last_time, const std::string & panel_name);
+  void SetBrightnessCB(
+    const SetLEDBrightnessSrv::Request::SharedPtr & request,
+    SetLEDBrightnessSrv::Response::SharedPtr response);
+  void SetPowerPin(const bool value) const;
+
+  int num_led_;
+  double frame_timeout_;
+  bool panels_initialised_ = false;
+
+  apa102::APA102 front_panel_;
+  apa102::APA102 rear_panel_;
+
+  rclcpp::Time front_panel_ts_;
+  rclcpp::Time rear_panel_ts_;
+  rclcpp::Service<SetLEDBrightnessSrv>::SharedPtr set_brightness_server_;
+  std::shared_ptr<image_transport::ImageTransport> it_;
+  std::shared_ptr<image_transport::Subscriber> rear_light_sub_;
+  std::shared_ptr<image_transport::Subscriber> front_light_sub_;
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_DRIVER_NODE_HPP_

panther_lights/include/panther_lights/dummy_scheduler_node.hpp

@@ -0,0 +1,72 @@
+// Copyright 2023 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PANTHER_LIGHTS_DUMMY_SCHEDULER_NODE_HPP_
+#define PANTHER_LIGHTS_DUMMY_SCHEDULER_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+namespace panther_lights
+{
+
+using BoolMsg = std_msgs::msg::Bool;
+using BatteryStateMsg = sensor_msgs::msg::BatteryState;
+
+struct RGBAColor
+{
+  uint8_t r;
+  uint8_t g;
+  uint8_t b;
+  uint8_t a;
+};
+
+class SchedulerNode : public rclcpp::Node
+{
+public:
+  SchedulerNode(
+    const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  void Initialize();
+
+private:
+  void ControllerTimerCB();
+  void PublishColor(const RGBAColor & color);
+
+  static constexpr RGBAColor kColorRed = {255, 0, 0, 255};
+  static constexpr RGBAColor kColorGreen = {0, 255, 0, 255};
+  static constexpr RGBAColor kColorOrange = {255, 140, 0, 255};
+
+  bool e_stop_state_ = true;
+  int num_led_;
+  float battery_percentage_;
+
+  rclcpp::TimerBase::SharedPtr controller_timer_;
+  rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
+  rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_state_sub_;
+
+  std::shared_ptr<image_transport::ImageTransport> it_;
+  std::shared_ptr<image_transport::Publisher> rear_light_pub_;
+  std::shared_ptr<image_transport::Publisher> front_light_pub_;
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_DUMMY_SCHEDULER_NODE_HPP_

panther_lights/launch/lights.launch.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 ros2_control Development Team
+# Copyright 2023 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import Shutdown
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    lights_driver_node = Node(
+        package="panther_lights",
+        executable="driver_node",
+        name="lights_driver_node",
+        on_exit=Shutdown(),
+    )
+
+    dummy_scheduler_node = Node(
+        package="panther_lights",
+        executable="dummy_scheduler_node",
+        name="dummy_scheduler_node",
+    )
+
+    actions = [
+        lights_driver_node,
+        dummy_scheduler_node,
+    ]
+
+    return LaunchDescription(actions)