Skip to content

Commit

Permalink
Merge branch 'ros2-lynx-devel' into ros2-lynx-sim
Browse files Browse the repository at this point in the history
  • Loading branch information
@rafal-gorecki
rafal-gorecki committed Sep 4, 2024
2 parents e11b9ea + c056820 commit 1d8a416
Show file tree
Hide file tree
Showing 28 changed files with 1,453 additions and 1,018 deletions.
41 changes: 24 additions & 17 deletions panther_hardware_interfaces/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ add_library(
src/panther_imu_sensor/panther_imu_sensor.cpp
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp
src/panther_system/motors_controller/canopen_controller.cpp
src/panther_system/motors_controller/canopen_manager.cpp
src/panther_system/motors_controller/motors_controller.cpp
src/panther_system/motors_controller/roboteq_data_converters.cpp
src/panther_system/motors_controller/roboteq_driver.cpp
Expand Down Expand Up @@ -84,6 +84,8 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
find_package(ros_testing REQUIRED)
find_package(panther_utils REQUIRED)

install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)
Expand Down Expand Up @@ -118,24 +120,24 @@ if(BUILD_TESTING)
PkgConfig::LIBLELY_COAPP)

ament_add_gtest(
${PROJECT_NAME}_test_canopen_controller
test/panther_system/motors_controller/test_canopen_controller.cpp
src/panther_system/motors_controller/canopen_controller.cpp
${PROJECT_NAME}_test_canopen_manager
test/unit/panther_system/motors_controller/test_canopen_manager.cpp
src/panther_system/motors_controller/canopen_manager.cpp
src/panther_system/motors_controller/roboteq_driver.cpp
src/utils.cpp)
target_include_directories(
${PROJECT_NAME}_test_canopen_controller
${PROJECT_NAME}_test_canopen_manager
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp
ament_target_dependencies(${PROJECT_NAME}_test_canopen_manager rclcpp
panther_msgs panther_utils)
target_link_libraries(${PROJECT_NAME}_test_canopen_controller
target_link_libraries(${PROJECT_NAME}_test_canopen_manager
PkgConfig::LIBLELY_COAPP)

ament_add_gtest(
${PROJECT_NAME}_test_roboteq_driver
test/panther_system/motors_controller/test_roboteq_driver.cpp
src/panther_system/motors_controller/canopen_controller.cpp
test/unit/panther_system/motors_controller/test_roboteq_driver.cpp
src/panther_system/motors_controller/canopen_manager.cpp
src/panther_system/motors_controller/roboteq_driver.cpp
src/utils.cpp)
target_include_directories(
Expand All @@ -150,7 +152,7 @@ if(BUILD_TESTING)
ament_add_gtest(
${PROJECT_NAME}_test_motors_controller
test/panther_system/motors_controller/test_motors_controller.cpp
src/panther_system/motors_controller/canopen_controller.cpp
src/panther_system/motors_controller/canopen_manager.cpp
src/panther_system/motors_controller/roboteq_driver.cpp
src/panther_system/motors_controller/roboteq_data_converters.cpp
src/panther_system/motors_controller/motors_controller.cpp
Expand All @@ -164,13 +166,7 @@ if(BUILD_TESTING)
target_link_libraries(${PROJECT_NAME}_test_motors_controller
PkgConfig::LIBLELY_COAPP)

ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
test/panther_system/gpio/test_gpio_driver.cpp)
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(
ament_add_gmock(
${PROJECT_NAME}_test_gpiod_controller
test/panther_system/gpio/test_gpio_controller.cpp
src/panther_system/gpio/gpio_controller.cpp
Expand Down Expand Up @@ -216,6 +212,17 @@ if(BUILD_TESTING)
target_link_libraries(${PROJECT_NAME}_test_panther_system
PkgConfig::LIBLELY_COAPP)

# Integration tests
option(TEST_INTEGRATION "Run integration tests" OFF)
if(TEST_INTEGRATION)
# Hardware integration
ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
test/panther_system/gpio/test_gpio_driver.cpp)
ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME}
PkgConfig::LIBGPIOD)
endif()

endif()

ament_export_include_directories(include)
Expand Down
126 changes: 85 additions & 41 deletions ...re_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,10 @@

#include "gpiod.hpp"

#include "panther_utils/common_utilities.hpp"

#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp"
#include "panther_hardware_interfaces/panther_system/gpio/types.hpp"

namespace panther_hardware_interfaces
{
Expand All @@ -54,7 +57,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<GPIODriver> gpio_driver);
Watchdog(std::shared_ptr<GPIODriverInterface> gpio_driver);

/**
* @brief Destructor for Watchdog class. Turns off the watchdog thread.
Expand Down Expand Up @@ -84,7 +87,7 @@ class Watchdog
void WatchdogThread();

GPIOPin watchdog_pin_ = GPIOPin::WATCHDOG;
std::shared_ptr<GPIODriver> gpio_driver_;
std::shared_ptr<GPIODriverInterface> gpio_driver_;
std::thread watchdog_thread_;
std::atomic_bool watchdog_thread_enabled_ = false;
};
Expand Down Expand Up @@ -139,12 +142,20 @@ class GPIOControllerInterface
bool IsPinAvailable(const GPIOPin pin) const;

protected:
std::shared_ptr<GPIODriver> gpio_driver_;
std::shared_ptr<GPIODriverInterface> gpio_driver_;
};

class GPIOControllerPTH12X : public GPIOControllerInterface
{
public:
/**
* @brief Constructor for GPIOControllerPTH12X class.
*
* @param gpio_driver Pointer to the GPIODriver object.
* @throw `std::runtime_error` When the GPIO driver is not initialized.
*/
GPIOControllerPTH12X(std::shared_ptr<GPIODriverInterface> gpio_driver);

/**
* @brief Initializes the GPIODriver, Watchdog, and powers on the motors.
*/
Expand All @@ -162,17 +173,19 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
* @brief Resets the E-stop.
*
* This method verifies the status of the E_STOP_RESET pin, which is configured as an input.
* If the pin is active, it attempts to reset the E-stop by momentarily setting it to an inactive
* state. During this reset process, the pin is configured as an output for a specific duration.
* If the attempt to reset the E-stop fails (the pin reads its value as an input again), it throws
* a runtime error. The Watchdog thread is temporarily activated during the E-stop reset process.
* If the pin is active, it attempts to reset the E-stop by momentarily setting it to an
* inactive state. During this reset process, the pin is configured as an output for a specific
* duration. If the attempt to reset the E-stop fails (the pin reads its value as an input
* again), it throws a runtime error. The Watchdog thread is temporarily activated during the
* E-stop reset process.
* @return true if the E-stop is successfully reset.
* @exception std::runtime_error when the E-stop reset fails.
*/
void EStopReset() override;

/**
* @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter.
* @brief Controls the motor power by enabling or disabling them based on the 'enable'
* parameter.
*
* @param enable Set to 'true' to enable the motors, 'false' to disable.
* @return 'true' if the motor control pin value is successfully set, 'false' otherwise.
Expand Down Expand Up @@ -204,7 +217,8 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
bool DigitalPowerEnable(const bool enable) override;

/**
* @brief Enables or disables the use of an external charger according to the 'enable' parameter.
* @brief Enables or disables the use of an external charger according to the 'enable'
* parameter.
*
* @param enable Set to 'true' to enable external charger, 'false' to disable.
* @return 'true' if the charger control pin value is successfully set, 'false' otherwise.
Expand All @@ -226,6 +240,11 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
*/
std::unordered_map<GPIOPin, bool> QueryControlInterfaceIOStates() const override;

/**
* @brief Returns the GPIO pin configuration information for the PTH12X.
*/
static const std::vector<GPIOInfo> & GetGPIOConfigInfoStorage();

void InterruptEStopReset() override;

protected:
Expand All @@ -235,9 +254,9 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
/**
* @brief Waits for a specific duration or until an interruption is signaled.
*
* This method is designed to block execution for the specified timeout duration. It also monitors
* for an interruption signal which, if received, will cause the method to return early. The
* interruption is controlled by the `should_abort_e_stop_reset_` flag.
* This method is designed to block execution for the specified timeout duration. It also
* monitors for an interruption signal which, if received, will cause the method to return
* early. The interruption is controlled by the `should_abort_e_stop_reset_` flag.
*
* @param timeout Duration to wait for in milliseconds.
* @return `true` if the wait completed without interruption, `false` if an interruption was
Expand All @@ -246,25 +265,10 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
bool WaitFor(std::chrono::milliseconds timeout);

/**
* @brief Vector containing GPIO pin configuration information such as pin direction, value, etc.
*/
const std::vector<GPIOInfo> gpio_config_info_storage_{
GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT},
GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT},
GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT},
GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT},
GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT},
GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true},
GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true},
};
* @brief Vector containing GPIO pin configuration information such as pin direction, value,
* etc.
*/
static const std::vector<GPIOInfo> gpio_config_info_storage_;

std::mutex e_stop_cv_mtx_;
std::condition_variable e_stop_cv_;
Expand All @@ -274,6 +278,14 @@ class GPIOControllerPTH12X : public GPIOControllerInterface
class GPIOControllerPTH10X : public GPIOControllerInterface
{
public:
/**
* @brief Constructor for GPIOControllerPTH10X class.
*
* @param gpio_driver Pointer to the GPIODriver object.
* @throw `std::runtime_error` When the GPIO driver is not initialized.
*/
GPIOControllerPTH10X(std::shared_ptr<GPIODriverInterface> gpio_driver);

/**
* @brief Initializes the GPIODriver and powers on the motors.
*/
Expand All @@ -297,7 +309,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
void EStopReset() override;

/**
* @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter.
* @brief Controls the motor power by enabling or disabling them based on the 'enable'
* parameter.
*
* This method checks if the motors are powered up by verifying the 'STAGE2_INPUT' pin.
*
Expand Down Expand Up @@ -333,8 +346,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
* robot version.
*
* @param enable Ignored parameter in this version.
* @exception std::runtime_error Always throws a runtime error due to lack of support for digital
* power control.
* @exception std::runtime_error Always throws a runtime error due to lack of support for
* digital power control.
*/
bool DigitalPowerEnable(const bool /* enable */) override;

Expand All @@ -343,8 +356,8 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
* robot version.
*
* @param enable Ignored parameter in this version.
* @exception std::runtime_error Always throws a runtime error due to lack of support for charging
* process control.
* @exception std::runtime_error Always throws a runtime error due to lack of support for
* charging process control.
*/
bool ChargerEnable(const bool /* enable */) override;

Expand All @@ -364,14 +377,45 @@ class GPIOControllerPTH10X : public GPIOControllerInterface
*/
std::unordered_map<GPIOPin, bool> QueryControlInterfaceIOStates() const override;

/**
* @brief Returns the GPIO pin configuration information for the PTH10X.
*/
static const std::vector<GPIOInfo> & GetGPIOConfigInfoStorage();

private:
/**
* @brief Vector containing GPIO pin configuration information such as pin direction, value, etc.
* @brief Vector containing GPIO pin configuration information such as pin direction, value,
* etc.
*/
const std::vector<GPIOInfo> 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},
static const std::vector<GPIOInfo> gpio_config_info_storage_;
};

class GPIOControllerFactory
{
public:
/**
* @brief Creates a GPIO controller based on the robot version.
*
* @param robot_version The robot version to create the GPIO controller for.
* @return A unique pointer to the created GPIO controller.
*/
static std::unique_ptr<GPIOControllerInterface> CreateGPIOController(const float robot_version)
{
std::unique_ptr<GPIOControllerInterface> gpio_controller;

if (panther_utils::common_utilities::MeetsVersionRequirement(robot_version, 1.2)) {
auto config_info_storage = GPIOControllerPTH12X::GetGPIOConfigInfoStorage();
auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);

gpio_controller = std::make_unique<GPIOControllerPTH12X>(gpio_driver);
} else {
auto config_info_storage = GPIOControllerPTH10X::GetGPIOConfigInfoStorage();
auto gpio_driver = std::make_shared<GPIODriver>(config_info_storage);

gpio_controller = std::make_unique<GPIOControllerPTH10X>(gpio_driver);
}

return gpio_controller;
};
};

Expand Down
Loading

0 comments on commit 1d8a416

Please sign in to comment.