aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
David Sharpe
2025-08-20 12:48:57 -05:00
committed by David
parent 508fa8e2ae
commit 1b05202efa
6 changed files with 463 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
src/arm_hardware_controller/CMakeLists.txt Normal file
View File

@@ -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()

16
src/arm_hardware_controller/arm_hardware_controller.xml Normal file
View File

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

122
src/arm_hardware_controller/bringup/launch/arm_controller.launch.py Normal file
View File

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

91
src/arm_hardware_controller/hardware/arm_controller.cpp Normal file
View File

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

60
src/arm_hardware_controller/hardware/include/arm_hardware_controller/arm_controller.hpp Normal file
View File

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

54
src/arm_hardware_controller/package.xml Normal file
View File

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