diff --git a/littlebot_base/include/littlebot_base/hardware_communication.hpp b/littlebot_base/include/littlebot_base/hardware_communication.hpp new file mode 100644 index 0000000..e903c9d --- /dev/null +++ b/littlebot_base/include/littlebot_base/hardware_communication.hpp @@ -0,0 +1,69 @@ +// // @ Copyright 2024 Nestor Neto + +#pragma once + +#include +#include + +namespace littlebot_base +{ + +class HardwareCommunication{ +public: + /** + * @brief Deconstructor for the HardwareCommunication class + * + */ + virtual ~HardwareCommunication() = default; + + /** + * @brief Set the command velocities + */ + virtual void setCommandVelocities(std::vector velocities) = 0; + + /** + * @brief Get the status velocities + */ + virtual std::vector getStatusVelocities() const = 0; + + /** + * @brief Get the status positions + */ + virtual std::vector getStatusPositionsStatus() const = 0; + +protected: + /** + * @brief Constructor for the LittlebotHardwareCommunication class + */ + HardwareCommunication() = default; + +private: + /** + * @brief Receive data from the hardware + */ + virtual bool receive() = 0; + + /** + * @brief Send data to the hardware + */ + virtual bool send() = 0; + + /** + * @brief + */ + std::vector command_velocities_; + + /** + * @brief + */ + std::vector status_positions_; + + /** + * @brief + */ + std::vector status_velocities_; + + +}; + +} // namespace littlebot_base \ No newline at end of file diff --git a/littlebot_base/include/littlebot_base/littlebot_hardware_component.hpp b/littlebot_base/include/littlebot_base/littlebot_hardware_component.hpp new file mode 100644 index 0000000..4931b6e --- /dev/null +++ b/littlebot_base/include/littlebot_base/littlebot_hardware_component.hpp @@ -0,0 +1,103 @@ +// // @ Copyright 2023 Nestor Neto + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace littlebot_base +{ + +class LittlebotHardwareComponent : public hardware_interface::SystemInterface +{ +public: + /** + * @brief Deconstructor for the LittlebotHardwareComponent class + * + */ + ~LittlebotHardwareComponent() = default; + + /** + * @brief + */ + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + /** + * @brief + */ + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state) override; + + /** + * @brief + */ + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state) override; + + /** + * @brief + */ + std::vector export_state_interfaces() override; + + /** + * @brief + */ + std::vector export_command_interfaces() override; + + /** + * @brief + */ + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief + */ + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + +private: + /** + * @brief The name of the hardware component. + */ + const std::string hardware_component_name_{"LittlebotHardwareComponent"}; + + /** + * @brief command interface. + */ + std::vector hw_commands_; + + /** + * @brief position state interface. + */ + std::vector hw_positions_; + + /** + * @brief velocity state interface. + */ + std::vector hw_velocities_; + + /** + * @brief + */ + static constexpr int kNumCommandInterface_{1}; + + /** + * @brief + */ + static constexpr int kNumStateInterface_{2}; +}; + +} // namespace littlebot_base \ No newline at end of file diff --git a/littlebot_base/include/littlebot_base/littlebot_hardware_comunication.hpp b/littlebot_base/include/littlebot_base/littlebot_hardware_comunication.hpp new file mode 100644 index 0000000..e175476 --- /dev/null +++ b/littlebot_base/include/littlebot_base/littlebot_hardware_comunication.hpp @@ -0,0 +1,72 @@ +// // @ Copyright 2024 Nestor Neto + +#pragma once + +#include +#include + +#include + +#include "littlebot_base/hardware_communication.hpp" + +namespace littlebot_base +{ + +class LittlebotHardwareCommunication : public HardwareCommunication +{ +public: + /** + * @brief Constructor for the LittlebotLittlebotHardwareCommunication class + */ + LittlebotHardwareCommunication(const std::string serial_port); + + /** + * @brief Deconstructor for the LittlebotHardwareCommunication class + */ + ~LittlebotHardwareCommunication(); + + /** + * @brief Set the command velocities + */ + void setCommandVelocities(std::vector velocities) override; + + /** + * @brief Get the status velocities + */ + std::vector getStatusVelocities() const override; + + /** + * @brief Get the status positions + */ + std::vector getStatusPositionsStatus() const override; + +private: + /** + * @brief Receive data from the hardware + */ + bool receive() override; + + /** + * @brief Send data to the hardware + */ + bool send() override; + + /** + * @brief + */ + std::vector command_velocities_; + + /** + * @brief + */ + std::vector status_positions_; + + /** + * @brief + */ + std::vector status_velocities_; + + +}; + +} // namespace littlebot_base \ No newline at end of file diff --git a/littlebot_base/littlebot_hardware_component.xml b/littlebot_base/littlebot_hardware_component.xml new file mode 100644 index 0000000..7d788b4 --- /dev/null +++ b/littlebot_base/littlebot_hardware_component.xml @@ -0,0 +1,9 @@ + + + + The ros2_control LittlebotHardwareComponent using a system hardware interface-type. + + + diff --git a/littlebot_base/src/littlebot_hardware_communication.cpp b/littlebot_base/src/littlebot_hardware_communication.cpp new file mode 100644 index 0000000..8f40625 --- /dev/null +++ b/littlebot_base/src/littlebot_hardware_communication.cpp @@ -0,0 +1,49 @@ +// @ Copyright 2024 Nestor Neto + +#include +#include + +#include "littlebot_base/littlebot_hardware_comunication.hpp" + +namespace littlebot_base +{ + LittlebotHardwareCommunication::LittlebotHardwareCommunication(const std::string serial_port) + { + std::cout << "LittlebotHardwareCommunication constructor" << serial_port << std::endl; + } + + LittlebotHardwareCommunication::~LittlebotHardwareCommunication() + { + std::cout << "LittlebotHardwareCommunication deconstructor" << std::endl; + } + + void LittlebotHardwareCommunication::setCommandVelocities(std::vector velocities) + { + std::cout << "LittlebotHardwareCommunication setCommandVelocities" << std::endl; + } + + std::vector LittlebotHardwareCommunication::getStatusVelocities() const + { + std::cout << "LittlebotHardwareCommunication getStatusVelocities" << std::endl; + return std::vector{1.2, 3.4}; + } + + std::vector LittlebotHardwareCommunication::getStatusPositionsStatus() const + { + std::cout << "LittlebotHardwareCommunication getStatusPositionsStatus" << std::endl; + return std::vector{1.2, 3.4}; + } + + bool LittlebotHardwareCommunication::receive() + { + std::cout << "LittlebotHardwareCommunication receive" << std::endl; + return true; + } + + bool LittlebotHardwareCommunication::send() + { + std::cout << "LittlebotHardwareCommunication send" << std::endl; + return true; + } + +} // namespace littlebot_base \ No newline at end of file diff --git a/littlebot_base/src/littlebot_hardware_component.cpp b/littlebot_base/src/littlebot_hardware_component.cpp new file mode 100644 index 0000000..8377f8c --- /dev/null +++ b/littlebot_base/src/littlebot_hardware_component.cpp @@ -0,0 +1,150 @@ +// @ Copyright 2023 Nestor Neto + +#include +#include +#include + +#include "littlebot_base/littlebot_hardware_component.hpp" + +namespace littlebot_base +{ + +hardware_interface::CallbackReturn LittlebotHardwareComponent::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::FAILURE; + } + + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != kNumCommandInterface_) + { + RCLCPP_FATAL( + rclcpp::get_logger("LittlebotSystemHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("LittlebotSystemHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != kNumStateInterface_) + { + RCLCPP_FATAL( + rclcpp::get_logger("LittlebotSystemHardware"), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("LittlebotSystemHardware"), + "Joint '%s' have %s state interfaces found. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("LittlebotSystemHardware"), + "Joint '%s' have %s state interfaces found. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LittlebotHardwareComponent::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (auto i = 0u; i < hw_positions_.size(); i++) + { + if (std::isnan(hw_positions_[i])) + { + hw_positions_[i] = 0; + hw_velocities_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger("LittlebotSystemHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LittlebotHardwareComponent::on_deactivate( + [[maybe_unused]] const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(rclcpp::get_logger("LittlebotSystemHardware"), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector LittlebotHardwareComponent::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + } + + return state_interfaces; +} + +std::vector LittlebotHardwareComponent::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::return_type LittlebotHardwareComponent::read( + [[maybe_unused]] const rclcpp::Time & time, + [[maybe_unused]] const rclcpp::Duration & period) +{ + return hardware_interface::return_type::OK; +} + + +hardware_interface::return_type LittlebotHardwareComponent::write( + [[maybe_unused]] const rclcpp::Time & time, + [[maybe_unused]] const rclcpp::Duration & period) +{ + return hardware_interface::return_type::OK; +} + +} // namespace littlebot_base + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + littlebot_base::LittlebotHardwareComponent, + hardware_interface::SystemInterface) +