From 1559a1c999046c0ae98edf6854d4e90a70f3e39e Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 7 Jun 2024 11:13:14 +0000 Subject: [PATCH 01/14] Move gpio_driver to panther_hardware_interfaces --- panther_hardware_interfaces/CMakeLists.txt | 22 +- .../cmake/SuperBuild.cmake | 18 +- .../gpio_driver.hpp | 296 +++++++++++++++++ panther_hardware_interfaces/package.xml | 6 +- .../src/gpio_driver.cpp | 311 ++++++++++++++++++ .../src/panther_system.cpp | 3 +- 6 files changed, 642 insertions(+), 14 deletions(-) create mode 100644 panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp create mode 100644 panther_hardware_interfaces/src/gpio_driver.cpp diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index bb4b6a8ee..d64e4dd56 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -53,25 +53,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 +103,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) 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_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp new file mode 100644 index 000000000..b329339ab --- /dev/null +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_driver.hpp @@ -0,0 +1,296 @@ +// 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. + +#ifndef PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ +#define PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpiod.hpp" + +namespace panther_hardware_interfaces +{ + +/** + * @brief Enumeration representing available GPIO pins in the Panther system. + */ +enum class GPIOPin { + AUX_PW_EN, + CHRG_DISABLE, + CHRG_SENSE, + DRIVER_EN, + E_STOP_RESET, + FAN_SW, + GPOUT1, + GPOUT2, + GPIN1, + GPIN2, + LED_SBC_SEL, + SHDN_INIT, + STAGE2_INPUT, + VDIG_OFF, + VMOT_ON, + MOTOR_ON, + WATCHDOG +}; + +/** + * @brief Structure containing information related to GPIO pins such as pin configuration, + * direction, value, etc. This information is required during the initialization process. + */ +struct GPIOInfo +{ + GPIOPin pin; + gpiod::line::direction direction; + bool active_low = false; + gpiod::line::value init_value = gpiod::line::value::INACTIVE; + gpiod::line::value value = gpiod::line::value::INACTIVE; + gpiod::line::offset offset = -1; +}; + +/** + * @brief Class responsible for managing GPIO pins on Panther robots, handling tasks such as + * setting pin values, changing pin directions, monitoring pin events, and more. + */ +class GPIODriver +{ +public: + /** + * @brief Constructs the GPIODriver object with information about GPIO pin configurations. + * This information is necessary for initializing the GPIO functionality. + * + * @param gpio_info_storage Vector containing information about GPIO pin configurations. + * + * @throws std::runtime_error if the provided `gpio_info_storage` vector is empty. + * + * @note To enable reading pin values, it is required to enable GPIO monitoring. + * See GPIOMonitorEnable method for more info. + * + * @par Example + * An example of constructing the GPIODriver object by providing GPIO pin information: + * @code{.cpp} + * std::vector gpio_configurations = { + * GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT}, + * GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, + * GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true, + * gpiod::line::value::ACTIVE} + * // ... additional GPIO pin configurations + * }; + * GPIODriver gpio_driver(gpio_configurations); + * @endcode + */ + GPIODriver(std::vector gpio_info_storage); + + /** + * @brief The destructor sets the GPIO pin values back to their initial values to ensure proper + * cleanup. It then releases the line request and turns off the GPIO monitoring thread. + */ + ~GPIODriver(); + + /** + * @brief Enables GPIO pin state monitoring asynchronously. Optionally, configure the + * Real-Time (RT) FIFO scheduling policy for the monitor thread. + * @details When called, this method starts the GPIO state monitoring thread asynchronously. + * If `use_rt` is set to `true`, it configures the FIFO RT scheduling policy for the monitor + * thread with the specified `gpio_monit_thread_sched_priority`. The default priority is 60. + * + * @param use_rt Whether to configure RT FIFO scheduling policy for the monitor thread. + * Default is set to false. + * @param gpio_monit_thread_sched_priority Priority for the GPIO monitoring thread. + * Set within the range of 0-99 to enable and configure the FIFO RT scheduling + * policy for the monitor thread. This parameter is considered only if `use_rt` is set to + * true. The default priority is 60. + * + * @note Calling `GPIOMonitorEnable` is optional after constructing the driver object. It allows + * asynchronous monitoring of GPIO pin states. Not invoking this method will result in the + * lack of functionality to read pin values. + */ + void GPIOMonitorEnable( + const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60); + + /** + * @brief This method sets the provided callback function to be executed upon GPIO edge events. + * + * @param callback The callback function to handle GPIO edge events. + * + * @throws std::runtime_error if GPIO monitor thread is not running. + * + * @par Example + * An example of using this method to bind a member function as a callback: + * @code{.cpp} + * class MyClass { + * public: + * void HandleGPIOEvent(const GPIOInfo & gpio_info) { + * // Handle GPIO event here, i.e: + * std::cout << gpio_info.offset << ": " << gpio_info.value << std::endl; + * } + * }; + * + * MyClass my_obj; + * GPIODriver gpio_driver; + * gpio_driver.GPIOMonitorEnable(true, 50); + * gpio_driver.ConfigureEdgeEventCallback( + * std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1)); + * @endcode + */ + void ConfigureEdgeEventCallback(const std::function & callback); + + /** + * @brief Changes the direction of a specific GPIO pin. + * + * @param pin GPIOPin to change the direction for. + * @param direction New direction for the pin. + */ + void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction); + + /** + * @brief Returns true if a specific pin is configured and stored in GPIO info storage + * + * @param pin The GPIO pin to check availability for + * @return true if the pin is available, false otherwise + */ + bool IsPinAvailable(const GPIOPin pin) const; + + /** + * @brief Checks if a specific GPIO pin is active. This method returns the value stored in the + * class read during the last edge event. + * + * @param pin GPIOPin to check. + * + * @throws std::runtime_error if GPIO monitor thread is not running. + * + * @return True if the pin is active, false otherwise. + */ + bool IsPinActive(const GPIOPin pin); + + /** + * @brief Sets the value for a specific GPIO pin. + * + * @param pin GPIOPin to set the value for. + * @param value The boolean value to set for the pin. + * + * @throws std::invalid_argument if trying to set the value for an INPUT pin. + * @throws std::runtime_error if changing the GPIO state fails. + * + * @return true if the pin value is successfully set, false otherwise. + */ + bool SetPinValue(const GPIOPin pin, const bool value); + +private: + std::unique_ptr CreateLineRequest(gpiod::chip & chip); + gpiod::line_settings GenerateLineSettings(const GPIOInfo & pin_info); + GPIOPin GetPinFromOffset(const gpiod::line::offset & offset) const; + GPIOInfo & GetGPIOInfoRef(const GPIOPin pin); + void ConfigureLineRequest( + gpiod::chip & chip, gpiod::request_builder & builder, GPIOInfo & gpio_info); + void MonitorAsyncEvents(); + void HandleEdgeEvent(const gpiod::edge_event & event); + bool IsGPIOMonitorThreadRunning() const; + + /** + * @brief Enables asynchronous monitoring of GPIO pin events. + * + * @throws std::runtime_error if a timeout occurs while waiting for the GPIO monitor thread. + */ + void GPIOMonitorOn(); + + /** + * @brief Disables asynchronous monitoring of GPIO pin events. + */ + void GPIOMonitorOff(); + + /** + * @brief Callback function for GPIO edge events. + * + * @param gpio_info Information related to the state of the GPIO pin for which the event took + * place. + */ + std::function GPIOEdgeEventCallback; + + /** + * @brief Mapping of GPIO pins to their respective names. + */ + const std::map pin_names_{ + {GPIOPin::WATCHDOG, "WATCHDOG"}, + {GPIOPin::AUX_PW_EN, "AUX_PW_EN"}, + {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"}, + {GPIOPin::CHRG_SENSE, "CHRG_SENSE"}, + {GPIOPin::DRIVER_EN, "DRIVER_EN"}, + {GPIOPin::E_STOP_RESET, "E_STOP_RESET"}, + {GPIOPin::FAN_SW, "FAN_SW"}, + {GPIOPin::GPOUT1, "GPOUT1"}, + {GPIOPin::GPOUT2, "GPOUT2"}, + {GPIOPin::GPIN1, "GPIN1"}, + {GPIOPin::GPIN2, "GPIN2"}, + {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"}, + {GPIOPin::SHDN_INIT, "SHDN_INIT"}, + {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"}, + {GPIOPin::VDIG_OFF, "VDIG_OFF"}, + {GPIOPin::VMOT_ON, "VMOT_ON"}, + {GPIOPin::MOTOR_ON, "MOTOR_ON"}, + }; + + /** + * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. + */ + std::vector gpio_info_storage_; + + /** + * @brief Mutex for managing access to GPIO pin information. + */ + mutable std::shared_mutex gpio_info_storage_mutex_; + + /** + * @brief Request object for controlling GPIO lines. + */ + std::unique_ptr line_request_; + + /** + * @brief Thread object for monitoring GPIO events asynchronously. + */ + std::unique_ptr gpio_monitor_thread_; + + /** + * @brief When enabled, the GPIODriver object will initiate the GPIO monitor + * thread with real-time scheduling if a valid priority is provided. + */ + bool use_rt_; + + /** + * @brief Priority for the GPIO monitoring thread. The value ranges from 0 to 99, inclusive, + * and is used to configure the FIFO real-time scheduling policy for the monitor thread + * if real-time monitoring is enabled. + */ + unsigned gpio_monit_thread_sched_priority_; + std::atomic_bool gpio_monitor_thread_enabled_ = false; + std::condition_variable monitor_init_cond_var_; + std::mutex monitor_init_mtx_; + static constexpr unsigned gpio_debounce_period_ = 10; + static constexpr unsigned edge_event_buffer_size_ = 2; + const std::filesystem::path gpio_chip_path_ = "/dev/gpiochip0"; +}; + +} // namespace panther_hardware_interfaces + +#endif // PANTHER_HARDWARE_INTERFACES_GPIO_DRIVER_HPP_ diff --git a/panther_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml index 91f91a2b8..a2d7bd01a 100644 --- a/panther_hardware_interfaces/package.xml +++ b/panther_hardware_interfaces/package.xml @@ -23,7 +23,6 @@ geometry_msgs hardware_interface imu_filter_madgwick - panther_gpiod panther_msgs panther_utils phidgets_api @@ -35,6 +34,11 @@ tf2_geometry_msgs tf2_ros + autoconf + autoconf-archive + libtool + m4 + pkg-config pluginlib diff --git a/panther_hardware_interfaces/src/gpio_driver.cpp b/panther_hardware_interfaces/src/gpio_driver.cpp new file mode 100644 index 000000000..3aa9a0c1f --- /dev/null +++ b/panther_hardware_interfaces/src/gpio_driver.cpp @@ -0,0 +1,311 @@ +// 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 "panther_hardware_interfaces/gpio_driver.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpiod.hpp" + +#include "panther_utils/configure_rt.hpp" + +namespace panther_hardware_interfaces +{ + +GPIODriver::GPIODriver(std::vector gpio_info_storage) +: gpio_info_storage_(std::move(gpio_info_storage)) +{ + if (gpio_info_storage_.empty()) { + throw std::runtime_error("The GPIO information vector is empty."); + } + + auto gpio_chip = gpiod::chip(gpio_chip_path_); + line_request_ = CreateLineRequest(gpio_chip); +} + +GPIODriver::~GPIODriver() +{ + for (GPIOInfo & gpio_info : gpio_info_storage_) { + if (gpio_info.direction == gpiod::line::direction::OUTPUT) { + line_request_->set_value(gpio_info.offset, gpio_info.init_value); + } + } + + GPIOMonitorOff(); + line_request_->release(); +} + +void GPIODriver::GPIOMonitorEnable( + const bool use_rt, const unsigned gpio_monit_thread_sched_priority) +{ + use_rt_ = use_rt; + gpio_monit_thread_sched_priority_ = gpio_monit_thread_sched_priority; + + GPIOMonitorOn(); +} + +void GPIODriver::ConfigureEdgeEventCallback(const std::function & callback) +{ + if (!IsGPIOMonitorThreadRunning()) { + throw std::runtime_error("GPIO monitor thread is not running!"); + } + + GPIOEdgeEventCallback = callback; +} + +std::unique_ptr GPIODriver::CreateLineRequest(gpiod::chip & chip) +{ + auto request_builder = chip.prepare_request(); + request_builder.set_consumer("panther_gpiod"); + + for (GPIOInfo & gpio_info : gpio_info_storage_) { + ConfigureLineRequest(chip, request_builder, gpio_info); + } + + return std::make_unique(request_builder.do_request()); +} + +void GPIODriver::ConfigureLineRequest( + gpiod::chip & chip, gpiod::request_builder & builder, GPIOInfo & gpio_info) +{ + gpiod::line_settings settings = GenerateLineSettings(gpio_info); + + std::string pin_name; + try { + pin_name = pin_names_.at(gpio_info.pin); + } catch (const std::out_of_range & e) { + throw std::runtime_error("No name defined for one of pins: " + std::string(e.what())); + } + + gpiod::line::offset offset = chip.get_line_offset_from_name(pin_name); + + builder.add_line_settings(offset, settings); + gpio_info.offset = offset; +} + +gpiod::line_settings GPIODriver::GenerateLineSettings(const GPIOInfo & gpio_info) +{ + auto settings = gpiod::line_settings(); + settings.set_direction(gpio_info.direction); + settings.set_active_low(gpio_info.active_low); + + // Set the initial value only when the line is configured for the first time; + // otherwise, set the last known value + gpiod::line::value new_output_value = line_request_ ? gpio_info.value : gpio_info.init_value; + settings.set_output_value(new_output_value); + + if (gpio_info.direction == gpiod::line::direction::INPUT) { + settings.set_edge_detection(gpiod::line::edge::BOTH); + settings.set_debounce_period(std::chrono::milliseconds(gpio_debounce_period_)); + } + + return settings; +} + +void GPIODriver::ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) +{ + std::lock_guard lock(gpio_info_storage_mutex_); + GPIOInfo & gpio_info = GetGPIOInfoRef(pin); + + if (gpio_info.direction == direction) { + return; + } + + gpio_info.direction = direction; + + auto line_config = gpiod::line_config(); + + for (const auto & gpio_info : gpio_info_storage_) { + gpiod::line_settings settings = GenerateLineSettings(gpio_info); + line_config.add_line_settings(gpio_info.offset, settings); + } + + line_request_->reconfigure_lines(line_config); + gpio_info.value = line_request_->get_value(gpio_info.offset); +} + +bool GPIODriver::IsPinAvailable(const GPIOPin pin) const +{ + return std::any_of(gpio_info_storage_.begin(), gpio_info_storage_.end(), [&](const auto & info) { + return info.pin == pin; + }); +} + +bool GPIODriver::IsPinActive(const GPIOPin pin) +{ + if (!IsGPIOMonitorThreadRunning()) { + throw std::runtime_error("GPIO monitor thread is not running!"); + } + + std::lock_guard lock(gpio_info_storage_mutex_); + const GPIOInfo & pin_info = GetGPIOInfoRef(pin); + return pin_info.value == gpiod::line::value::ACTIVE; +} + +bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) +{ + GPIOInfo & gpio_info = GetGPIOInfoRef(pin); + + if (gpio_info.direction == gpiod::line::direction::INPUT) { + throw std::invalid_argument("Cannot set value for INPUT pin."); + } + + gpiod::line::value gpio_value = value ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE; + + std::lock_guard lock(gpio_info_storage_mutex_); + try { + line_request_->set_value(gpio_info.offset, gpio_value); + + if (line_request_->get_value(gpio_info.offset) != gpio_value) { + throw std::runtime_error("Failed to change GPIO state."); + } + + gpio_info.value = gpio_value; + if (GPIOEdgeEventCallback) { + GPIOEdgeEventCallback(gpio_info); + } + + return true; + } catch (const std::exception & e) { + std::cerr << "An exception ocurred while setting GPIO pin value: " << e.what() << std::endl; + return false; + } +} + +void GPIODriver::GPIOMonitorOn() +{ + if (IsGPIOMonitorThreadRunning()) { + return; + } + + { + std::lock_guard lock(gpio_info_storage_mutex_); + for (auto & info : gpio_info_storage_) { + info.value = line_request_->get_value(info.offset); + } + } + + std::unique_lock lck(monitor_init_mtx_); + + gpio_monitor_thread_enabled_ = true; + gpio_monitor_thread_ = std::make_unique(&GPIODriver::MonitorAsyncEvents, this); + + if ( + monitor_init_cond_var_.wait_for(lck, std::chrono::milliseconds(100)) == + std::cv_status::timeout) { + throw std::runtime_error("Timeout while waiting for GPIO monitor thread."); + } +} + +void GPIODriver::MonitorAsyncEvents() +{ + if (use_rt_) { + panther_utils::ConfigureRT(gpio_monit_thread_sched_priority_); + } + + auto edge_event_buffer = gpiod::edge_event_buffer(edge_event_buffer_size_); + + { + std::lock_guard lck(monitor_init_mtx_); + } + monitor_init_cond_var_.notify_all(); + + while (gpio_monitor_thread_enabled_) { + if (line_request_->wait_edge_events(std::chrono::milliseconds(10))) { + line_request_->read_edge_events(edge_event_buffer); + + for (const auto & event : edge_event_buffer) { + HandleEdgeEvent(event); + } + } + } +} + +void GPIODriver::HandleEdgeEvent(const gpiod::edge_event & event) +{ + std::lock_guard lock(gpio_info_storage_mutex_); + GPIOPin pin; + + try { + pin = GetPinFromOffset(event.line_offset()); + } catch (const std::out_of_range & e) { + std::cerr << "An exception ocurred while handling edge event: " << e.what() << std::endl; + return; + } + + GPIOInfo & gpio_info = GetGPIOInfoRef(pin); + gpiod::line::value new_value; + + if (event.type() == gpiod::edge_event::event_type::RISING_EDGE) { + new_value = gpiod::line::value::ACTIVE; + } else { + new_value = gpiod::line::value::INACTIVE; + } + + gpio_info.value = new_value; + + if (GPIOEdgeEventCallback) { + GPIOEdgeEventCallback(gpio_info); + } +} + +void GPIODriver::GPIOMonitorOff() +{ + gpio_monitor_thread_enabled_ = false; + + if (IsGPIOMonitorThreadRunning()) { + gpio_monitor_thread_->join(); + } +} + +bool GPIODriver::IsGPIOMonitorThreadRunning() const +{ + return gpio_monitor_thread_ && gpio_monitor_thread_->joinable(); +} + +GPIOInfo & GPIODriver::GetGPIOInfoRef(const GPIOPin pin) +{ + for (auto & info : gpio_info_storage_) { + if (info.pin == pin) { + return info; + } + } + + throw std::invalid_argument("Pin not found in GPIO info storage."); +} + +GPIOPin GPIODriver::GetPinFromOffset(const gpiod::line::offset & offset) const +{ + for (const auto & gpio_info : gpio_info_storage_) { + if (gpio_info.offset == offset) { + return gpio_info.pin; + } + } + + throw std::out_of_range( + "Pin with offset " + std::to_string(offset) + " not found in GPIO info storage."); +} + +} // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system.cpp b/panther_hardware_interfaces/src/panther_system.cpp index 0d3db5bf1..98e13d1c4 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; } From b9abe41417973fd9fb8f0bfcf9075fc105a70916 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Fri, 7 Jun 2024 11:13:48 +0000 Subject: [PATCH 02/14] Replace namespace --- panther_hardware_interfaces/CMakeLists.txt | 8 +-- panther_hardware_interfaces/CODE_STRUCTURE.md | 2 +- .../gpio_controller.hpp | 63 ++++++++-------- .../panther_system_ros_interface.hpp | 7 +- .../src/gpio_controller.cpp | 72 +++++++++---------- .../src/panther_system_e_stop.cpp | 6 +- .../src/panther_system_ros_interface.cpp | 23 +++--- .../test/test_gpio_controller.cpp | 50 ++++++------- .../include/panther_utils/configure_rt.hpp | 2 +- 9 files changed, 110 insertions(+), 123 deletions(-) diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index d64e4dd56..71944ff39 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -26,7 +26,6 @@ set(PACKAGE_INCLUDE_DEPENDS controller_interface diagnostic_updater hardware_interface - panther_gpiod panther_msgs panther_utils PkgConfig @@ -174,8 +173,8 @@ if(BUILD_TESTING) ${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 @@ -191,13 +190,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::LIBLELY_COAPP) 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..170495d22 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/panther_hardware_interfaces/CODE_STRUCTURE.md @@ -43,7 +43,7 @@ As they usually are rare and singular occurrences, it is better to filter some o ## 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/include/panther_hardware_interfaces/gpio_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp index 0ec430678..b615323bc 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; }; @@ -103,8 +103,7 @@ class GPIOControllerInterface virtual void EStopTrigger() = 0; virtual void EStopReset() = 0; - virtual std::unordered_map QueryControlInterfaceIOStates() - const = 0; + virtual std::unordered_map QueryControlInterfaceIOStates() const = 0; virtual void InterruptEStopReset(){}; @@ -130,15 +129,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 @@ -215,7 +213,7 @@ class GPIOControllerPTH12X : public GPIOControllerInterface * * @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 +237,21 @@ 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}, }; std::mutex e_stop_cv_mtx_; @@ -345,15 +342,15 @@ class GPIOControllerPTH10X : public GPIOControllerInterface * * @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}, }; }; 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/src/gpio_controller.cpp b/panther_hardware_interfaces/src/gpio_controller.cpp index 673e98191..8d8f9ca7c 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,39 @@ 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 +std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const { - std::unordered_map io_state; + 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 +212,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 +227,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 +235,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 +263,17 @@ bool GPIOControllerPTH10X::ChargerEnable(const bool /* enable */) throw std::runtime_error("This robot version does not support this functionality."); } -std::unordered_map -GPIOControllerPTH10X::QueryControlInterfaceIOStates() const +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_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..5b7a21127 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,32 +226,31 @@ 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; default: diff --git a/panther_hardware_interfaces/test/test_gpio_controller.cpp b/panther_hardware_interfaces/test/test_gpio_controller.cpp index fdb085650..70f04d89f 100644 --- a/panther_hardware_interfaces/test/test_gpio_controller.cpp +++ b/panther_hardware_interfaces/test/test_gpio_controller.cpp @@ -19,21 +19,21 @@ #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}, +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}, }; class GPIOControllerWrapper : public panther_hardware_interfaces::GPIOControllerPTH12X @@ -80,8 +80,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 +89,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 +117,33 @@ 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, 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_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( From 906eea8f33a357680f922b550258c774cd58809f Mon Sep 17 00:00:00 2001 From: pawelirh Date: Mon, 10 Jun 2024 08:46:32 +0000 Subject: [PATCH 03/14] Fix test compiling --- panther_hardware_interfaces/CMakeLists.txt | 25 +++++++++++-------- .../test/test_gpio_controller.cpp | 3 +++ 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index 71944ff39..4a27d1f0e 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -20,26 +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 + 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) @@ -167,14 +167,16 @@ 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_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) - target_link_libraries(${PROJECT_NAME}_test_gpiod_controller PkgConfig::LIBGPIOD) + target_link_libraries(${PROJECT_NAME}_test_gpiod_controller + PkgConfig::LIBGPIOD) ament_add_gtest( ${PROJECT_NAME}_test_panther_system_ros_interface @@ -182,7 +184,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) @@ -195,7 +198,7 @@ if(BUILD_TESTING) realtime_tools std_srvs) target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface - PkgConfig::LIBLELY_COAPP 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/test/test_gpio_controller.cpp b/panther_hardware_interfaces/test/test_gpio_controller.cpp index 70f04d89f..975388f8e 100644 --- a/panther_hardware_interfaces/test/test_gpio_controller.cpp +++ b/panther_hardware_interfaces/test/test_gpio_controller.cpp @@ -19,6 +19,9 @@ #include +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}, From cc076388e6f94fa0a02a252b9546aa51150595cf Mon Sep 17 00:00:00 2001 From: pawelirh Date: Mon, 10 Jun 2024 11:42:59 +0000 Subject: [PATCH 04/14] Add LED service in panther_system --- panther_hardware_interfaces/CMakeLists.txt | 9 + panther_hardware_interfaces/CODE_STRUCTURE.md | 5 + .../gpio_controller.hpp | 25 ++- panther_hardware_interfaces/package.xml | 15 +- .../src/gpio_controller.cpp | 10 + .../src/panther_system.cpp | 3 + .../test/test_gpio_driver.cpp | 190 ++++++++++++++++++ 7 files changed, 248 insertions(+), 9 deletions(-) create mode 100644 panther_hardware_interfaces/test/test_gpio_driver.cpp diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index 4a27d1f0e..24e81fded 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -167,6 +167,15 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_motors_controller PkgConfig::LIBLELY_COAPP) + 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) diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/panther_hardware_interfaces/CODE_STRUCTURE.md index 170495d22..2ab1b39ff 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/panther_hardware_interfaces/CODE_STRUCTURE.md @@ -41,6 +41,11 @@ 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, handling reading and writing pins of the RPi GPIO. It includes the following utilities: 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 b615323bc..724a39c5d 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/gpio_controller.hpp @@ -95,13 +95,16 @@ 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; @@ -208,6 +211,14 @@ 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. * @@ -252,6 +263,7 @@ class GPIOControllerPTH12X : public GPIOControllerInterface 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_; @@ -336,6 +348,14 @@ 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. @@ -351,6 +371,7 @@ class GPIOControllerPTH10X : public GPIOControllerInterface 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_hardware_interfaces/package.xml b/panther_hardware_interfaces/package.xml index a2d7bd01a..4d5f7bb60 100644 --- a/panther_hardware_interfaces/package.xml +++ b/panther_hardware_interfaces/package.xml @@ -14,9 +14,17 @@ 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 @@ -34,13 +42,6 @@ tf2_geometry_msgs tf2_ros - autoconf - autoconf-archive - libtool - m4 - pkg-config - pluginlib - ament_cmake diff --git a/panther_hardware_interfaces/src/gpio_controller.cpp b/panther_hardware_interfaces/src/gpio_controller.cpp index 8d8f9ca7c..61f4b079c 100644 --- a/panther_hardware_interfaces/src/gpio_controller.cpp +++ b/panther_hardware_interfaces/src/gpio_controller.cpp @@ -175,6 +175,11 @@ bool GPIOControllerPTH12X::ChargerEnable(const bool enable) return gpio_driver_->SetPinValue(GPIOPin::CHRG_DISABLE, !enable); } +bool GPIOControllerPTH12X::LEDControlEnable(const bool enable) +{ + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); +} + std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const { std::unordered_map io_state; @@ -263,6 +268,11 @@ bool GPIOControllerPTH10X::ChargerEnable(const bool /* enable */) throw std::runtime_error("This robot version does not support this functionality."); } +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; diff --git a/panther_hardware_interfaces/src/panther_system.cpp b/panther_hardware_interfaces/src/panther_system.cpp index 98e13d1c4..812f31e55 100644 --- a/panther_hardware_interfaces/src/panther_system.cpp +++ b/panther_hardware_interfaces/src/panther_system.cpp @@ -126,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/test/test_gpio_driver.cpp b/panther_hardware_interfaces/test/test_gpio_driver.cpp new file mode 100644 index 000000000..5ad0b8336 --- /dev/null +++ b/panther_hardware_interfaces/test/test_gpio_driver.cpp @@ -0,0 +1,190 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "gpiod.hpp" +#include "gtest/gtest.h" + +#include "panther_hardware_interfaces/gpio_driver.hpp" +#include "panther_utils/test/test_utils.hpp" + +using GPIOInfo = panther_hardware_interfaces::GPIOInfo; +using GPIOPin = panther_hardware_interfaces::GPIOPin; + +class TestGPIODriver : public ::testing::Test +{ +public: + TestGPIODriver() = default; + virtual ~TestGPIODriver() = default; + + void GPIOEventCallback(const GPIOInfo & gpio_info); + +protected: + void SetUp() override; + void TearDown() override; + void SetAndVerifyPinState(const GPIOPin & pin); + + 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}, + }; +}; + +void TestGPIODriver::SetUp() +{ + gpio_driver_ = std::make_unique(gpio_config_info_); + + last_gpio_event_summary_.first = static_cast(-1); + last_gpio_event_summary_.second = static_cast(-1); +} + +void TestGPIODriver::TearDown() { gpio_driver_.reset(); } + +void TestGPIODriver::GPIOEventCallback(const GPIOInfo & gpio_info) +{ + last_gpio_event_summary_.first = gpio_info.pin; + last_gpio_event_summary_.second = gpio_info.value; +} + +void TestGPIODriver::SetAndVerifyPinState(const GPIOPin & pin) +{ + EXPECT_TRUE(gpio_driver_->SetPinValue(pin, true)); + EXPECT_TRUE(gpio_driver_->IsPinActive(pin)); + + EXPECT_TRUE(gpio_driver_->SetPinValue(pin, false)); + EXPECT_FALSE(gpio_driver_->IsPinActive(pin)); +} + +TEST(TestGPIODriverInitialization, EmptyInfoStorage) +{ + EXPECT_THROW( + { + auto gpio_driver = + std::make_unique(std::vector{}); + }, + std::runtime_error); +} + +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( + std::vector{{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}}); + gpio_driver.reset(); + + gpio_driver = std::make_unique( + std::vector{{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}}); + }, + std::invalid_argument); +} + +TEST_F(TestGPIODriver, SetWrongPinValue) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { this->gpio_driver_->SetPinValue(static_cast(-1), true); }, + "Pin not found in GPIO info storage.")); +} + +TEST_F(TestGPIODriver, IsPinAvailable) +{ + EXPECT_TRUE(this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL)); + EXPECT_FALSE(this->gpio_driver_->IsPinAvailable(static_cast(-1))); +} + +TEST_F(TestGPIODriver, GPIOMonitorEnableNoRT) +{ + this->gpio_driver_->GPIOMonitorEnable(); + + SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); +} + +TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT) +{ + // Redirect std::cerr warning to a stringstream + std::stringstream buffer; + std::streambuf * prev_cerr_buf = std::cerr.rdbuf(buffer.rdbuf()); + + this->gpio_driver_->GPIOMonitorEnable(true); + + std::string captured_scheduling_rt_policy_waring = buffer.str(); + + // Restore std::cerr to its previous buffer + std::cerr.rdbuf(prev_cerr_buf); + + EXPECT_EQ(captured_scheduling_rt_policy_waring, ""); + + SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); +} + +TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { + this->gpio_driver_->ConfigureEdgeEventCallback( + std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); + }, + "GPIO monitor thread is not running!")); +} + +TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState) +{ + auto tested_pin = GPIOPin::LED_SBC_SEL; + + this->gpio_driver_->GPIOMonitorEnable(); + this->gpio_driver_->ConfigureEdgeEventCallback( + std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); + + EXPECT_TRUE(this->gpio_driver_->SetPinValue(tested_pin, true)); + EXPECT_TRUE(this->gpio_driver_->IsPinActive(tested_pin)); + + EXPECT_EQ(this->last_gpio_event_summary_.first, tested_pin); + EXPECT_EQ(this->last_gpio_event_summary_.second, gpiod::line::value::ACTIVE); + + EXPECT_TRUE(this->gpio_driver_->SetPinValue(tested_pin, false)); + EXPECT_FALSE(this->gpio_driver_->IsPinActive(tested_pin)); + + EXPECT_EQ(this->last_gpio_event_summary_.first, tested_pin); + EXPECT_EQ(this->last_gpio_event_summary_.second, gpiod::line::value::INACTIVE); +} + +TEST_F(TestGPIODriver, ChangePinDirection) +{ + this->gpio_driver_->GPIOMonitorEnable(); + this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); }, + "Cannot set value for INPUT pin.")); + + this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT); + + SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From bff2fb7d5948c4acb6c074ec89115bde0b403a2a Mon Sep 17 00:00:00 2001 From: pawelirh Date: Mon, 10 Jun 2024 14:20:08 +0000 Subject: [PATCH 05/14] Implement changes in driver node --- .../launch/controller.launch.py | 1 + panther_lights/CMakeLists.txt | 10 +-- .../include/panther_lights/driver_node.hpp | 18 ++++- panther_lights/package.xml | 2 +- panther_lights/src/driver_node.cpp | 71 ++++++++++--------- 5 files changed, 61 insertions(+), 41 deletions(-) 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_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/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index bc76a2481..7e4e65acc 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; /** @@ -54,12 +56,20 @@ class DriverNode : public rclcpp::Node int num_led_; double frame_timeout_; bool panels_initialised_ = false; + rclcpp::Time chanel_1_ts_; rclcpp::Time chanel_2_ts_; private: void OnShutdown(); + /** + * @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 a message with new frame is received. * @@ -76,16 +86,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 106b69a89..20077784b 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..3b1ee4c0f 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 @@ -67,10 +66,14 @@ void DriverNode::Initialize(const std::shared_ptrget_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(); + enable_led_control_client_ = this->create_client( + "hardware/led_control_enable", rmw_qos_profile_services_default); + + if (!enable_led_control_client_->wait_for_service(std::chrono::seconds(1))) { + throw std::runtime_error( + "Timeout occurred while waiting for service '" + + std::string(enable_led_control_client_->get_service_name()) + "'!"); + } chanel_1_ts_ = this->get_clock()->now(); chanel_2_ts_ = this->get_clock()->now(); @@ -104,10 +107,36 @@ void DriverNode::OnShutdown() // Give back control over LEDs if (panels_initialised_) { - SetPowerPin(false); + ToggleLEDControl(false); + } +} + +void DriverNode::ToggleLEDControl(const bool enable) +{ + auto service_name = std::string(enable_led_control_client_->get_service_name()); + std::string enable_str = enable ? "true" : "false"; + + auto request = std::make_shared(); + request->data = enable; + + auto result = enable_led_control_client_->async_send_request(request); + RCLCPP_DEBUG(get_logger(), "Sent request on '%s' service.", service_name.c_str()); + + auto status = result.wait_for(std::chrono::seconds(1)); + + if (status != std::future_status::ready) { + throw std::runtime_error("Timeout on '" + service_name + "' - service didn't response!"); } - gpio_driver_.reset(); + if (result.get()->success) { + RCLCPP_INFO( + this->get_logger(), "Service '%s' call is successful! Toggled LED control to '%s'.", + service_name.c_str(), enable_str.c_str()); + } else { + RCLCPP_WARN( + this->get_logger(), "Service '%s' call is not successful: %s", service_name.c_str(), + result.get()->message.c_str()); + } } void DriverNode::FrameCB( @@ -145,19 +174,13 @@ void DriverNode::FrameCB( } if (!panels_initialised_) { - // Take control over LEDs - SetPowerPin(true); + ToggleLEDControl(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 +205,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."}; if (!panels_initialised_) { 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); } - status.values = key_values; status.summary(error_level, message); } From af4f8035c40e39e9a4bcbc38cb4a3253dfc9626f Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 11 Jun 2024 14:20:12 +0000 Subject: [PATCH 06/14] Redesign initialization --- .../include/panther_lights/driver_node.hpp | 13 ++- panther_lights/src/driver_node.cpp | 94 ++++++++++--------- panther_lights/test/test_driver_node.cpp | 2 +- 3 files changed, 60 insertions(+), 49 deletions(-) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index 7e4e65acc..df9004a81 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -55,7 +55,7 @@ class DriverNode : public rclcpp::Node 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_; @@ -63,13 +63,22 @@ class DriverNode : public rclcpp::Node private: void OnShutdown(); + void ClearLEDs(); + /** - * @brief Toggles LED control on or off. + * @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 the future result of the service call. + */ + void ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future); + /** * @brief Callback to execute when a message with new frame is received. * diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 3b1ee4c0f..448fb02ec 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -52,6 +52,9 @@ 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(); + diagnostic_updater_.setHardwareID("Bumper Lights"); diagnostic_updater_.add("Lights driver status", this, &DriverNode::DiagnoseLights); @@ -63,30 +66,28 @@ void DriverNode::Initialize(const std::shared_ptrget_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(); + chanel_1_.SetGlobalBrightness(global_brightness); + chanel_2_.SetGlobalBrightness(global_brightness); - enable_led_control_client_ = this->create_client( + enable_led_control_client_ = this->create_client( "hardware/led_control_enable", rmw_qos_profile_services_default); - if (!enable_led_control_client_->wait_for_service(std::chrono::seconds(1))) { + 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()) + "'!"); } - chanel_1_ts_ = this->get_clock()->now(); - chanel_2_ts_ = this->get_clock()->now(); - - chanel_1_.SetGlobalBrightness(global_brightness); - chanel_2_.SetGlobalBrightness(global_brightness); + ToggleLEDControl(true); + chanel_1_ts_ = this->get_clock()->now(); 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"); @@ -101,41 +102,49 @@ void DriverNode::Initialize(const std::shared_ptr(num_led_ * 4, 0)); - chanel_2_.SetPanel(std::vector(num_led_ * 4, 0)); + ClearLEDs(); - // Give back control over LEDs - if (panels_initialised_) { + if (led_control_granted_) { ToggleLEDControl(false); } } -void DriverNode::ToggleLEDControl(const bool enable) +void DriverNode::ClearLEDs() { - auto service_name = std::string(enable_led_control_client_->get_service_name()); - std::string enable_str = enable ? "true" : "false"; + 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; - auto result = enable_led_control_client_->async_send_request(request); - RCLCPP_DEBUG(get_logger(), "Sent request on '%s' service.", service_name.c_str()); + auto future_result = enable_led_control_client_->async_send_request( + request, std::bind(&DriverNode::ToggleLEDControlCB, this, std::placeholders::_1)); - auto status = result.wait_for(std::chrono::seconds(1)); + RCLCPP_DEBUG( + this->get_logger(), "Sent request toggling LED control to '%s'.", enable ? "true" : "false"); +} - if (status != std::future_status::ready) { - throw std::runtime_error("Timeout on '" + service_name + "' - service didn't response!"); - } +void DriverNode::ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future) +{ + RCLCPP_DEBUG(this->get_logger(), "Received response after toggling LED control."); + + auto result = future.get(); + + auto request = result.first; + auto response = result.second; - if (result.get()->success) { - RCLCPP_INFO( - this->get_logger(), "Service '%s' call is successful! Toggled LED control to '%s'.", - service_name.c_str(), enable_str.c_str()); + if (request->data == true && response->success) { + led_control_granted_ = true; + ClearLEDs(); + RCLCPP_INFO(this->get_logger(), "LED control granted."); + } else if (request->data == false && response->success) { + led_control_granted_ = false; + RCLCPP_INFO(this->get_logger(), "LED control revoked."); } else { - RCLCPP_WARN( - this->get_logger(), "Service '%s' call is not successful: %s", service_name.c_str(), - result.get()->message.c_str()); + RCLCPP_WARN(this->get_logger(), "Failed to toggle LED control."); } } @@ -144,7 +153,10 @@ void DriverNode::FrameCB( const rclcpp::Time & last_time, const std::string & panel_name) { std::string message; - if ( + + if (!led_control_granted_) { + message = "LED control not granted, ignoring frame"; + } else if ( (this->get_clock()->now() - rclcpp::Time(msg->header.stamp)) > rclcpp::Duration::from_seconds(frame_timeout_)) { message = "Timeout exceeded, ignoring frame"; @@ -159,13 +171,8 @@ void DriverNode::FrameCB( } if (!message.empty()) { - if (panel_name == "channel_1") { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); - } else if (panel_name == "channel_2") { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); - } + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); auto warn_msg = message + " on " + panel_name + "!"; diagnostic_updater_.broadcast(diagnostic_msgs::msg::DiagnosticStatus::WARN, warn_msg); @@ -173,11 +180,6 @@ void DriverNode::FrameCB( return; } - if (!panels_initialised_) { - ToggleLEDControl(true); - panels_initialised_ = true; - } - panel.SetPanel(msg->data); } @@ -206,11 +208,11 @@ void DriverNode::SetBrightnessCB( void DriverNode::DiagnoseLights(diagnostic_updater::DiagnosticStatusWrapper & status) { 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."; + message = "Control over LEDs not granted, initialization failed!"; } status.summary(error_level, message); diff --git a/panther_lights/test/test_driver_node.cpp b/panther_lights/test/test_driver_node.cpp index a3821b619..202653987 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; } }; From 51639f25e1f1417a515e0624ba62078f4cb62524 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 11 Jun 2024 14:20:34 +0000 Subject: [PATCH 07/14] Update documentation panther_lights and panther_hardware_interfaces --- panther_hardware_interfaces/README.md | 21 ++++++++++++++++-- panther_lights/README.md | 31 +++++++++++++++++---------- 2 files changed, 39 insertions(+), 13 deletions(-) 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_lights/README.md b/panther_lights/README.md index a8a2eb26e..c28900845 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*]: allows setting animation on Bumper Lights based on animation ID. + +[//]: # (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). From 00499a451496d3590a483350ca3466b922f2fc4c Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 11 Jun 2024 14:21:21 +0000 Subject: [PATCH 08/14] Remove panther_gpiod package --- panther_gpiod/CHANGELOG.rst | 286 ---------------- panther_gpiod/CMakeLists.txt | 71 ---- panther_gpiod/README.md | 40 --- panther_gpiod/cmake/SuperBuild.cmake | 41 --- .../include/panther_gpiod/gpio_driver.hpp | 307 ----------------- panther_gpiod/package.xml | 31 -- panther_gpiod/src/gpio_driver.cpp | 311 ------------------ panther_gpiod/test/test_gpio_driver.cpp | 186 ----------- 8 files changed, 1273 deletions(-) delete mode 100644 panther_gpiod/CHANGELOG.rst delete mode 100644 panther_gpiod/CMakeLists.txt delete mode 100644 panther_gpiod/README.md delete mode 100644 panther_gpiod/cmake/SuperBuild.cmake delete mode 100644 panther_gpiod/include/panther_gpiod/gpio_driver.hpp delete mode 100644 panther_gpiod/package.xml delete mode 100644 panther_gpiod/src/gpio_driver.cpp delete mode 100644 panther_gpiod/test/test_gpio_driver.cpp diff --git a/panther_gpiod/CHANGELOG.rst b/panther_gpiod/CHANGELOG.rst deleted file mode 100644 index c0a97cd93..000000000 --- a/panther_gpiod/CHANGELOG.rst +++ /dev/null @@ -1,286 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package panther_gpiod -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -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/include/panther_gpiod/gpio_driver.hpp b/panther_gpiod/include/panther_gpiod/gpio_driver.hpp deleted file mode 100644 index 22cbde9e1..000000000 --- a/panther_gpiod/include/panther_gpiod/gpio_driver.hpp +++ /dev/null @@ -1,307 +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. - -/** - * @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_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpiod.hpp" - -namespace panther_gpiod -{ - -/** - * @brief Enumeration representing available GPIO pins in the Panther system. - */ -enum class GPIOPin { - AUX_PW_EN, - CHRG_DISABLE, - CHRG_SENSE, - DRIVER_EN, - E_STOP_RESET, - FAN_SW, - GPOUT1, - GPOUT2, - GPIN1, - GPIN2, - LED_SBC_SEL, - SHDN_INIT, - STAGE2_INPUT, - VDIG_OFF, - VMOT_ON, - MOTOR_ON, - WATCHDOG -}; - -/** - * @brief Structure containing information related to GPIO pins such as pin configuration, - * direction, value, etc. This information is required during the initialization process. - */ -struct GPIOInfo -{ - GPIOPin pin; - gpiod::line::direction direction; - bool active_low = false; - gpiod::line::value init_value = gpiod::line::value::INACTIVE; - gpiod::line::value value = gpiod::line::value::INACTIVE; - gpiod::line::offset offset = -1; -}; - -/** - * @brief Class responsible for managing GPIO pins on Panther robots, handling tasks such as - * setting pin values, changing pin directions, monitoring pin events, and more. - */ -class GPIODriver -{ -public: - /** - * @brief Constructs the GPIODriver object with information about GPIO pin configurations. - * This information is necessary for initializing the GPIO functionality. - * - * @param gpio_info_storage Vector containing information about GPIO pin configurations. - * - * @throws std::runtime_error if the provided `gpio_info_storage` vector is empty. - * - * @note To enable reading pin values, it is required to enable GPIO monitoring. - * See GPIOMonitorEnable method for more info. - * - * @par Example - * An example of constructing the GPIODriver object by providing GPIO pin information: - * @code{.cpp} - * std::vector gpio_configurations = { - * GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT}, - * GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - * GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true, - * gpiod::line::value::ACTIVE} - * // ... additional GPIO pin configurations - * }; - * GPIODriver gpio_driver(gpio_configurations); - * @endcode - */ - GPIODriver(std::vector gpio_info_storage); - - /** - * @brief The destructor sets the GPIO pin values back to their initial values to ensure proper - * cleanup. It then releases the line request and turns off the GPIO monitoring thread. - */ - ~GPIODriver(); - - /** - * @brief Enables GPIO pin state monitoring asynchronously. Optionally, configure the - * Real-Time (RT) FIFO scheduling policy for the monitor thread. - * @details When called, this method starts the GPIO state monitoring thread asynchronously. - * If `use_rt` is set to `true`, it configures the FIFO RT scheduling policy for the monitor - * thread with the specified `gpio_monit_thread_sched_priority`. The default priority is 60. - * - * @param use_rt Whether to configure RT FIFO scheduling policy for the monitor thread. - * Default is set to false. - * @param gpio_monit_thread_sched_priority Priority for the GPIO monitoring thread. - * Set within the range of 0-99 to enable and configure the FIFO RT scheduling - * policy for the monitor thread. This parameter is considered only if `use_rt` is set to - * true. The default priority is 60. - * - * @note Calling `GPIOMonitorEnable` is optional after constructing the driver object. It allows - * asynchronous monitoring of GPIO pin states. Not invoking this method will result in the - * lack of functionality to read pin values. - */ - void GPIOMonitorEnable( - const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60); - - /** - * @brief This method sets the provided callback function to be executed upon GPIO edge events. - * - * @param callback The callback function to handle GPIO edge events. - * - * @throws std::runtime_error if GPIO monitor thread is not running. - * - * @par Example - * An example of using this method to bind a member function as a callback: - * @code{.cpp} - * class MyClass { - * public: - * void HandleGPIOEvent(const GPIOInfo & gpio_info) { - * // Handle GPIO event here, i.e: - * std::cout << gpio_info.offset << ": " << gpio_info.value << std::endl; - * } - * }; - * - * MyClass my_obj; - * GPIODriver gpio_driver; - * gpio_driver.GPIOMonitorEnable(true, 50); - * gpio_driver.ConfigureEdgeEventCallback( - * std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1)); - * @endcode - */ - void ConfigureEdgeEventCallback(const std::function & callback); - - /** - * @brief Changes the direction of a specific GPIO pin. - * - * @param pin GPIOPin to change the direction for. - * @param direction New direction for the pin. - */ - void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction); - - /** - * @brief Returns true if a specific pin is configured and stored in GPIO info storage - * - * @param pin The GPIO pin to check availability for - * @return true if the pin is available, false otherwise - */ - bool IsPinAvailable(const GPIOPin pin) const; - - /** - * @brief Checks if a specific GPIO pin is active. This method returns the value stored in the - * class read during the last edge event. - * - * @param pin GPIOPin to check. - * - * @throws std::runtime_error if GPIO monitor thread is not running. - * - * @return True if the pin is active, false otherwise. - */ - bool IsPinActive(const GPIOPin pin); - - /** - * @brief Sets the value for a specific GPIO pin. - * - * @param pin GPIOPin to set the value for. - * @param value The boolean value to set for the pin. - * - * @throws std::invalid_argument if trying to set the value for an INPUT pin. - * @throws std::runtime_error if changing the GPIO state fails. - * - * @return true if the pin value is successfully set, false otherwise. - */ - bool SetPinValue(const GPIOPin pin, const bool value); - -private: - std::unique_ptr CreateLineRequest(gpiod::chip & chip); - gpiod::line_settings GenerateLineSettings(const GPIOInfo & pin_info); - GPIOPin GetPinFromOffset(const gpiod::line::offset & offset) const; - GPIOInfo & GetGPIOInfoRef(const GPIOPin pin); - void ConfigureLineRequest( - gpiod::chip & chip, gpiod::request_builder & builder, GPIOInfo & gpio_info); - void MonitorAsyncEvents(); - void HandleEdgeEvent(const gpiod::edge_event & event); - bool IsGPIOMonitorThreadRunning() const; - - /** - * @brief Enables asynchronous monitoring of GPIO pin events. - * - * @throws std::runtime_error if a timeout occurs while waiting for the GPIO monitor thread. - */ - void GPIOMonitorOn(); - - /** - * @brief Disables asynchronous monitoring of GPIO pin events. - */ - void GPIOMonitorOff(); - - /** - * @brief Callback function for GPIO edge events. - * - * @param gpio_info Information related to the state of the GPIO pin for which the event took - * place. - */ - std::function GPIOEdgeEventCallback; - - /** - * @brief Mapping of GPIO pins to their respective names. - */ - const std::map pin_names_{ - {GPIOPin::WATCHDOG, "WATCHDOG"}, - {GPIOPin::AUX_PW_EN, "AUX_PW_EN"}, - {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"}, - {GPIOPin::CHRG_SENSE, "CHRG_SENSE"}, - {GPIOPin::DRIVER_EN, "DRIVER_EN"}, - {GPIOPin::E_STOP_RESET, "E_STOP_RESET"}, - {GPIOPin::FAN_SW, "FAN_SW"}, - {GPIOPin::GPOUT1, "GPOUT1"}, - {GPIOPin::GPOUT2, "GPOUT2"}, - {GPIOPin::GPIN1, "GPIN1"}, - {GPIOPin::GPIN2, "GPIN2"}, - {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"}, - {GPIOPin::SHDN_INIT, "SHDN_INIT"}, - {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"}, - {GPIOPin::VDIG_OFF, "VDIG_OFF"}, - {GPIOPin::VMOT_ON, "VMOT_ON"}, - {GPIOPin::MOTOR_ON, "MOTOR_ON"}, - }; - - /** - * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. - */ - std::vector gpio_info_storage_; - - /** - * @brief Mutex for managing access to GPIO pin information. - */ - mutable std::shared_mutex gpio_info_storage_mutex_; - - /** - * @brief Request object for controlling GPIO lines. - */ - std::unique_ptr line_request_; - - /** - * @brief Thread object for monitoring GPIO events asynchronously. - */ - std::unique_ptr gpio_monitor_thread_; - - /** - * @brief When enabled, the GPIODriver object will initiate the GPIO monitor - * thread with real-time scheduling if a valid priority is provided. - */ - bool use_rt_; - - /** - * @brief Priority for the GPIO monitoring thread. The value ranges from 0 to 99, inclusive, - * and is used to configure the FIFO real-time scheduling policy for the monitor thread - * if real-time monitoring is enabled. - */ - unsigned gpio_monit_thread_sched_priority_; - std::atomic_bool gpio_monitor_thread_enabled_ = false; - std::condition_variable monitor_init_cond_var_; - std::mutex monitor_init_mtx_; - static constexpr unsigned gpio_debounce_period_ = 10; - static constexpr unsigned edge_event_buffer_size_ = 2; - const std::filesystem::path gpio_chip_path_ = "/dev/gpiochip0"; -}; - -} // namespace panther_gpiod - -#endif // PANTHER_GPIOD_GPIO_DRIVER_HPP_ diff --git a/panther_gpiod/package.xml b/panther_gpiod/package.xml deleted file mode 100644 index a5fbdf541..000000000 --- a/panther_gpiod/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - panther_gpiod - 2.0.2 - 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_gpiod/src/gpio_driver.cpp b/panther_gpiod/src/gpio_driver.cpp deleted file mode 100644 index b72d1923e..000000000 --- a/panther_gpiod/src/gpio_driver.cpp +++ /dev/null @@ -1,311 +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 "panther_gpiod/gpio_driver.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpiod.hpp" - -#include "panther_utils/configure_rt.hpp" - -namespace panther_gpiod -{ - -GPIODriver::GPIODriver(std::vector gpio_info_storage) -: gpio_info_storage_(std::move(gpio_info_storage)) -{ - if (gpio_info_storage_.empty()) { - throw std::runtime_error("The GPIO information vector is empty."); - } - - auto gpio_chip = gpiod::chip(gpio_chip_path_); - line_request_ = CreateLineRequest(gpio_chip); -} - -GPIODriver::~GPIODriver() -{ - for (GPIOInfo & gpio_info : gpio_info_storage_) { - if (gpio_info.direction == gpiod::line::direction::OUTPUT) { - line_request_->set_value(gpio_info.offset, gpio_info.init_value); - } - } - - GPIOMonitorOff(); - line_request_->release(); -} - -void GPIODriver::GPIOMonitorEnable( - const bool use_rt, const unsigned gpio_monit_thread_sched_priority) -{ - use_rt_ = use_rt; - gpio_monit_thread_sched_priority_ = gpio_monit_thread_sched_priority; - - GPIOMonitorOn(); -} - -void GPIODriver::ConfigureEdgeEventCallback(const std::function & callback) -{ - if (!IsGPIOMonitorThreadRunning()) { - throw std::runtime_error("GPIO monitor thread is not running!"); - } - - GPIOEdgeEventCallback = callback; -} - -std::unique_ptr GPIODriver::CreateLineRequest(gpiod::chip & chip) -{ - auto request_builder = chip.prepare_request(); - request_builder.set_consumer("panther_gpiod"); - - for (GPIOInfo & gpio_info : gpio_info_storage_) { - ConfigureLineRequest(chip, request_builder, gpio_info); - } - - return std::make_unique(request_builder.do_request()); -} - -void GPIODriver::ConfigureLineRequest( - gpiod::chip & chip, gpiod::request_builder & builder, GPIOInfo & gpio_info) -{ - gpiod::line_settings settings = GenerateLineSettings(gpio_info); - - std::string pin_name; - try { - pin_name = pin_names_.at(gpio_info.pin); - } catch (const std::out_of_range & e) { - throw std::runtime_error("No name defined for one of pins: " + std::string(e.what())); - } - - gpiod::line::offset offset = chip.get_line_offset_from_name(pin_name); - - builder.add_line_settings(offset, settings); - gpio_info.offset = offset; -} - -gpiod::line_settings GPIODriver::GenerateLineSettings(const GPIOInfo & gpio_info) -{ - auto settings = gpiod::line_settings(); - settings.set_direction(gpio_info.direction); - settings.set_active_low(gpio_info.active_low); - - // Set the initial value only when the line is configured for the first time; - // otherwise, set the last known value - gpiod::line::value new_output_value = line_request_ ? gpio_info.value : gpio_info.init_value; - settings.set_output_value(new_output_value); - - if (gpio_info.direction == gpiod::line::direction::INPUT) { - settings.set_edge_detection(gpiod::line::edge::BOTH); - settings.set_debounce_period(std::chrono::milliseconds(gpio_debounce_period_)); - } - - return settings; -} - -void GPIODriver::ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) -{ - std::lock_guard lock(gpio_info_storage_mutex_); - GPIOInfo & gpio_info = GetGPIOInfoRef(pin); - - if (gpio_info.direction == direction) { - return; - } - - gpio_info.direction = direction; - - auto line_config = gpiod::line_config(); - - for (const auto & gpio_info : gpio_info_storage_) { - gpiod::line_settings settings = GenerateLineSettings(gpio_info); - line_config.add_line_settings(gpio_info.offset, settings); - } - - line_request_->reconfigure_lines(line_config); - gpio_info.value = line_request_->get_value(gpio_info.offset); -} - -bool GPIODriver::IsPinAvailable(const GPIOPin pin) const -{ - return std::any_of(gpio_info_storage_.begin(), gpio_info_storage_.end(), [&](const auto & info) { - return info.pin == pin; - }); -} - -bool GPIODriver::IsPinActive(const GPIOPin pin) -{ - if (!IsGPIOMonitorThreadRunning()) { - throw std::runtime_error("GPIO monitor thread is not running!"); - } - - std::lock_guard lock(gpio_info_storage_mutex_); - const GPIOInfo & pin_info = GetGPIOInfoRef(pin); - return pin_info.value == gpiod::line::value::ACTIVE; -} - -bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) -{ - GPIOInfo & gpio_info = GetGPIOInfoRef(pin); - - if (gpio_info.direction == gpiod::line::direction::INPUT) { - throw std::invalid_argument("Cannot set value for INPUT pin."); - } - - gpiod::line::value gpio_value = value ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE; - - std::lock_guard lock(gpio_info_storage_mutex_); - try { - line_request_->set_value(gpio_info.offset, gpio_value); - - if (line_request_->get_value(gpio_info.offset) != gpio_value) { - throw std::runtime_error("Failed to change GPIO state."); - } - - gpio_info.value = gpio_value; - if (GPIOEdgeEventCallback) { - GPIOEdgeEventCallback(gpio_info); - } - - return true; - } catch (const std::exception & e) { - std::cerr << "An exception ocurred while setting GPIO pin value: " << e.what() << std::endl; - return false; - } -} - -void GPIODriver::GPIOMonitorOn() -{ - if (IsGPIOMonitorThreadRunning()) { - return; - } - - { - std::lock_guard lock(gpio_info_storage_mutex_); - for (auto & info : gpio_info_storage_) { - info.value = line_request_->get_value(info.offset); - } - } - - std::unique_lock lck(monitor_init_mtx_); - - gpio_monitor_thread_enabled_ = true; - gpio_monitor_thread_ = std::make_unique(&GPIODriver::MonitorAsyncEvents, this); - - if ( - monitor_init_cond_var_.wait_for(lck, std::chrono::milliseconds(100)) == - std::cv_status::timeout) { - throw std::runtime_error("Timeout while waiting for GPIO monitor thread."); - } -} - -void GPIODriver::MonitorAsyncEvents() -{ - if (use_rt_) { - panther_utils::ConfigureRT(gpio_monit_thread_sched_priority_); - } - - auto edge_event_buffer = gpiod::edge_event_buffer(edge_event_buffer_size_); - - { - std::lock_guard lck(monitor_init_mtx_); - } - monitor_init_cond_var_.notify_all(); - - while (gpio_monitor_thread_enabled_) { - if (line_request_->wait_edge_events(std::chrono::milliseconds(10))) { - line_request_->read_edge_events(edge_event_buffer); - - for (const auto & event : edge_event_buffer) { - HandleEdgeEvent(event); - } - } - } -} - -void GPIODriver::HandleEdgeEvent(const gpiod::edge_event & event) -{ - std::lock_guard lock(gpio_info_storage_mutex_); - GPIOPin pin; - - try { - pin = GetPinFromOffset(event.line_offset()); - } catch (const std::out_of_range & e) { - std::cerr << "An exception ocurred while handling edge event: " << e.what() << std::endl; - return; - } - - GPIOInfo & gpio_info = GetGPIOInfoRef(pin); - gpiod::line::value new_value; - - if (event.type() == gpiod::edge_event::event_type::RISING_EDGE) { - new_value = gpiod::line::value::ACTIVE; - } else { - new_value = gpiod::line::value::INACTIVE; - } - - gpio_info.value = new_value; - - if (GPIOEdgeEventCallback) { - GPIOEdgeEventCallback(gpio_info); - } -} - -void GPIODriver::GPIOMonitorOff() -{ - gpio_monitor_thread_enabled_ = false; - - if (IsGPIOMonitorThreadRunning()) { - gpio_monitor_thread_->join(); - } -} - -bool GPIODriver::IsGPIOMonitorThreadRunning() const -{ - return gpio_monitor_thread_ && gpio_monitor_thread_->joinable(); -} - -GPIOInfo & GPIODriver::GetGPIOInfoRef(const GPIOPin pin) -{ - for (auto & info : gpio_info_storage_) { - if (info.pin == pin) { - return info; - } - } - - throw std::invalid_argument("Pin not found in GPIO info storage."); -} - -GPIOPin GPIODriver::GetPinFromOffset(const gpiod::line::offset & offset) const -{ - for (const auto & gpio_info : gpio_info_storage_) { - if (gpio_info.offset == offset) { - return gpio_info.pin; - } - } - - throw std::out_of_range( - "Pin with offset " + std::to_string(offset) + " not found in GPIO info storage."); -} - -} // namespace panther_gpiod diff --git a/panther_gpiod/test/test_gpio_driver.cpp b/panther_gpiod/test/test_gpio_driver.cpp deleted file mode 100644 index 7c9755507..000000000 --- a/panther_gpiod/test/test_gpio_driver.cpp +++ /dev/null @@ -1,186 +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 -#include -#include -#include -#include -#include -#include -#include - -#include "gpiod.hpp" -#include "gtest/gtest.h" - -#include "panther_gpiod/gpio_driver.hpp" -#include "panther_utils/test/test_utils.hpp" - -using namespace panther_gpiod; - -class TestGPIODriver : public ::testing::Test -{ -public: - TestGPIODriver() = default; - virtual ~TestGPIODriver() = default; - - void GPIOEventCallback(const GPIOInfo & gpio_info); - -protected: - void SetUp() override; - void TearDown() override; - void SetAndVerifyPinState(const GPIOPin & pin); - - 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}, - }; -}; - -void TestGPIODriver::SetUp() -{ - gpio_driver_ = std::make_unique(gpio_config_info_); - - last_gpio_event_summary_.first = static_cast(-1); - last_gpio_event_summary_.second = static_cast(-1); -} - -void TestGPIODriver::TearDown() { gpio_driver_.reset(); } - -void TestGPIODriver::GPIOEventCallback(const GPIOInfo & gpio_info) -{ - last_gpio_event_summary_.first = gpio_info.pin; - last_gpio_event_summary_.second = gpio_info.value; -} - -void TestGPIODriver::SetAndVerifyPinState(const GPIOPin & pin) -{ - EXPECT_TRUE(gpio_driver_->SetPinValue(pin, true)); - EXPECT_TRUE(gpio_driver_->IsPinActive(pin)); - - EXPECT_TRUE(gpio_driver_->SetPinValue(pin, false)); - EXPECT_FALSE(gpio_driver_->IsPinActive(pin)); -} - -TEST(TestGPIODriverInitialization, EmptyInfoStorage) -{ - EXPECT_THROW( - { auto gpio_driver = std::make_unique(std::vector{}); }, - std::runtime_error); -} - -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( - std::vector{{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}}); - gpio_driver.reset(); - - gpio_driver = std::make_unique( - std::vector{{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}}); - }, - std::invalid_argument); -} - -TEST_F(TestGPIODriver, SetWrongPinValue) -{ - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { this->gpio_driver_->SetPinValue(static_cast(-1), true); }, - "Pin not found in GPIO info storage.")); -} - -TEST_F(TestGPIODriver, IsPinAvailable) -{ - EXPECT_TRUE(this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL)); - EXPECT_FALSE(this->gpio_driver_->IsPinAvailable(static_cast(-1))); -} - -TEST_F(TestGPIODriver, GPIOMonitorEnableNoRT) -{ - this->gpio_driver_->GPIOMonitorEnable(); - - SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); -} - -TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT) -{ - // Redirect std::cerr warning to a stringstream - std::stringstream buffer; - std::streambuf * prev_cerr_buf = std::cerr.rdbuf(buffer.rdbuf()); - - this->gpio_driver_->GPIOMonitorEnable(true); - - std::string captured_scheduling_rt_policy_waring = buffer.str(); - - // Restore std::cerr to its previous buffer - std::cerr.rdbuf(prev_cerr_buf); - - EXPECT_EQ(captured_scheduling_rt_policy_waring, ""); - - SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); -} - -TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread) -{ - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { - this->gpio_driver_->ConfigureEdgeEventCallback( - std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); - }, - "GPIO monitor thread is not running!")); -} - -TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState) -{ - auto tested_pin = GPIOPin::LED_SBC_SEL; - - this->gpio_driver_->GPIOMonitorEnable(); - this->gpio_driver_->ConfigureEdgeEventCallback( - std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); - - EXPECT_TRUE(this->gpio_driver_->SetPinValue(tested_pin, true)); - EXPECT_TRUE(this->gpio_driver_->IsPinActive(tested_pin)); - - EXPECT_EQ(this->last_gpio_event_summary_.first, tested_pin); - EXPECT_EQ(this->last_gpio_event_summary_.second, gpiod::line::value::ACTIVE); - - EXPECT_TRUE(this->gpio_driver_->SetPinValue(tested_pin, false)); - EXPECT_FALSE(this->gpio_driver_->IsPinActive(tested_pin)); - - EXPECT_EQ(this->last_gpio_event_summary_.first, tested_pin); - EXPECT_EQ(this->last_gpio_event_summary_.second, gpiod::line::value::INACTIVE); -} - -TEST_F(TestGPIODriver, ChangePinDirection) -{ - this->gpio_driver_->GPIOMonitorEnable(); - this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT); - - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); }, - "Cannot set value for INPUT pin.")); - - this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT); - - SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 13924e6f83925f5889d45797e752d36a90105c9b Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 12 Jun 2024 14:16:40 +0000 Subject: [PATCH 09/14] Grant hardware access on frame callback --- .../include/panther_lights/driver_node.hpp | 6 +- panther_lights/src/driver_node.cpp | 63 ++++++++++--------- panther_lights/src/driver_node_main.cpp | 6 +- panther_lights/test/test_driver_node.cpp | 2 +- 4 files changed, 40 insertions(+), 37 deletions(-) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index df9004a81..b9bf6b688 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -45,12 +45,12 @@ 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_; diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 448fb02ec..f8c0f9b1a 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -55,16 +55,6 @@ DriverNode::DriverNode(const std::string & node_name, const rclcpp::NodeOptions frame_timeout_ = this->get_parameter("frame_timeout").as_double(); num_led_ = this->get_parameter("num_led").as_int(); - 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) -{ - RCLCPP_INFO(this->get_logger(), "Initializing."); - const float global_brightness = this->get_parameter("global_brightness").as_double(); chanel_1_.SetGlobalBrightness(global_brightness); chanel_2_.SetGlobalBrightness(global_brightness); @@ -72,13 +62,18 @@ void DriverNode::Initialize(const std::shared_ptrcreate_client( "hardware/led_control_enable", rmw_qos_profile_services_default); - 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()) + "'!"); - } + set_brightness_server_ = this->create_service( + "lights/driver/set/brightness", std::bind(&DriverNode::SetBrightnessCB, this, _1, _2)); - ToggleLEDControl(true); + diagnostic_updater_.setHardwareID("Bumper Lights"); + diagnostic_updater_.add("Lights driver status", this, &DriverNode::DiagnoseLights); + + RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); +} + +void DriverNode::InitializeSubscribers(const std::shared_ptr & it) +{ + RCLCPP_INFO(this->get_logger(), "Initializing ImageTransport subscribers."); chanel_1_ts_ = this->get_clock()->now(); chanel_1_sub_ = std::make_shared( @@ -94,10 +89,7 @@ void DriverNode::Initialize(const std::shared_ptrheader.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() @@ -120,7 +112,13 @@ void DriverNode::ToggleLEDControl(const bool enable) auto request = std::make_shared(); request->data = enable; - auto future_result = enable_led_control_client_->async_send_request( + 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()) + "'!"); + } + + enable_led_control_client_->async_send_request( request, std::bind(&DriverNode::ToggleLEDControlCB, this, std::placeholders::_1)); RCLCPP_DEBUG( @@ -139,12 +137,13 @@ void DriverNode::ToggleLEDControlCB(rclcpp::Client::SharedFutureWith if (request->data == true && response->success) { led_control_granted_ = true; ClearLEDs(); - RCLCPP_INFO(this->get_logger(), "LED control granted."); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "LED control granted."); } else if (request->data == false && response->success) { led_control_granted_ = false; - RCLCPP_INFO(this->get_logger(), "LED control revoked."); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "LED control revoked."); } else { - RCLCPP_WARN(this->get_logger(), "Failed to toggle LED control."); + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Failed to toggle LED control."); } } @@ -152,11 +151,17 @@ void DriverNode::FrameCB( const ImageMsg::ConstSharedPtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, const std::string & panel_name) { - std::string message; - if (!led_control_granted_) { - message = "LED control not granted, ignoring frame"; - } else if ( + 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)) > rclcpp::Duration::from_seconds(frame_timeout_)) { message = "Timeout exceeded, ignoring frame"; @@ -212,7 +217,7 @@ void DriverNode::DiagnoseLights(diagnostic_updater::DiagnosticStatusWrapper & st if (!led_control_granted_) { error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - message = "Control over LEDs not granted, initialization failed!"; + message = "Control over LEDs not granted, driver is not functional!"; } 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 202653987..5cf5c89fa 100644 --- a/panther_lights/test/test_driver_node.cpp +++ b/panther_lights/test/test_driver_node.cpp @@ -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(); } From 8819ae0c6bc5a524bb0c6786232f7744ae736008 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 12 Jun 2024 14:25:06 +0000 Subject: [PATCH 10/14] Improve description in header --- panther_lights/include/panther_lights/driver_node.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/panther_lights/include/panther_lights/driver_node.hpp b/panther_lights/include/panther_lights/driver_node.hpp index b9bf6b688..a4fa85cec 100644 --- a/panther_lights/include/panther_lights/driver_node.hpp +++ b/panther_lights/include/panther_lights/driver_node.hpp @@ -63,6 +63,9 @@ class DriverNode : public rclcpp::Node private: void OnShutdown(); + /** + * @brief Clears all LEDs on both channels. + */ void ClearLEDs(); /** @@ -75,7 +78,7 @@ class DriverNode : public rclcpp::Node /** * @brief Callback to execute when service invoked to toggle LED control returns response. * - * @param future Future object with request and the future result of the service call. + * @param future Future object with request and response of the service call. */ void ToggleLEDControlCB(rclcpp::Client::SharedFutureWithRequest future); From 5c059bfb0c9d62cc42a749b9e8ced1af8ed8e446 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 12 Jun 2024 15:23:22 +0000 Subject: [PATCH 11/14] Update GPIO controller tests --- .../test/test_gpio_controller.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/panther_hardware_interfaces/test/test_gpio_controller.cpp b/panther_hardware_interfaces/test/test_gpio_controller.cpp index 975388f8e..fe5990eac 100644 --- a/panther_hardware_interfaces/test/test_gpio_controller.cpp +++ b/panther_hardware_interfaces/test/test_gpio_controller.cpp @@ -37,6 +37,7 @@ const std::vector gpio_config_info_storage{ 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 @@ -144,6 +145,15 @@ TEST_F(TestGPIOController, TestChargerEnable) 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 = From f75c437938767130d1bd14a306aad610fc132230 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Irzyk?= <108666440+pawelirh@users.noreply.github.com> Date: Fri, 14 Jun 2024 10:15:04 +0200 Subject: [PATCH 12/14] Update panther_hardware_interfaces/CODE_STRUCTURE.md Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_hardware_interfaces/CODE_STRUCTURE.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/panther_hardware_interfaces/CODE_STRUCTURE.md index 2ab1b39ff..d7a448d4f 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/panther_hardware_interfaces/CODE_STRUCTURE.md @@ -43,7 +43,7 @@ As they usually are rare and singular occurrences, it is better to filter some o ## GPIODriver -The GPIODriver is a low-level class responsible for direct interaction with the GPIO (General Purpose Input/Output) pins on the Raspberry Pi +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 From 08b938665a654ab2deeff719fcbd464e09846c97 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 18 Jun 2024 09:48:54 +0000 Subject: [PATCH 13/14] Minor changes based on PR review --- .../src/gpio_driver.cpp | 2 +- panther_lights/README.md | 2 +- panther_lights/src/driver_node.cpp | 29 ++++++++++++------- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/panther_hardware_interfaces/src/gpio_driver.cpp b/panther_hardware_interfaces/src/gpio_driver.cpp index 3aa9a0c1f..48ae9a93f 100644 --- a/panther_hardware_interfaces/src/gpio_driver.cpp +++ b/panther_hardware_interfaces/src/gpio_driver.cpp @@ -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); diff --git a/panther_lights/README.md b/panther_lights/README.md index c28900845..e15befd2c 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -55,7 +55,7 @@ This node is responsible for displaying frames on the Husarion Panther robot's B [//]: # (ROS_API_NODE_SERVICE_CLIENTS_START) -- `hardware/led_control_enable` [*std_srvs/SetBool*]: allows setting animation on Bumper Lights based on animation ID. +- `hardware/led_control_enable` [*std_srvs/SetBool*]: makes SBC controlling LEDs. [//]: # (ROS_API_NODE_SERVICE_CLIENTS_END) diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index f8c0f9b1a..75483176a 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -129,21 +129,24 @@ void DriverNode::ToggleLEDControlCB(rclcpp::Client::SharedFutureWith { RCLCPP_DEBUG(this->get_logger(), "Received response after toggling LED control."); - auto result = future.get(); + const auto result = future.get(); - auto request = result.first; - auto response = result.second; + const auto request = result.first; + const auto response = result.second; - if (request->data == true && response->success) { + 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 if (request->data == false && response->success) { + } else { led_control_granted_ = false; RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "LED control revoked."); - } else { - RCLCPP_ERROR_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "Failed to toggle LED control."); } } @@ -176,8 +179,14 @@ void DriverNode::FrameCB( } if (!message.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); + // 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 << "!"); + } else if (panel_name == "channel_2") { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, message << " on " << panel_name << "!"); + } auto warn_msg = message + " on " + panel_name + "!"; diagnostic_updater_.broadcast(diagnostic_msgs::msg::DiagnosticStatus::WARN, warn_msg); From 21e729ba15ebb24f6322ed86c6f2767a4cfa7b97 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 18 Jun 2024 10:55:32 +0000 Subject: [PATCH 14/14] Add publishing led_control io state --- .../src/panther_system_ros_interface.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp index 5b7a21127..5db2e6fd1 100644 --- a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp +++ b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp @@ -253,6 +253,9 @@ bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool p 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; }