Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros …

…into ros2-lynx-devel

husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp

@@ -45,7 +45,9 @@ class GPIODriverInterface
   virtual void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) = 0;
   virtual bool IsPinAvailable(const GPIOPin pin) const = 0;
   virtual bool IsPinActive(const GPIOPin pin) = 0;
-  virtual bool SetPinValue(const GPIOPin pin, const bool value) = 0;
+  virtual bool SetPinValue(
+    const GPIOPin pin, const bool value,
+    const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)) = 0;
 };
 
 /**
@@ -168,13 +170,18 @@ class GPIODriver : public GPIODriverInterface
    *
    * @param pin GPIOPin to set the value for.
    * @param value The boolean value to set for the pin.
+   * @param pin_validation_wait_time The time duration to wait for the pin value to change before
+   * checking if change was successful.
    *
    * @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) override;
+  bool SetPinValue(
+    const GPIOPin pin, const bool value,
+    const std::chrono::milliseconds & pin_validation_wait_time =
+      std::chrono::milliseconds(0)) override;
 
 private:
   std::unique_ptr<gpiod::line_request> CreateLineRequest(gpiod::chip & chip);

husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp

@@ -189,7 +189,9 @@ bool GPIOController::ChargerEnable(const bool enable)
 
 bool GPIOController::LEDControlEnable(const bool enable)
 {
-  return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable);
+  // pin_validation_wait_time=10ms used due to slow pin state transition
+  // on pin loaded by high 100nF capacity in SBC Overlay v1.4
+  return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable, std::chrono::milliseconds(10));
 }
 
 std::unordered_map<GPIOPin, bool> GPIOController::QueryControlInterfaceIOStates() const

husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp

@@ -164,7 +164,8 @@ bool GPIODriver::IsPinActive(const GPIOPin pin)
   return pin_info.value == gpiod::line::value::ACTIVE;
 }
 
-bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value)
+bool GPIODriver::SetPinValue(
+  const GPIOPin pin, const bool value, const std::chrono::milliseconds & pin_validation_wait_time)
 {
   GPIOInfo & gpio_info = GetGPIOInfoRef(pin);
 
@@ -174,10 +175,17 @@ bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value)
 
   gpiod::line::value gpio_value = value ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE;
 
-  std::lock_guard lock(gpio_info_storage_mutex_);
+  std::unique_lock lock(gpio_info_storage_mutex_);
+
   try {
     line_request_->set_value(gpio_info.offset, gpio_value);
 
+    if (pin_validation_wait_time.count() > 0) {
+      lock.unlock();
+      std::this_thread::sleep_for(pin_validation_wait_time);
+      lock.lock();
+    }
+
     if (line_request_->get_value(gpio_info.offset) != gpio_value) {
       throw std::runtime_error("Failed to change GPIO state.");
     }

husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp

@@ -39,7 +39,13 @@ class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterf
     (override));
   MOCK_METHOD(bool, IsPinAvailable, (const GPIOPin pin), (const, override));
   MOCK_METHOD(bool, IsPinActive, (const GPIOPin pin), (override));
-  MOCK_METHOD(bool, SetPinValue, (const GPIOPin pin, const bool value), (override));
+  MOCK_METHOD(
+    bool, SetPinValue,
+    (const GPIOPin pin, const bool value,
+     const std::chrono::milliseconds & pin_validation_wait_time),
+    (override));
+
+  using NiceMock = testing::NiceMock<MockGPIODriver>;
 };
 
 class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOController
@@ -64,18 +70,19 @@ class TestGPIOController : public ::testing::Test
 protected:
   float GetRobotVersion();
 
-  std::shared_ptr<MockGPIODriver> gpio_driver_;
+  std::shared_ptr<MockGPIODriver::NiceMock> gpio_driver_;
   std::unique_ptr<GPIOControllerWrapper> gpio_controller_wrapper_;
   static constexpr int watchdog_edges_per_100ms_ = 10;
 };
 
 TestGPIOController::TestGPIOController()
 {
-  gpio_driver_ = std::make_shared<MockGPIODriver>();
+  gpio_driver_ = std::make_shared<MockGPIODriver::NiceMock>();
 
   // Mock methods called during the initialization process
-  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true)).WillByDefault(testing::Return(true));
-  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true))
+  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true, testing::_))
+    .WillByDefault(testing::Return(true));
+  ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true, testing::_))
     .WillByDefault(testing::Return(true));
   ON_CALL(*gpio_driver_, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(true));
 
@@ -106,7 +113,7 @@ TEST(TestGPIOControllerInitialization, WatchdogPinNotAvailable)
 {
   auto gpio_driver = std::make_shared<MockGPIODriver>();
 
-  EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true))
+  EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true, testing::_))
     .Times(2)
     .WillRepeatedly(testing::Return(true));
   ON_CALL(*gpio_driver, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(false));
@@ -142,14 +149,16 @@ TEST_P(ParametrizedTestGPIOController, TestGPIOEnableDisable)
   auto const disable = !enable;
 
   // Enable GPIO
-  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable)).WillOnce(testing::Return(true));
+  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable, testing::_))
+    .WillOnce(testing::Return(true));
   EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(enable));
 
   param.enable_method(gpio_controller_wrapper_.get(), true);
   EXPECT_EQ(enable, gpio_controller_wrapper_->IsPinActive(param.pin));
 
   // Disable GPIO
-  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable)).WillOnce(testing::Return(true));
+  EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable, testing::_))
+    .WillOnce(testing::Return(true));
   EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(disable));
 
   param.enable_method(gpio_controller_wrapper_.get(), false);

husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp

@@ -66,7 +66,10 @@ class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterf
     bool, IsPinAvailable, (const husarion_ugv_hardware_interfaces::GPIOPin), (const, override));
   MOCK_METHOD(bool, IsPinActive, (const husarion_ugv_hardware_interfaces::GPIOPin), (override));
   MOCK_METHOD(
-    bool, SetPinValue, (const husarion_ugv_hardware_interfaces::GPIOPin, const bool), (override));
+    bool, SetPinValue,
+    (const husarion_ugv_hardware_interfaces::GPIOPin, const bool,
+     const std::chrono::milliseconds & pin_validation_wait_time),
+    (override));
 
   using NiceMock = testing::NiceMock<MockGPIODriver>;
 };