From bbc07d6c5734026166e2f6529c9064536a96dc43 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Tue, 18 Nov 2025 06:02:57 -0500 Subject: [PATCH] removed the actuator hwi in arm, replaced with servo hwi --- .../arm/arm.actuator.ros2_control.xacro | 29 --- .../arm/arm.servo.ros2_control.xacro | 22 ++ src/description/urdf/athena_arm.urdf.xacro | 6 +- .../actuator_ros2_control/CMakeLists.txt | 77 ------ .../actuator_hardware_interface.xml | 9 - .../actuator_hardware_interface.hpp | 125 ---------- .../actuator_ros2_control/package.xml | 32 --- .../src/actuator_hardware_interface.cpp | 222 ------------------ .../servo_hardware_interface.xml | 2 +- 9 files changed, 26 insertions(+), 498 deletions(-) delete mode 100644 src/description/ros2_control/arm/arm.actuator.ros2_control.xacro create mode 100644 src/description/ros2_control/arm/arm.servo.ros2_control.xacro delete mode 100644 src/hardware_interfaces/actuator_ros2_control/CMakeLists.txt delete mode 100644 src/hardware_interfaces/actuator_ros2_control/actuator_hardware_interface.xml delete mode 100644 src/hardware_interfaces/actuator_ros2_control/include/actuator_ros2_control/actuator_hardware_interface.hpp delete mode 100644 src/hardware_interfaces/actuator_ros2_control/package.xml delete mode 100644 src/hardware_interfaces/actuator_ros2_control/src/actuator_hardware_interface.cpp diff --git a/src/description/ros2_control/arm/arm.actuator.ros2_control.xacro b/src/description/ros2_control/arm/arm.actuator.ros2_control.xacro deleted file mode 100644 index 7d5f57c7..00000000 --- a/src/description/ros2_control/arm/arm.actuator.ros2_control.xacro +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - actuator_ros2_control/ACTUATORHardwareInterface - - - - 0 - - 0 - ${2*pi} - - - - 0.0 - - - - - - - - - diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro new file mode 100644 index 00000000..9deb797e --- /dev/null +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -0,0 +1,22 @@ + + + + + + + + + servo_ros2_control/SERVOHardwareInterface + + + + + + + + + + + + + diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/athena_arm.urdf.xacro index 1976fe3b..11cd29e2 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/athena_arm.urdf.xacro @@ -25,8 +25,8 @@ - - + + @@ -47,7 +47,7 @@ - + diff --git a/src/hardware_interfaces/actuator_ros2_control/CMakeLists.txt b/src/hardware_interfaces/actuator_ros2_control/CMakeLists.txt deleted file mode 100644 index 301ff5d6..00000000 --- a/src/hardware_interfaces/actuator_ros2_control/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(actuator_ros2_control) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - msgs -) - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(ament_cmake_auto REQUIRED) # allow the following two lines -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -ament_auto_find_build_dependencies() -ament_export_dependencies(rosidl_default_runtime) - - -include_directories(include) - -add_library( - actuator_ros2_control - SHARED - src/actuator_hardware_interface.cpp -) - -target_compile_features(actuator_ros2_control PUBLIC cxx_std_20) -target_include_directories(actuator_ros2_control PUBLIC -$ -$ -) - -ament_target_dependencies( - actuator_ros2_control PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -# Export hardware plugins -pluginlib_export_plugin_description_file(hardware_interface actuator_hardware_interface.xml) - -# INSTALL -install( - DIRECTORY include/ - DESTINATION include/actuator_ros2_control -) - -install(TARGETS actuator_ros2_control - EXPORT export_actuator_ros2_control - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - set(ament_cmake_copyright_FOUND TRUE) - - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_export_targets(export_actuator_ros2_control) -ament_export_include_directories(include) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/src/hardware_interfaces/actuator_ros2_control/actuator_hardware_interface.xml b/src/hardware_interfaces/actuator_ros2_control/actuator_hardware_interface.xml deleted file mode 100644 index b63e83d5..00000000 --- a/src/hardware_interfaces/actuator_ros2_control/actuator_hardware_interface.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - UMDLoop's ros2_control plugin for the ACTUATOR motors - - - diff --git a/src/hardware_interfaces/actuator_ros2_control/include/actuator_ros2_control/actuator_hardware_interface.hpp b/src/hardware_interfaces/actuator_ros2_control/include/actuator_ros2_control/actuator_hardware_interface.hpp deleted file mode 100644 index 6b45efb3..00000000 --- a/src/hardware_interfaces/actuator_ros2_control/include/actuator_ros2_control/actuator_hardware_interface.hpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - -#ifndef ACTUATOR_HARDWARE_INTERACE_HPP_ -#define ACTUATOR_HARDWARE_INTERACE_HPP_ - -#include -#include -#include -#include -#include - - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - - -#include -#include -#include -#include "msgs/msg/cana.hpp" - -namespace actuator_ros2_control -{ -class ACTUATORHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(ACTUATORHardwareInterface) - - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - // Exports/exposes Interfaces that are available so that the controllers - // know what to read and write to - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - // Establish communications and set initial values for state and command interfaces - // hardware_interface::CallbackReturn on_configure( - // const rclcpp_lifecycle::State & previous_state) override; - - // Lifecycle - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::return_type perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces - ) override; - - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -private: - - int num_joints; - - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; - - double encoder_position; - double motor_speed; - - rclcpp::Publisher::SharedPtr actuator_can_publisher_; - rclcpp::Subscription::SharedPtr actuator_can_subscriber_; - rclcpp::Node::SharedPtr node_; - - msgs::msg::CANA received_joint_data_; - - std::vector joint_node_ids; - std::vector joint_gear_ratios; - - - enum integration_level_t : std::uint8_t - { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, - }; - - // Active control mode for each actuator - std::vector control_level_; - -}; - -} // namespace actuator_hardware_interface - -#endif // ACTUATOR_HARDWARE_INTERACE_HPP_ - diff --git a/src/hardware_interfaces/actuator_ros2_control/package.xml b/src/hardware_interfaces/actuator_ros2_control/package.xml deleted file mode 100644 index 19219500..00000000 --- a/src/hardware_interfaces/actuator_ros2_control/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - actuator_ros2_control - 0.0.0 - UMDLoop's ros2_control hardware interface for the ACTUATOR motors - duttaishan01 - Apache-2.0 - - ament_cmake - ament_cmake - - hardware_interface - pluginlib - - rclcpp - rclcpp_lifecycle - realtime_tools - msgs - - ament_lint_auto - ament_lint_common - - ament_cmake - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/hardware_interfaces/actuator_ros2_control/src/actuator_hardware_interface.cpp b/src/hardware_interfaces/actuator_ros2_control/src/actuator_hardware_interface.cpp deleted file mode 100644 index a7704cd0..00000000 --- a/src/hardware_interfaces/actuator_ros2_control/src/actuator_hardware_interface.cpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - -#include "actuator_ros2_control/actuator_hardware_interface.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/lexical_casts.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace actuator_ros2_control -{ -hardware_interface::CallbackReturn ACTUATORHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file -{ - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor - for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - } - - num_joints = static_cast(info_.joints.size()); - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0); - joint_state_velocity_.assign(num_joints, 0); - - joint_command_position_.assign(num_joints, 0); - joint_command_velocity_.assign(num_joints, 0); - - encoder_position = 0; - motor_speed = 0; - - control_level_.resize(num_joints, integration_level_t::POSITION); - - node_ = rclcpp::Node::make_shared("actuator_hardware_node"); - actuator_can_publisher_ = node_->create_publisher("can_tx", 10); - - // Lambda function that takes the message as a shared pointer, dereferences it, - // and stores it in received_joint_data_ to be used - actuator_can_subscriber_ = node_->create_subscription( - "can_rx", - 10, - [this](const msgs::msg::CANA::SharedPtr received_message) - { - received_joint_data_ = *received_message; - }); - - return hardware_interface::CallbackReturn::SUCCESS; -} - - -hardware_interface::CallbackReturn ACTUATORHardwareInterface::on_shutdown( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // may not need this either - return hardware_interface::CallbackReturn::SUCCESS; -} - - -std::vector ACTUATORHardwareInterface::export_state_interfaces() -{ - std::vector state_interfaces; - - // Each ACTUATOR motor corresponds to a different joint. - for(int i = 0; i < num_joints; i++){ - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_state_position_[i])); - - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_state_velocity_[i])); - } - - return state_interfaces; -} - - -std::vector -ACTUATORHardwareInterface::export_command_interfaces() -{ - std::vector command_interfaces; - - for(int i = 0; i < num_joints; i++){ - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); - } - - return command_interfaces; -} - - -hardware_interface::CallbackReturn ACTUATORHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("ACTUATORHardwareInterface"), "Activating ...please wait..."); - - RCLCPP_INFO(rclcpp::get_logger("ACTUATORHardwareInterface"), "Successfully activated!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - - -hardware_interface::CallbackReturn ACTUATORHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - auto joint_tx = msgs::msg::CANA(); - RCLCPP_INFO(rclcpp::get_logger("ACTUATORHardwareInterface"), "Deactivating ...please wait..."); - - RCLCPP_INFO(rclcpp::get_logger("ACTUATORHardwareInterface"), "Successfully deactivated all ACTUATOR motors!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - - -hardware_interface::return_type ACTUATORHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - return hardware_interface::return_type::OK; -} - - -hardware_interface::return_type actuator_ros2_control::ACTUATORHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type ACTUATORHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) -{ - std::vector new_modes = {}; - for (std::string key : start_interfaces) - { - for (int i = 0; i < num_joints; i++){ - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ - new_modes.push_back(integration_level_t::POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ - new_modes.push_back(integration_level_t::VELOCITY); - } - } - } - // All joints must be given new command mode at the same time - if (new_modes.size() != num_joints){ - return hardware_interface::return_type::ERROR; - } - // All joints must have the same command mode - if (!std::all_of( - new_modes.begin() + 1, new_modes.end(), - [&](integration_level_t mode) { return mode == new_modes[0]; })) - { - return hardware_interface::return_type::ERROR; - } - - // Stop motion on all relevant joints that are stopping - for (std::string key : stop_interfaces) { - for (int i = 0; i < num_joints; i++) { - if (key.find(info_.joints[i].name) != std::string::npos) { - joint_command_position_[i] = 0; - joint_command_velocity_[i] = 0; - control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined - } - } - } - // Set the new command modes. By this point everything should be undefined after the "stop motion" loop - for (int i = 0; i < num_joints; i++) { - if (control_level_[i] != integration_level_t::UNDEFINED) { - // Something else is using the joint! Abort! - return hardware_interface::return_type::ERROR; - } - control_level_[i] = new_modes[i]; - } -} - -} // namespace actuator_hardware_interface - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - actuator_ros2_control::ACTUATORHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/servo_ros2_control/servo_hardware_interface.xml b/src/hardware_interfaces/servo_ros2_control/servo_hardware_interface.xml index 76a711e3..87bd0110 100644 --- a/src/hardware_interfaces/servo_ros2_control/servo_hardware_interface.xml +++ b/src/hardware_interfaces/servo_ros2_control/servo_hardware_interface.xml @@ -1,4 +1,4 @@ - +