diff --git a/src/arm_hardware_controller/CMakeLists.txt b/src/arm_hardware_controller/CMakeLists.txt new file mode 100644 index 0000000..bcc504e --- /dev/null +++ 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 + $ + # $ + $ + ${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() diff --git a/src/arm_hardware_controller/arm_hardware_controller.xml b/src/arm_hardware_controller/arm_hardware_controller.xml new file mode 100644 index 0000000..f27d1c4 --- /dev/null +++ b/src/arm_hardware_controller/arm_hardware_controller.xml @@ -0,0 +1,16 @@ + + + + Robot hardware plugin for 6 dof robot tutorial. + + + + diff --git a/src/arm_hardware_controller/bringup/launch/arm_controller.launch.py b/src/arm_hardware_controller/bringup/launch/arm_controller.launch.py new file mode 100644 index 0000000..f397e53 --- /dev/null +++ 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) diff --git a/src/arm_hardware_controller/hardware/arm_controller.cpp b/src/arm_hardware_controller/hardware/arm_controller.cpp new file mode 100644 index 0000000..9e5929a --- /dev/null +++ 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 +#include + +#include + +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) diff --git 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 new file mode 100644 index 0000000..40b4c85 --- /dev/null +++ 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::SharedPtr command_pub_; + void state_sub_callback(const sensor_msgs::msg::JointState::SharedPtr msg); + rclcpp::Subscription::SharedPtr state_sub_; + + std::vector cmd_; + std::vector pos_, vel_; + std::chrono::steady_clock::time_point last_state_time_; + +protected: +}; + +} // namespace astra_arm + +#endif // ASTRA_ARM__HARDWARE_HPP_ diff --git a/src/arm_hardware_controller/package.xml b/src/arm_hardware_controller/package.xml new file mode 100644 index 0000000..481c093 --- /dev/null +++ b/src/arm_hardware_controller/package.xml @@ -0,0 +1,54 @@ + + + + arm_hardware_controller + 0.0.0 + Demo for 6 DOF robot. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + Sai Kishor Kothakota + + Apache-2.0 + + Paul Gesel + + ament_cmake + ros2_control_cmake + ros2_interfaces_pkg + + backward_ros + control_msgs + controller_interface + hardware_interface + kdl_parser + pluginlib + rclcpp_lifecycle + rclcpp + realtime_tools + trajectory_msgs + + controller_manager + joint_state_broadcaster + joint_state_publisher_gui + launch_ros + launch + robot_state_publisher + ros2_control_demo_description + ros2controlcli + ros2launch + rviz2 + urdf + xacro + + ament_cmake_pytest + launch_testing + launch + liburdfdom-tools + rclpy + + + ament_cmake + +