diff --git a/src/arm_hardware_controller/CMakeLists.txt b/src/arm_hardware_controller/CMakeLists.txt deleted file mode 100644 index bcc504e..0000000 --- a/src/arm_hardware_controller/CMakeLists.txt +++ /dev/null @@ -1,120 +0,0 @@ -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 deleted file mode 100644 index f27d1c4..0000000 --- a/src/arm_hardware_controller/arm_hardware_controller.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index f397e53..0000000 --- a/src/arm_hardware_controller/bringup/launch/arm_controller.launch.py +++ /dev/null @@ -1,122 +0,0 @@ -# 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 deleted file mode 100644 index 9e5929a..0000000 --- a/src/arm_hardware_controller/hardware/arm_controller.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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 deleted file mode 100644 index 40b4c85..0000000 --- a/src/arm_hardware_controller/hardware/include/arm_hardware_controller/arm_controller.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 deleted file mode 100644 index 481c093..0000000 --- a/src/arm_hardware_controller/package.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - 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 - -