diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro index 8443fe58..4e1fdf17 100644 --- a/lynx_description/urdf/lynx_macro.urdf.xacro +++ b/lynx_description/urdf/lynx_macro.urdf.xacro @@ -61,8 +61,8 @@ true - - panther_hardware_interfaces/PantherSystem + + panther_hardware_interfaces/LynxSystem 1600 @@ -77,8 +77,7 @@ panther_can 3 - 1 - 2 + 1 100 +Since some of the tests create and access a virtual CAN interface, it is necessary to install `kmod` and `iproute2`. ```bash -sudo modprobe vcan -sudo ip link add dev panther_can type vcan -sudo ip link set up panther_can -sudo ip link set panther_can down -sudo ip link set panther_can txqueuelen 1000 -sudo ip link set panther_can up +sudo apt update +sudo apt install -y kmod iproute2 ``` ### Running tests diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp new file mode 100644 index 00000000..b5927e59 --- /dev/null +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/lynx_system.hpp @@ -0,0 +1,60 @@ +// 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_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_ +#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_ + +#include +#include + +#include "panther_hardware_interfaces/panther_system/ugv_system.hpp" + +namespace panther_hardware_interfaces +{ + +/** + * @brief Class that implements UGVSystem for Lynx robot + */ +class LynxSystem : public UGVSystem +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LynxSystem) + + LynxSystem() : UGVSystem(kJointOrder) {} + + ~LynxSystem() = default; + +protected: + void ReadCANopenSettingsDriverCANIDs() override; + + virtual void DefineRobotDriver() override; // virtual for testing + + void UpdateHwStates() override; + void UpdateMotorsStateDataTimedOut() override; + + void UpdateDriverStateMsg() override; + void UpdateFlagErrors() override; + void UpdateDriverStateDataTimedOut() override; + + std::vector GetSpeedCommands() const; + + void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override; + + static const inline std::vector kJointOrder = {"fl", "fr", "rl", "rr"}; +}; + +} // namespace panther_hardware_interfaces + +#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp index 373ff0ee..80cf06bb 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp @@ -15,141 +15,44 @@ #ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_ #define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_ -#include -#include -#include #include #include -#include "diagnostic_updater/diagnostic_status_wrapper.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" - -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp" -#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp" -#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp" +#include "panther_hardware_interfaces/panther_system/ugv_system.hpp" namespace panther_hardware_interfaces { -using return_type = hardware_interface::return_type; -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using StateInterface = hardware_interface::StateInterface; -using CommandInterface = hardware_interface::CommandInterface; - /** - * @brief Class that implements SystemInterface from ros2_control for Panther + * @brief Class that implements UGVSystem for Panther robot */ -class PantherSystem : public hardware_interface::SystemInterface +class PantherSystem : public UGVSystem { public: RCLCPP_SHARED_PTR_DEFINITIONS(PantherSystem) - CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + PantherSystem() : UGVSystem(kJointOrder) {} - std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) override; - return_type write( - const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override; + ~PantherSystem() = default; protected: - void CheckJointSize() const; - void SortAndCheckJointNames(); - void SetInitialValues(); - void CheckInterfaces() const; - void ReadPantherVersion(); - void ReadDrivetrainSettings(); - void ReadCANopenSettings(); - void ReadInitializationActivationAttempts(); - void ReadParametersAndCreateRoboteqErrorFilter(); - void ReadDriverStatesUpdateFrequency(); - - void ConfigureGPIOController(); - void ConfigureMotorsController(); - void ConfigureEStop(); - - void UpdateMotorsState(); - void UpdateDriverState(); - void UpdateEStopState(); - - void UpdateHwStates(); - void UpdateMotorsStateDataTimedOut(); - bool AreVelocityCommandsNearZero(); - - void UpdateDriverStateMsg(); - void UpdateFlagErrors(); - void UpdateDriverStateDataTimedOut(); - - void HandlePDOWriteOperation(std::function pdo_write_operation); - - void MotorsPowerEnable(const bool enable); - - void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status); - void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status); - - static constexpr size_t kJointsSize = 4; - - // Currently only velocity command mode is supported - although Roboteq driver support position - // and torque mode, in 2.1 firmware both modes aren't really stable and safe. - // In torque mode sometimes after killing the software motor moves and it generally isn't well - // tuned. Position mode also isn't really stable (reacts abruptly to spikes). If updating the - // firmware to 2.1a will solve these issues, it may be worth to enable other modes. - std::array hw_commands_velocities_; - - std::array hw_states_positions_; - std::array hw_states_velocities_; - std::array hw_states_efforts_; - - // Define expected joint order, so that it doesn't matter what order is defined in the URDF. - // It is expected that the joint name should contain these specifiers. - static const inline std::array joint_order_ = {"fl", "fr", "rl", "rr"}; - std::array joints_names_sorted_; - - std::shared_ptr gpio_controller_; - std::shared_ptr motors_controller_; - std::shared_ptr e_stop_; - - DrivetrainSettings drivetrain_settings_; - CANopenSettings canopen_settings_; - - std::unique_ptr panther_system_ros_interface_; + void ReadCANopenSettingsDriverCANIDs() override; - // Sometimes SDO errors can happen during initialization and activation of Roboteq drivers, - // in these cases it is better to retry - // Example errors: - // SDO abort code 05040000 received on upload request of object 1000 (Device type) to - // node 02: SDO protocol timed out - // SDO abort code 05040000 received on upload request of sub-object 1018:01 (Vendor-ID) to - // node 02: SDO protocol timed out - unsigned max_roboteq_initialization_attempts_; - unsigned max_roboteq_activation_attempts_; + virtual void DefineRobotDriver() override; // virtual for testing - rclcpp::Logger logger_{rclcpp::get_logger("PantherSystem")}; - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + void UpdateHwStates() override; + void UpdateMotorsStateDataTimedOut() override; - std::shared_ptr roboteq_error_filter_; + void UpdateDriverStateMsg() override; + void UpdateFlagErrors() override; + void UpdateDriverStateDataTimedOut() override; - float panther_version_; + std::vector GetSpeedCommands() const; - std::shared_ptr motor_controller_write_mtx_; + void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override; - rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME}; - rclcpp::Duration driver_states_update_period_{0, 0}; + static const inline std::vector kJointOrder = {"fl", "fr", "rl", "rr"}; }; } // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp deleted file mode 100644 index 146bd909..00000000 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp +++ /dev/null @@ -1,191 +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. - -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ - -#include -#include -#include - -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp" - -namespace panther_hardware_interfaces -{ - -/** - * @class EStopInterface - * @brief Abstract base class defining the interface for emergency stop detailed implementations. - */ -class EStopInterface -{ -public: - EStopInterface( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : gpio_controller_(gpio_controller), - roboteq_error_filter_(roboteq_error_filter), - motors_controller_(motors_controller), - motor_controller_write_mtx_(motor_controller_write_mtx), - ZeroVelocityCheck(zero_velocity_check) {}; - - virtual ~EStopInterface() = default; - - virtual bool ReadEStopState() = 0; - virtual void TriggerEStop() = 0; - virtual void ResetEStop() = 0; - -protected: - std::shared_ptr gpio_controller_; - std::shared_ptr roboteq_error_filter_; - std::shared_ptr motors_controller_; - std::shared_ptr motor_controller_write_mtx_; - - std::function ZeroVelocityCheck; - - std::mutex e_stop_manipulation_mtx_; - std::atomic_bool e_stop_triggered_ = true; -}; - -/** - * @class EStopPTH12X - * @brief Implements the emergency stop for the PTH12X hardware variant. - */ -class EStopPTH12X : public EStopInterface -{ -public: - EStopPTH12X( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : EStopInterface( - gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, - zero_velocity_check) {}; - - virtual ~EStopPTH12X() override = default; - - /** - * @brief Checks the emergency stop state. - * - * 1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by - * another device within the robot's system (e.g., Roboteq controller or Safety Board), - * disabling the software Watchdog is necessary to prevent an uncontrolled reset. - * 2. If there is a need, disable software Watchdog using - * GPIOControllerInterface::EStopTrigger method. - * 3. Return ESTOP GPIO pin state. - */ - bool ReadEStopState() override; - - /** - * @brief Triggers the emergency stop. - * - * 1. Interrupt the E-Stop resetting process if it is in progress. - * 2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog. - * 3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error - * exception. - * - * @throws std::runtime_error if triggering the E-stop using GPIO fails. - */ - void TriggerEStop() override; - - /** - * @brief Resets the emergency stop. - * - * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement - * after an E-stop state change. - * 2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation - * may take some time and can be interrupted by the E-Stop trigger process. - * 3. Set the clear_error flag to allow for clearing of Roboteq errors. - * 4. Confirm the E-Stop reset was successful with the ReadEStopState method. - * - * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was - * triggered again. - * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO. - */ - void ResetEStop() override; -}; - -/** - * @class EStopPTH10X - * @brief Implements the emergency stop for the PTH10X hardware variant. In this robot - * version, only a software-based E-Stop is supported. There are no hardware components that - * implement E-Stop functionality. - */ -class EStopPTH10X : public EStopInterface -{ -public: - EStopPTH10X( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : EStopInterface( - gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, - zero_velocity_check) {}; - - virtual ~EStopPTH10X() override = default; - - /** - * @brief Checks the emergency stop state. - * - * 1. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered - * up. - * 2. Check for any errors reported by the Roboteq controller. - * 3. If the E-Stop is not currently triggered, initiate an E-Stop if the motors are not powered - * up or if a driver error has occurred. - * 4. Return the actual state of the E-Stop. - * - * @return A boolean indicating whether the E-Stop is currently triggered `true` or not `false`. - */ - bool ReadEStopState() override; - - /** - * @brief Trigger the emergency stop state. - * - * 1. Send a command to the Roboteq controllers to enable the Safety Stop. - * Note: The Safety Stop is a specific state of the Roboteq controller, distinct from - * the E-Stop state of the Panther robot. - * 2. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. - */ - void TriggerEStop() override; - - /** - * @brief Resets the emergency stop. - * - * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement - * after an E-stop state change. - * 2. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered - * up. - * 3. Check for any errors reported by the Roboteq controller. - * 4. Set the clear_error flag to allow for clearing of Roboteq errors. - * 5. Confirm the E-Stop reset was successful with the ReadEStopState method. - * 6. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. - * - * @throws std::runtime_error if the E-stop reset operation was halted because the E-stop was - * triggered again, motors are not powered or motor controller is in an error state. - */ - void ResetEStop() override; -}; - -} // namespace panther_hardware_interfaces - -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp index 6449d7c6..25b61114 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp @@ -63,7 +63,7 @@ class LynxRobotDriver : public RoboteqRobotDriver * @brief This method defines driver objects and adds motor drivers for them. It is virtual to * allow mocking drivers in tests. */ - virtual void DefineDrivers(); + void DefineDrivers() override; }; } // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp index bccd1f64..78e38064 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp @@ -107,6 +107,13 @@ class RobotDriverInterface * the error flags should remain active. */ virtual void AttemptErrorFlagReset() = 0; + + /** + * @brief Check if communication with drivers is working properly + * + * @return true if communication error occurred + */ + virtual bool CommunicationError() = 0; }; } // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp index 3d4e8440..6f872307 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp @@ -132,6 +132,8 @@ class RoboteqRobotDriver : public RobotDriverInterface */ const DriverData & GetData(const std::string & name) override; + bool CommunicationError() override; + protected: /** * @brief This method defines driver objects and adds motor drivers for them. @@ -156,6 +158,7 @@ class RoboteqRobotDriver : public RobotDriverInterface bool DataTimeout( const timespec & current_time, const timespec & data_timestamp, const std::chrono::milliseconds & timeout); + void BootDrivers(); bool initialized_ = false; diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp new file mode 100644 index 00000000..a0d1088e --- /dev/null +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_e_stop.hpp @@ -0,0 +1,120 @@ +// 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_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_ +#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_ + +#include +#include +#include + +#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp" + +namespace panther_hardware_interfaces +{ + +/** + * @class EStopInterface + * @brief Abstract base class defining the interface for emergency stop detailed implementations. + */ +class EStopInterface +{ +public: + EStopInterface() {} + + virtual ~EStopInterface() = default; + + virtual bool ReadEStopState() = 0; + virtual void TriggerEStop() = 0; + virtual void ResetEStop() = 0; +}; + +/** + * @class EStop + * @brief Implements the emergency stop interface. + */ +class EStop : public EStopInterface +{ +public: + EStop( + std::shared_ptr gpio_controller, + std::shared_ptr roboteq_error_filter, + std::shared_ptr robot_driver, + std::shared_ptr robot_driver_write_mtx, std::function zero_velocity_check) + : EStopInterface(), + gpio_controller_(gpio_controller), + roboteq_error_filter_(roboteq_error_filter), + robot_driver_(robot_driver), + robot_driver_write_mtx_(robot_driver_write_mtx), + ZeroVelocityCheck(zero_velocity_check) {}; + + virtual ~EStop() override = default; + + /** + * @brief Checks the emergency stop state. + * + * 1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by + * another device within the robot's system (e.g., Roboteq controller or Safety Board), + * disabling the software Watchdog is necessary to prevent an uncontrolled reset. + * 2. If there is a need, disable software Watchdog using + * GPIOControllerInterface::EStopTrigger method. + * 3. Return ESTOP GPIO pin state. + */ + bool ReadEStopState() override; + + /** + * @brief Triggers the emergency stop. + * + * 1. Interrupt the E-Stop resetting process if it is in progress. + * 2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog. + * 3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error + * exception. + * + * @throws std::runtime_error if triggering the E-stop using GPIO fails. + */ + void TriggerEStop() override; + + /** + * @brief Resets the emergency stop. + * + * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement + * after an E-stop state change. + * 2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation + * may take some time and can be interrupted by the E-Stop trigger process. + * 3. Set the clear_error flag to allow for clearing of Roboteq errors. + * 4. Confirm the E-Stop reset was successful with the ReadEStopState method. + * + * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was + * triggered again. + * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO. + */ + void ResetEStop() override; + +protected: + std::shared_ptr gpio_controller_; + std::shared_ptr roboteq_error_filter_; + std::shared_ptr robot_driver_; + std::shared_ptr robot_driver_write_mtx_; + + std::function ZeroVelocityCheck; + + std::mutex e_stop_manipulation_mtx_; + std::atomic_bool e_stop_triggered_ = true; +}; + +} // namespace panther_hardware_interfaces + +#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_E_STOP_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp similarity index 85% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp rename to panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp index ca04943d..856d4b72 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/system_ros_interface.hpp @@ -12,30 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ +#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ +#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ #include -#include #include +#include #include #include #include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" -#include "realtime_tools/realtime_publisher.h" +#include +#include +#include -#include "std_msgs/msg/bool.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" +#include +#include +#include #include "panther_msgs/msg/driver_state_named.hpp" #include "panther_msgs/msg/io_state.hpp" #include "panther_msgs/msg/robot_driver_state.hpp" #include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp" using namespace std::placeholders; @@ -49,6 +49,14 @@ using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed; using SetBoolSrv = std_srvs::srv::SetBool; using TriggerSrv = std_srvs::srv::Trigger; +struct DriverCANErrors +{ + bool motor_states_data_timed_out; + bool driver_state_data_timed_out; + bool can_error; + bool heartbeat_timeout; +}; + struct CANErrors { bool error; @@ -57,17 +65,7 @@ struct CANErrors bool read_pdo_motor_states_error; bool read_pdo_driver_state_error; - bool front_motor_states_data_timed_out; - bool rear_motor_states_data_timed_out; - - bool front_driver_state_data_timed_out; - bool rear_driver_state_data_timed_out; - - bool front_can_error; - bool rear_can_error; - - bool front_heartbeat_timeout; - bool rear_heartbeat_timeout; + std::unordered_map driver_errors; }; /** @@ -143,7 +141,7 @@ class ROSServiceWrapper * @brief Class that takes care of additional ROS interface of panther system, such as publishing * driver state and providing service for clearing errors */ -class PantherSystemRosInterface +class SystemROSInterface { public: /** @@ -152,10 +150,10 @@ class PantherSystemRosInterface * @param node_name * @param node_options */ - PantherSystemRosInterface( + SystemROSInterface( const std::string & node_name, const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); - ~PantherSystemRosInterface(); + ~SystemROSInterface(); /** * @brief Registers a new service server associated with a specific service type and callback on @@ -222,14 +220,21 @@ class PantherSystemRosInterface } /** - * @brief Updates fault flags, script flags, and runtime errors in the driver state msg + * @brief Updates fault flags, script flags, and runtime errors in the robot driver state msg + * + * @param name The name of the driver to update the flags for + * @param data The data to update the flags with */ - void UpdateMsgErrorFlags(const RoboteqData & front, const RoboteqData & rear); + void UpdateMsgErrorFlags(const std::string & name, const DriverData & data); /** - * @brief Updates parameters of the drivers: voltage, current and temperature + * @brief Updates parameters of the driver (voltage, current and temperature) in robot driver + * state msg + * + * @param name The name of the driver to update the parameters for + * @param state The data to update the parameters with */ - void UpdateMsgDriversStates(const DriverState & front, const DriverState & rear); + void UpdateMsgDriversStates(const std::string & name, const RoboteqDriverState & state); /** * @brief Updates the current state of communication errors and general error state @@ -242,7 +247,7 @@ class PantherSystemRosInterface void InitializeAndPublishIOStateMsg(const std::unordered_map & io_state); void PublishIOState(const GPIOInfo & gpio_info); -private: +protected: /** * @brief Updates the IOState message and indicates whether its state has changed. * @@ -271,8 +276,15 @@ class PantherSystemRosInterface rclcpp::CallbackGroup::SharedPtr GetOrCreateNodeCallbackGroup( const unsigned group_id, rclcpp::CallbackGroupType callback_group_type); - void InitializeRobotDriverStateMsg(); - + /** + * @brief Retrieves the driver state message associated with the specified name. If the driver + * state is not found, it is created. + * + * @param robot_driver_state The robot driver state message to retrieve driver state from. + * @param name The name of the driver state to be retrieved. + * + * @return Reference to the driver state message associated with the specified name. + */ DriverStateNamedMsg & GetDriverStateByName( RobotDriverStateMsg & robot_driver_state, const std::string & name); @@ -298,4 +310,4 @@ class PantherSystemRosInterface } // namespace panther_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ +#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp new file mode 100644 index 00000000..03fd02b5 --- /dev/null +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/ugv_system.hpp @@ -0,0 +1,168 @@ +// 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_PANTHER_SYSTEM_UGV_SYSTEM_HPP_ +#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/canopen_manager.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_error_filter.hpp" +#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp" +#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp" + +namespace panther_hardware_interfaces +{ + +using return_type = hardware_interface::return_type; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using StateInterface = hardware_interface::StateInterface; +using CommandInterface = hardware_interface::CommandInterface; + +/** + * @brief Class that implements SystemInterface from ros2_control for Husarion UGV robots + */ +class UGVSystem : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(UGVSystem) + UGVSystem(const std::vector & joint_order) + : SystemInterface(), + joint_size_(joint_order.size()), + joint_order_(joint_order), + joints_names_sorted_(joint_size_) + { + } + + virtual ~UGVSystem() = default; + + CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) override; + return_type write( + const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override; + +protected: + void CheckJointSize() const; + void SortAndCheckJointNames(); + void SetInitialValues(); + void CheckInterfaces() const; + + void ReadDrivetrainSettings(); + void ReadCANopenSettings(); + virtual void ReadCANopenSettingsDriverCANIDs() = 0; + + void ReadInitializationActivationAttempts(); + void ReadParametersAndCreateRoboteqErrorFilter(); + void ReadDriverStatesUpdateFrequency(); + + virtual void ConfigureGPIOController(); // virtual for mocking + void ConfigureRobotDriver(); + virtual void DefineRobotDriver() = 0; + virtual void ConfigureEStop(); // virtual for mocking + + void UpdateMotorsState(); + void UpdateDriverState(); + void UpdateEStopState(); + + virtual void UpdateHwStates() = 0; + virtual void UpdateMotorsStateDataTimedOut() = 0; // possible but needs changes in robot driver + bool AreVelocityCommandsNearZero(); + + virtual void UpdateDriverStateMsg() = 0; + virtual void UpdateFlagErrors() = 0; // possible but needs changes in robot driver + virtual void UpdateDriverStateDataTimedOut() = 0; // possible but needs changes in robot driver + + void HandleRobotDriverWriteOperation(std::function write_operation); + virtual std::vector GetSpeedCommands() const = 0; + + void MotorsPowerEnable(const bool enable); + + virtual void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) = 0; + virtual void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) = 0; + + const size_t joint_size_; + + // Currently only velocity command mode is supported - although Roboteq driver support position + // and torque mode, in 2.1 firmware both modes aren't really stable and safe. + // In torque mode sometimes after killing the software motor moves and it generally isn't well + // tuned. Position mode also isn't really stable (reacts abruptly to spikes). If updating the + // firmware to 2.1a will solve these issues, it may be worth to enable other modes. + std::vector hw_commands_velocities_; + + std::vector hw_states_positions_; + std::vector hw_states_velocities_; + std::vector hw_states_efforts_; + + // Define expected joint order, so that it doesn't matter what order is defined in the URDF. + // It is expected that the joint name should contain these specifiers. + const std::vector joint_order_; + std::vector joints_names_sorted_; + + std::shared_ptr gpio_controller_; + std::shared_ptr robot_driver_; + std::shared_ptr e_stop_; + + DrivetrainSettings drivetrain_settings_; + CANopenSettings canopen_settings_; + + std::unique_ptr system_ros_interface_; + + // Sometimes SDO errors can happen during initialization and activation of Roboteq drivers, + // in these cases it is better to retry + // Example errors: + // SDO abort code 05040000 received on upload request of object 1000 (Device type) to + // node 02: SDO protocol timed out + // SDO abort code 05040000 received on upload request of sub-object 1018:01 (Vendor-ID) to + // node 02: SDO protocol timed out + unsigned max_roboteq_initialization_attempts_; + unsigned max_roboteq_activation_attempts_; + + rclcpp::Logger logger_{rclcpp::get_logger("UGVSystem")}; + rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + + std::shared_ptr roboteq_error_filter_; + + std::shared_ptr robot_driver_write_mtx_; + + rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Duration driver_states_update_period_{0, 0}; +}; + +} // namespace panther_hardware_interfaces + +#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_UGV_SYSTEM_HPP_ diff --git a/panther_hardware_interfaces/panther_hardware_interfaces.xml b/panther_hardware_interfaces/panther_hardware_interfaces.xml index 8462de75..dbb3512e 100644 --- a/panther_hardware_interfaces/panther_hardware_interfaces.xml +++ b/panther_hardware_interfaces/panther_hardware_interfaces.xml @@ -7,6 +7,14 @@ + + + Hardware system controller for Panther's Roboteq motor controller + + + diff --git a/panther_hardware_interfaces/src/panther_system/lynx_system.cpp b/panther_hardware_interfaces/src/panther_system/lynx_system.cpp new file mode 100644 index 00000000..8d491af5 --- /dev/null +++ b/panther_hardware_interfaces/src/panther_system/lynx_system.cpp @@ -0,0 +1,176 @@ +// 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/panther_system/lynx_system.hpp" + +#include +#include +#include + +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" +#include "rclcpp/logging.hpp" + +#include "panther_hardware_interfaces/panther_system/robot_driver/lynx_robot_driver.hpp" + +#include "panther_utils/diagnostics.hpp" + +namespace panther_hardware_interfaces +{ + +void LynxSystem::ReadCANopenSettingsDriverCANIDs() +{ + const auto driver_can_id = std::stoi(info_.hardware_parameters["driver_can_id"]); + canopen_settings_.driver_can_ids.emplace(DriverNames::DEFAULT, driver_can_id); +} + +void LynxSystem::DefineRobotDriver() +{ + robot_driver_ = std::make_shared(canopen_settings_, drivetrain_settings_); +} + +void LynxSystem::UpdateHwStates() +{ + const auto data = robot_driver_->GetData(DriverNames::DEFAULT); + + const auto left = data.GetMotorState(MotorChannels::LEFT); + const auto right = data.GetMotorState(MotorChannels::RIGHT); + + hw_states_positions_[0] = left.GetPosition(); + hw_states_positions_[1] = right.GetPosition(); + hw_states_positions_[2] = left.GetPosition(); + hw_states_positions_[3] = right.GetPosition(); + + hw_states_velocities_[0] = left.GetVelocity(); + hw_states_velocities_[1] = right.GetVelocity(); + hw_states_velocities_[2] = left.GetVelocity(); + hw_states_velocities_[3] = right.GetVelocity(); + + hw_states_efforts_[0] = left.GetTorque(); + hw_states_efforts_[1] = right.GetTorque(); + hw_states_efforts_[2] = left.GetTorque(); + hw_states_efforts_[3] = right.GetTorque(); +} + +void LynxSystem::UpdateMotorsStateDataTimedOut() +{ + if (robot_driver_->GetData(DriverNames::DEFAULT).IsMotorStatesDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false); + } +} + +void LynxSystem::UpdateDriverStateMsg() +{ + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + + system_ros_interface_->UpdateMsgDriversStates(DriverNames::DEFAULT, driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::DEFAULT, driver_data); + + CANErrors can_errors; + can_errors.error = roboteq_error_filter_->IsError(); + can_errors.write_pdo_cmds_error = roboteq_error_filter_->IsError(ErrorsFilterIds::WRITE_PDO_CMDS); + can_errors.read_pdo_motor_states_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_MOTOR_STATES); + can_errors.read_pdo_driver_state_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE); + + DriverCANErrors driver_can_errors; + driver_can_errors.motor_states_data_timed_out = driver_data.IsMotorStatesDataTimedOut(); + driver_can_errors.driver_state_data_timed_out = driver_data.IsDriverStateDataTimedOut(); + driver_can_errors.can_error = driver_data.IsCANError(); + driver_can_errors.heartbeat_timeout = driver_data.IsHeartbeatTimeout(); + + can_errors.driver_errors.emplace(DriverNames::DEFAULT, driver_can_errors); + + system_ros_interface_->UpdateMsgErrors(can_errors); +} + +void LynxSystem::UpdateFlagErrors() +{ + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + if (driver_data.IsFlagError()) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, steady_clock_, 1000, + "Error state on the default driver: " << driver_data.GetFlagErrorLog()); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true); + + HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); }); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); + } +} + +void LynxSystem::UpdateDriverStateDataTimedOut() +{ + if (robot_driver_->GetData(DriverNames::DEFAULT).IsDriverStateDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false); + } +} + +void LynxSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"No error detected."}; + + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + if (driver_data.IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + panther_utils::diagnostics::AddKeyValueIfTrue( + status, driver_data.GetErrorMap(), "Driver error: "); + } + + if (roboteq_error_filter_->IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + panther_utils::diagnostics::AddKeyValueIfTrue( + status, roboteq_error_filter_->GetErrorMap(), "", " error"); + } + + status.summary(level, message); +} + +void LynxSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"Panther system status monitoring."}; + + const auto driver_state = robot_driver_->GetData(DriverNames::DEFAULT).GetDriverState(); + + status.add("Default driver voltage (V)", driver_state.GetVoltage()); + status.add("Default driver current (A)", driver_state.GetCurrent()); + status.add("Default driver temperature (\u00B0C)", driver_state.GetTemperature()); + status.add( + "Default driver heatsink temperature (\u00B0C)", driver_state.GetHeatsinkTemperature()); + + status.summary(level, message); +} + +std::vector LynxSystem::GetSpeedCommands() const +{ + return { + static_cast(hw_commands_velocities_[0]), static_cast(hw_commands_velocities_[1])}; +} + +} // namespace panther_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(panther_hardware_interfaces::LynxSystem, hardware_interface::SystemInterface) diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp index a624c58a..e87ded9b 100644 --- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp +++ b/panther_hardware_interfaces/src/panther_system/panther_system.cpp @@ -14,542 +14,65 @@ #include "panther_hardware_interfaces/panther_system/panther_system.hpp" -#include -#include -#include -#include #include -#include #include #include #include "diagnostic_updater/diagnostic_status_wrapper.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" -#include "panther_hardware_interfaces/utils.hpp" -#include "panther_utils/common_utilities.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp" + #include "panther_utils/diagnostics.hpp" namespace panther_hardware_interfaces { -CallbackReturn PantherSystem::on_init(const hardware_interface::HardwareInfo & hardware_info) -{ - if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } - - try { - CheckJointSize(); - SortAndCheckJointNames(); - SetInitialValues(); - CheckInterfaces(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what()); - return CallbackReturn::ERROR; - } - - try { - ReadPantherVersion(); - ReadDrivetrainSettings(); - ReadCANopenSettings(); - ReadInitializationActivationAttempts(); - ReadParametersAndCreateRoboteqErrorFilter(); - ReadDriverStatesUpdateFrequency(); - } catch (const std::invalid_argument & e) { - RCLCPP_ERROR_STREAM( - logger_, "An exception occurred while reading the parameters: " << e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG_STREAM( - logger_, "Creating system entities for the Panther version: " << panther_version_); - - try { - ConfigureGPIOController(); - ConfigureMotorsController(); - ConfigureEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_cleanup(const rclcpp_lifecycle::State &) -{ - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) -{ - hw_commands_velocities_.fill(0.0); - hw_states_positions_.fill(0.0); - hw_states_velocities_.fill(0.0); - hw_states_efforts_.fill(0.0); - - if (!OperationWithAttempts( - std::bind(&MotorsController::Activate, motors_controller_), - max_roboteq_activation_attempts_)) { - RCLCPP_ERROR_STREAM( - logger_, "Failed to activate Panther System: Couldn't activate MotorsController in " - << max_roboteq_activation_attempts_ << " attempts."); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_ = - std::make_unique("hardware_controller"); - - panther_system_ros_interface_->AddService>( - "~/fan_enable", - std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/aux_power_enable", - std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/digital_power_enable", - std::bind( - &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1)); - 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)); - - panther_system_ros_interface_->AddService>( - "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, - rclcpp::CallbackGroupType::MutuallyExclusive); - - auto e_stop_reset_qos = rmw_qos_profile_services_default; - e_stop_reset_qos.depth = 1; - panther_system_ros_interface_->AddService>( - "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, - rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos); - - panther_system_ros_interface_->AddDiagnosticTask( - std::string("system errors"), this, &PantherSystem::DiagnoseErrors); - - panther_system_ros_interface_->AddDiagnosticTask( - std::string("system status"), this, &PantherSystem::DiagnoseStatus); - - gpio_controller_->RegisterGPIOEventCallback( - [this](const auto & state) { panther_system_ros_interface_->PublishIOState(state); }); - - const auto io_state = gpio_controller_->QueryControlInterfaceIOStates(); - panther_system_ros_interface_->InitializeAndPublishIOStateMsg(io_state); - - panther_system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState()); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what()); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_shutdown(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what()); - return CallbackReturn::ERROR; - } - - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - - panther_system_ros_interface_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what()); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_->BroadcastOnDiagnosticTasks( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, - "An error has occurred during a node state transition."); - - panther_system_ros_interface_.reset(); - - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -std::vector PantherSystem::export_state_interfaces() -{ - std::vector state_interfaces; - for (std::size_t i = 0; i < kJointsSize; i++) { - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i])); - } - - return state_interfaces; -} - -std::vector PantherSystem::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < kJointsSize; i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); - } - - return command_interfaces; -} - -return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) -{ - UpdateMotorsState(); - - if (time >= next_driver_state_update_time_) { - UpdateDriverState(); - UpdateDriverStateMsg(); - panther_system_ros_interface_->PublishRobotDriverState(); - next_driver_state_update_time_ = time + driver_states_update_period_; - } - - UpdateEStopState(); - - return return_type::OK; -} - -return_type PantherSystem::write( - const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) -{ - const bool e_stop = e_stop_->ReadEStopState(); - - if (!e_stop) { - HandlePDOWriteOperation([this] { - motors_controller_->SendSpeedCommands( - hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2], - hw_commands_velocities_[3]); - }); - } - - return return_type::OK; -} - -void PantherSystem::CheckJointSize() const -{ - if (info_.joints.size() != kJointsSize) { - throw std::runtime_error( - "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " + - std::to_string(kJointsSize) + "expected."); - } -} - -void PantherSystem::SortAndCheckJointNames() -{ - // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it - // is necessary to make sure that joints are in specific order and order of definitions in URDF - // doesn't matter - for (std::size_t i = 0; i < kJointsSize; i++) { - std::size_t match_count = 0; - - for (std::size_t j = 0; j < kJointsSize; j++) { - if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) { - joints_names_sorted_[i] = info_.joints[j].name; - ++match_count; - } - } - - if (match_count != 1) { - throw std::runtime_error( - "There should be exactly one joint containing " + joint_order_[i] + ", " + - std::to_string(match_count) + " found."); - } - } -} - -void PantherSystem::SetInitialValues() -{ - // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it - // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best - // option. - hw_commands_velocities_.fill(0.0); - - hw_states_positions_.fill(std::numeric_limits::quiet_NaN()); - hw_states_velocities_.fill(std::numeric_limits::quiet_NaN()); - hw_states_efforts_.fill(std::numeric_limits::quiet_NaN()); -} - -void PantherSystem::CheckInterfaces() const -{ - for (const hardware_interface::ComponentInfo & joint : info_.joints) { - // Commands - if (joint.command_interfaces.size() != 1) { - throw std::runtime_error( - "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) + - " command interfaces. 1 expected."); - } - - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.command_interfaces[0].name + - " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); - } - - // States - if (joint.state_interfaces.size() != 3) { - throw std::runtime_error( - "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) + - " state " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") + - " 3 expected."); - } - - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[0].name + - " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected."); - } - - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[1].name + - " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); - } - - if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[2].name + - " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected."); - } - } -} - -void PantherSystem::ReadPantherVersion() -{ - panther_version_ = std::stof(info_.hardware_parameters["panther_version"]); -} - -void PantherSystem::ReadDrivetrainSettings() -{ - drivetrain_settings_.motor_torque_constant = - std::stof(info_.hardware_parameters["motor_torque_constant"]); - drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]); - drivetrain_settings_.gearbox_efficiency = - std::stof(info_.hardware_parameters["gearbox_efficiency"]); - drivetrain_settings_.encoder_resolution = - std::stof(info_.hardware_parameters["encoder_resolution"]); - drivetrain_settings_.max_rpm_motor_speed = - std::stof(info_.hardware_parameters["max_rpm_motor_speed"]); -} - -void PantherSystem::ReadCANopenSettings() -{ - canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"]; - canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]); - canopen_settings_.front_driver_can_id = - std::stoi(info_.hardware_parameters["front_driver_can_id"]); - canopen_settings_.rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]); - canopen_settings_.pdo_motor_states_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"])); - canopen_settings_.pdo_driver_state_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"])); - canopen_settings_.sdo_operation_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"])); -} - -void PantherSystem::ReadInitializationActivationAttempts() +void PantherSystem::ReadCANopenSettingsDriverCANIDs() { - max_roboteq_initialization_attempts_ = - std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]); - max_roboteq_activation_attempts_ = - std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]); -} - -void PantherSystem::ReadParametersAndCreateRoboteqErrorFilter() -{ - const unsigned max_write_pdo_cmds_errors_count = - std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]); - const unsigned max_read_pdo_motor_states_errors_count = - std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]); - const unsigned max_read_pdo_driver_state_errors_count = - std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]); - - roboteq_error_filter_ = std::make_shared( - max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count, - max_read_pdo_driver_state_errors_count, 1); -} - -void PantherSystem::ReadDriverStatesUpdateFrequency() -{ - const float driver_states_update_frequency = - std::stof(info_.hardware_parameters["driver_states_update_frequency"]); - driver_states_update_period_ = - rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency); -} - -void PantherSystem::ConfigureGPIOController() -{ - gpio_controller_ = GPIOControllerFactory::CreateGPIOController(panther_version_); - gpio_controller_->Start(); - - RCLCPP_INFO(logger_, "Successfully configured GPIO controller."); -} - -void PantherSystem::ConfigureMotorsController() -{ - motor_controller_write_mtx_ = std::make_shared(); - motors_controller_ = std::make_shared(canopen_settings_, drivetrain_settings_); - - if (!OperationWithAttempts( - std::bind(&MotorsController::Initialize, motors_controller_), - max_roboteq_initialization_attempts_, - std::bind(&MotorsController::Deinitialize, motors_controller_))) { - throw std::runtime_error("Roboteq drivers initialization failed."); - } - - RCLCPP_INFO(logger_, "Successfully configured motors controller"); -} - -void PantherSystem::ConfigureEStop() -{ - if ( - !gpio_controller_ || !roboteq_error_filter_ || !motors_controller_ || - !motor_controller_write_mtx_) { - throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first."); - } - - if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2)) { - e_stop_ = std::make_shared( - gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, - std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); - } else { - e_stop_ = std::make_shared( - gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, - std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); - } - - RCLCPP_INFO(logger_, "Successfully configured E-Stop"); + const auto front_driver_can_id = std::stoi(info_.hardware_parameters["front_driver_can_id"]); + const auto rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]); + canopen_settings_.driver_can_ids.emplace(DriverNames::FRONT, front_driver_can_id); + canopen_settings_.driver_can_ids.emplace(DriverNames::REAR, rear_driver_can_id); } -void PantherSystem::UpdateMotorsState() +void PantherSystem::DefineRobotDriver() { - try { - motors_controller_->UpdateMotorsState(); - UpdateHwStates(); - UpdateMotorsStateDataTimedOut(); - } catch (const std::runtime_error & e) { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); - - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, steady_clock_, 5000, - "An exception occurred while updating motors states: " << e.what()); - } + robot_driver_ = std::make_shared(canopen_settings_, drivetrain_settings_); } -void PantherSystem::UpdateDriverState() +void PantherSystem::UpdateHwStates() { - try { - motors_controller_->UpdateDriversState(); - UpdateFlagErrors(); - UpdateDriverStateDataTimedOut(); - } catch (const std::runtime_error & e) { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + const auto front_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_data = robot_driver_->GetData(DriverNames::REAR); - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, steady_clock_, 5000, - "An exception occurred while updating drivers states: " << e.what()); - } -} + const auto fl_motor_state = front_data.GetMotorState(MotorChannels::LEFT); + const auto fr_motor_state = front_data.GetMotorState(MotorChannels::RIGHT); + const auto rl_motor_state = rear_data.GetMotorState(MotorChannels::LEFT); + const auto rr_motor_state = rear_data.GetMotorState(MotorChannels::RIGHT); -void PantherSystem::UpdateEStopState() -{ - if ( - motors_controller_->GetFrontData().IsHeartbeatTimeout() || - motors_controller_->GetRearData().IsHeartbeatTimeout()) { - e_stop_->TriggerEStop(); - } + hw_states_positions_[0] = fl_motor_state.GetPosition(); + hw_states_positions_[1] = fr_motor_state.GetPosition(); + hw_states_positions_[2] = rl_motor_state.GetPosition(); + hw_states_positions_[3] = rr_motor_state.GetPosition(); - const bool e_stop = e_stop_->ReadEStopState(); - panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop); -} + hw_states_velocities_[0] = fl_motor_state.GetVelocity(); + hw_states_velocities_[1] = fr_motor_state.GetVelocity(); + hw_states_velocities_[2] = rl_motor_state.GetVelocity(); + hw_states_velocities_[3] = rr_motor_state.GetVelocity(); -void PantherSystem::UpdateHwStates() -{ - const auto front = motors_controller_->GetFrontData(); - const auto rear = motors_controller_->GetRearData(); - - const auto fl = front.GetLeftMotorState(); - const auto fr = front.GetRightMotorState(); - const auto rl = rear.GetLeftMotorState(); - const auto rr = rear.GetRightMotorState(); - - hw_states_positions_[0] = fl.GetPosition(); - hw_states_positions_[1] = fr.GetPosition(); - hw_states_positions_[2] = rl.GetPosition(); - hw_states_positions_[3] = rr.GetPosition(); - - hw_states_velocities_[0] = fl.GetVelocity(); - hw_states_velocities_[1] = fr.GetVelocity(); - hw_states_velocities_[2] = rl.GetVelocity(); - hw_states_velocities_[3] = rr.GetVelocity(); - - hw_states_efforts_[0] = fl.GetTorque(); - hw_states_efforts_[1] = fr.GetTorque(); - hw_states_efforts_[2] = rl.GetTorque(); - hw_states_efforts_[3] = rr.GetTorque(); + hw_states_efforts_[0] = fl_motor_state.GetTorque(); + hw_states_efforts_[1] = fr_motor_state.GetTorque(); + hw_states_efforts_[2] = rl_motor_state.GetTorque(); + hw_states_efforts_[3] = rr_motor_state.GetTorque(); } void PantherSystem::UpdateMotorsStateDataTimedOut() { if ( - motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() || - motors_controller_->GetRearData().IsMotorStatesDataTimedOut()) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, steady_clock_, 1000, - (motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() ? "Front " : "") - << (motors_controller_->GetRearData().IsMotorStatesDataTimedOut() ? "Rear " : "") - << "PDO motor state data timeout."); + robot_driver_->GetData(DriverNames::FRONT).IsMotorStatesDataTimedOut() || + robot_driver_->GetData(DriverNames::REAR).IsMotorStatesDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout."); roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); } else { roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false); @@ -558,12 +81,15 @@ void PantherSystem::UpdateMotorsStateDataTimedOut() void PantherSystem::UpdateDriverStateMsg() { - panther_system_ros_interface_->UpdateMsgDriversStates( - motors_controller_->GetFrontData().GetDriverState(), - motors_controller_->GetRearData().GetDriverState()); + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); - panther_system_ros_interface_->UpdateMsgErrorFlags( - motors_controller_->GetFrontData(), motors_controller_->GetRearData()); + system_ros_interface_->UpdateMsgDriversStates( + DriverNames::FRONT, front_driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgDriversStates( + DriverNames::REAR, rear_driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::FRONT, front_driver_data); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::REAR, rear_driver_data); CANErrors can_errors; can_errors.error = roboteq_error_filter_->IsError(); @@ -573,38 +99,40 @@ void PantherSystem::UpdateDriverStateMsg() can_errors.read_pdo_driver_state_error = roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE); - can_errors.front_motor_states_data_timed_out = - motors_controller_->GetFrontData().IsMotorStatesDataTimedOut(); - can_errors.rear_motor_states_data_timed_out = - motors_controller_->GetRearData().IsMotorStatesDataTimedOut(); + DriverCANErrors front_driver_can_errors; + DriverCANErrors rear_driver_can_errors; - can_errors.front_driver_state_data_timed_out = - motors_controller_->GetFrontData().IsDriverStateDataTimedOut(); - can_errors.rear_driver_state_data_timed_out = - motors_controller_->GetRearData().IsDriverStateDataTimedOut(); + front_driver_can_errors.motor_states_data_timed_out = + front_driver_data.IsMotorStatesDataTimedOut(); + front_driver_can_errors.driver_state_data_timed_out = + front_driver_data.IsDriverStateDataTimedOut(); + front_driver_can_errors.can_error = front_driver_data.IsCANError(); + front_driver_can_errors.heartbeat_timeout = front_driver_data.IsHeartbeatTimeout(); - can_errors.front_can_error = motors_controller_->GetFrontData().IsCANError(); - can_errors.rear_can_error = motors_controller_->GetRearData().IsCANError(); + rear_driver_can_errors.motor_states_data_timed_out = rear_driver_data.IsMotorStatesDataTimedOut(); + rear_driver_can_errors.driver_state_data_timed_out = rear_driver_data.IsDriverStateDataTimedOut(); + rear_driver_can_errors.can_error = rear_driver_data.IsCANError(); + rear_driver_can_errors.heartbeat_timeout = rear_driver_data.IsHeartbeatTimeout(); - can_errors.front_heartbeat_timeout = motors_controller_->GetFrontData().IsHeartbeatTimeout(); - can_errors.rear_heartbeat_timeout = motors_controller_->GetRearData().IsHeartbeatTimeout(); + can_errors.driver_errors.emplace(DriverNames::FRONT, front_driver_can_errors); + can_errors.driver_errors.emplace(DriverNames::REAR, rear_driver_can_errors); - panther_system_ros_interface_->UpdateMsgErrors(can_errors); + system_ros_interface_->UpdateMsgErrors(can_errors); } void PantherSystem::UpdateFlagErrors() { - if ( - motors_controller_->GetFrontData().IsFlagError() || - motors_controller_->GetRearData().IsFlagError()) { + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); + if (front_driver_data.IsFlagError() || rear_driver_data.IsFlagError()) { RCLCPP_WARN_STREAM_THROTTLE( logger_, steady_clock_, 1000, "Error state on one of the drivers:\n" - << "\tFront: " << motors_controller_->GetFrontData().GetFlagErrorLog() - << "\tRear: " << motors_controller_->GetRearData().GetFlagErrorLog()); + << "\tFront: " << front_driver_data.GetFlagErrorLog() + << "\tRear: " << rear_driver_data.GetFlagErrorLog()); roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true); - HandlePDOWriteOperation([this] { motors_controller_->AttemptErrorFlagResetWithZeroSpeed(); }); + HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); }); } else { roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); } @@ -613,77 +141,21 @@ void PantherSystem::UpdateFlagErrors() void PantherSystem::UpdateDriverStateDataTimedOut() { if ( - motors_controller_->GetFrontData().IsDriverStateDataTimedOut() || - motors_controller_->GetRearData().IsDriverStateDataTimedOut()) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, steady_clock_, 1000, - (motors_controller_->GetFrontData().IsDriverStateDataTimedOut() ? "Front " : "") - << (motors_controller_->GetRearData().IsDriverStateDataTimedOut() ? "Rear " : "") - << "PDO driver state timeout."); + robot_driver_->GetData(DriverNames::FRONT).IsDriverStateDataTimedOut() || + robot_driver_->GetData(DriverNames::REAR).IsDriverStateDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout."); roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); } else { roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false); } } -void PantherSystem::HandlePDOWriteOperation(std::function pdo_write_operation) -{ - try { - { - std::unique_lock motor_controller_write_lck( - *motor_controller_write_mtx_, std::defer_lock); - if (!motor_controller_write_lck.try_lock()) { - throw std::runtime_error( - "Can't acquire mutex for writing commands - E-stop is being triggered."); - } - pdo_write_operation(); - } - - roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what()); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); - } -} - -bool PantherSystem::AreVelocityCommandsNearZero() -{ - for (const auto & cmd : hw_commands_velocities_) { - if (std::abs(cmd) > std::numeric_limits::epsilon()) { - return false; - } - } - return true; -} - -void PantherSystem::MotorsPowerEnable(const bool enable) -{ - try { - { - std::lock_guard lck_g(*motor_controller_write_mtx_); - - if (!enable) { - motors_controller_->TurnOnEStop(); - } else { - motors_controller_->TurnOffEStop(); - } - } - - e_stop_->TriggerEStop(); - - roboteq_error_filter_->SetClearErrorsFlag(); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what()); - } -} - void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) { unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; std::string message{"No error detected."}; - const auto front_driver_data = motors_controller_->GetFrontData(); + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); if (front_driver_data.IsError()) { level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; message = "Error detected."; @@ -692,7 +164,7 @@ void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status, front_driver_data.GetErrorMap(), "Front driver error: "); } - const auto rear_driver_data = motors_controller_->GetRearData(); + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); if (rear_driver_data.IsError()) { level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; message = "Error detected."; @@ -717,8 +189,8 @@ void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; std::string message{"Panther system status monitoring."}; - const auto front_driver_state = motors_controller_->GetFrontData().GetDriverState(); - const auto rear_driver_state = motors_controller_->GetRearData().GetDriverState(); + const auto front_driver_state = robot_driver_->GetData(DriverNames::FRONT).GetDriverState(); + const auto rear_driver_state = robot_driver_->GetData(DriverNames::REAR).GetDriverState(); auto driver_states_with_names = { std::make_pair(std::string("Front"), front_driver_state), @@ -732,10 +204,16 @@ void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & driver_name + " driver heatsink temperature (\u00B0C)", driver_state.GetHeatsinkTemperature()); } - status.summary(level, message); } +std::vector PantherSystem::GetSpeedCommands() const +{ + return { + static_cast(hw_commands_velocities_[0]), static_cast(hw_commands_velocities_[1]), + static_cast(hw_commands_velocities_[2]), static_cast(hw_commands_velocities_[3])}; +} + } // namespace panther_hardware_interfaces #include "pluginlib/class_list_macros.hpp" diff --git a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp index 81026556..9a3626c0 100644 --- a/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp +++ b/panther_hardware_interfaces/src/panther_system/robot_driver/roboteq_robot_driver.cpp @@ -56,8 +56,11 @@ void RoboteqRobotDriver::Initialize() DefineDrivers(); for (auto & [name, driver] : drivers_) { data_.emplace(name, DriverData(drivetrain_settings_)); - driver->Boot(); } + + canopen_manager_.Activate(); + BootDrivers(); + } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to initialize robot driver: " + std::string(e.what())); } @@ -81,7 +84,7 @@ void RoboteqRobotDriver::Activate() driver->ResetScript(); } catch (const std::runtime_error & e) { throw std::runtime_error( - "Reset Roboteq script exception on " + name + "driver : " + std::string(e.what())); + "Reset Roboteq script exception on " + name + " driver : " + std::string(e.what())); } } @@ -190,6 +193,13 @@ void RoboteqRobotDriver::TurnOffEStop() } } +bool RoboteqRobotDriver::CommunicationError() +{ + return std::any_of(data_.begin(), data_.end(), [](const auto & data) { + return data.second.IsCANError() || data.second.IsHeartbeatTimeout(); + }); +} + void RoboteqRobotDriver::SetMotorsStates( DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state, const timespec & current_time) @@ -222,4 +232,29 @@ bool RoboteqRobotDriver::DataTimeout( timeout; } +void RoboteqRobotDriver::BootDrivers() +{ + for (auto & [name, driver] : drivers_) { + try { + auto driver_future = driver->Boot(); + auto driver_status = driver_future.wait_for(std::chrono::seconds(5)); + + if (driver_status == std::future_status::ready) { + try { + driver_future.get(); + } catch (const std::exception & e) { + throw std::runtime_error( + "Boot for " + name + " driver failed with exception: " + std::string(e.what())); + } + } else { + throw std::runtime_error("Boot for " + name + " driver timed out or failed."); + } + + } catch (const std::system_error & e) { + throw std::runtime_error( + "An exception occurred while trying to Boot " + name + " driver " + std::string(e.what())); + } + } +} + } // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp b/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp similarity index 57% rename from panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp rename to panther_hardware_interfaces/src/panther_system/system_e_stop.cpp index 9a776b40..35efc987 100644 --- a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp +++ b/panther_hardware_interfaces/src/panther_system/system_e_stop.cpp @@ -17,12 +17,12 @@ #include #include -#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp" +#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp" namespace panther_hardware_interfaces { -bool EStopPTH12X::ReadEStopState() +bool EStop::ReadEStopState() { if (e_stop_manipulation_mtx_.try_lock()) { std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); @@ -39,7 +39,7 @@ bool EStopPTH12X::ReadEStopState() return e_stop_triggered_; } -void EStopPTH12X::TriggerEStop() +void EStop::TriggerEStop() { gpio_controller_->InterruptEStopReset(); @@ -53,7 +53,7 @@ void EStopPTH12X::TriggerEStop() e_stop_triggered_ = true; } -void EStopPTH12X::ResetEStop() +void EStop::ResetEStop() { if (e_stop_manipulation_mtx_.try_lock()) { std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); @@ -83,61 +83,4 @@ void EStopPTH12X::ResetEStop() } } -bool EStopPTH10X::ReadEStopState() -{ - if (e_stop_manipulation_mtx_.try_lock()) { - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); - 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_) { - TriggerEStop(); - } - } - - return e_stop_triggered_; -} - -void EStopPTH10X::TriggerEStop() -{ - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_); - std::lock_guard lck_g(*motor_controller_write_mtx_); - - try { - motors_controller_->TurnOnSafetyStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Error when trying to set safety stop using CAN command: " + std::string(e.what())); - } - - e_stop_triggered_ = true; -} - -void EStopPTH10X::ResetEStop() -{ - if (e_stop_manipulation_mtx_.try_lock()) { - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); - - if (!ZeroVelocityCheck()) { - throw std::runtime_error( - "Can't reset E-stop - last velocity commands are different than zero. Make sure that your " - "controller sends zero commands before trying to reset E-stop."); - } - - if (!gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT)) { - throw std::runtime_error("Can't reset E-stop - motors are not powered."); - } - - if (roboteq_error_filter_->IsError()) { - throw std::runtime_error("Can't reset E-stop - motor controller is in an error state."); - } - - roboteq_error_filter_->SetClearErrorsFlag(); - - e_stop_triggered_ = false; - } else { - std::fprintf(stdout, "E-stop trigger operation is in progress, skipping reset."); - } -} - } // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp similarity index 61% rename from panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp rename to panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp index 9b2d2cbc..f6b5564e 100644 --- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp +++ b/panther_hardware_interfaces/src/panther_system/system_ros_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp" +#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp" #include #include @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "realtime_tools/realtime_publisher.h" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_data_converters.hpp" namespace panther_hardware_interfaces { @@ -52,7 +52,7 @@ void ROSServiceWrapper::CallbackWrapper( response->message = err.what(); RCLCPP_WARN_STREAM( - rclcpp::get_logger("PantherSystem"), + rclcpp::get_logger("UGVSystem"), "An exception occurred while handling the request: " << err.what()); } } @@ -71,11 +71,11 @@ void ROSServiceWrapper>::ProccessC callback_(); } -PantherSystemRosInterface::PantherSystemRosInterface( +SystemROSInterface::SystemROSInterface( const std::string & node_name, const rclcpp::NodeOptions & node_options) : node_(rclcpp::Node::make_shared(node_name, node_options)), diagnostic_updater_(node_) { - RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Constructing node."); + RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Constructing node."); executor_ = std::make_unique(); executor_->add_node(node_); @@ -87,8 +87,6 @@ PantherSystemRosInterface::PantherSystemRosInterface( std::make_unique>( driver_state_publisher_); - InitializeRobotDriverStateMsg(); - io_state_publisher_ = node_->create_publisher( "~/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); realtime_io_state_publisher_ = @@ -99,12 +97,12 @@ PantherSystemRosInterface::PantherSystemRosInterface( realtime_e_stop_state_publisher_ = std::make_unique>(e_stop_state_publisher_); - diagnostic_updater_.setHardwareID("Panther System"); + diagnostic_updater_.setHardwareID("UGV System"); - RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Node constructed successfully."); + RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Node constructed successfully."); } -PantherSystemRosInterface::~PantherSystemRosInterface() +SystemROSInterface::~SystemROSInterface() { if (executor_) { executor_->cancel(); @@ -128,72 +126,55 @@ PantherSystemRosInterface::~PantherSystemRosInterface() node_.reset(); } -void PantherSystemRosInterface::UpdateMsgErrorFlags( - const RoboteqData & front, const RoboteqData & rear) +void SystemROSInterface::UpdateMsgErrorFlags(const std::string & name, const DriverData & data) { auto & driver_state = realtime_driver_state_publisher_->msg_; - auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT); - auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR); + auto & driver_state_named = GetDriverStateByName(driver_state, name); driver_state.header.stamp = node_->get_clock()->now(); - front_driver_state.state.fault_flag = front.GetFaultFlag().GetMessage(); - front_driver_state.state.script_flag = front.GetScriptFlag().GetMessage(); - front_driver_state.state.channel_2_motor_runtime_error = front.GetLeftRuntimeError().GetMessage(); - front_driver_state.state.channel_1_motor_runtime_error = - front.GetRightRuntimeError().GetMessage(); - - rear_driver_state.state.fault_flag = rear.GetFaultFlag().GetMessage(); - rear_driver_state.state.script_flag = rear.GetScriptFlag().GetMessage(); - rear_driver_state.state.channel_2_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage(); - rear_driver_state.state.channel_1_motor_runtime_error = rear.GetRightRuntimeError().GetMessage(); + driver_state_named.state.fault_flag = data.GetFaultFlag().GetMessage(); + driver_state_named.state.script_flag = data.GetScriptFlag().GetMessage(); + driver_state_named.state.channel_1_motor_runtime_error = + data.GetRuntimeError(RoboteqDriver::kChannel1).GetMessage(); + driver_state_named.state.channel_2_motor_runtime_error = + data.GetRuntimeError(RoboteqDriver::kChannel2).GetMessage(); } -void PantherSystemRosInterface::UpdateMsgDriversStates( - const DriverState & front, const DriverState & rear) +void SystemROSInterface::UpdateMsgDriversStates( + const std::string & name, const RoboteqDriverState & state) { auto & driver_state = realtime_driver_state_publisher_->msg_; - auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT); - auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR); - - front_driver_state.state.voltage = front.GetVoltage(); - front_driver_state.state.current = front.GetCurrent(); - front_driver_state.state.temperature = front.GetTemperature(); - front_driver_state.state.heatsink_temperature = front.GetHeatsinkTemperature(); - - rear_driver_state.state.voltage = rear.GetVoltage(); - rear_driver_state.state.current = rear.GetCurrent(); - rear_driver_state.state.temperature = rear.GetTemperature(); - rear_driver_state.state.heatsink_temperature = rear.GetHeatsinkTemperature(); + auto & driver_state_named = GetDriverStateByName(driver_state, name); + + driver_state_named.state.voltage = state.GetVoltage(); + driver_state_named.state.current = state.GetCurrent(); + driver_state_named.state.temperature = state.GetTemperature(); + driver_state_named.state.heatsink_temperature = state.GetHeatsinkTemperature(); } -void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors) +void SystemROSInterface::UpdateMsgErrors(const CANErrors & can_errors) { auto & driver_state = realtime_driver_state_publisher_->msg_; - auto & front_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_FRONT); - auto & rear_driver_state = GetDriverStateByName(driver_state, DriverStateNamedMsg::NAME_REAR); driver_state.error = can_errors.error; driver_state.write_pdo_cmds_error = can_errors.write_pdo_cmds_error; driver_state.read_pdo_motor_states_error = can_errors.read_pdo_motor_states_error; driver_state.read_pdo_driver_state_error = can_errors.read_pdo_driver_state_error; - front_driver_state.state.motor_states_data_timed_out = - can_errors.front_motor_states_data_timed_out; - rear_driver_state.state.motor_states_data_timed_out = can_errors.rear_motor_states_data_timed_out; + for (const auto & [name, driver_errors] : can_errors.driver_errors) { + auto & driver_state_named = GetDriverStateByName(driver_state, name); - front_driver_state.state.driver_state_data_timed_out = - can_errors.front_driver_state_data_timed_out; - rear_driver_state.state.driver_state_data_timed_out = can_errors.rear_driver_state_data_timed_out; - - front_driver_state.state.can_error = can_errors.front_can_error; - rear_driver_state.state.can_error = can_errors.rear_can_error; - - front_driver_state.state.heartbeat_timeout = can_errors.front_heartbeat_timeout; - rear_driver_state.state.heartbeat_timeout = can_errors.rear_heartbeat_timeout; + driver_state_named.state.motor_states_data_timed_out = + driver_errors.motor_states_data_timed_out; + driver_state_named.state.driver_state_data_timed_out = + driver_errors.driver_state_data_timed_out; + driver_state_named.state.can_error = driver_errors.can_error; + driver_state_named.state.heartbeat_timeout = driver_errors.heartbeat_timeout; + } } -void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop) +void SystemROSInterface::PublishEStopStateMsg(const bool e_stop) { realtime_e_stop_state_publisher_->msg_.data = e_stop; if (realtime_e_stop_state_publisher_->trylock()) { @@ -201,21 +182,21 @@ void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop) } } -void PantherSystemRosInterface::PublishEStopStateIfChanged(const bool e_stop) +void SystemROSInterface::PublishEStopStateIfChanged(const bool e_stop) { if (realtime_e_stop_state_publisher_->msg_.data != e_stop) { PublishEStopStateMsg(e_stop); } } -void PantherSystemRosInterface::PublishRobotDriverState() +void SystemROSInterface::PublishRobotDriverState() { if (realtime_driver_state_publisher_->trylock()) { realtime_driver_state_publisher_->unlockAndPublish(); } } -void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( +void SystemROSInterface::InitializeAndPublishIOStateMsg( const std::unordered_map & io_state) { for (const auto & [pin, pin_value] : io_state) { @@ -227,7 +208,7 @@ void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( } } -void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info) +void SystemROSInterface::PublishIOState(const GPIOInfo & gpio_info) { const bool pin_value = (gpio_info.value == gpiod::line::value::ACTIVE); @@ -240,7 +221,7 @@ void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info) } } -bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value) +bool SystemROSInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value) { auto & io_state_msg = realtime_io_state_publisher_->msg_; @@ -277,7 +258,7 @@ bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool p return true; } -rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallbackGroup( +rclcpp::CallbackGroup::SharedPtr SystemROSInterface::GetOrCreateNodeCallbackGroup( const unsigned group_id, rclcpp::CallbackGroupType callback_group_type) { if (group_id == 0) { @@ -302,19 +283,7 @@ rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallb return callback_group; } -void PantherSystemRosInterface::InitializeRobotDriverStateMsg() -{ - DriverStateNamedMsg front_driver_state; - DriverStateNamedMsg rear_driver_state; - front_driver_state.name = DriverStateNamedMsg::NAME_FRONT; - rear_driver_state.name = DriverStateNamedMsg::NAME_REAR; - - auto & driver_state = realtime_driver_state_publisher_->msg_; - driver_state.driver_states.push_back(front_driver_state); - driver_state.driver_states.push_back(rear_driver_state); -} - -DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName( +DriverStateNamedMsg & SystemROSInterface::GetDriverStateByName( RobotDriverStateMsg & robot_driver_state, const std::string & name) { auto & driver_states = robot_driver_state.driver_states; @@ -324,7 +293,10 @@ DriverStateNamedMsg & PantherSystemRosInterface::GetDriverStateByName( [&name](const DriverStateNamedMsg & msg) { return msg.name == name; }); if (it == driver_states.end()) { - throw std::runtime_error("Driver with name '" + name + "' not found."); + DriverStateNamedMsg driver_state_named; + driver_state_named.name = name; + driver_states.push_back(driver_state_named); + it = driver_states.end() - 1; } return *it; diff --git a/panther_hardware_interfaces/src/panther_system/ugv_system.cpp b/panther_hardware_interfaces/src/panther_system/ugv_system.cpp new file mode 100644 index 00000000..fcdf497a --- /dev/null +++ b/panther_hardware_interfaces/src/panther_system/ugv_system.cpp @@ -0,0 +1,550 @@ +// 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/panther_system/ugv_system.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "panther_hardware_interfaces/panther_system/robot_driver/panther_robot_driver.hpp" +#include "panther_hardware_interfaces/panther_system/system_ros_interface.hpp" +#include "panther_hardware_interfaces/utils.hpp" + +#include "panther_utils/common_utilities.hpp" +#include "panther_utils/diagnostics.hpp" + +namespace panther_hardware_interfaces +{ + +CallbackReturn UGVSystem::on_init(const hardware_interface::HardwareInfo & hardware_info) +{ + if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + try { + CheckJointSize(); + SortAndCheckJointNames(); + SetInitialValues(); + CheckInterfaces(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what()); + return CallbackReturn::ERROR; + } + + try { + ReadDrivetrainSettings(); + ReadCANopenSettings(); + ReadInitializationActivationAttempts(); + ReadParametersAndCreateRoboteqErrorFilter(); + ReadDriverStatesUpdateFrequency(); + } catch (const std::invalid_argument & e) { + RCLCPP_ERROR_STREAM( + logger_, "An exception occurred while reading the parameters: " << e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_configure(const rclcpp_lifecycle::State &) +{ + try { + ConfigureGPIOController(); + ConfigureRobotDriver(); + ConfigureEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_cleanup(const rclcpp_lifecycle::State &) +{ + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &) +{ + std::fill(hw_commands_velocities_.begin(), hw_commands_velocities_.end(), 0.0); + std::fill(hw_states_positions_.begin(), hw_states_positions_.end(), 0.0); + std::fill(hw_states_velocities_.begin(), hw_states_velocities_.end(), 0.0); + std::fill(hw_states_efforts_.begin(), hw_states_efforts_.end(), 0.0); + + if (!OperationWithAttempts( + std::bind(&RobotDriverInterface::Activate, robot_driver_), + max_roboteq_activation_attempts_)) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to activate UGV System: Couldn't activate RobotDriver in " + << max_roboteq_activation_attempts_ << " attempts."); + return CallbackReturn::ERROR; + } + + system_ros_interface_ = std::make_unique("hardware_controller"); + + system_ros_interface_->AddService>( + "~/fan_enable", + std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/aux_power_enable", + std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/digital_power_enable", + std::bind( + &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/charger_enable", + std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/led_control_enable", + std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/motor_power_enable", std::bind(&UGVSystem::MotorsPowerEnable, this, std::placeholders::_1)); + + system_ros_interface_->AddService>( + "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto e_stop_reset_qos = rmw_qos_profile_services_default; + e_stop_reset_qos.depth = 1; + system_ros_interface_->AddService>( + "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, + rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos); + + system_ros_interface_->AddDiagnosticTask( + std::string("system errors"), this, &UGVSystem::DiagnoseErrors); + + system_ros_interface_->AddDiagnosticTask( + std::string("system status"), this, &UGVSystem::DiagnoseStatus); + + gpio_controller_->RegisterGPIOEventCallback( + [this](const auto & state) { system_ros_interface_->PublishIOState(state); }); + + const auto io_state = gpio_controller_->QueryControlInterfaceIOStates(); + system_ros_interface_->InitializeAndPublishIOStateMsg(io_state); + + system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState()); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_deactivate(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what()); + return CallbackReturn::ERROR; + } + + system_ros_interface_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_shutdown(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what()); + return CallbackReturn::ERROR; + } + + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + + system_ros_interface_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_error(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what()); + return CallbackReturn::ERROR; + } + + if (system_ros_interface_) { + system_ros_interface_->BroadcastOnDiagnosticTasks( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "An error has occurred during a node state transition."); + + system_ros_interface_.reset(); + } + + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +std::vector UGVSystem::export_state_interfaces() +{ + std::vector state_interfaces; + for (std::size_t i = 0; i < joint_size_; i++) { + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i])); + } + + return state_interfaces; +} + +std::vector UGVSystem::export_command_interfaces() +{ + std::vector command_interfaces; + for (std::size_t i = 0; i < joint_size_; i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); + } + + return command_interfaces; +} + +return_type UGVSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) +{ + UpdateMotorsState(); + + if (time >= next_driver_state_update_time_) { + UpdateDriverState(); + UpdateDriverStateMsg(); + system_ros_interface_->PublishRobotDriverState(); + next_driver_state_update_time_ = time + driver_states_update_period_; + } + + UpdateEStopState(); + + return return_type::OK; +} + +return_type UGVSystem::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + const bool e_stop = e_stop_->ReadEStopState(); + + if (!e_stop) { + HandleRobotDriverWriteOperation([this] { + const auto speed_cmds = GetSpeedCommands(); + robot_driver_->SendSpeedCommands(speed_cmds); + }); + } + + return return_type::OK; +} + +void UGVSystem::CheckJointSize() const +{ + if (info_.joints.size() != joint_size_) { + throw std::runtime_error( + "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " + + std::to_string(joint_size_) + " expected."); + } +} + +void UGVSystem::SortAndCheckJointNames() +{ + // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it + // is necessary to make sure that joints are in specific order and order of definitions in URDF + // doesn't matter + for (std::size_t i = 0; i < joint_size_; i++) { + std::size_t match_count = 0; + + for (std::size_t j = 0; j < joint_size_; j++) { + if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) { + joints_names_sorted_[i] = info_.joints[j].name; + ++match_count; + } + } + + if (match_count != 1) { + throw std::runtime_error( + "There should be exactly one joint containing " + joint_order_[i] + ", " + + std::to_string(match_count) + " found."); + } + } +} + +void UGVSystem::SetInitialValues() +{ + // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it + // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best + // option. + hw_commands_velocities_.resize(joint_size_, 0.0); + + hw_states_positions_.resize(joint_size_, std::numeric_limits::quiet_NaN()); + hw_states_velocities_.resize(joint_size_, std::numeric_limits::quiet_NaN()); + hw_states_efforts_.resize(joint_size_, std::numeric_limits::quiet_NaN()); +} + +void UGVSystem::CheckInterfaces() const +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + // Commands + if (joint.command_interfaces.size() != 1) { + throw std::runtime_error( + "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) + + " command interfaces. 1 expected."); + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.command_interfaces[0].name + + " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); + } + + // States + if (joint.state_interfaces.size() != 3) { + throw std::runtime_error( + "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) + + " state " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") + + " 3 expected."); + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[0].name + + " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected."); + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[1].name + + " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); + } + + if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[2].name + + " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected."); + } + } +} + +void UGVSystem::ReadDrivetrainSettings() +{ + drivetrain_settings_.motor_torque_constant = + std::stof(info_.hardware_parameters["motor_torque_constant"]); + drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]); + drivetrain_settings_.gearbox_efficiency = + std::stof(info_.hardware_parameters["gearbox_efficiency"]); + drivetrain_settings_.encoder_resolution = + std::stof(info_.hardware_parameters["encoder_resolution"]); + drivetrain_settings_.max_rpm_motor_speed = + std::stof(info_.hardware_parameters["max_rpm_motor_speed"]); +} + +void UGVSystem::ReadCANopenSettings() +{ + canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"]; + canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]); + canopen_settings_.pdo_motor_states_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"])); + canopen_settings_.pdo_driver_state_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"])); + canopen_settings_.sdo_operation_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"])); + + ReadCANopenSettingsDriverCANIDs(); +} + +void UGVSystem::ReadInitializationActivationAttempts() +{ + max_roboteq_initialization_attempts_ = + std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]); + max_roboteq_activation_attempts_ = + std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]); +} + +void UGVSystem::ReadParametersAndCreateRoboteqErrorFilter() +{ + const unsigned max_write_pdo_cmds_errors_count = + std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]); + const unsigned max_read_pdo_motor_states_errors_count = + std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]); + const unsigned max_read_pdo_driver_state_errors_count = + std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]); + + roboteq_error_filter_ = std::make_shared( + max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count, + max_read_pdo_driver_state_errors_count, 1); +} + +void UGVSystem::ReadDriverStatesUpdateFrequency() +{ + const float driver_states_update_frequency = + std::stof(info_.hardware_parameters["driver_states_update_frequency"]); + driver_states_update_period_ = + rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency); +} + +void UGVSystem::ConfigureGPIOController() +{ + gpio_controller_ = GPIOControllerFactory::CreateGPIOController(1.2); + gpio_controller_->Start(); + + RCLCPP_INFO(logger_, "Successfully configured GPIO controller."); +} + +void UGVSystem::ConfigureRobotDriver() +{ + robot_driver_write_mtx_ = std::make_shared(); + DefineRobotDriver(); + + if (!OperationWithAttempts( + std::bind(&RobotDriverInterface::Initialize, robot_driver_), + max_roboteq_initialization_attempts_, + std::bind(&RobotDriverInterface::Deinitialize, robot_driver_))) { + throw std::runtime_error("Roboteq drivers initialization failed."); + } + + RCLCPP_INFO(logger_, "Successfully configured robot driver"); +} + +void UGVSystem::ConfigureEStop() +{ + if (!gpio_controller_ || !roboteq_error_filter_ || !robot_driver_ || !robot_driver_write_mtx_) { + throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first."); + } + + e_stop_ = std::make_shared( + gpio_controller_, roboteq_error_filter_, robot_driver_, robot_driver_write_mtx_, + std::bind(&UGVSystem::AreVelocityCommandsNearZero, this)); + + RCLCPP_INFO(logger_, "Successfully configured E-Stop"); +} + +void UGVSystem::UpdateMotorsState() +{ + try { + robot_driver_->UpdateMotorsState(); + UpdateHwStates(); + UpdateMotorsStateDataTimedOut(); + } catch (const std::runtime_error & e) { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); + + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, steady_clock_, 5000, + "An exception occurred while updating motors states: " << e.what()); + } +} + +void UGVSystem::UpdateDriverState() +{ + try { + robot_driver_->UpdateDriversState(); + UpdateFlagErrors(); + UpdateDriverStateDataTimedOut(); + } catch (const std::runtime_error & e) { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, steady_clock_, 5000, + "An exception occurred while updating drivers states: " << e.what()); + } +} + +void UGVSystem::UpdateEStopState() +{ + if (robot_driver_->CommunicationError()) { + e_stop_->TriggerEStop(); + } + + const bool e_stop = e_stop_->ReadEStopState(); + system_ros_interface_->PublishEStopStateIfChanged(e_stop); +} + +void UGVSystem::HandleRobotDriverWriteOperation(std::function write_operation) +{ + try { + { + std::unique_lock robot_driver_write_lck( + *robot_driver_write_mtx_, std::defer_lock); + if (!robot_driver_write_lck.try_lock()) { + throw std::runtime_error( + "Can't acquire mutex for writing commands - E-stop is being triggered."); + } + write_operation(); + } + + roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what()); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); + } +} + +bool UGVSystem::AreVelocityCommandsNearZero() +{ + for (const auto & cmd : hw_commands_velocities_) { + if (std::abs(cmd) > std::numeric_limits::epsilon()) { + return false; + } + } + return true; +} + +void UGVSystem::MotorsPowerEnable(const bool enable) +{ + try { + { + std::lock_guard lck_g(*robot_driver_write_mtx_); + + if (!enable) { + robot_driver_->TurnOnEStop(); + } else { + robot_driver_->TurnOffEStop(); + } + } + + e_stop_->TriggerEStop(); + + roboteq_error_filter_->SetClearErrorsFlag(); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what()); + } +} + +} // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp deleted file mode 100644 index 3b8c0829..00000000 --- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp +++ /dev/null @@ -1,777 +0,0 @@ -// Copyright 2023 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -#include - -#include - -#include - -#include "utils/panther_system_test_utils.hpp" -#include "utils/roboteqs_mock.hpp" - -using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; - -class TestPantherSystem : public ::testing::Test -{ -public: - TestPantherSystem() { pth_test_.Start(pth_test_.GetDefaultPantherSystemUrdf()); } - ~TestPantherSystem() { pth_test_.Stop(); } - - void CheckInterfaces() - { - EXPECT_EQ(pth_test_.GetResourceManager()->system_components_size(), 1u); - ASSERT_EQ(pth_test_.GetResourceManager()->state_interface_keys().size(), 12u); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/position")); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/velocity")); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/effort")); - - ASSERT_EQ(pth_test_.GetResourceManager()->command_interface_keys().size(), 4u); - - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("fl_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("fr_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("rl_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("rr_wheel_joint/velocity")); - } - - void CheckInitialValues() - { - using hardware_interface::LoanedCommandInterface; - using hardware_interface::LoanedStateInterface; - - LoanedStateInterface fl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position"); - LoanedStateInterface fr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position"); - LoanedStateInterface rl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position"); - LoanedStateInterface rr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position"); - - LoanedStateInterface fl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity"); - LoanedStateInterface fr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity"); - LoanedStateInterface rl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity"); - LoanedStateInterface rr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity"); - - LoanedStateInterface fl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort"); - LoanedStateInterface fr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort"); - LoanedStateInterface rl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort"); - LoanedStateInterface rr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort"); - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - ASSERT_FLOAT_EQ(fl_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_p.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_v.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_e.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.0); - } - - // 100 Hz - const float period_ = 0.01; - - panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; -}; - -// TRANSITIONS -TEST_F(TestPantherSystem, ConfigureActivateFinalizePantherSystem) -{ - using panther_hardware_interfaces_test::kPantherSystemName; - - // check if hardware is configured - auto status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); - - try { - pth_test_.ConfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.ActivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); - - // Check interfaces - CheckInterfaces(); - - // Check initial values - CheckInitialValues(); - - try { - pth_test_.ShutdownPantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ShutdownPantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::FINALIZED); -} - -TEST_F(TestPantherSystem, ConfigureActivateDeactivateDeconfigurePantherSystem) -{ - using panther_hardware_interfaces_test::kPantherSystemName; - - auto status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); - - try { - pth_test_.ConfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.ActivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); - - // Check interfaces - CheckInterfaces(); - - // Check initial values - CheckInitialValues(); - - try { - pth_test_.DeactivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to DeactivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.UnconfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to UnconfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); -} - -// WRITING -TEST_F(TestPantherSystem, WriteCommandsPantherSystem) -{ - using hardware_interface::LoanedCommandInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd; - - const float fl_v = 0.1; - const float fr_v = 0.2; - const float rl_v = 0.3; - const float rr_v = 0.4; - - pth_test_.ConfigureActivatePantherSystem(); - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(fl_v); - fr_c_v.set_value(fr_v); - rl_c_v.set_value(rl_v); - rr_c_v.set_value(rr_v); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v); - - const auto TIME = rclcpp::Time(0); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(fl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(fr_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(rl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(rr_v * kRadPerSecToRbtqCmd)); - - pth_test_.ShutdownPantherSystem(); -} - -// READING -TEST_F(TestPantherSystem, ReadFeedbackPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; - using panther_hardware_interfaces_test::kRbtqPosFbToRad; - using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; - - const std::int32_t fl_val = 100; - const std::int32_t fr_val = 200; - const std::int32_t rl_val = 300; - const std::int32_t rr_val = 400; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_val); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_val); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_val); - - pth_test_.ConfigureActivatePantherSystem(); - - LoanedStateInterface fl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position"); - LoanedStateInterface fr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position"); - LoanedStateInterface rl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position"); - LoanedStateInterface rr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position"); - - LoanedStateInterface fl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity"); - LoanedStateInterface fr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity"); - LoanedStateInterface rl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity"); - LoanedStateInterface rr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity"); - - LoanedStateInterface fl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort"); - LoanedStateInterface fr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort"); - LoanedStateInterface rl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort"); - LoanedStateInterface rr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort"); - - const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - try { - pth_test_.GetResourceManager()->read(TIME, PERIOD); - } catch (const std::exception & e) { - FAIL() << "Exception: " << e.what(); - return; - } - - ASSERT_FLOAT_EQ(fl_s_p.get_value(), fl_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(fr_s_p.get_value(), fr_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(rl_s_p.get_value(), rl_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(rr_s_p.get_value(), rr_val * kRbtqPosFbToRad); - - ASSERT_FLOAT_EQ(fl_s_v.get_value(), fl_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(fr_s_v.get_value(), fr_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(rl_s_v.get_value(), rl_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(rr_s_v.get_value(), rr_val * kRbtqVelFbToRadPerSec); - - ASSERT_FLOAT_EQ(fl_s_e.get_value(), fl_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(fr_s_e.get_value(), fr_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(rl_s_e.get_value(), rl_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(rr_s_e.get_value(), rr_val * kRbtqCurrentFbToNewtonMeters); - - pth_test_.ShutdownPantherSystem(); -} - -TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - - const std::int16_t f_temp = 30; - const std::int16_t r_temp = 32; - const std::int16_t f_heatsink_temp = 31; - const std::int16_t r_heatsink_temp = 33; - const std::uint16_t f_volt = 400; - const std::uint16_t r_volt = 430; - const std::int16_t f_battery_current_1 = 10; - const std::int16_t r_battery_current_1 = 30; - const std::int16_t f_battery_current_2 = 30; - const std::int16_t r_battery_current_2 = 40; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetTemperature(f_temp); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetTemperature(r_temp); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVoltage(f_volt); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVoltage(r_volt); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2); - - rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); - - pth_test_.ConfigureActivatePantherSystem(); - - RobotDriverStateMsg::SharedPtr state_msg; - unsigned state_msg_count = 0; - auto sub = node->create_subscription( - panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(), - [&](const RobotDriverStateMsg::SharedPtr msg) { - state_msg = msg; - ++state_msg_count; - }); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto simulated_time = node->get_clock()->now(); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - try { - pth_test_.GetResourceManager()->read(simulated_time, PERIOD); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); - } catch (const std::exception & e) { - FAIL() << "Exception: " << e.what(); - return; - } - - ASSERT_TRUE(state_msg); - - ASSERT_EQ(static_cast(state_msg->driver_states.at(0).state.temperature), f_temp); - ASSERT_EQ(static_cast(state_msg->driver_states.at(1).state.temperature), r_temp); - - ASSERT_EQ( - static_cast(state_msg->driver_states.at(0).state.heatsink_temperature), - f_heatsink_temp); - ASSERT_EQ( - static_cast(state_msg->driver_states.at(1).state.heatsink_temperature), - r_heatsink_temp); - - ASSERT_EQ( - static_cast(state_msg->driver_states.at(0).state.voltage * 10.0), f_volt); - ASSERT_EQ( - static_cast(state_msg->driver_states.at(1).state.voltage * 10.0), r_volt); - - ASSERT_EQ( - static_cast(state_msg->driver_states.at(0).state.current * 10.0), - (f_battery_current_1 + f_battery_current_2)); - ASSERT_EQ( - static_cast(state_msg->driver_states.at(1).state.current * 10.0), - (r_battery_current_1 + r_battery_current_2)); - - pth_test_.ShutdownPantherSystem(); -} - -// ENCODER DISCONNECTED -TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem) -{ - using hardware_interface::LoanedCommandInterface; - using panther_hardware_interfaces_test::DriverChannel; - using panther_hardware_interfaces_test::DriverScriptFlags; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetDriverScriptFlag( - DriverScriptFlags::ENCODER_DISCONNECTED); - - rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); - - pth_test_.ConfigureActivatePantherSystem(); - - RobotDriverStateMsg::SharedPtr state_msg; - auto sub = node->create_subscription( - panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(), - [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; }); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->read(TIME, PERIOD); - - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); - ASSERT_TRUE(state_msg->driver_states.at(0).state.script_flag.encoder_disconnected); - - // writing should be blocked - error - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(0.1); - fr_c_v.set_value(0.1); - rl_c_v.set_value(0.1); - rr_c_v.set_value(0.1); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.1); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - - pth_test_.ShutdownPantherSystem(); -} - -// INITIAL PROCEDURE -TEST_F(TestPantherSystem, InitialProcedureTestPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - using panther_hardware_interfaces_test::DriverChannel; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 234); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 32); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 54); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 12); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetResetRoboteqScript(65); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetResetRoboteqScript(23); - - pth_test_.ConfigureActivatePantherSystem(); - - ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetResetRoboteqScript(), 2); - ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetRearDriver()->GetResetRoboteqScript(), 2); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - - pth_test_.ShutdownPantherSystem(); -} - -// ERROR HANDLING -// TODO: FIX - return code -10, but otherwise seems to work -// TEST(TestPantherSystemOthers, test_error_state) -// { -// using panther_hardware_interfaces_test::kDefaultJoints; -// using panther_hardware_interfaces_test::kDefaultParamMap; -// using panther_hardware_interfaces_test::kPantherSystemName; - -// panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - -// auto param_map = kDefaultParamMap; - -// param_map["max_write_pdo_cmds_errors_count"] = "1"; -// param_map["max_read_pdo_motor_states_errors_count"] = "1"; -// param_map["max_read_pdo_driver_state_errors_count"] = "1"; - -// const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints); -// const float period_ = 0.01; - -// pth_test_.Start(panther_system_urdf_); - -// pth_test_.ConfigureActivatePantherSystem(); - -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetOnWriteWait(0x202C, 0, 200000); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->SetOnWriteWait(0x202C, 0, 200000); - -// auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); -// const auto PERIOD = rclcpp::Duration::from_seconds(period_); - -// pth_test_.GetResourceManager()->read(TIME, PERIOD); -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// auto status_map = pth_test_.GetResourceManager()->get_components_status(); -// ASSERT_EQ( -// status_map[kPantherSystemName].state.label(), -// hardware_interface::lifecycle_state_names::FINALIZED); - -// pth_test_.Stop(); -// } - -// WRONG ORDER URDF -TEST(TestPantherSystemOthers, WrongOrderURDF) -{ - using hardware_interface::LoanedCommandInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kDefaultParamMap; - using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd; - - const float period_ = 0.01; - - panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - - std::vector joints = { - "rr_wheel_joint", "fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint"}; - - const std::string panther_system_urdf_ = pth_test_.BuildUrdf(kDefaultParamMap, joints); - - pth_test_.Start(panther_system_urdf_); - - const float fl_v = 0.1; - const float fr_v = 0.2; - const float rl_v = 0.3; - const float rr_v = 0.4; - - pth_test_.ConfigureActivatePantherSystem(); - - // loaned command interfaces have to be destroyed before running Stop - { - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(fl_v); - fr_c_v.set_value(fr_v); - rl_c_v.set_value(rl_v); - rr_c_v.set_value(rr_v); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v); - - const auto TIME = rclcpp::Time(0); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(fl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(fr_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(rl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(rr_v * kRadPerSecToRbtqCmd)); - } - - pth_test_.ShutdownPantherSystem(); - - pth_test_.Stop(); -} - -// TIMEOUT TESTS - -// TODO: fix -// TEST(TestPantherSystemOthers, pdo_read_motors_states_timeout_test) -// { -// using panther_hardware_interfaces_test::kDefaultJoints; -// using panther_hardware_interfaces_test::kDefaultParamMap; - -// panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - -// auto param_map = kDefaultParamMap; - -// // It is necessary to set max_read_pdo_errors_count to some higher value, because -// // adding wait time to Roboteq mock block all communication (also PDO), and PDO timeouts -// // happen -// param_map["pdo_motor_states_timeout_ms"] = "15"; -// param_map["max_write_pdo_cmds_errors_count"] = "100"; -// param_map["max_read_pdo_motor_states_errors_count"] = "2"; -// param_map["max_read_pdo_driver_state_errors_count"] = "2"; - -// const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints); -// const float period_ = 0.01; - -// pth_test_.Start(panther_system_urdf_); - -// rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); -// pth_test_.ConfigureActivatePantherSystem(); - -// RobotDriverStateMsg::SharedPtr state_msg; -// auto sub = node->create_subscription( -// panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(), -// [&](const RobotDriverStateMsg::SharedPtr msg) { state_msg = msg; }); - -// std::this_thread::sleep_for(std::chrono::seconds(2)); - -// auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); -// const auto PERIOD = rclcpp::Duration::from_seconds(period_); - -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_FALSE(state_msg->read_pdo_motor_states_error); -// state_msg.reset(); - -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing(); - -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// std::this_thread::sleep_for(PERIOD.to_chrono()); - -// TIME += PERIOD; -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_FALSE(state_msg->read_pdo_motor_states_error); -// state_msg.reset(); - -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// std::this_thread::sleep_for(PERIOD.to_chrono()); - -// TIME += PERIOD; -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_TRUE(state_msg->read_pdo_motor_states_error); - -// pth_test_.ShutdownPantherSystem(); - -// pth_test_.Stop(); -// } - -// todo E-stop tests - it will the best to add them along with GPIO, as it will change the E-stop -// procedure - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - // For testing individual tests: - // testing::GTEST_FLAG(filter) = "TestPantherSystemOthers.pdo_read_motors_states_timeout_test"; - - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp deleted file mode 100644 index 5db85fc1..00000000 --- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp +++ /dev/null @@ -1,286 +0,0 @@ -// Copyright 2023 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include - -#include - -#include "utils/test_constants.hpp" - -using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; -class TestPantherSystemRosInterface : public ::testing::Test -{ -public: - TestPantherSystemRosInterface() - { - using panther_hardware_interfaces::PantherSystemRosInterface; - - test_node_ = std::make_shared("test_panther_system_node"); - - driver_state_sub_ = test_node_->create_subscription( - panther_hardware_interfaces_test::kRobotDriverStateTopic, rclcpp::SensorDataQoS(), - [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_msg_ = msg; }); - - panther_system_ros_interface_ = - std::make_unique("hardware_controller"); - } - - ~TestPantherSystemRosInterface() { panther_system_ros_interface_.reset(); } - -protected: - rclcpp::Node::SharedPtr test_node_; - rclcpp::Subscription::SharedPtr driver_state_sub_; - RobotDriverStateMsg::SharedPtr driver_state_msg_; - std::unique_ptr - panther_system_ros_interface_; -}; - -TEST(TestPantherSystemRosInterfaceInitialization, NodeCreation) -{ - using panther_hardware_interfaces::PantherSystemRosInterface; - - std::vector node_names; - const std::string panther_system_node_name = "hardware_controller"; - const std::string panther_system_node_name_with_ns = "/" + panther_system_node_name; - - rclcpp::Node::SharedPtr test_node = std::make_shared("test_panther_system_node"); - - std::unique_ptr panther_system_ros_interface; - - panther_system_ros_interface = - std::make_unique(panther_system_node_name); - node_names = test_node->get_node_names(); - ASSERT_TRUE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - panther_system_ros_interface.reset(); - node_names = test_node->get_node_names(); - ASSERT_FALSE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - // Check if it is possible to create a node once again (if everything was cleaned up properly) - panther_system_ros_interface = - std::make_unique(panther_system_node_name); - node_names = test_node->get_node_names(); - ASSERT_TRUE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - panther_system_ros_interface.reset(); -} - -TEST(TestPantherSystemRosInterfaceInitialization, Activation) -{ - using panther_hardware_interfaces::PantherSystemRosInterface; - using panther_hardware_interfaces_test::kRobotDriverStateTopic; - - std::map> service_names_and_types; - std::map> topic_names_and_types; - - rclcpp::Node::SharedPtr test_node = std::make_shared("test_panther_system_node"); - - std::unique_ptr panther_system_ros_interface = - std::make_unique("hardware_controller"); - - // Necessary to add some waiting, so that topic lists are updated - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface.reset(); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_FALSE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface = std::make_unique("hardware_controller"); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_TRUE(topic_names_and_types.find(kRobotDriverStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface.reset(); -} - -TEST_F(TestPantherSystemRosInterface, ErrorFlags) -{ - panther_hardware_interfaces::RoboteqData front( - panther_hardware_interfaces_test::kDrivetrainSettings); - panther_hardware_interfaces::RoboteqData rear( - panther_hardware_interfaces_test::kDrivetrainSettings); - - panther_hardware_interfaces::RoboteqDriverState front_driver_state; - front_driver_state.fault_flags = 0b00000001; - front_driver_state.script_flags = 0b00000010; - front_driver_state.runtime_stat_flag_motor_1 = 0b00001000; - front_driver_state.runtime_stat_flag_motor_2 = 0b00000100; - - panther_hardware_interfaces::RoboteqDriverState rear_driver_state; - rear_driver_state.fault_flags = 0b00000010; - rear_driver_state.script_flags = 0b00000001; - rear_driver_state.runtime_stat_flag_motor_1 = 0b00100000; - rear_driver_state.runtime_stat_flag_motor_2 = 0b00010000; - - front.SetDriverState(front_driver_state, false); - rear.SetDriverState(rear_driver_state, false); - - panther_system_ros_interface_->UpdateMsgErrorFlags(front, rear); - panther_system_ros_interface_->PublishRobotDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.fault_flag.overheat); - EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.script_flag.encoder_disconnected); - EXPECT_TRUE( - driver_state_msg_->driver_states.at(0).state.channel_2_motor_runtime_error.loop_error); - EXPECT_TRUE( - driver_state_msg_->driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active); - - EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.fault_flag.overvoltage); - EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.script_flag.loop_error); - EXPECT_TRUE(driver_state_msg_->driver_states.at(1) - .state.channel_2_motor_runtime_error.forward_limit_triggered); - EXPECT_TRUE(driver_state_msg_->driver_states.at(1) - .state.channel_1_motor_runtime_error.reverse_limit_triggered); -} - -TEST_F(TestPantherSystemRosInterface, DriversStates) -{ - panther_hardware_interfaces::DriverState front; - panther_hardware_interfaces::DriverState rear; - - const std::int16_t f_temp = 36; - const std::int16_t f_heatsink_temp = 37; - const std::uint16_t f_volt = 405; - const std::int16_t f_battery_current_1 = 15; - const std::int16_t f_battery_current_2 = 12; - const std::int16_t r_temp = 35; - const std::int16_t r_heatsink_temp = 36; - const std::uint16_t r_volt = 404; - const std::int16_t r_battery_current_1 = 14; - const std::int16_t r_battery_current_2 = 11; - - front.SetTemperature(f_temp); - front.SetHeatsinkTemperature(f_heatsink_temp); - front.SetVoltage(f_volt); - front.SetBatteryCurrent1(f_battery_current_1); - front.SetBatteryCurrent2(f_battery_current_2); - - rear.SetTemperature(r_temp); - rear.SetHeatsinkTemperature(r_heatsink_temp); - rear.SetVoltage(r_volt); - rear.SetBatteryCurrent1(r_battery_current_1); - rear.SetBatteryCurrent2(r_battery_current_2); - - panther_system_ros_interface_->UpdateMsgDriversStates(front, rear); - panther_system_ros_interface_->PublishRobotDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(0).state.temperature), f_temp); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(1).state.temperature), r_temp); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(0).state.heatsink_temperature), - f_heatsink_temp); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(1).state.heatsink_temperature), - r_heatsink_temp); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(0).state.voltage * 10.0), - f_volt); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(1).state.voltage * 10.0), - r_volt); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(0).state.current * 10.0), - (f_battery_current_1 + f_battery_current_2)); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->driver_states.at(1).state.current * 10.0), - (r_battery_current_1 + r_battery_current_2)); -} - -TEST_F(TestPantherSystemRosInterface, Errors) -{ - panther_hardware_interfaces::CANErrors can_errors; - can_errors.error = true; - - can_errors.write_pdo_cmds_error = true; - can_errors.read_pdo_motor_states_error = false; - can_errors.read_pdo_driver_state_error = false; - - can_errors.front_motor_states_data_timed_out = true; - can_errors.rear_motor_states_data_timed_out = false; - - can_errors.front_driver_state_data_timed_out = false; - can_errors.rear_driver_state_data_timed_out = true; - - can_errors.front_can_error = false; - can_errors.rear_can_error = true; - - panther_system_ros_interface_->UpdateMsgErrors(can_errors); - - panther_system_ros_interface_->PublishRobotDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_TRUE(driver_state_msg_->error); - - EXPECT_TRUE(driver_state_msg_->write_pdo_cmds_error); - EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error); - EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error); - - EXPECT_TRUE(driver_state_msg_->driver_states.at(0).state.motor_states_data_timed_out); - EXPECT_FALSE(driver_state_msg_->driver_states.at(1).state.motor_states_data_timed_out); - - EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.driver_state_data_timed_out); - EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.driver_state_data_timed_out); - - EXPECT_FALSE(driver_state_msg_->driver_states.at(0).state.can_error); - EXPECT_TRUE(driver_state_msg_->driver_states.at(1).state.can_error); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - - auto run_tests = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - return run_tests; -} diff --git a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp index 9c990a7f..714de2d9 100644 --- a/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp +++ b/panther_hardware_interfaces/test/unit/panther_system/robot_driver/test_roboteq_robot_driver.cpp @@ -44,21 +44,19 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob { // Assume 2 drivers and 4 motor drivers mock_fl_motor_driver = - std::make_shared<::testing::NiceMock>(); + std::make_shared(); mock_fr_motor_driver = - std::make_shared<::testing::NiceMock>(); + std::make_shared(); mock_rl_motor_driver = - std::make_shared<::testing::NiceMock>(); + std::make_shared(); mock_rr_motor_driver = - std::make_shared<::testing::NiceMock>(); + std::make_shared(); - mock_front_driver = - std::make_shared<::testing::NiceMock>(); + mock_front_driver = std::make_shared(); mock_front_driver->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver); mock_front_driver->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver); - mock_rear_driver = - std::make_shared<::testing::NiceMock>(); + mock_rear_driver = std::make_shared(); mock_rear_driver->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver); mock_rear_driver->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver); } @@ -77,18 +75,12 @@ class RoboteqRobotDriverWrapper : public panther_hardware_interfaces::RoboteqRob static constexpr char kLeftMotorDriverName[] = "left"; static constexpr char kRightMotorDriverName[] = "right"; - std::shared_ptr<::testing::NiceMock> - mock_front_driver; - std::shared_ptr<::testing::NiceMock> - mock_rear_driver; - std::shared_ptr<::testing::NiceMock> - mock_fl_motor_driver; - std::shared_ptr<::testing::NiceMock> - mock_fr_motor_driver; - std::shared_ptr<::testing::NiceMock> - mock_rl_motor_driver; - std::shared_ptr<::testing::NiceMock> - mock_rr_motor_driver; + std::shared_ptr mock_front_driver; + std::shared_ptr mock_rear_driver; + std::shared_ptr mock_fl_motor_driver; + std::shared_ptr mock_fr_motor_driver; + std::shared_ptr mock_rl_motor_driver; + std::shared_ptr mock_rr_motor_driver; }; class TestRoboteqRobotDriverInitialization : public ::testing::Test @@ -510,6 +502,29 @@ TEST_F(TestRoboteqRobotDriver, TurnOffEStopError) EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error); } +TEST_F(TestRoboteqRobotDriver, CommunicationError) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(false)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + EXPECT_FALSE(robot_driver_->CommunicationError()); + + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + EXPECT_TRUE(robot_driver_->CommunicationError()); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.cpp new file mode 100644 index 00000000..ed1ecd65 --- /dev/null +++ b/panther_hardware_interfaces/test/unit/panther_system/test_lynx_system.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 +#include +#include +#include + +#include +#include + +#include + +#include "panther_hardware_interfaces/panther_system/lynx_system.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp" + +#include "utils/system_test_utils.hpp" +#include "utils/test_constants.hpp" + +class LynxSystemWrapper : public panther_hardware_interfaces::LynxSystem +{ +public: + LynxSystemWrapper() : LynxSystem() + { + mock_robot_driver = + std::make_shared(); + mock_gpio_controller = + std::make_shared(); + mock_e_stop = std::make_shared(); + } + + void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; } + void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; } + void ConfigureEStop() override { e_stop_ = mock_e_stop; } + + void ReadCANopenSettingsDriverCANIDs() { LynxSystem::ReadCANopenSettingsDriverCANIDs(); } + void UpdateHwStates() { LynxSystem::UpdateHwStates(); } + void UpdateMotorsStateDataTimedOut() { LynxSystem::UpdateMotorsStateDataTimedOut(); } + void UpdateDriverStateDataTimedOut() { LynxSystem::UpdateDriverStateDataTimedOut(); } + + void UpdateDriverStateMsg() { LynxSystem::UpdateDriverStateMsg(); } + void UpdateFlagErrors() { LynxSystem::UpdateFlagErrors(); } + std::vector GetSpeedCommands() const { return LynxSystem::GetSpeedCommands(); } + + void SetHwCommandsVelocities(std::vector & velocities) + { + hw_commands_velocities_[0] = velocities[0]; + hw_commands_velocities_[1] = velocities[1]; + hw_commands_velocities_[2] = velocities[2]; + hw_commands_velocities_[3] = velocities[3]; + } + + panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; } + std::vector GetHwStatesPositions() { return hw_states_positions_; } + std::vector GetHwStatesVelocities() { return hw_states_velocities_; } + std::vector GetHwStatesEfforts() { return hw_states_efforts_; } + + std::shared_ptr GetRoboteqErrorFilter() + { + return roboteq_error_filter_; + } + + std::shared_ptr mock_robot_driver; + std::shared_ptr + mock_gpio_controller; + std::shared_ptr mock_e_stop; +}; + +class TestLynxSystem : public ::testing::Test +{ +public: + TestLynxSystem() + { + lynx_system_ = std::make_shared(); + + hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + hardware_info_.hardware_parameters.emplace("driver_can_id", "1"); + + lynx_system_->on_init(hardware_info_); + lynx_system_->on_configure(rclcpp_lifecycle::State()); + } + + ~TestLynxSystem() {} + +protected: + std::shared_ptr lynx_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestLynxSystem, ReadCANopenSettingsDriverCANIDs) +{ + ASSERT_NO_THROW(lynx_system_->ReadCANopenSettingsDriverCANIDs()); + + const auto canopen_settings = lynx_system_->GetCANopenSettings(); + + EXPECT_EQ(canopen_settings.driver_can_ids.size(), 1); + EXPECT_EQ( + canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::DEFAULT), 1); +} + +TEST_F(TestLynxSystem, UpdateHwStates) +{ + const std::int32_t left_pos = 10; + const std::int16_t left_vel = 20; + const std::int16_t left_eff = 30; + const std::int32_t right_pos = 40; + const std::int16_t right_vel = 50; + const std::int16_t right_eff = 60; + + const auto left_expected_pos = left_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto left_expected_vel = left_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto left_expected_eff = left_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto right_expected_pos = right_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto right_expected_vel = right_vel * + panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto right_expected_eff = right_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + + panther_hardware_interfaces::MotorDriverState left_motor_driver_state = { + left_pos, left_vel, left_eff, {0, 0}, {0, 0}}; + panther_hardware_interfaces::MotorDriverState right_motor_driver_state = { + right_pos, right_vel, right_eff, {0, 0}, {0, 0}}; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + // left - channel 2, right - channel 1 + roboteq_data.SetMotorsStates(right_motor_driver_state, left_motor_driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + ASSERT_NO_THROW(lynx_system_->UpdateHwStates()); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[0], left_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[1], right_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[2], left_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[3], right_expected_pos); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[0], left_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[1], right_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[2], left_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[3], right_expected_vel); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[0], left_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[1], right_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[2], left_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[3], right_expected_eff); +} + +TEST_F(TestLynxSystem, UpdateMotorsStateDataTimedOut) +{ + panther_hardware_interfaces::MotorDriverState motor_driver_state; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateMotorsStateDataTimedOut(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateMotorsStateDataTimedOut(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, UpdateDriverStateMsg) +{ + // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for + // system_ros_interface_ +} + +TEST_F(TestLynxSystem, UpdateFlagErrors) +{ + auto driver_state = panther_hardware_interfaces::DriverState(); + driver_state.fault_flags = 0b01; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateFlagErrors(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_TRUE(error); + + // check if reset error works + driver_state.fault_flags = 0; + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateFlagErrors(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, UpdateDriverStateDataTimedOut) +{ + panther_hardware_interfaces::DriverState driver_state; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, true); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateDriverStateDataTimedOut(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateDriverStateDataTimedOut(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, GetSpeedCommands) +{ + const auto fl_v = 0.1; + const auto fr_v = 0.2; + const auto rl_v = 0.3; + const auto rr_v = 0.4; + + std::vector velocities = {fl_v, fr_v, rl_v, rr_v}; + lynx_system_->SetHwCommandsVelocities(velocities); + const auto speed_cmd = lynx_system_->GetSpeedCommands(); + + // only front left and front right motors are used for speed commands + ASSERT_EQ(speed_cmd.size(), 2); + EXPECT_FLOAT_EQ(speed_cmd[0], fl_v); + EXPECT_FLOAT_EQ(speed_cmd[1], fr_v); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp new file mode 100644 index 00000000..f75704ef --- /dev/null +++ b/panther_hardware_interfaces/test/unit/panther_system/test_panther_system.cpp @@ -0,0 +1,375 @@ +// 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 "panther_hardware_interfaces/panther_system/panther_system.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/roboteq_robot_driver.hpp" + +#include "utils/system_test_utils.hpp" +#include "utils/test_constants.hpp" + +class PantherSystemWrapper : public panther_hardware_interfaces::PantherSystem +{ +public: + PantherSystemWrapper() : PantherSystem() + { + mock_robot_driver = + std::make_shared(); + mock_gpio_controller = + std::make_shared(); + mock_e_stop = std::make_shared(); + + ON_CALL( + *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .WillByDefault(::testing::ReturnRef(default_driver_data)); + ON_CALL( + *mock_robot_driver, GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .WillByDefault(::testing::ReturnRef(default_driver_data)); + } + + void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; } + void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; } + void ConfigureEStop() override { e_stop_ = mock_e_stop; } + + void ReadCANopenSettingsDriverCANIDs() { PantherSystem::ReadCANopenSettingsDriverCANIDs(); } + void UpdateHwStates() { PantherSystem::UpdateHwStates(); } + void UpdateMotorsStateDataTimedOut() { PantherSystem::UpdateMotorsStateDataTimedOut(); } + void UpdateDriverStateDataTimedOut() { PantherSystem::UpdateDriverStateDataTimedOut(); } + + void UpdateDriverStateMsg() { PantherSystem::UpdateDriverStateMsg(); } + void UpdateFlagErrors() { PantherSystem::UpdateFlagErrors(); } + std::vector GetSpeedCommands() const { return PantherSystem::GetSpeedCommands(); } + + void SetHwCommandsVelocities(std::vector & velocities) + { + hw_commands_velocities_[0] = velocities[0]; + hw_commands_velocities_[1] = velocities[1]; + hw_commands_velocities_[2] = velocities[2]; + hw_commands_velocities_[3] = velocities[3]; + } + + panther_hardware_interfaces::CANopenSettings GetCANopenSettings() { return canopen_settings_; } + std::vector GetHwStatesPositions() { return hw_states_positions_; } + std::vector GetHwStatesVelocities() { return hw_states_velocities_; } + std::vector GetHwStatesEfforts() { return hw_states_efforts_; } + + std::shared_ptr GetRoboteqErrorFilter() + { + return roboteq_error_filter_; + } + + std::shared_ptr mock_robot_driver; + std::shared_ptr + mock_gpio_controller; + std::shared_ptr mock_e_stop; + +private: + panther_hardware_interfaces::DriverData default_driver_data = + panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings); +}; + +class TestPantherSystem : public ::testing::Test +{ +public: + TestPantherSystem() + { + panther_system_ = std::make_shared(); + + hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + hardware_info_.hardware_parameters.emplace("front_driver_can_id", "1"); + hardware_info_.hardware_parameters.emplace("rear_driver_can_id", "2"); + + panther_system_->on_init(hardware_info_); + panther_system_->on_configure(rclcpp_lifecycle::State()); + } + + ~TestPantherSystem() {} + +protected: + std::shared_ptr panther_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestPantherSystem, ReadCANopenSettingsDriverCANIDs) +{ + ASSERT_NO_THROW(panther_system_->ReadCANopenSettingsDriverCANIDs()); + + const auto canopen_settings = panther_system_->GetCANopenSettings(); + + EXPECT_EQ(canopen_settings.driver_can_ids.size(), 2); + EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::FRONT), 1); + EXPECT_EQ(canopen_settings.driver_can_ids.at(panther_hardware_interfaces::DriverNames::REAR), 2); +} + +TEST_F(TestPantherSystem, UpdateHwStates) +{ + const std::int32_t fl_pos = 10; + const std::int16_t fl_vel = 20; + const std::int16_t fl_eff = 30; + const std::int32_t fr_pos = 40; + const std::int16_t fr_vel = 50; + const std::int16_t fr_eff = 60; + const std::int32_t rl_pos = 70; + const std::int16_t rl_vel = 80; + const std::int16_t rl_eff = 90; + const std::int32_t rr_pos = 100; + const std::int16_t rr_vel = 110; + const std::int16_t rr_eff = 120; + + const auto fl_expected_pos = fl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto fl_expected_vel = fl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto fl_expected_eff = fl_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto fr_expected_pos = fr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto fr_expected_vel = fr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto fr_expected_eff = fr_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto rl_expected_pos = rl_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto rl_expected_vel = rl_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto rl_expected_eff = rl_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto rr_expected_pos = rr_pos * panther_hardware_interfaces_test::kRbtqPosFbToRad; + const auto rr_expected_vel = rr_vel * panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto rr_expected_eff = rr_eff * + panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + + panther_hardware_interfaces::MotorDriverState fl_motor_driver_state = { + fl_pos, fl_vel, fl_eff, {0, 0}, {0, 0}}; + panther_hardware_interfaces::MotorDriverState fr_motor_driver_state = { + fr_pos, fr_vel, fr_eff, {0, 0}, {0, 0}}; + panther_hardware_interfaces::MotorDriverState rl_motor_driver_state = { + rl_pos, rl_vel, rl_eff, {0, 0}, {0, 0}}; + panther_hardware_interfaces::MotorDriverState rr_motor_driver_state = { + rr_pos, rr_vel, rr_eff, {0, 0}, {0, 0}}; + + panther_hardware_interfaces::DriverData front_roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + panther_hardware_interfaces::DriverData rear_roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + // left - channel 2, right - channel 1 + front_roboteq_data.SetMotorsStates(fr_motor_driver_state, fl_motor_driver_state, false); + rear_roboteq_data.SetMotorsStates(rr_motor_driver_state, rl_motor_driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(front_roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(rear_roboteq_data)); + + ASSERT_NO_THROW(panther_system_->UpdateHwStates()); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[0], fl_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[1], fr_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[2], rl_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[3], rr_expected_pos); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[0], fl_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[1], fr_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[2], rl_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[3], rr_expected_vel); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[0], fl_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[1], fr_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[2], rl_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[3], rr_expected_eff); +} + +TEST_F(TestPantherSystem, UpdateMotorsStateDataTimedOut) +{ + panther_hardware_interfaces::MotorDriverState motor_driver_state; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .Times(0); + + panther_system_->UpdateMotorsStateDataTimedOut(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .Times(1); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateMotorsStateDataTimedOut(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, UpdateDriverStateMsg) +{ + // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for + // system_ros_interface_ +} + +TEST_F(TestPantherSystem, UpdateFlagErrors) +{ + panther_hardware_interfaces::DriverState driver_state; + driver_state.fault_flags = 0b01; + driver_state.script_flags = 0; + driver_state.runtime_stat_flag_channel_1 = 0; + driver_state.runtime_stat_flag_channel_2 = 0; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + panther_system_->UpdateFlagErrors(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_TRUE(error); + + // check if reset error works + driver_state.fault_flags = 0; + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateFlagErrors(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, UpdateDriverStateDataTimedOut) +{ + panther_hardware_interfaces::DriverState driver_state; + + panther_hardware_interfaces::DriverData roboteq_data( + panther_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, true); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .Times(0); + + panther_system_->UpdateDriverStateDataTimedOut(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(panther_hardware_interfaces::DriverNames::REAR))) + .Times(1); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateDriverStateDataTimedOut(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(panther_hardware_interfaces::error_filter_id_names.at( + panther_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, GetSpeedCommands) +{ + const auto fl_v = 0.1; + const auto fr_v = 0.2; + const auto rl_v = 0.3; + const auto rr_v = 0.4; + + std::vector velocities = {fl_v, fr_v, rl_v, rr_v}; + panther_system_->SetHwCommandsVelocities(velocities); + const auto speed_cmd = panther_system_->GetSpeedCommands(); + + ASSERT_EQ(speed_cmd.size(), 4); + EXPECT_FLOAT_EQ(speed_cmd[0], fl_v); + EXPECT_FLOAT_EQ(speed_cmd[1], fr_v); + EXPECT_FLOAT_EQ(speed_cmd[2], rl_v); + EXPECT_FLOAT_EQ(speed_cmd[3], rr_v); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp new file mode 100644 index 00000000..68e2fbff --- /dev/null +++ b/panther_hardware_interfaces/test/unit/panther_system/test_system_ros_interface.cpp @@ -0,0 +1,207 @@ +// 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 "utils/test_constants.hpp" + +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; + +class SystemROSInterfaceWrapper : public panther_hardware_interfaces::SystemROSInterface +{ +public: + SystemROSInterfaceWrapper(const std::string & node_name) : SystemROSInterface(node_name) {} + + RobotDriverStateMsg & GetRobotDriverStateMsg() { return realtime_driver_state_publisher_->msg_; } +}; + +class TestSystemROSInterface : public ::testing::Test +{ +public: + TestSystemROSInterface() + { + system_ros_interface_ = std::make_unique("hardware_controller"); + } + + ~TestSystemROSInterface() { system_ros_interface_.reset(); } + +protected: + std::unique_ptr system_ros_interface_; +}; + +TEST(TestSystemROSInterfaceInitialization, NodeCreation) +{ + using panther_hardware_interfaces::SystemROSInterface; + + std::vector node_names; + const std::string system_node_name = "hardware_controller"; + const std::string system_node_name_with_ns = "/" + system_node_name; + + rclcpp::Node::SharedPtr test_node = std::make_shared("test_system_node"); + + std::unique_ptr system_ros_interface; + + system_ros_interface = std::make_unique(system_node_name); + + node_names = test_node->get_node_names(); + ASSERT_TRUE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); + + system_ros_interface.reset(); + node_names = test_node->get_node_names(); + ASSERT_FALSE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); + + // Check if it is possible to create a node once again (if everything was cleaned up properly) + system_ros_interface = std::make_unique(system_node_name); + node_names = test_node->get_node_names(); + ASSERT_TRUE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); +} + +TEST_F(TestSystemROSInterface, UpdateMsgErrorFlags) +{ + panther_hardware_interfaces::DriverData data( + panther_hardware_interfaces_test::kDrivetrainSettings); + + panther_hardware_interfaces::DriverState driver_state; + driver_state.fault_flags = 0b00000001; + driver_state.script_flags = 0b00000010; + driver_state.runtime_stat_flag_channel_1 = 0b00001000; + driver_state.runtime_stat_flag_channel_2 = 0b00000100; + + data.SetDriverState(driver_state, false); + + system_ros_interface_->UpdateMsgErrorFlags("driver", data); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.fault_flag.overheat); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.script_flag.encoder_disconnected); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.channel_2_motor_runtime_error.loop_error); + EXPECT_TRUE( + driver_state_msg.driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active); +} + +TEST_F(TestSystemROSInterface, UpdateMsgDriversStates) +{ + panther_hardware_interfaces::RoboteqDriverState state; + + const std::int16_t temp = 36; + const std::int16_t heatsink_temp = 37; + const std::uint16_t volt = 405; + const std::int16_t battery_current_1 = 15; + const std::int16_t battery_current_2 = 12; + + state.SetTemperature(temp); + state.SetHeatsinkTemperature(heatsink_temp); + state.SetVoltage(volt); + state.SetBatteryCurrent1(battery_current_1); + state.SetBatteryCurrent2(battery_current_2); + + system_ros_interface_->UpdateMsgDriversStates("driver", state); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.temperature), temp); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.heatsink_temperature), + heatsink_temp); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.voltage * 10.0), volt); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.current * 10.0), + (battery_current_1 + battery_current_2)); +} + +TEST_F(TestSystemROSInterface, UpdateMsgErrors) +{ + panther_hardware_interfaces::CANErrors can_errors; + can_errors.error = true; + + can_errors.write_pdo_cmds_error = true; + can_errors.read_pdo_motor_states_error = false; + can_errors.read_pdo_driver_state_error = false; + + panther_hardware_interfaces::DriverCANErrors driver_can_errors; + driver_can_errors.motor_states_data_timed_out = true; + driver_can_errors.driver_state_data_timed_out = false; + driver_can_errors.can_error = false; + driver_can_errors.heartbeat_timeout = true; + + can_errors.driver_errors.emplace("driver", driver_can_errors); + + system_ros_interface_->UpdateMsgErrors(can_errors); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_TRUE(driver_state_msg.error); + + EXPECT_TRUE(driver_state_msg.write_pdo_cmds_error); + EXPECT_FALSE(driver_state_msg.read_pdo_motor_states_error); + EXPECT_FALSE(driver_state_msg.read_pdo_driver_state_error); + + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.motor_states_data_timed_out); + EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.driver_state_data_timed_out); + EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.can_error); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.heartbeat_timeout); +} + +TEST_F(TestSystemROSInterface, CreateDriverStateEntryInMsg) +{ + const auto driver_1_name = "driver_1"; + const auto driver_2_name = "driver_2"; + const auto driver_3_name = "driver_3"; + + auto & driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + ASSERT_EQ(driver_state_msg.driver_states.size(), 0); + + // check 3 different methods that can create a new entry in the message + system_ros_interface_->UpdateMsgErrorFlags( + driver_1_name, + panther_hardware_interfaces::DriverData(panther_hardware_interfaces_test::kDrivetrainSettings)); + system_ros_interface_->UpdateMsgDriversStates(driver_2_name, {}); + + panther_hardware_interfaces::CANErrors can_errors; + can_errors.driver_errors.emplace(driver_3_name, panther_hardware_interfaces::DriverCANErrors()); + system_ros_interface_->UpdateMsgErrors(can_errors); + + EXPECT_EQ(driver_state_msg.driver_states.size(), 3); + EXPECT_EQ(driver_state_msg.driver_states.at(0).name, driver_1_name); + EXPECT_EQ(driver_state_msg.driver_states.at(1).name, driver_2_name); + EXPECT_EQ(driver_state_msg.driver_states.at(2).name, driver_3_name); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} diff --git a/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp b/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp new file mode 100644 index 00000000..0474f8cd --- /dev/null +++ b/panther_hardware_interfaces/test/unit/panther_system/test_ugv_system.cpp @@ -0,0 +1,378 @@ +// 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 "panther_hardware_interfaces/panther_system/ugv_system.hpp" + +#include "utils/system_test_utils.hpp" + +class MockUGVSystem : public panther_hardware_interfaces::UGVSystem +{ +public: + MockUGVSystem(const std::vector & joint_order) + : panther_hardware_interfaces::UGVSystem(joint_order) + { + mock_robot_driver_ = + std::make_shared(); + mock_gpio_controller_ = + std::make_shared(); + mock_e_stop_ = std::make_shared(); + + ON_CALL(*this, DefineRobotDriver()).WillByDefault(::testing::Invoke([&]() { + robot_driver_ = mock_robot_driver_; + })); + ON_CALL(*this, ConfigureGPIOController()).WillByDefault(::testing::Invoke([&]() { + gpio_controller_ = mock_gpio_controller_; + })); + ON_CALL(*this, ConfigureEStop()).WillByDefault(::testing::Invoke([&]() { + e_stop_ = mock_e_stop_; + })); + } + + MOCK_METHOD(void, ReadCANopenSettingsDriverCANIDs, (), (override)); + MOCK_METHOD(void, ConfigureGPIOController, (), (override)); + MOCK_METHOD(void, ConfigureEStop, (), (override)); + + MOCK_METHOD(void, DefineRobotDriver, (), (override)); + + MOCK_METHOD(void, UpdateHwStates, (), (override)); + MOCK_METHOD(void, UpdateMotorsStateDataTimedOut, (), (override)); + MOCK_METHOD(void, UpdateDriverStateMsg, (), (override)); + MOCK_METHOD(void, UpdateFlagErrors, (), (override)); + MOCK_METHOD(void, UpdateDriverStateDataTimedOut, (), (override)); + + MOCK_METHOD(std::vector, GetSpeedCommands, (), (const, override)); + + MOCK_METHOD(void, DiagnoseErrors, (diagnostic_updater::DiagnosticStatusWrapper &), (override)); + MOCK_METHOD(void, DiagnoseStatus, (diagnostic_updater::DiagnosticStatusWrapper &), (override)); + + void DefaultDefineRobotDriver() + { + robot_driver_ = std::make_shared(); + } + + void SetHwCommandVelocity(const std::vector & velocity) + { + hw_commands_velocities_ = velocity; + } + void SetHwStatePosition(const std::vector & position) { hw_states_positions_ = position; } + void SetHwStateVelocity(const std::vector & velocity) + { + hw_states_velocities_ = velocity; + } + void SetHwStateEffort(const std::vector & effort) { hw_states_efforts_ = effort; } + + std::shared_ptr GetMockRobotDriver() + { + return mock_robot_driver_; + } + + std::shared_ptr + GetMockGPIOController() + { + return mock_gpio_controller_; + } + + std::shared_ptr GetMockEStop() + { + return mock_e_stop_; + } + + using NiceMock = testing::NiceMock; + +private: + std::shared_ptr mock_robot_driver_; + std::shared_ptr + mock_gpio_controller_; + std::shared_ptr mock_e_stop_; +}; + +class TestUGVSystem : public ::testing::Test +{ +public: + TestUGVSystem() + { + ugv_system_ = + std::make_shared(std::vector{"fl", "fr", "rl", "rr"}); + + hardware_info_ = panther_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + } + + ~TestUGVSystem() {} + +protected: + std::shared_ptr ugv_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestUGVSystem, OnInit) +{ + EXPECT_CALL(*ugv_system_, ReadCANopenSettingsDriverCANIDs()).Times(1); + + auto callback_return = ugv_system_->on_init(hardware_info_); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnConfigure) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + EXPECT_CALL(*ugv_system_, DefineRobotDriver()).WillOnce(::testing::Invoke([&]() { + ugv_system_->DefaultDefineRobotDriver(); + })); + EXPECT_CALL(*ugv_system_, ConfigureGPIOController()).Times(1); + EXPECT_CALL(*ugv_system_, ConfigureEStop()).Times(1); + auto callback_return = ugv_system_->on_configure(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnCleanup) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_cleanup(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnActivate) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Activate()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockGPIOController(), QueryControlInterfaceIOStates()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + auto callback_return = ugv_system_->on_activate(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, OnDeactivate) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + auto callback_return = ugv_system_->on_deactivate(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, OnShutdown) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_shutdown(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnError) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_error(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, ExportStateInterfacesInitialValues) +{ + std::vector prefixes = {"fl", "fr", "rl", "rr"}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + std::vector state_interfaces = + ugv_system_->export_state_interfaces(); + + ASSERT_EQ(state_interfaces.size(), 12); + + int i = 0; + for (const auto & prefix : prefixes) { + EXPECT_EQ( + state_interfaces[i].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + state_interfaces[i + 1].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + state_interfaces[i + 2].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_EFFORT); + + i += 3; + } + + auto all_interfaces_are_nan = std::all_of( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & state) { return std::isnan(state.get_value()); }); + + EXPECT_TRUE(all_interfaces_are_nan); +} + +TEST_F(TestUGVSystem, ExportStateInterfaces) +{ + std::vector position = {1.0, 2.0, 3.0, 4.0}; + std::vector velocity = {5.0, 6.0, 7.0, 8.0}; + std::vector effort = {9.0, 10.0, 11.0, 12.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + ugv_system_->SetHwStatePosition(position); + ugv_system_->SetHwStateVelocity(velocity); + ugv_system_->SetHwStateEffort(effort); + + std::vector state_interfaces = + ugv_system_->export_state_interfaces(); + + ASSERT_EQ(state_interfaces.size(), 12); + + for (std::size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(state_interfaces[i * 3].get_value(), position[i]); + EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 1].get_value(), velocity[i]); + EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 2].get_value(), effort[i]); + } +} + +TEST_F(TestUGVSystem, ExportCommandInterfacesInitialValues) +{ + std::vector prefixes = {"fl", "fr", "rl", "rr"}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + std::vector command_interfaces = + ugv_system_->export_command_interfaces(); + + ASSERT_EQ(command_interfaces.size(), 4); + + EXPECT_EQ( + command_interfaces[0].get_name(), + std::string("fl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[1].get_name(), + std::string("fr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[2].get_name(), + std::string("rl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[3].get_name(), + std::string("rr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + + std::for_each( + command_interfaces.begin(), command_interfaces.end(), + [](const hardware_interface::CommandInterface & command) { + EXPECT_FLOAT_EQ(command.get_value(), 0.0); + }); +} + +TEST_F(TestUGVSystem, ExportCommandInterfaces) +{ + std::vector velocity = {1.0, 2.0, 3.0, 4.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + ugv_system_->SetHwCommandVelocity(velocity); + + std::vector command_interfaces = + ugv_system_->export_command_interfaces(); + + ASSERT_EQ(command_interfaces.size(), 4); + + for (std::size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(command_interfaces[i].get_value(), velocity[i]); + } +} + +TEST_F(TestUGVSystem, Read) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateMotorsState()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateHwStates()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateMotorsStateDataTimedOut()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateDriversState()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateDriverStateMsg()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateFlagErrors()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateDriverStateDataTimedOut()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + + auto callback_return = ugv_system_->read( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0)); + + EXPECT_EQ(callback_return, hardware_interface::return_type::OK); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, Write) +{ + rclcpp::init(0, nullptr); + + const auto velocity = std::vector{1.0, 2.0, 3.0, 4.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_, GetSpeedCommands()).WillOnce(::testing::Return(velocity)); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(1); + + auto callback_return = ugv_system_->write( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0)); + + EXPECT_EQ(callback_return, hardware_interface::return_type::OK); + + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/panther_hardware_interfaces/test/utils/mock_driver.hpp b/panther_hardware_interfaces/test/utils/mock_driver.hpp index 7af1cca1..da2b3390 100644 --- a/panther_hardware_interfaces/test/utils/mock_driver.hpp +++ b/panther_hardware_interfaces/test/utils/mock_driver.hpp @@ -30,6 +30,15 @@ namespace panther_hardware_interfaces_test class MockDriver : public panther_hardware_interfaces::DriverInterface { public: + MockDriver() + { + ON_CALL(*this, Boot()).WillByDefault(::testing::Invoke([]() { + std::promise promise; + promise.set_value(); + return promise.get_future(); + })); + } + MOCK_METHOD(std::future, Boot, (), (override)); MOCK_METHOD(bool, IsCANError, (), (const, override)); MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override)); @@ -52,6 +61,8 @@ class MockDriver : public panther_hardware_interfaces::DriverInterface motor_drivers_.emplace(name, motor_driver); } + using NiceMock = testing::NiceMock; + private: std::map> motor_drivers_; @@ -63,6 +74,8 @@ class MockMotorDriver : public panther_hardware_interfaces::MotorDriverInterface MOCK_METHOD(panther_hardware_interfaces::MotorDriverState, ReadState, (), (override)); MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override)); MOCK_METHOD(void, TurnOnSafetyStop, (), (override)); + + using NiceMock = testing::NiceMock; }; } // namespace panther_hardware_interfaces_test diff --git a/panther_hardware_interfaces/test/utils/system_test_utils.hpp b/panther_hardware_interfaces/test/utils/system_test_utils.hpp new file mode 100644 index 00000000..db294b4e --- /dev/null +++ b/panther_hardware_interfaces/test/utils/system_test_utils.hpp @@ -0,0 +1,174 @@ +// 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_TEST_SYSTEM_TEST_UTILS_HPP_ +#define PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_ + +#include +#include +#include +#include + +#include + +#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" +#include "panther_hardware_interfaces/panther_system/robot_driver/robot_driver.hpp" +#include "panther_hardware_interfaces/panther_system/system_e_stop.hpp" + +#include "utils/mock_driver.hpp" + +namespace panther_hardware_interfaces_test +{ + +class MockRobotDriver : public panther_hardware_interfaces::RobotDriverInterface +{ +public: + MOCK_METHOD(void, Initialize, (), (override)); + MOCK_METHOD(void, Deinitialize, (), (override)); + MOCK_METHOD(void, Activate, (), (override)); + MOCK_METHOD(void, UpdateCommunicationState, (), (override)); + MOCK_METHOD(void, UpdateMotorsState, (), (override)); + MOCK_METHOD(void, UpdateDriversState, (), (override)); + MOCK_METHOD( + const panther_hardware_interfaces::DriverData &, GetData, (const std::string &), (override)); + MOCK_METHOD(void, SendSpeedCommands, (const std::vector &), (override)); + MOCK_METHOD(void, TurnOnEStop, (), (override)); + MOCK_METHOD(void, TurnOffEStop, (), (override)); + MOCK_METHOD(void, AttemptErrorFlagReset, (), (override)); + MOCK_METHOD(bool, CommunicationError, (), (override)); + + using NiceMock = testing::NiceMock; +}; + +class MockGPIODriver : public panther_hardware_interfaces::GPIODriverInterface +{ +public: + MOCK_METHOD(void, GPIOMonitorEnable, (const bool, const unsigned), (override)); + MOCK_METHOD( + void, ConfigureEdgeEventCallback, + (const std::function &), (override)); + MOCK_METHOD( + void, ChangePinDirection, + (const panther_hardware_interfaces::GPIOPin, const gpiod::line::direction), (override)); + MOCK_METHOD( + bool, IsPinAvailable, (const panther_hardware_interfaces::GPIOPin), (const, override)); + MOCK_METHOD(bool, IsPinActive, (const panther_hardware_interfaces::GPIOPin), (override)); + MOCK_METHOD( + bool, SetPinValue, (const panther_hardware_interfaces::GPIOPin, const bool), (override)); + + using NiceMock = testing::NiceMock; +}; + +class MockGPIOController : public panther_hardware_interfaces::GPIOControllerInterface +{ +public: + MockGPIOController() : GPIOControllerInterface() + { + gpio_driver_ = std::make_shared<::testing::NiceMock>(); + } + + MOCK_METHOD(void, Start, (), (override)); + MOCK_METHOD(void, EStopTrigger, (), (override)); + MOCK_METHOD(void, EStopReset, (), (override)); + MOCK_METHOD(bool, MotorPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, FanEnable, (const bool), (override)); + MOCK_METHOD(bool, AUXPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, DigitalPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, ChargerEnable, (const bool), (override)); + MOCK_METHOD(bool, LEDControlEnable, (const bool), (override)); + MOCK_METHOD( + (std::unordered_map), QueryControlInterfaceIOStates, + (), (const, override)); + + using NiceMock = testing::NiceMock; +}; + +class MockEStop : public panther_hardware_interfaces::EStopInterface +{ +public: + MockEStop() : EStopInterface() {} + + MOCK_METHOD(bool, ReadEStopState, (), (override)); + MOCK_METHOD(void, TriggerEStop, (), (override)); + MOCK_METHOD(void, ResetEStop, (), (override)); + + using NiceMock = testing::NiceMock; +}; + +hardware_interface::HardwareInfo GenerateDefaultHardwareInfo() +{ + hardware_interface::HardwareInfo hardware_info; + hardware_info.name = "test"; + hardware_info.hardware_class_type = "UGVSystem"; + + hardware_interface::InterfaceInfo vel_command_interface; + vel_command_interface.name = "velocity"; + hardware_interface::InterfaceInfo pos_state_interface; + pos_state_interface.name = "position"; + hardware_interface::InterfaceInfo vel_state_interface; + vel_state_interface.name = "velocity"; + hardware_interface::InterfaceInfo eff_state_interface; + eff_state_interface.name = "effort"; + + hardware_interface::ComponentInfo wheel_joint; + wheel_joint.command_interfaces = {vel_command_interface}; + wheel_joint.state_interfaces = {pos_state_interface, vel_state_interface, eff_state_interface}; + + auto fl_wheel_joint = wheel_joint; + fl_wheel_joint.name = "fl_wheel_joint"; + auto fr_wheel_joint = wheel_joint; + fr_wheel_joint.name = "fr_wheel_joint"; + auto rl_wheel_joint = wheel_joint; + rl_wheel_joint.name = "rl_wheel_joint"; + auto rr_wheel_joint = wheel_joint; + rr_wheel_joint.name = "rr_wheel_joint"; + + hardware_info.joints = {fl_wheel_joint, fr_wheel_joint, rl_wheel_joint, rr_wheel_joint}; + + std::unordered_map hardware_paremeters = { + // drivetrain settings + {"motor_torque_constant", "0.11"}, + {"gear_ratio", "30.08"}, + {"gearbox_efficiency", "0.75"}, + {"encoder_resolution", "1600"}, + {"max_rpm_motor_speed", "3600.0"}, + + // CANopen settings + {"can_interface_name", "panther_can"}, + {"master_can_id", "3"}, + {"pdo_motor_states_timeout_ms", "15"}, + {"pdo_driver_state_timeout_ms", "75"}, + {"sdo_operation_timeout_ms", "100"}, + + // Driver states update frequency + {"driver_states_update_frequency", "20.0"}, + + // Roboteq initialization and activation attempts + {"max_roboteq_initialization_attempts", "1"}, + {"max_roboteq_activation_attempts", "1"}, + + // Roboteq error filter params + {"max_write_pdo_cmds_errors_count", "1"}, + {"max_read_pdo_motor_states_errors_count", "1"}, + {"max_read_pdo_driver_state_errors_count", "1"}, + }; + + hardware_info.hardware_parameters = hardware_paremeters; + + return hardware_info; +} + +} // namespace panther_hardware_interfaces_test + +#endif // PANTHER_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_