diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index b9f435eb2..2554e8f47 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -196,6 +196,7 @@ def generate_launch_description(): ("panther_system_node/aux_power_enable", "hardware/aux_power_enable"), ("panther_system_node/charger_enable", "hardware/charger_enable"), ("panther_system_node/digital_power_enable", "hardware/digital_power_enable"), + ("panther_system_node/led_control_enable", "hardware/led_control_enable"), ("panther_system_node/motor_power_enable", "hardware/motor_power_enable"), ], condition=UnlessCondition(use_sim), diff --git a/panther_gpiod/CHANGELOG.rst b/panther_gpiod/CHANGELOG.rst deleted file mode 100644 index 2cc3bb2c3..000000000 --- a/panther_gpiod/CHANGELOG.rst +++ /dev/null @@ -1,292 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package panther_gpiod -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.3 (2024-06-06) ------------------- -* Merge pull request `#320 `_ from husarion/ros2-clear-logs -* Exception changes in hardware_interfaces -* Contributors: Dawid Kmak, pawelirh - -2.0.2 (2024-06-05) ------------------- -* Merge branch 'ros2' of https://github.com/husarion/panther_ros into ros2-manager-refactor -* Increase the timeout and notify outside the scope (`#312 `_) -* Merge branch 'ros2' of https://github.com/husarion/panther_ros into ros2-manager-refactor -* Contributors: Dawid, Paweł Irzyk - -2.0.1 (2024-05-01) ------------------- -* Merge branch 'ros2-devel' into ros2-readme -* Merge remote-tracking branch 'origin/ros2-devel' into ros2-os-diagnostics -* Merge remote-tracking branch 'origin/ros2-devel' into ros2-os-diagnostics -* Contributors: Jakub Delicat, rafal-gorecki - -2.0.0 (2024-03-29) ------------------- -* Merge pull request `#257 `_ from husarion/ros2-headers - Divide Headers into std and local liblaries -* Rest of fils -* Headers + Copyright -* Merge branch 'ros2-devel' into ros2-ekf-optimalization -* Merge branch 'ros2-devel' into ros2-lights-tests -* Merge branch 'ros2-manager-plugins' of https://github.com/husarion/panther_ros into ros2-panther-manager -* Merge remote-tracking branch 'origin/ros2-devel' into ros2-manager-plugins -* undo test remove (`#244 `_) -* Ros2 lights controller (`#241 `_) - * ROS 2 lights animations (`#221 `_) - * add animation and image_animation class - * controller node and pluginlib - * add tests and fix issues - * add animation images - * add alpha channel - * add charging animation with tests - * update dummy controller - * fix missing includes - * add missing dep - * Update panther_lights/include/panther_lights/animation/animation.hpp - Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> - * Update panther_lights/include/panther_lights/animation/animation.hpp - Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> - * review changes - * update tests - --------- - Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> - * ROS 2 lights converter (`#223 `_) - * add led_segment - * WIP led_panel and segment converter - * simplify converter - * update segment conversion - * add test for led panel, segment, and converter - * review fixes - * update copyright year - * update controller so it somehow works - * Update tests - * Apply review fixes - * fix gpio tests - * parse controller configuration - * add default animation - * add yaml_utils to panther_utils - * add led animation and queue - * Fix queuing - * fix bug - * priority and timeout queue validation - * move queue to separate file - * add briefs - * param and brightness handle - * user animations, bugs, briefs - * use yaml utils - * fix tests - * update tests - * add led_animation test - * test fixxes - * add led animations queue tests - * clean up code | clean up code - * Update documentation | add launching controller node - * make it work - * update scheduler - * Update panther_lights/LIGHTS_API.md - Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> - * review fixes - * update pre-commit and fix typos - * Update panther_bringup/README.md - Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> - * Update panther_hardware_interfaces/README.md - Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> - * Update panther_lights/README.md - Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> - * Update panther_lights/test/test_controller_node.cpp - Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> - * review fixes - * Update README.md - --------- - Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> - Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> - Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> -* ROS 2 control liblely instalation (`#239 `_) - * install liblely with cmake - * simplify instalation - * fix condition - * add super build - * libgpiod super build - * remove obsolate info - * add missing PKG_CONFIG_PATH -* Merge remote-tracking branch 'origin/ros2-devel' into ros2-add-mecanum-controller -* Merge pull request `#208 `_ from husarion/ros2-control - Add ROS 2 control -* Fix IsPinAvailable calls -* Merge branch 'ros2-devel' into ros2-control - Conflicts: - panther_gpiod/CMakeLists.txt - panther_gpiod/package.xml - panther_gpiod/src/gpio_driver.cpp -* Add GPIO controller (`#222 `_) - * add GPIO controller - * Basic integration of gpio controller and panther system - * [WIP] Add panther version - * add io state topic - * Remove unnecessary parts from cmakelists - * Cleanup gpio controller - * Add estop to panther system - * Add todo comment - * Add ServiceWrapper - * Add estop services - * Add remaps to ros2 control - * Add publishing estop state, change iostate to latched and fix publishing initial state - * revise e-stop logic in initial stage - * same, but in better way - * small changes - * remove clear_errors service - * Fix test - * Add resetting gpio controller - * Change wheel separation multiplier to 1.0 - * fix pin names list - * add robot version check before GPIO read - * Change lock in gpio driver - * Fix order in cmakelists - * Change throws to exception in briefs - * Remove unnecessary includes - * Fix controller_manager topic remaps - * Add checking if last commands were 0 before resetting estop - * Change estop variable to atomic bool - * Add motor controller mutex - * Change order of operations when setting estop - * Fix order of methods - * Fixes in panther system - change methods order, use ReadDriverStatesUpdateFrequency, remove unnecessary logs - * Remove max_safety_stop_attempts (no longer needed after adding gpio controller) - * Refactor setting estop in write method - * Fix estop naming convention - * Remove old todos - * Fix typo - * Review fixes - * fix formatting - * Update panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> - * review fixes - * rename some methods - * draft of InitializeAndPublishIOStateMsg functionality - * fix io_state topic - * fix service warappers - * small fix - * rewiew fixes - * add briefs in gpio_controler - * review fixes - * small fix - --------- - Co-authored-by: Paweł Kowalski - Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> -* Ros2 code style fixes (`#215 `_) - * Fix style of cstdint usage in battery - * Fix style of cstdint usage in lights - * Unify handling exceptions - * Fix formatting -* Merge branch 'ros2-devel' into ros2-add-mecanum-controller -* Ros2 - Add GPIO Tests (`#220 `_) - * initial aproach - * Update README.md - * fix typo - * Resolve GPIOMonitor thread blocking issue - * add tests - * fix typo - * update readme - * fix cmake - * small fixes - * Update panther_gpiod/test/test_gpiod.cpp - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> - * review fixes - * add missing tests - * review fixes - * review fixes - * fix package.xml - --------- - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> -* Merge branch 'ros2-control' into ros2-control-pdo-commands - Conflicts: - panther_controller/config/WH01_controller.yaml - panther_controller/config/WH02_controller.yaml - panther_controller/config/WH04_controller.yaml - panther_description/urdf/panther_macro.urdf.xacro - panther_hardware_interfaces/CMakeLists.txt - panther_hardware_interfaces/CODE_STRUCTURE.md - panther_hardware_interfaces/README.md - panther_hardware_interfaces/include/panther_hardware_interfaces/canopen_controller.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/roboteq_data_converters.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/roboteq_driver.hpp - panther_hardware_interfaces/src/canopen_controller.cpp - panther_hardware_interfaces/src/motors_controller.cpp - panther_hardware_interfaces/src/panther_system.cpp - panther_hardware_interfaces/src/panther_system_ros_interface.cpp - panther_hardware_interfaces/src/roboteq_driver.cpp -* Merge branch 'ros2-devel' into ros2-control-pdo-commands - Conflicts: - panther_bringup/launch/bringup.launch.py - panther_controller/config/WH01_controller.yaml - panther_controller/config/WH02_controller.yaml - panther_controller/config/WH04_controller.yaml -* CR suggestions - move configureRT to panther_utils -* Merge branch 'ros2-devel' into ros2-control - Conflicts: - panther_bringup/launch/bringup.launch.py - panther_controller/config/WH01_controller.yaml - panther_controller/config/WH02_controller.yaml - panther_controller/config/WH04_controller.yaml -* Manuall merge of ros2-prealpha to ros2-dev (`#218 `_) - * manually merge prealpha with ros2-dev - * typo and formatting - * change locks and simplify code - * add missing library - * fix build -* Merge branch 'ros2-control' into ros2-control-pdo-commands - Conflicts: - panther_hardware_interfaces/README.md - panther_hardware_interfaces/include/panther_hardware_interfaces/canopen_controller.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/roboteq_data_converters.hpp - panther_hardware_interfaces/include/panther_hardware_interfaces/roboteq_driver.hpp - panther_hardware_interfaces/src/motors_controller.cpp - panther_hardware_interfaces/src/panther_system.cpp - panther_hardware_interfaces/src/roboteq_driver.cpp -* Merge branch 'ros2-devel' into ros2-control - Conflicts: - panther_bringup/launch/bringup.launch.py -* ROS 2 - Add GPIO driver package (`#211 `_) - * driver draft - * refactor and briefs - * unique_lock small fixes - * fix typo - * Update panther_gpiod/include/panther_gpiod/gpio_driver.hpp - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> - * Update panther_gpiod/src/gpio_driver.cpp - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> - * review fixes - * remove bias from GPIOInfo - * add rt config - * change thread priority - * add REDME - * fix typo - * small fixes - * fix rt cofig handle || update pins release - * review fixes - * almost there, closer than ever before - * fix typo - * brief update - * add enable_gpio_monitoring param - * review fixes - * small fix - * add condition_variable - * add GPIOMonitorEnable() - * update brief - * Update panther_gpiod/src/gpio_driver.cpp - Co-authored-by: Maciej Stępień - * Update panther_gpiod/src/gpio_driver.cpp - Co-authored-by: Maciej Stępień - * Update README.md - * Very small update - --------- - Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> - Co-authored-by: Maciej Stępień -* Contributors: Dawid, Dawid Kmak, Jakub Delicat, Maciej Stępień, Paweł Irzyk, Paweł Kowalski, rafal-gorecki diff --git a/panther_gpiod/CMakeLists.txt b/panther_gpiod/CMakeLists.txt deleted file mode 100644 index e0940a6ec..000000000 --- a/panther_gpiod/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) - -# Handle superbuild first -option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) - -if(USE_SUPERBUILD) - project(SUPERBUILD NONE) - include(cmake/SuperBuild.cmake) - return() -else() - project(panther_gpiod) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -set(PACKAGE_INCLUDE_DEPENDS ament_cmake panther_utils PkgConfig) - -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -set(ENV{PKG_CONFIG_PATH} - "${CMAKE_INSTALL_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") - -pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx) - -add_library(${PROJECT_NAME} SHARED src/gpio_driver.cpp) - -target_include_directories(${PROJECT_NAME} PUBLIC include) - -target_link_libraries(${PROJECT_NAME} PkgConfig::LIBGPIOD) - -ament_target_dependencies(${PROJECT_NAME} panther_utils) - -install(DIRECTORY include/ DESTINATION include) - -install( - TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) - -ament_export_include_directories(include) - -ament_export_libraries(${PROJECT_NAME}) - -ament_export_dependencies(panther_utils) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - - ament_add_gtest(${PROJECT_NAME}_test_gpiod test/test_gpio_driver.cpp) - - target_include_directories( - ${PROJECT_NAME}_test_gpiod - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_gpiod panther_utils) - target_link_libraries(${PROJECT_NAME}_test_gpiod ${PROJECT_NAME} - PkgConfig::LIBGPIOD) -endif() - -ament_package() diff --git a/panther_gpiod/README.md b/panther_gpiod/README.md deleted file mode 100644 index 5a935bb81..000000000 --- a/panther_gpiod/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# panther_gpiod - -The package comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Panther robot. It simplifies integration within robotic applications. - -## GPIODriver Usage Guide - -Below is a minimal example demonstrating how to use the GPIODriver class: - -```cpp -#include -#include - -#include "panther_gpiod/gpio_driver.hpp" - -int main() { - // Prepare vector with GPIO pin configurations - std::vector gpio_configurations = { - GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true}, - GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - // Add more GPIO pin configurations... - }; - - // Create a GPIODriver object and pass GPIO configurations - GPIODriver gpio_driver(gpio_configurations); - - // Enable GPIO monitoring with Real-Time (RT) configuration and a specific thread priority value - gpio_driver.GPIOMonitorEnable(/* use_rt = */ true, /* gpio_monit_thread_sched_priority = */ 50); - - // Set callback function for GPIO edge events (optional) - gpio_driver.ConfigureEdgeEventCallback(your_callback_function); - - // Perform GPIO operations, such as reading pin values - - return 0; -} -``` - -> **NOTE** -> -> Not invoking the `GPIOMonitorEnable()` method will result in the absence of functionality to read pin values. It is not mandatory to call this method, but the `IsPinActive()` method throws a runtime error when the GPIO monitor thread is not running. diff --git a/panther_gpiod/cmake/SuperBuild.cmake b/panther_gpiod/cmake/SuperBuild.cmake deleted file mode 100644 index dc3c3d93b..000000000 --- a/panther_gpiod/cmake/SuperBuild.cmake +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -include(ExternalProject) - -set(DEPENDENCIES) - -file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_libgpiod/include) - -list(APPEND DEPENDENCIES ep_libgpiod) -ExternalProject_Add( - ep_libgpiod - GIT_REPOSITORY git://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git - GIT_TAG v2.0.2 - PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_libgpiod - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} - CONFIGURE_COMMAND - sh -c - "/autogen.sh --prefix= --enable-tools=no --enable-bindings-cxx" - BUILD_COMMAND make -j ${N} - INSTALL_COMMAND make install INSTALL_PREFIX= - BUILD_IN_SOURCE 1) - -ExternalProject_Add( - ep_panther_gpiod - DEPENDS ${DEPENDENCIES} - SOURCE_DIR ${PROJECT_SOURCE_DIR} - CMAKE_ARGS -DUSE_SUPERBUILD=OFF - INSTALL_COMMAND "" - BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/panther_gpiod/package.xml b/panther_gpiod/package.xml deleted file mode 100644 index e511b0d11..000000000 --- a/panther_gpiod/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - panther_gpiod - 2.0.3 - libgpiod wrapper for Panther - Husarion - Apache License 2.0 - - https://husarion.com/ - https://github.com/husarion/panther_ros - https://github.com/husarion/panther_ros/issues - - Paweł Kowalski - - ament_cmake - - panther_utils - - autoconf - autoconf-archive - libtool - m4 - pkg-config - - ament_cmake_gtest - - - ament_cmake - - diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index bb4b6a8ee..24e81fded 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -20,27 +20,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# Find dependencies set(PACKAGE_INCLUDE_DEPENDS ament_cmake controller_interface diagnostic_updater + generate_parameter_library + geometry_msgs hardware_interface - panther_gpiod + imu_filter_madgwick panther_msgs panther_utils + phidgets_api PkgConfig pluginlib rclcpp rclcpp_lifecycle realtime_tools std_srvs - imu_filter_madgwick - phidgets_api - generate_parameter_library - tf2_ros tf2_geometry_msgs - geometry_msgs) + tf2_ros) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -53,25 +52,29 @@ set(ENV{PKG_CONFIG_PATH} "${CMAKE_INSTALL_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") pkg_check_modules(LIBLELY_COAPP REQUIRED IMPORTED_TARGET liblely-coapp) +pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx) add_library( ${PROJECT_NAME} SHARED - src/panther_system.cpp - src/panther_system_ros_interface.cpp - src/panther_system_e_stop.cpp - src/motors_controller.cpp src/canopen_controller.cpp src/gpio_controller.cpp - src/roboteq_driver.cpp + src/gpio_driver.cpp + src/motors_controller.cpp + src/panther_imu.cpp + src/panther_system_e_stop.cpp + src/panther_system_ros_interface.cpp + src/panther_system.cpp src/roboteq_data_converters.cpp + src/roboteq_driver.cpp src/roboteq_error_filter.cpp - src/utils.cpp - src/panther_imu.cpp) + src/utils.cpp) + target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_INCLUDE_DEPENDS}) + target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP - phidgets_spatial_parameters) + PkgConfig::LIBGPIOD phidgets_spatial_parameters) target_compile_definitions(${PROJECT_NAME} PRIVATE "PANTHER_HARDWARE_INTERFACES_BUILDING_DLL") @@ -99,9 +102,9 @@ if(BUILD_TESTING) PUBLIC $ include) ament_target_dependencies(${PROJECT_NAME}_test_panther_imu hardware_interface rclcpp panther_utils panther_msgs phidgets_api) - target_link_libraries(${PROJECT_NAME}_test_panther_imu ${PROJECT_NAME} phidgets_spatial_parameters) + ament_add_gtest( ${PROJECT_NAME}_test_roboteq_error_filter test/test_roboteq_error_filter.cpp src/roboteq_error_filter.cpp) @@ -164,14 +167,25 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_motors_controller PkgConfig::LIBLELY_COAPP) - ament_add_gtest(${PROJECT_NAME}_test_gpiod_controller - test/test_gpio_controller.cpp src/gpio_controller.cpp) + ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver test/test_gpio_driver.cpp) + target_include_directories( + ${PROJECT_NAME}_test_gpiod_driver + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils) + target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME} + PkgConfig::LIBGPIOD) + + ament_add_gtest( + ${PROJECT_NAME}_test_gpiod_controller test/test_gpio_controller.cpp + src/gpio_controller.cpp src/gpio_driver.cpp) target_include_directories( ${PROJECT_NAME}_test_gpiod_controller PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils - panther_gpiod) + ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils) + target_link_libraries(${PROJECT_NAME}_test_gpiod_controller + PkgConfig::LIBGPIOD) ament_add_gtest( ${PROJECT_NAME}_test_panther_system_ros_interface @@ -179,7 +193,8 @@ if(BUILD_TESTING) src/panther_system_ros_interface.cpp src/roboteq_data_converters.cpp src/utils.cpp - src/gpio_controller.cpp) + src/gpio_controller.cpp + src/gpio_driver.cpp) target_include_directories( ${PROJECT_NAME}_test_panther_system_ros_interface PRIVATE $ include) @@ -187,13 +202,12 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_panther_system_ros_interface diagnostic_updater rclcpp - panther_gpiod panther_msgs panther_utils realtime_tools std_srvs) target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface - PkgConfig::LIBLELY_COAPP) + PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) ament_add_gtest( ${PROJECT_NAME}_test_panther_system test/test_panther_system.cpp diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/panther_hardware_interfaces/CODE_STRUCTURE.md index 1c18ea95f..d7a448d4f 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/panther_hardware_interfaces/CODE_STRUCTURE.md @@ -41,9 +41,14 @@ Feedback converters are combined in the `RoboteqData` class to provide the full A class that keeps track of different types of errors. In some rare cases, Roboteq controllers can miss for example the SDO response, or PDO can be received a bit later, which results in a timeout. As they usually are rare and singular occurrences, it is better to filter some of these errors and escalate only when a certain number of errors happen. +## GPIODriver + +The GPIODriver is a low-level class responsible for direct interaction with the GPIO (General Purpose Input/Output) pins on the Raspberry Pi. +It comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Panther robot. It simplifies integration within robotic applications. + ## GPIOController -The GPIOController provides wrappers for the GPIO driver from the `panther_gpiod` package, handling reading and writing pins of the RPi GPIO. It includes the following utilities: +The GPIOController provides wrappers for the GPIO driver, handling reading and writing pins of the RPi GPIO. It includes the following utilities: * `GPIOControllerInterface`: Interface for all wrappers that handle GPIO control tasks. * `GPIOControllerPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above. diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md index 4eae47772..8f906c4d8 100644 --- a/panther_hardware_interfaces/README.md +++ b/panther_hardware_interfaces/README.md @@ -32,11 +32,28 @@ That said apart from the usual interface provided by the ros2_control, this plug [//]: # (ROS_API_NODE_PUBLISHERS_START) -- `/diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages. -- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers state and error flags. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages. +- `driver/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers state and error flags. +- `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state. +- `hardware/io_state` [*panther_msgs/IOState*]: Current IO state. [//]: # (ROS_API_NODE_PUBLISHERS_END) +#### Service Servers + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) + +- `hardware/aux_power_enable` [*std_srvs/SetBool*]: Enables or disables AUX power. +- `hardware/charger_enable` [*std_srvs/SetBool*]: Enables or disables charger. +- `hardware/digital_power_enable` [*std_srvs/SetBool*]: Enables or disables digital power. +- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. +- `hardware/fan_enable` [*std_srvs/SetBool*]: Enables or disables fan. +- `hardware/led_control_enable` [*std_srvs/SetBool*]: Enables or disables SBC (Single Board Computer) control over the LEDs. +- `hardware/motor_power_enable` [*std_srvs/SetBool*]: Enables or disables motor power. + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) + #### Parameters [//]: # (ROS_API_NODE_PARAMETERS_START) diff --git a/panther_hardware_interfaces/cmake/SuperBuild.cmake b/panther_hardware_interfaces/cmake/SuperBuild.cmake index 67ac187da..c7fca72e8 100644 --- a/panther_hardware_interfaces/cmake/SuperBuild.cmake +++ b/panther_hardware_interfaces/cmake/SuperBuild.cmake @@ -14,11 +14,9 @@ include(ExternalProject) -set(DEPENDENCIES) +set(DEPENDENCIES ep_liblely ep_libgpiod) file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_liblely/include) - -list(APPEND DEPENDENCIES ep_liblely) ExternalProject_Add( ep_liblely SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/ep_liblely/upstream @@ -33,6 +31,20 @@ ExternalProject_Add( BUILD_COMMAND $(MAKE) -C INSTALL_COMMAND make install INSTALL_PREFIX=) +file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_libgpiod/include) +ExternalProject_Add( + ep_libgpiod + GIT_REPOSITORY git://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git + GIT_TAG v2.0.2 + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_libgpiod + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} + CONFIGURE_COMMAND + sh -c + "/autogen.sh --prefix= --enable-tools=no --enable-bindings-cxx" + BUILD_COMMAND make -j ${N} + INSTALL_COMMAND make install INSTALL_PREFIX= + BUILD_IN_SOURCE 1) + ExternalProject_Add( ep_panther_hardware_interfaces DEPENDS ${DEPENDENCIES} diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp index 0ec430678..724a39c5d 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp @@ -14,7 +14,7 @@ /** * @file gpio_controller.hpp - * @brief Header file containing a higher-level wrapper for the panther_gpiod GPIO driver. + * @brief Header file containing a higher-level wrapper for the GPIO driver. */ #ifndef PANTHER_HARDWARE_INTERFACES_GPIO_CONTROLLER_HPP_ @@ -29,7 +29,7 @@ #include "gpiod.hpp" -#include "panther_gpiod/gpio_driver.hpp" +#include "panther_hardware_interfaces/gpio_driver.hpp" namespace panther_hardware_interfaces { @@ -54,7 +54,7 @@ class Watchdog * @exception std::runtime_error if the Watchdog pin is not configured by GPIODriver or not * described in GPIOController gpio_info storage */ - Watchdog(std::shared_ptr gpio_driver); + Watchdog(std::shared_ptr gpio_driver); /** * @brief Destructor for Watchdog class. Turns off the watchdog thread. @@ -83,8 +83,8 @@ class Watchdog */ void WatchdogThread(); - panther_gpiod::GPIOPin watchdog_pin_ = panther_gpiod::GPIOPin::WATCHDOG; - std::shared_ptr gpio_driver_; + GPIOPin watchdog_pin_ = GPIOPin::WATCHDOG; + std::shared_ptr gpio_driver_; std::thread watchdog_thread_; std::atomic_bool watchdog_thread_enabled_ = false; }; @@ -95,16 +95,18 @@ class GPIOControllerInterface virtual ~GPIOControllerInterface() = default; virtual void Start() = 0; + + virtual void EStopTrigger() = 0; + virtual void EStopReset() = 0; + virtual bool MotorPowerEnable(const bool enable) = 0; virtual bool FanEnable(const bool enable) = 0; virtual bool AUXPowerEnable(const bool enable) = 0; virtual bool DigitalPowerEnable(const bool enable) = 0; virtual bool ChargerEnable(const bool enable) = 0; - virtual void EStopTrigger() = 0; - virtual void EStopReset() = 0; + virtual bool LEDControlEnable(const bool enable) = 0; - virtual std::unordered_map QueryControlInterfaceIOStates() - const = 0; + virtual std::unordered_map QueryControlInterfaceIOStates() const = 0; virtual void InterruptEStopReset(){}; @@ -130,15 +132,14 @@ class GPIOControllerInterface * std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1)); * @endcode */ - void RegisterGPIOEventCallback( - const std::function & callback); + void RegisterGPIOEventCallback(const std::function & callback); - bool IsPinActive(const panther_gpiod::GPIOPin pin) const; + bool IsPinActive(const GPIOPin pin) const; - bool IsPinAvailable(const panther_gpiod::GPIOPin pin) const; + bool IsPinAvailable(const GPIOPin pin) const; protected: - std::shared_ptr gpio_driver_; + std::shared_ptr gpio_driver_; }; class GPIOControllerPTH12X : public GPIOControllerInterface @@ -210,12 +211,20 @@ class GPIOControllerPTH12X : public GPIOControllerInterface */ bool ChargerEnable(const bool enable) override; + /** + * @brief Enables or disables the LED control based on the 'enable' parameter. + * + * @param enable Set to 'true' to enable the LED control, 'false' to disable. + * @return 'true' if the LED control pin value is successfully set, 'false' otherwise. + */ + bool LEDControlEnable(const bool enable) override; + /** * @brief Queries the current IO states of the control interface. * * @return An unordered map containing the GPIOPin as the key and its active state as the value. */ - std::unordered_map QueryControlInterfaceIOStates() const override; + std::unordered_map QueryControlInterfaceIOStates() const override; void InterruptEStopReset() override; @@ -239,22 +248,22 @@ class GPIOControllerPTH12X : public GPIOControllerInterface /** * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. */ - const std::vector gpio_config_info_storage_{ - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPIN1, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPIN2, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{ - panther_gpiod::GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true}, + const std::vector gpio_config_info_storage_{ + GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true}, + GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}, }; std::mutex e_stop_cv_mtx_; @@ -339,21 +348,30 @@ class GPIOControllerPTH10X : public GPIOControllerInterface */ bool ChargerEnable(const bool /* enable */) override; + /** + * @brief Enables or disables the LED control based on the 'enable' parameter. + * + * @param enable Set to 'true' to enable the LED control, 'false' to disable. + * @return 'true' if the LED control pin value is successfully set, 'false' otherwise. + */ + bool LEDControlEnable(const bool enable) override; + /** * @brief Returns imitation of the IO states of the control interface. In this version of the * robot, there is a lack of support for controlling these IOs. * * @return An unordered map containing the GPIOPin as the key and its active state as the value. */ - std::unordered_map QueryControlInterfaceIOStates() const override; + std::unordered_map QueryControlInterfaceIOStates() const override; private: /** * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. */ - const std::vector gpio_config_info_storage_{ - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}, + const std::vector gpio_config_info_storage_{ + GPIOInfo{GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}, }; }; diff --git a/panther_gpiod/include/panther_gpiod/gpio_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp similarity index 94% rename from panther_gpiod/include/panther_gpiod/gpio_driver.hpp rename to panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp index 22cbde9e1..b329339ab 100644 --- a/panther_gpiod/include/panther_gpiod/gpio_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp @@ -12,19 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file gpio_driver.hpp - * - * @brief Header file containing declarations for the GPIODriver class - * responsible for GPIO operations on Panther robot. - * - * This file contains the declarations for the GPIODriver class, which - * manages GPIO pins on the Panther robot by providing functionalities to set pin values, - * change pin directions, monitor pin events, and more. - */ - -#ifndef PANTHER_GPIOD_GPIO_DRIVER_HPP_ -#define PANTHER_GPIOD_GPIO_DRIVER_HPP_ +#ifndef PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ +#define PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ #include #include @@ -40,7 +29,7 @@ #include "gpiod.hpp" -namespace panther_gpiod +namespace panther_hardware_interfaces { /** @@ -302,6 +291,6 @@ class GPIODriver const std::filesystem::path gpio_chip_path_ = "/dev/gpiochip0"; }; -} // namespace panther_gpiod +} // namespace panther_hardware_interfaces -#endif // PANTHER_GPIOD_GPIO_DRIVER_HPP_ +#endif // PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp index 2dfeca29d..cb079e460 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp @@ -228,9 +228,8 @@ class PantherSystemRosInterface void PublishEStopStateMsg(const bool e_stop); void PublishEStopStateIfChanged(const bool e_stop); void PublishDriverState(); - void InitializeAndPublishIOStateMsg( - const std::unordered_map & io_state); - void PublishIOState(const panther_gpiod::GPIOInfo & gpio_info); + void InitializeAndPublishIOStateMsg(const std::unordered_map & io_state); + void PublishIOState(const GPIOInfo & gpio_info); private: /** @@ -242,7 +241,7 @@ class PantherSystemRosInterface * @return True if the state update caused a change in the IO state message; returns * false otherwise. */ - bool UpdateIOStateMsg(const panther_gpiod::GPIOPin pin, const bool pin_value); + bool UpdateIOStateMsg(const GPIOPin pin, const bool pin_value); /** * @brief Retrieves an existing callback group from the internal map or creates diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml index d442da970..f4b6dea56 100644 --- a/panther_hardware_interfaces/package.xml +++ b/panther_hardware_interfaces/package.xml @@ -14,16 +14,23 @@ Maciej Stępień Paweł Kowalski Jakub Delicat + Paweł Irzyk ament_cmake + autoconf + autoconf-archive + libtool + m4 + pkg-config + pluginlib + controller_interface diagnostic_updater generate_parameter_library geometry_msgs hardware_interface imu_filter_madgwick - panther_gpiod panther_msgs panther_utils phidgets_api @@ -35,8 +42,6 @@ tf2_geometry_msgs tf2_ros - pluginlib - ament_cmake diff --git a/panther_hardware_interfaces/src/gpio_controller.cpp b/panther_hardware_interfaces/src/gpio_controller.cpp index 673e98191..61f4b079c 100644 --- a/panther_hardware_interfaces/src/gpio_controller.cpp +++ b/panther_hardware_interfaces/src/gpio_controller.cpp @@ -27,8 +27,7 @@ namespace panther_hardware_interfaces { -Watchdog::Watchdog(std::shared_ptr gpio_driver) -: gpio_driver_(std::move(gpio_driver)) +Watchdog::Watchdog(std::shared_ptr gpio_driver) : gpio_driver_(std::move(gpio_driver)) { if (!gpio_driver_->IsPinAvailable(watchdog_pin_)) { throw std::runtime_error("Watchdog pin is not configured."); @@ -80,7 +79,7 @@ void Watchdog::WatchdogThread() bool Watchdog::IsWatchdogEnabled() const { return watchdog_thread_.joinable(); } void GPIOControllerInterface::RegisterGPIOEventCallback( - const std::function & callback) + const std::function & callback) { if (!gpio_driver_) { throw std::runtime_error("GPIO driver has not been initialized yet."); @@ -89,22 +88,22 @@ void GPIOControllerInterface::RegisterGPIOEventCallback( gpio_driver_->ConfigureEdgeEventCallback(callback); } -bool GPIOControllerInterface::IsPinActive(const panther_gpiod::GPIOPin pin) const +bool GPIOControllerInterface::IsPinActive(const GPIOPin pin) const { return gpio_driver_->IsPinActive(pin); } -bool GPIOControllerInterface::IsPinAvailable(const panther_gpiod::GPIOPin pin) const +bool GPIOControllerInterface::IsPinAvailable(const GPIOPin pin) const { return gpio_driver_->IsPinAvailable(pin); } void GPIOControllerPTH12X::Start() { - gpio_driver_ = std::make_shared(gpio_config_info_storage_); + gpio_driver_ = std::make_shared(gpio_config_info_storage_); gpio_driver_->GPIOMonitorEnable(true, 60); - gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::VMOT_ON, true); + gpio_driver_->SetPinValue(GPIOPin::VMOT_ON, true); MotorPowerEnable(true); watchdog_ = std::make_unique(gpio_driver_); @@ -119,7 +118,7 @@ void GPIOControllerPTH12X::EStopTrigger() void GPIOControllerPTH12X::EStopReset() { - const auto e_stop_pin = panther_gpiod::GPIOPin::E_STOP_RESET; + const auto e_stop_pin = GPIOPin::E_STOP_RESET; bool e_stop_state = !gpio_driver_->IsPinActive(e_stop_pin); if (!e_stop_state) { @@ -153,42 +152,44 @@ void GPIOControllerPTH12X::EStopReset() bool GPIOControllerPTH12X::MotorPowerEnable(const bool enable) { - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::DRIVER_EN, enable); + return gpio_driver_->SetPinValue(GPIOPin::DRIVER_EN, enable); }; bool GPIOControllerPTH12X::AUXPowerEnable(const bool enable) { - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::AUX_PW_EN, enable); + return gpio_driver_->SetPinValue(GPIOPin::AUX_PW_EN, enable); }; bool GPIOControllerPTH12X::FanEnable(const bool enable) { - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::FAN_SW, enable); + return gpio_driver_->SetPinValue(GPIOPin::FAN_SW, enable); }; bool GPIOControllerPTH12X::DigitalPowerEnable(const bool enable) { - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::VDIG_OFF, !enable); + return gpio_driver_->SetPinValue(GPIOPin::VDIG_OFF, !enable); }; bool GPIOControllerPTH12X::ChargerEnable(const bool enable) { - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::CHRG_DISABLE, !enable); + return gpio_driver_->SetPinValue(GPIOPin::CHRG_DISABLE, !enable); } -std::unordered_map -GPIOControllerPTH12X::QueryControlInterfaceIOStates() const +bool GPIOControllerPTH12X::LEDControlEnable(const bool enable) { - std::unordered_map io_state; + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); +} + +std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const +{ + std::unordered_map io_state; - std::vector pins_to_query = { - panther_gpiod::GPIOPin::AUX_PW_EN, panther_gpiod::GPIOPin::CHRG_SENSE, - panther_gpiod::GPIOPin::CHRG_DISABLE, panther_gpiod::GPIOPin::VDIG_OFF, - panther_gpiod::GPIOPin::FAN_SW, panther_gpiod::GPIOPin::SHDN_INIT, - panther_gpiod::GPIOPin::VMOT_ON, + std::vector pins_to_query = { + GPIOPin::AUX_PW_EN, GPIOPin::CHRG_SENSE, GPIOPin::CHRG_DISABLE, GPIOPin::VDIG_OFF, + GPIOPin::FAN_SW, GPIOPin::SHDN_INIT, GPIOPin::VMOT_ON, }; - std::for_each(pins_to_query.begin(), pins_to_query.end(), [&](panther_gpiod::GPIOPin pin) { + std::for_each(pins_to_query.begin(), pins_to_query.end(), [&](GPIOPin pin) { bool is_active = gpio_driver_->IsPinActive(pin); io_state.emplace(pin, is_active); }); @@ -216,10 +217,10 @@ bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout) void GPIOControllerPTH10X::Start() { - gpio_driver_ = std::make_shared(gpio_config_info_storage_); + gpio_driver_ = std::make_shared(gpio_config_info_storage_); gpio_driver_->GPIOMonitorEnable(true, 60); - gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::MOTOR_ON, true); + gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, true); } void GPIOControllerPTH10X::EStopTrigger() @@ -231,7 +232,7 @@ void GPIOControllerPTH10X::EStopTrigger() void GPIOControllerPTH10X::EStopReset() { - if (!gpio_driver_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT)) { + if (!gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) { throw std::runtime_error( "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position."); } @@ -239,12 +240,12 @@ void GPIOControllerPTH10X::EStopReset() bool GPIOControllerPTH10X::MotorPowerEnable(const bool enable) { - if (enable && !gpio_driver_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT)) { + if (enable && !gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) { throw std::runtime_error( "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position."); } - return gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::MOTOR_ON, enable); + return gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, enable); } bool GPIOControllerPTH10X::AUXPowerEnable(const bool /* enable */) @@ -267,19 +268,22 @@ bool GPIOControllerPTH10X::ChargerEnable(const bool /* enable */) throw std::runtime_error("This robot version does not support this functionality."); } -std::unordered_map -GPIOControllerPTH10X::QueryControlInterfaceIOStates() const +bool GPIOControllerPTH10X::LEDControlEnable(const bool enable) +{ + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); +} + +std::unordered_map GPIOControllerPTH10X::QueryControlInterfaceIOStates() const { - std::unordered_map io_state; - - io_state.emplace(panther_gpiod::GPIOPin::AUX_PW_EN, true); - io_state.emplace(panther_gpiod::GPIOPin::CHRG_SENSE, false); - io_state.emplace(panther_gpiod::GPIOPin::CHRG_DISABLE, false); - io_state.emplace(panther_gpiod::GPIOPin::VDIG_OFF, false); - io_state.emplace(panther_gpiod::GPIOPin::FAN_SW, false); - io_state.emplace(panther_gpiod::GPIOPin::SHDN_INIT, false); - io_state.emplace( - panther_gpiod::GPIOPin::MOTOR_ON, gpio_driver_->IsPinActive(panther_gpiod::GPIOPin::MOTOR_ON)); + std::unordered_map io_state; + + io_state.emplace(GPIOPin::AUX_PW_EN, true); + io_state.emplace(GPIOPin::CHRG_SENSE, false); + io_state.emplace(GPIOPin::CHRG_DISABLE, false); + io_state.emplace(GPIOPin::VDIG_OFF, false); + io_state.emplace(GPIOPin::FAN_SW, false); + io_state.emplace(GPIOPin::SHDN_INIT, false); + io_state.emplace(GPIOPin::MOTOR_ON, gpio_driver_->IsPinActive(GPIOPin::MOTOR_ON)); return io_state; } diff --git a/panther_gpiod/src/gpio_driver.cpp b/panther_hardware_interfaces/src/gpio_driver.cpp similarity index 97% rename from panther_gpiod/src/gpio_driver.cpp rename to panther_hardware_interfaces/src/gpio_driver.cpp index b72d1923e..48ae9a93f 100644 --- a/panther_gpiod/src/gpio_driver.cpp +++ b/panther_hardware_interfaces/src/gpio_driver.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_gpiod/gpio_driver.hpp" +#include "panther_hardware_interfaces/gpio_driver.hpp" #include #include @@ -31,7 +31,7 @@ #include "panther_utils/configure_rt.hpp" -namespace panther_gpiod +namespace panther_hardware_interfaces { GPIODriver::GPIODriver(std::vector gpio_info_storage) @@ -78,7 +78,7 @@ void GPIODriver::ConfigureEdgeEventCallback(const std::function GPIODriver::CreateLineRequest(gpiod::chip & chip) { auto request_builder = chip.prepare_request(); - request_builder.set_consumer("panther_gpiod"); + request_builder.set_consumer("panther_hardware_interfaces"); for (GPIOInfo & gpio_info : gpio_info_storage_) { ConfigureLineRequest(chip, request_builder, gpio_info); @@ -308,4 +308,4 @@ GPIOPin GPIODriver::GetPinFromOffset(const gpiod::line::offset & offset) const "Pin with offset " + std::to_string(offset) + " not found in GPIO info storage."); } -} // namespace panther_gpiod +} // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system.cpp b/panther_hardware_interfaces/src/panther_system.cpp index 0d3db5bf1..812f31e55 100644 --- a/panther_hardware_interfaces/src/panther_system.cpp +++ b/panther_hardware_interfaces/src/panther_system.cpp @@ -75,7 +75,8 @@ CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &) ConfigureMotorsController(); ConfigureEStop(); } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); + RCLCPP_ERROR_STREAM( + logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); return CallbackReturn::ERROR; } @@ -125,6 +126,9 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) panther_system_ros_interface_->AddService>( "~/charger_enable", std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1)); + panther_system_ros_interface_->AddService>( + "~/led_control_enable", + std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1)); panther_system_ros_interface_->AddService>( "~/motor_power_enable", std::bind(&PantherSystem::MotorsPowerEnable, this, std::placeholders::_1)); diff --git a/panther_hardware_interfaces/src/panther_system_e_stop.cpp b/panther_hardware_interfaces/src/panther_system_e_stop.cpp index 92e5ccc17..4b7de54ef 100644 --- a/panther_hardware_interfaces/src/panther_system_e_stop.cpp +++ b/panther_hardware_interfaces/src/panther_system_e_stop.cpp @@ -24,7 +24,7 @@ namespace panther_hardware_interfaces bool EStopPTH12X::ReadEStopState() { - e_stop_triggered_ = !gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::E_STOP_RESET); + e_stop_triggered_ = !gpio_controller_->IsPinActive(GPIOPin::E_STOP_RESET); // In the case where E-Stop is triggered by another device within the robot's system (e.g., // Roboteq or Safety Board), disabling the software Watchdog is necessary to prevent an @@ -82,7 +82,7 @@ void EStopPTH12X::ResetEStop() bool EStopPTH10X::ReadEStopState() { - const bool motors_on = gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT); + const bool motors_on = gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT); const bool driver_error = roboteq_error_filter_->IsError(); if ((driver_error || !motors_on) && !e_stop_triggered_) { @@ -118,7 +118,7 @@ void EStopPTH10X::ResetEStop() "controller sends zero commands before trying to reset E-stop."); } - if (!gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT)) { + if (!gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT)) { throw std::runtime_error("Can't reset E-stop - motors are not powered."); } diff --git a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp index a9a2d5862..5db2e6fd1 100644 --- a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp +++ b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp @@ -202,7 +202,7 @@ void PantherSystemRosInterface::PublishDriverState() } void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( - const std::unordered_map & io_state) + const std::unordered_map & io_state) { for (const auto & [pin, pin_value] : io_state) { UpdateIOStateMsg(pin, pin_value); @@ -213,7 +213,7 @@ void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( } } -void PantherSystemRosInterface::PublishIOState(const panther_gpiod::GPIOInfo & gpio_info) +void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info) { const bool pin_value = (gpio_info.value == gpiod::line::value::ACTIVE); @@ -226,34 +226,36 @@ void PantherSystemRosInterface::PublishIOState(const panther_gpiod::GPIOInfo & g } } -bool PantherSystemRosInterface::UpdateIOStateMsg( - const panther_gpiod::GPIOPin pin, const bool pin_value) +bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value) { auto & io_state_msg = realtime_io_state_publisher_->msg_; switch (pin) { - case panther_gpiod::GPIOPin::AUX_PW_EN: + case GPIOPin::AUX_PW_EN: io_state_msg.aux_power = pin_value; break; - case panther_gpiod::GPIOPin::CHRG_SENSE: + case GPIOPin::CHRG_SENSE: io_state_msg.charger_connected = pin_value; break; - case panther_gpiod::GPIOPin::CHRG_DISABLE: + case GPIOPin::CHRG_DISABLE: io_state_msg.charger_enabled = !pin_value; break; - case panther_gpiod::GPIOPin::VDIG_OFF: + case GPIOPin::VDIG_OFF: io_state_msg.digital_power = !pin_value; break; - case panther_gpiod::GPIOPin::FAN_SW: + case GPIOPin::FAN_SW: io_state_msg.fan = pin_value; break; - case panther_gpiod::GPIOPin::VMOT_ON: - case panther_gpiod::GPIOPin::MOTOR_ON: + case GPIOPin::VMOT_ON: + case GPIOPin::MOTOR_ON: io_state_msg.motor_on = pin_value; break; - case panther_gpiod::GPIOPin::SHDN_INIT: + case GPIOPin::SHDN_INIT: io_state_msg.power_button = pin_value; break; + case GPIOPin::LED_SBC_SEL: + io_state_msg.led_control = pin_value; + break; default: return false; } diff --git a/panther_hardware_interfaces/test/test_gpio_controller.cpp b/panther_hardware_interfaces/test/test_gpio_controller.cpp index fdb085650..fe5990eac 100644 --- a/panther_hardware_interfaces/test/test_gpio_controller.cpp +++ b/panther_hardware_interfaces/test/test_gpio_controller.cpp @@ -19,21 +19,25 @@ #include -const std::vector gpio_config_info_storage{ - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPIN1, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::GPIN2, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, - panther_gpiod::GPIOInfo{panther_gpiod::GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT}, +using GPIOInfo = panther_hardware_interfaces::GPIOInfo; +using GPIOPin = panther_hardware_interfaces::GPIOPin; + +const std::vector gpio_config_info_storage{ + GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT}, }; class GPIOControllerWrapper : public panther_hardware_interfaces::GPIOControllerPTH12X @@ -80,8 +84,8 @@ float TestGPIOController::GetRobotVersion() TEST_F(TestGPIOController, TestMotorsInit) { - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::VMOT_ON)); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::DRIVER_EN)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::VMOT_ON)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::DRIVER_EN)); } TEST_F(TestGPIOController, TestWatchdog) @@ -89,7 +93,7 @@ TEST_F(TestGPIOController, TestWatchdog) auto edge_cnt = std::make_shared(0); gpio_controller_wrapper_->RegisterGPIOEventCallback([this, edge_cnt](const auto & state) mutable { - if (state.pin == panther_gpiod::GPIOPin::WATCHDOG) { + if (state.pin == GPIOPin::WATCHDOG) { (*edge_cnt)++; } }); @@ -117,33 +121,42 @@ TEST_F(TestGPIOController, TestPinsAvailability) TEST_F(TestGPIOController, TestFanEnbale) { gpio_controller_wrapper_->FanEnable(true); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::FAN_SW)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW)); gpio_controller_wrapper_->FanEnable(false); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::FAN_SW)); + EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW)); } TEST_F(TestGPIOController, TestAUXPowerEnbale) { gpio_controller_wrapper_->AUXPowerEnable(true); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::AUX_PW_EN)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN)); gpio_controller_wrapper_->AUXPowerEnable(false); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::AUX_PW_EN)); + EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN)); } TEST_F(TestGPIOController, TestChargerEnable) { gpio_controller_wrapper_->ChargerEnable(true); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::CHRG_DISABLE)); + EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE)); gpio_controller_wrapper_->ChargerEnable(false); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(panther_gpiod::GPIOPin::CHRG_DISABLE)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE)); +} + +TEST_F(TestGPIOController, TestLEDControlEnable) +{ + gpio_controller_wrapper_->LEDControlEnable(true); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL)); + + gpio_controller_wrapper_->LEDControlEnable(false); + EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL)); } TEST_F(TestGPIOController, TestQueryControlInterfaceIOStates) { - std::unordered_map io_states = + std::unordered_map io_states = gpio_controller_wrapper_->QueryControlInterfaceIOStates(); ASSERT_EQ(io_states.size(), 7); diff --git a/panther_gpiod/test/test_gpio_driver.cpp b/panther_hardware_interfaces/test/test_gpio_driver.cpp similarity index 89% rename from panther_gpiod/test/test_gpio_driver.cpp rename to panther_hardware_interfaces/test/test_gpio_driver.cpp index 7c9755507..5ad0b8336 100644 --- a/panther_gpiod/test/test_gpio_driver.cpp +++ b/panther_hardware_interfaces/test/test_gpio_driver.cpp @@ -24,10 +24,11 @@ #include "gpiod.hpp" #include "gtest/gtest.h" -#include "panther_gpiod/gpio_driver.hpp" +#include "panther_hardware_interfaces/gpio_driver.hpp" #include "panther_utils/test/test_utils.hpp" -using namespace panther_gpiod; +using GPIOInfo = panther_hardware_interfaces::GPIOInfo; +using GPIOPin = panther_hardware_interfaces::GPIOPin; class TestGPIODriver : public ::testing::Test { @@ -42,7 +43,7 @@ class TestGPIODriver : public ::testing::Test void TearDown() override; void SetAndVerifyPinState(const GPIOPin & pin); - std::unique_ptr gpio_driver_; + std::unique_ptr gpio_driver_; std::pair last_gpio_event_summary_; const std::vector gpio_config_info_{ GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT}, @@ -51,7 +52,7 @@ class TestGPIODriver : public ::testing::Test void TestGPIODriver::SetUp() { - gpio_driver_ = std::make_unique(gpio_config_info_); + gpio_driver_ = std::make_unique(gpio_config_info_); last_gpio_event_summary_.first = static_cast(-1); last_gpio_event_summary_.second = static_cast(-1); @@ -77,7 +78,10 @@ void TestGPIODriver::SetAndVerifyPinState(const GPIOPin & pin) TEST(TestGPIODriverInitialization, EmptyInfoStorage) { EXPECT_THROW( - { auto gpio_driver = std::make_unique(std::vector{}); }, + { + auto gpio_driver = + std::make_unique(std::vector{}); + }, std::runtime_error); } @@ -86,11 +90,11 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail) // There is no OS version that supports simultaneous operation of MOTOR_ON and VMOT_ON pins. EXPECT_THROW( { - auto gpio_driver = std::make_unique( + auto gpio_driver = std::make_unique( std::vector{{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}}); gpio_driver.reset(); - gpio_driver = std::make_unique( + gpio_driver = std::make_unique( std::vector{{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}}); }, std::invalid_argument); diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index aa4d7137e..95177faab 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -8,12 +8,12 @@ endif() find_package(ament_cmake REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(image_transport REQUIRED) -find_package(panther_gpiod REQUIRED) find_package(panther_msgs REQUIRED) find_package(panther_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(yaml-cpp REQUIRED) include_directories(include) @@ -33,10 +33,10 @@ ament_target_dependencies( driver_node diagnostic_updater image_transport - panther_gpiod panther_msgs rclcpp - sensor_msgs) + sensor_msgs + std_srvs) add_executable( controller_node @@ -178,8 +178,8 @@ if(BUILD_TESTING) image_transport diagnostic_updater panther_msgs - panther_gpiod - panther_utils) + panther_utils + std_srvs) endif() ament_package() diff --git a/panther_lights/README.md b/panther_lights/README.md index 57955479b..47db13a69 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -26,11 +26,11 @@ This node is responsible for displaying frames on the Husarion Panther robot's B [//]: # (ROS_API_NODE_DESCRIPTION_END) -#### Publishes +#### Publishers [//]: # (ROS_API_NODE_PUBLISHERS_START) -- `/diagnostics` [*diagnostic_msgs/DiagnosticArray*]: lights diagnostic messages. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: lights diagnostic messages. [//]: # (ROS_API_NODE_PUBLISHERS_END) @@ -38,8 +38,8 @@ This node is responsible for displaying frames on the Husarion Panther robot's B [//]: # (ROS_API_NODE_SUBSCRIBERS_START) -- `~/lights/driver/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. -- `~/lights/driver/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. +- `lights/driver/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. +- `lights/driver/channel_2_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) @@ -47,10 +47,18 @@ This node is responsible for displaying frames on the Husarion Panther robot's B [//]: # (ROS_API_NODE_SERVICE_SERVERS_START) -- `~/lights/driver/set/brightness` [*panther_msgs/SetLEDBrightness*]: allows setting global LED brightness, value ranges from **0.0** to **1.0**. +- `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) +#### Service Clients + +[//]: # (ROS_API_NODE_SERVICE_CLIENTS_START) + +- `hardware/led_control_enable` [*std_srvs/SetBool*]: makes SBC controlling LEDs. + +[//]: # (ROS_API_NODE_SERVICE_CLIENTS_END) + #### Parameters [//]: # (ROS_API_NODE_PARAMETERS_START) @@ -80,8 +88,8 @@ This node is responsible for processing animations and publishing frames to be d [//]: # (ROS_API_NODE_PUBLISHERS_START) -- `~/lights/driver/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. -- `~/lights/driver/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. +- `lights/driver/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. +- `lights/driver/channel_2_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_PUBLISHERS_END) @@ -89,7 +97,7 @@ This node is responsible for processing animations and publishing frames to be d [//]: # (ROS_API_NODE_SERVICE_SERVERS_START) -- `~/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. +- `lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. [//]: # (ROS_API_NODE_SERVICE_SERVERS_END) @@ -143,9 +151,9 @@ The `led_animations` section contains list with definitions for various animatio - `id` [*int*, default: **None**]: unique ID of an animation. - `name` [*string*, default: **ANIMATION_**]: name of an animation. If not provided will default to **ANIMATION_**, where `` is equal to `id` parameter of the given animation. - `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: - - **1** interrupts and removes animation with priorities **2** and **3**. - - **2** interrupts animations with priority **3**. - - **3** adds animation to the end of the queue. + - **1** interrupts and removes animation with priorities **2** and **3**. + - **2** interrupts animations with priority **3**. + - **3** adds animation to the end of the queue. - `timeout` [*float*, default: **120.0**]: time in **[s]**, after which animation will be removed from the queue. Default animations can be found in the table below: @@ -270,6 +278,7 @@ ros2 service call /lights/controller/set/animation panther_msgs/srv/SetLEDAnimat ``` --- + ## Defining a Custom Animation Type It is possible to define your own animation type with expected, new behavior. For more information, see: [**Animation API**](https://github.com/husarion/panther_ros/panther_lights/LIGHTS_API.md). diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index bc76a2481..a4fa85cec 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -22,15 +22,17 @@ #include "image_transport/image_transport.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + #include "panther_msgs/srv/set_led_brightness.hpp" -#include "panther_gpiod/gpio_driver.hpp" #include "panther_lights/apa102.hpp" namespace panther_lights { using ImageMsg = sensor_msgs::msg::Image; +using SetBoolSrv = std_srvs::srv::SetBool; using SetLEDBrightnessSrv = panther_msgs::srv::SetLEDBrightness; /** @@ -43,23 +45,43 @@ class DriverNode : public rclcpp::Node const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** - * @brief Initializes the driver class. This method must be executed manually after creating the - * node. + * @brief Initializes the ImageTransport subscribers for to subscribe frames. IMPORTANT: this must + * be invoked after the node class is instantiated to establish whole functionality. * * @param it ImageTransport object used to create subscribers for Image topics. */ - void Initialize(const std::shared_ptr & it); + void InitializeSubscribers(const std::shared_ptr & it); protected: int num_led_; double frame_timeout_; - bool panels_initialised_ = false; + bool led_control_granted_ = false; + rclcpp::Time chanel_1_ts_; rclcpp::Time chanel_2_ts_; private: void OnShutdown(); + /** + * @brief Clears all LEDs on both channels. + */ + void ClearLEDs(); + + /** + * @brief Toggles LED control ON or OFF. + * + * @param enable True to enable LED control, false to disable. + */ + void ToggleLEDControl(const bool enable); + + /** + * @brief Callback to execute when service invoked to toggle LED control returns response. + * + * @param future Future object with request and response of the service call. + */ + void ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future); + /** * @brief Callback to execute when a message with new frame is received. * @@ -76,16 +98,18 @@ class DriverNode : public rclcpp::Node void SetBrightnessCB( const SetLEDBrightnessSrv::Request::SharedPtr & request, SetLEDBrightnessSrv::Response::SharedPtr response); - void SetPowerPin(const bool value); + void DiagnoseLights(diagnostic_updater::DiagnosticStatusWrapper & status); apa102::APA102 chanel_1_; apa102::APA102 chanel_2_; + rclcpp::Client::SharedPtr enable_led_control_client_; rclcpp::Service::SharedPtr set_brightness_server_; + std::shared_ptr chanel_2_sub_; std::shared_ptr chanel_1_sub_; - std::unique_ptr gpio_driver_; + diagnostic_updater::Updater diagnostic_updater_; }; diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 099415287..446bfc0be 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -19,12 +19,12 @@ diagnostic_updater image_transport libpng-dev - panther_gpiod panther_msgs panther_utils pluginlib rclcpp sensor_msgs + std_srvs ament_cmake diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 7ef8e05ac..75483176a 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -21,16 +21,15 @@ #include #include -#include "gpiod.hpp" - #include "diagnostic_updater/diagnostic_updater.hpp" #include "image_transport/image_transport.hpp" #include "rclcpp/rclcpp.hpp" + #include "sensor_msgs/image_encodings.hpp" +#include "std_srvs/srv/set_bool.hpp" #include "panther_msgs/srv/set_led_brightness.hpp" -#include "panther_gpiod/gpio_driver.hpp" #include "panther_lights/apa102.hpp" namespace panther_lights @@ -53,67 +52,117 @@ DriverNode::DriverNode(const std::string & node_name, const rclcpp::NodeOptions this->declare_parameter("frame_timeout", 0.1); this->declare_parameter("num_led", 46); + frame_timeout_ = this->get_parameter("frame_timeout").as_double(); + num_led_ = this->get_parameter("num_led").as_int(); + + const float global_brightness = this->get_parameter("global_brightness").as_double(); + chanel_1_.SetGlobalBrightness(global_brightness); + chanel_2_.SetGlobalBrightness(global_brightness); + + enable_led_control_client_ = this->create_client( + "hardware/led_control_enable", rmw_qos_profile_services_default); + + set_brightness_server_ = this->create_service( + "lights/driver/set/brightness", std::bind(&DriverNode::SetBrightnessCB, this, _1, _2)); + diagnostic_updater_.setHardwareID("Bumper Lights"); diagnostic_updater_.add("Lights driver status", this, &DriverNode::DiagnoseLights); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } -void DriverNode::Initialize(const std::shared_ptr & it) +void DriverNode::InitializeSubscribers(const std::shared_ptr & it) { - RCLCPP_INFO(this->get_logger(), "Initializing."); - - const float global_brightness = this->get_parameter("global_brightness").as_double(); - frame_timeout_ = this->get_parameter("frame_timeout").as_double(); - num_led_ = this->get_parameter("num_led").as_int(); - - std::vector gpio_info_storage = {panther_gpiod::GPIOInfo{ - panther_gpiod::GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}}; - gpio_driver_ = std::make_unique(gpio_info_storage); - gpio_driver_->GPIOMonitorEnable(); + RCLCPP_INFO(this->get_logger(), "Initializing ImageTransport subscribers."); chanel_1_ts_ = this->get_clock()->now(); - chanel_2_ts_ = this->get_clock()->now(); - - chanel_1_.SetGlobalBrightness(global_brightness); - chanel_2_.SetGlobalBrightness(global_brightness); - chanel_1_sub_ = std::make_shared( it->subscribe("lights/driver/channel_1_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, chanel_1_, chanel_1_ts_, "channel_1"); chanel_1_ts_ = msg->header.stamp; })); + chanel_2_ts_ = this->get_clock()->now(); chanel_2_sub_ = std::make_shared( it->subscribe("lights/driver/channel_2_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, chanel_2_, chanel_2_ts_, "channel_2"); chanel_2_ts_ = msg->header.stamp; })); - set_brightness_server_ = this->create_service( - "lights/driver/set/brightness", std::bind(&DriverNode::SetBrightnessCB, this, _1, _2)); - - RCLCPP_INFO(this->get_logger(), "Initialized successfully."); + RCLCPP_INFO(this->get_logger(), "Initialized ImageTransport successfully."); } void DriverNode::OnShutdown() { - // Clear LEDs + ClearLEDs(); + + if (led_control_granted_) { + ToggleLEDControl(false); + } +} + +void DriverNode::ClearLEDs() +{ chanel_1_.SetPanel(std::vector(num_led_ * 4, 0)); chanel_2_.SetPanel(std::vector(num_led_ * 4, 0)); +} + +void DriverNode::ToggleLEDControl(const bool enable) +{ + auto request = std::make_shared(); + request->data = enable; - // Give back control over LEDs - if (panels_initialised_) { - SetPowerPin(false); + if (!enable_led_control_client_->wait_for_service(std::chrono::seconds(3))) { + throw std::runtime_error( + "Timeout occurred while waiting for service '" + + std::string(enable_led_control_client_->get_service_name()) + "'!"); } - gpio_driver_.reset(); + enable_led_control_client_->async_send_request( + request, std::bind(&DriverNode::ToggleLEDControlCB, this, std::placeholders::_1)); + + RCLCPP_DEBUG( + this->get_logger(), "Sent request toggling LED control to '%s'.", enable ? "true" : "false"); +} + +void DriverNode::ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future) +{ + RCLCPP_DEBUG(this->get_logger(), "Received response after toggling LED control."); + + const auto result = future.get(); + + const auto request = result.first; + const auto response = result.second; + + if (!response->success) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Failed to toggle LED control."); + return; + } + + if (request->data == true) { + led_control_granted_ = true; + ClearLEDs(); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "LED control granted."); + } else { + led_control_granted_ = false; + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "LED control revoked."); + } } void DriverNode::FrameCB( const ImageMsg::ConstSharedPtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, const std::string & panel_name) { + if (!led_control_granted_) { + ToggleLEDControl(true); + + auto message = "LED control not granted, ignoring frame!"; + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); + return; + } + std::string message; if ( (this->get_clock()->now() - rclcpp::Time(msg->header.stamp)) > @@ -130,6 +179,7 @@ void DriverNode::FrameCB( } if (!message.empty()) { + // Since this is throttle warning, we need to add panel name condition to log from both panels if (panel_name == "channel_1") { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); @@ -144,20 +194,9 @@ void DriverNode::FrameCB( return; } - if (!panels_initialised_) { - // Take control over LEDs - SetPowerPin(true); - panels_initialised_ = true; - } - panel.SetPanel(msg->data); } -void DriverNode::SetPowerPin(const bool value) -{ - gpio_driver_->SetPinValue(panther_gpiod::GPIOPin::LED_SBC_SEL, value); -} - void DriverNode::SetBrightnessCB( const SetLEDBrightnessSrv::Request::SharedPtr & req, SetLEDBrightnessSrv::Response::SharedPtr res) { @@ -182,30 +221,14 @@ void DriverNode::SetBrightnessCB( void DriverNode::DiagnoseLights(diagnostic_updater::DiagnosticStatusWrapper & status) { - std::vector key_values; unsigned char error_level{diagnostic_updater::DiagnosticStatusWrapper::OK}; - std::string message{"LED panels initialized properly."}; + std::string message{"Control over LEDs granted."}; - if (!panels_initialised_) { + if (!led_control_granted_) { error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - message = "LED panels initialization failed."; - - auto pin_available = gpio_driver_->IsPinAvailable(panther_gpiod::GPIOPin::LED_SBC_SEL); - auto pin_active = gpio_driver_->IsPinActive(panther_gpiod::GPIOPin::LED_SBC_SEL); - - diagnostic_msgs::msg::KeyValue pin_available_kv; - pin_available_kv.key = "LED_SBC_SEL pin available"; - pin_available_kv.value = pin_available ? "true" : "false"; - - diagnostic_msgs::msg::KeyValue pin_active_kv; - pin_active_kv.key = "LED_SBC_SEL pin active"; - pin_active_kv.value = pin_active ? "true" : "false"; - - key_values.push_back(pin_available_kv); - key_values.push_back(pin_active_kv); + message = "Control over LEDs not granted, driver is not functional!"; } - status.values = key_values; status.summary(error_level, message); } diff --git a/panther_lights/src/driver_node_main.cpp b/panther_lights/src/driver_node_main.cpp index d25ea58e8..b948463c9 100644 --- a/panther_lights/src/driver_node_main.cpp +++ b/panther_lights/src/driver_node_main.cpp @@ -23,11 +23,9 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto lights_driver_node = std::make_shared("lights_driver_node"); + auto it = std::make_shared(lights_driver_node); - std::shared_ptr it; - it = std::make_shared(lights_driver_node); - - lights_driver_node->Initialize(it); + lights_driver_node->InitializeSubscribers(it); try { rclcpp::spin(lights_driver_node); diff --git a/panther_lights/test/test_driver_node.cpp b/panther_lights/test/test_driver_node.cpp index a3821b619..5cf5c89fa 100644 --- a/panther_lights/test/test_driver_node.cpp +++ b/panther_lights/test/test_driver_node.cpp @@ -38,7 +38,7 @@ class DriverNodeWrapper : public panther_lights::DriverNode int getNumLeds() const { return num_led_; } double getTimeout() const { return frame_timeout_; } - bool isInitialised() const { return panels_initialised_; } + bool isInitialised() const { return led_control_granted_; } rclcpp::Time setChanel1TS(const rclcpp::Time & ts) { return chanel_1_ts_ = ts; } rclcpp::Time setChanel2TS(const rclcpp::Time & ts) { return chanel_2_ts_ = ts; } }; @@ -59,7 +59,7 @@ class TestDriverNode : public testing::Test it_->advertise("lights/driver/channel_2_frame", 5)); set_brightness_client_ = node_->create_client("lights/driver/set/brightness"); - node_->Initialize(it_); + node_->InitializeSubscribers(it_); } ~TestDriverNode() { rclcpp::shutdown(); } diff --git a/panther_utils/include/panther_utils/configure_rt.hpp b/panther_utils/include/panther_utils/configure_rt.hpp index 9a181eac9..1d6d58ae4 100644 --- a/panther_utils/include/panther_utils/configure_rt.hpp +++ b/panther_utils/include/panther_utils/configure_rt.hpp @@ -28,7 +28,7 @@ namespace panther_utils * * @exception std::runtime_error if invalid priority is set, kernel isn't RT or configuration fails. */ -void ConfigureRT(const unsigned priority) +inline void ConfigureRT(const unsigned priority) { if (priority > 99) { throw std::runtime_error(