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 @@
-
+