feat: add arm_hardware_controller to act as a hardware interface for IK

2026-02-11 09:20:40 +00:00 · 2025-08-20 12:48:57 -05:00
parent 508fa8e2ae
commit 1b05202efa
6 changed files with 463 additions and 0 deletions
--- a/src/arm_hardware_controller/CMakeLists.txt
+++ b/src/arm_hardware_controller/CMakeLists.txt
@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 3.16)
+project(arm_hardware_controller LANGUAGES CXX)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+# find dependencies
+set(HW_IF_INCLUDE_DEPENDS
+  pluginlib
+  hardware_interface
+  ros2_interfaces_pkg
+)
+set(REF_GEN_INCLUDE_DEPENDS
+  kdl_parser
+  rclcpp
+  trajectory_msgs
+)
+set(CONTROLLER_INCLUDE_DEPENDS
+  pluginlib
+  controller_interface
+  realtime_tools
+  trajectory_msgs
+)
+
+# Specify the required version of ros2_control
+find_package(controller_manager 4.0.0)
+# Handle the case where the required version is not found
+if(NOT controller_manager_FOUND)
+  message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. "
+  "Are you using the correct branch of the ros2_control_demos repository?")
+endif()
+
+# find dependencies
+find_package(backward_ros REQUIRED)
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${REF_GEN_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+# add_executable(send_trajectory reference_generator/send_trajectory.cpp)
+
+# target_link_libraries(send_trajectory PUBLIC
+#   ${trajectory_msgs_TARGETS}
+#   kdl_parser::kdl_parser
+#   rclcpp::rclcpp
+# )
+
+add_library(
+  arm_hardware_controller
+  SHARED
+  hardware/arm_controller.cpp
+  # controller/r6bot_controller.cpp
+)
+
+target_compile_features(arm_hardware_controller PUBLIC cxx_std_17)
+target_include_directories(arm_hardware_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+  # $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/controller/include>
+  $<INSTALL_INTERFACE:include/arm_hardware_controller>
+  ${ros2_interfaces_pkg_INCLUDE_DIRS}  # fixes "no such file or directory" compilation error for "ros2_interfaces_pkg/msg/arm_ik.hpp"
+)
+target_link_libraries(arm_hardware_controller PUBLIC
+  ${trajectory_msgs_TARGETS}
+  # controller_interface::controller_interface
+  hardware_interface::hardware_interface
+  pluginlib::pluginlib
+  realtime_tools::realtime_tools
+)
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface arm_hardware_controller.xml)
+# Export controller plugins
+# pluginlib_export_plugin_description_file(controller_interface arm_hardware_controller.xml)
+
+# INSTALL
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include  #/arm_hardware_controller
+)
+# install(
+#   DIRECTORY description/launch description/ros2_control description/urdf
+#   DESTINATION share/arm_hardware_controller
+# )
+# install(
+#   DIRECTORY bringup/launch bringup/config
+#   DESTINATION share/arm_hardware_controller
+# )
+# install(
+#     TARGETS send_trajectory
+#     RUNTIME DESTINATION lib/arm_hardware_controller
+# )
+
+install(TARGETS arm_hardware_controller
+  EXPORT export_arm_hardware_controller
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# if(BUILD_TESTING)
+#   find_package(ament_cmake_pytest REQUIRED)
+
+#   ament_add_pytest_test(example_7_urdf_xacro test/test_urdf_xacro.py)
+#   ament_add_pytest_test(view_example_7_launch test/test_view_robot_launch.py)
+#   ament_add_pytest_test(run_example_7_launch test/test_r6bot_controller_launch.py)
+# endif()
+
+## EXPORTS
+ament_export_targets(export_arm_hardware_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS} ${REF_GEN_INCLUDE_DEPENDS} ${CONTROLLER_INCLUDE_DEPENDS})
+ament_package()
--- a/src/arm_hardware_controller/arm_hardware_controller.xml
+++ b/src/arm_hardware_controller/arm_hardware_controller.xml
@@ -0,0 +1,16 @@
+<library path="arm_hardware_controller">
+  <class name="arm_hardware_controller/ArmSystem"
+         type="arm_hardware_controller::ArmSystem"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Robot hardware plugin for 6 dof robot tutorial.
+    </description>
+  </class>
+  <!-- <class name="arm_hardware_controller/ArmController"
+         type="arm_hardware_controller::ArmController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Robot controller plugin for 6 dof robot tutorial.
+    </description>
+  </class> -->
+</library>
--- a/src/arm_hardware_controller/bringup/launch/arm_controller.launch.py
+++ b/src/arm_hardware_controller/bringup/launch/arm_controller.launch.py
@@ -0,0 +1,122 @@
+# Copyright 2023 ros2_control Development Team
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ros2_control_demo_example_7"),
+                    "urdf",
+                    "r6bot.urdf.xacro",
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_example_7"),
+            "config",
+            "r6bot_controller.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("ros2_control_demo_description"), "r6bot/rviz", "view_robot.rviz"]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+    )
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    gui = LaunchConfiguration("gui")
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["r6bot_controller", "--param-file", robot_controllers],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    # Delay start of joint_state_broadcaster after `robot_controller`
+    # TODO(anyone): This is a workaround for flaky tests. Remove when fixed.
+    delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_controller_spawner,
+            on_exit=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        robot_controller_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_joint_state_broadcaster_after_robot_controller_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)
--- a/src/arm_hardware_controller/hardware/arm_controller.cpp
+++ b/src/arm_hardware_controller/hardware/arm_controller.cpp
@@ -0,0 +1,91 @@
+// Copyright 2023 ros2_control Development Team
+//
+// 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 "arm_hardware_controller/arm_controller.hpp"
+#include <string>
+#include <vector>
+
+#include <iostream>
+
+namespace astra_arm
+{
+
+CallbackReturn ArmSystem::on_init(const hardware_interface::HardwareInfo & info)
+{
+    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+    {
+        return CallbackReturn::ERROR;
+    }
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+CallbackReturn ArmSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+    // reset values always when configuring hardware
+    for (const auto & [name, descr] : joint_state_interfaces_)
+    {
+        set_state(name, 0.0);
+    }
+    for (const auto & [name, descr] : joint_command_interfaces_)
+    {
+        set_command(name, 0.0);
+    }
+    for (const auto & [name, descr] : sensor_state_interfaces_)
+    {
+        set_state(name, 0.0);
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+return_type ArmSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
+{
+    // TODO(pac48) set sensor_states_ values from subscriber
+
+    for (std::size_t i = 0; i < info_.joints.size(); i++)
+    {
+        const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY;
+        const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION;
+        set_state(name_vel, get_command(name_vel));
+        set_state(name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds());
+    }
+    return return_type::OK;
+}
+
+return_type ArmSystem::write(const rclcpp::Time &, const rclcpp::Duration &)
+{
+    return return_type::OK;
+}
+
+void ArmSystem::state_sub_callback(
+    const sensor_msgs::msg::JointState::SharedPtr msg)
+{
+    for (std::size_t i = 0; i < msg->name.size(); i++)
+    {
+        if (joint_state_interfaces_.find(msg->name[i]) != joint_state_interfaces_.end())
+        {
+            set_state(msg->name[i] + "/" + hardware_interface::HW_IF_POSITION, msg->position[i]);
+            if (!msg->position.empty())
+                set_state(msg->name[i] + "/" + hardware_interface::HW_IF_VELOCITY, msg->velocity[i]);
+        }
+    }
+    last_state_time_ = std::chrono::steady_clock::now();
+}
+
+}  // namespace astra_arm
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  astra_arm::ArmSystem, hardware_interface::SystemInterface)
--- a/src/arm_hardware_controller/hardware/include/arm_hardware_controller/arm_controller.hpp
+++ b/src/arm_hardware_controller/hardware/include/arm_hardware_controller/arm_controller.hpp
@@ -0,0 +1,60 @@
+// Copyright 2023 ros2_control Development Team
+//
+// 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 ASTRA_ARM__HARDWARE_HPP_
+#define ASTRA_ARM__HARDWARE_HPP_
+
+#include "string"
+#include "unordered_map"
+#include "vector"
+
+#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 "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include "ros2_interfaces_pkg/msg/arm_ik.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+
+using hardware_interface::return_type;
+
+namespace astra_arm
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class ArmSystem : public hardware_interface::SystemInterface
+{
+public:
+    // Boilerplate
+    CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+    CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) 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;
+private:
+    // Comms with arm_pkg
+    rclcpp::Publisher<ros2_interfaces_pkg::msg::ArmIK>::SharedPtr command_pub_;
+    void state_sub_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub_;
+
+    std::vector<double> cmd_;
+    std::vector<double> pos_, vel_;
+    std::chrono::steady_clock::time_point last_state_time_;
+
+protected:
+};
+
+}  // namespace astra_arm
+
+#endif  // ASTRA_ARM__HARDWARE_HPP_
--- a/src/arm_hardware_controller/package.xml
+++ b/src/arm_hardware_controller/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_hardware_controller</name>
+  <version>0.0.0</version>
+  <description>Demo for 6 DOF robot.</description>
+
+  <maintainer email="denis.stogl@stoglrobotics.de">Dr.-Ing. Denis Štogl</maintainer>
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="christoph.froehlich@ait.ac.at">Christoph Froehlich</maintainer>
+  <maintainer email="sai.kishor@pal-robotics.com">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <author email="paul.gesel@picknik.ai">Paul Gesel</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ros2_control_cmake</build_depend>
+  <build_depend>ros2_interfaces_pkg</build_depend>
+
+  <depend>backward_ros</depend>
+  <depend>control_msgs</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>kdl_parser</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp</depend>
+  <depend>realtime_tools</depend>
+  <depend>trajectory_msgs</depend>
+
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_control_demo_description</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>liburdfdom-tools</test_depend>
+  <test_depend>rclpy</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>