From 7a320ff9623eecaf5f5bf128c55b471a2a7eedf1 Mon Sep 17 00:00:00 2001 From: younes Date: Wed, 26 Jan 2022 22:45:14 -0500 Subject: [PATCH 1/3] Initial commit: unsteady state --- uwrt_arm_cartesian_controller/CMakeLists.txt | 97 +++++++ .../uwrt_arm_cartesian_controller.hpp | 67 +++++ .../visibility_control.hpp | 53 ++++ uwrt_arm_cartesian_controller/package.xml | 18 ++ .../src/uwrt_arm_cartesian_controller.cpp | 243 ++++++++++++++++++ .../uwrt_arm_cartesian_controller_plugin.xml | 7 + 6 files changed, 485 insertions(+) create mode 100644 uwrt_arm_cartesian_controller/CMakeLists.txt create mode 100644 uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp create mode 100644 uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp create mode 100644 uwrt_arm_cartesian_controller/package.xml create mode 100644 uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp create mode 100644 uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller_plugin.xml diff --git a/uwrt_arm_cartesian_controller/CMakeLists.txt b/uwrt_arm_cartesian_controller/CMakeLists.txt new file mode 100644 index 00000000..9f6d6ad4 --- /dev/null +++ b/uwrt_arm_cartesian_controller/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) +project(uwrt_arm_cartesian_controller) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_library(uwrt_arm_cartesian_controller + SHARED + src/uwrt_arm_cartesian_controller.cpp +) +target_include_directories(uwrt_arm_cartesian_controller PRIVATE include) +ament_target_dependencies(uwrt_arm_cartesian_controller + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + geometry_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(uwrt_arm_cartesian_controller PRIVATE "UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface uwrt_arm_cartesian_controller_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + uwrt_arm_cartesian_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(FALSE) # younes todo add unit testing like in the forwarding controller example BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_uwrt_arm_cartesian_controller + test/test_load_uwrt_arm_cartesian_controller.cpp + ) + target_include_directories(test_load_uwrt_arm_cartesian_controller PRIVATE include) + ament_target_dependencies(test_load_uwrt_arm_cartesian_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_uwrt_arm_cartesian_controller + test/test_uwrt_arm_cartesian_controller.cpp + ) + target_include_directories(test_uwrt_arm_cartesian_controller PRIVATE include) + target_link_libraries(test_uwrt_arm_cartesian_controller + uwrt_arm_cartesian_controller + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + geometry_msgs +) +ament_export_include_directories( + include +) +ament_export_libraries( + uwrt_arm_cartesian_controller +) +ament_package() \ No newline at end of file diff --git a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp new file mode 100644 index 00000000..a9016252 --- /dev/null +++ b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "uwrt_arm_cartesian_controller/visibility_control.hpp" +#include "kdl/tree.hpp" +#include "kdl/chainiksolver.hpp" +#include "kdl/chainiksolvervel_pinv.hpp" + +#include + +namespace uwrt_arm_cartesian_controller +{ + using TwistStamped = geometry_msgs::msg::TwistStamped; + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class UWRTCartesianController : public controller_interface::ControllerInterface + { + public: + UWRTCartesianController(); + + ~UWRTCartesianController(); + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + protected: + rclcpp::Subscription::SharedPtr m_cart_command_sub; + // realtime_tools::RealtimeBuffer> m_cart_command_buffer; // younes todo dont this buffer. just always use the latest cmd just like aidan did. initialize the twist to 0 vector + + std::vector m_joint_names; + std::string m_interface_name, m_cart_command_topic, m_robot_description, m_root_name, m_tip_name; + + private: + bool parse_params(); + TwistStamped get_null_twist() const; + + KDL::Tree m_kdl_tree; + KDL::Chain m_kdl_chain; + std::unique_ptr m_ik_solver; + + TwistStamped m_last_twist; + }; + +} // namespace uwrt_arm_cartesian_controller diff --git a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp new file mode 100644 index 00000000..dc684eeb --- /dev/null +++ b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp @@ -0,0 +1,53 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#pragma once + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __attribute__((dllexport)) +#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __declspec(dllexport) +#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_DLL +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT +#else +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT +#endif +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC_TYPE UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC +#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL +#else +#define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC +#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL +#endif +#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC_TYPE +#endif diff --git a/uwrt_arm_cartesian_controller/package.xml b/uwrt_arm_cartesian_controller/package.xml new file mode 100644 index 00000000..17c145e0 --- /dev/null +++ b/uwrt_arm_cartesian_controller/package.xml @@ -0,0 +1,18 @@ + + + + uwrt_arm_cartesian_controller + 0.0.0 + TODO: Package description + younes + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp new file mode 100644 index 00000000..3836f320 --- /dev/null +++ b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp @@ -0,0 +1,243 @@ +#include "uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp" +#include "controller_interface/helpers.hpp" + +#include "kdl_parser/kdl_parser.hpp" + +#include +#include + +namespace uwrt_arm_cartesian_controller +{ + UWRTCartesianController::UWRTCartesianController() : m_cart_command_sub(nullptr), m_ik_solver(nullptr) {} + + CallbackReturn UWRTCartesianController::on_init() + { + try + { + auto_declare>("joint_names", std::vector()); + auto_declare("interface_name", ""); + auto_declare("end_effector_velocity_topic", ""); + auto_declare("robot_description", ""); + auto_declare("root_name", ""); + auto_declare("tip_name", ""); + } + catch (const std::exception &e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn UWRTCartesianController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) + { + if (!parse_params()) + { + return CallbackReturn::ERROR; + } + + // Generate tree from urdf + if (!kdl_parser::treeFromParam(m_robot_description, m_kdl_tree)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from robot description"); + return CallbackReturn::ERROR; + } + + // Populate the KDL chain + const bool chain_generated_successfully = m_kdl_tree.getChain(m_root_name, m_tip_name, m_kdl_chain); + std::stringstream tree_info = "\tTree has " << m_kdl_tree.getNrOfJoints() << " joints \n\tThe joint names are:"; + for (auto it = m_kdl_chain.segments.begin(); it != m_kdl_chain.segments.end(); ++it) + { + tree_info << "\n\t\t" << it->getJoint().getName()); + } + tree_info << "\n\tTree has " << m_kdl_tree.getNrOfSegments() << " segments" + << "\n\tThe segments are:"; + KDL::SegmentMap segment_map = m_kdl_tree.getSegments(); + for (auto it = segment_map.begin(); it != segment_map.end(); it++) + { + tree_info << "\n\t\t" << it->first); + } + + if (!chain_generated_successfully) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get KDL chain from tree: \n\t" << m_root_name << " --> " << m_tip_name); + RCLCPP_ERROR(get_node()->get_logger(), tree_info.str()); + return CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_node()->get_logger(), "Successfully got KDL chain from tree: \n\t" << m_root_name << " --> " << m_tip_name); + RCLCPP_INFO(get_node()->get_logger(), tree_info.str()); + + m_ik_solver.reset(new KDL::ChainIkSolverVel_pinv(m_kdl_chain)); + + m_cart_command_sub = get_node()->create_subscription( + m_cart_command_topic, rclcpp::SystemDefaultsQoS(), + [this](const TwistStamped::SharedPtr msg) + { m_last_twist = *msg; }); // younes todo any chance msg could be null? + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration + UWRTCartesianController::command_interface_configuration() const + { + // younes todo better understand this. what is command_interfaces_? + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto &joint : m_joint_names) + { + command_interfaces_config.names.push_back(joint + "/" + m_interface_name); + } + + return command_interfaces_config; + } + + controller_interface::InterfaceConfiguration + UWRTCartesianController::state_interface_configuration() const + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + CallbackReturn UWRTCartesianController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) + { + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + std::vector> ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, m_joint_names, m_interface_name, ordered_interfaces) || + command_interfaces_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + node_->get_logger(), "Expected %zu position command interfaces, got %zu", m_joint_names.size(), + ordered_interfaces.size()); + return CallbackReturn::ERROR; + } + + // reset command if a command came through callback when controller was inactive + m_last_twist = get_null_twist(); + + return CallbackReturn::SUCCESS; + } + + CallbackReturn UWRTCartesianController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) + { + for (auto &command_interface : command_interfaces_) + { + command_interface.set_value(0.0); + } + + m_last_twist = get_null_twist(); + + return CallbackReturn::SUCCESS; + } + + controller_interface::return_type UWRTCartesianController::update(const rclcpp::Time &time, const rclcpp::Duration & /*period*/) + { + // expired command + if (time - m_last_twist.header.stamp > rclcpp::Duration(5, 0)) // younes todo make this a param + { + KDL::SetToZero(m_cart_vel); // younes todo this var doesnt exist and its a geometry msg not a kdl type + for (uint8_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interface.set_value(0.0); + } + } + else + { + KDL::JntArray joint_positions(command_interfaces_.size()); + + KDL::Twist end_effector_velocity = KDL::Twist::Zero(); + end_effector_velocity(0) = m_last_twist.twist.linear.x; + end_effector_velocity(1) = m_last_twist.twist.linear.y; + end_effector_velocity(2) = m_last_twist.twist.linear.z; + end_effector_velocity(3) = m_last_twist.twist.angular.x; + end_effector_velocity(4) = m_last_twist.twist.angular.y; + end_effector_velocity(5) = m_last_twist.twist.angular.z; + + KDL::JntArray joint_velocities(command_interfaces_.size(); + + for (uint8_t i = 0; i < command_interfaces_.size(); ++i) + { + joint_positions(i) = command_interafces_.at(i).getPosition(); + } + + if (m_ik_solver->CartToJnt(joint_positions, end_effector_velocity, joint_velocities) < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to compute joint velocities for given end-effector velocity"); + KDL::SetToZero(m_cart_vel); // younes todo var doesnt exist anymroe + for (uint8_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interface.set_value(0.0); + } + return controller_interface::return_type::ERROR; + } + + for (uint8_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_.at(i).setCommand(joint_velocities(i)); // younes todo use set_values + } + } + + return controller_interface::return_type::OK; + } + + bool UWRTCartesianController::parse_params() + { + m_joint_names = node_->get_parameter("joints").as_string_array(); + + if (m_joint_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return false; + } + + // Loop over string params + const std::array, std::string>, 5> string_params = {{m_interface_name, "interface_name"}, + {m_cart_command_topic, "end_effector_velocity_topic"}, + {m_robot_description, "robot_description"}, + {m_root_name, "root_name"}, + {m_tip_name, "tip_name"}}; + for (auto &string_param : string_params) + { + if (string_param.first.empty()) + { + string_param.first = node_->get_parameter(string_param.second).as_string(); + if (string_param.first.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), string_param.second + " parameter was empty"); + return false; + } + } + } + + return true; + } + + TwistStamped UWRTCartesianController::get_null_twist() const + { + TwistStamped ret; + ret.twist.linear.x = 0; + ret.twist.linear.y = 0; + ret.twist.linear.z = 0; + ret.twist.angular.x = 0; + ret.twist.angular.y = 0; + ret.twist.angular.z = 0; + ret.header.stamp = rclcpp::Time::now(); + // younes todo whats the name of the arm's base frame ret.header.frame_id = "foo"; + return ret; + } + +} // namespace uwrt_arm_cartesian_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + uwrt_arm_cartesian_controller::UWRTCartesianController, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller_plugin.xml b/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller_plugin.xml new file mode 100644 index 00000000..d203814b --- /dev/null +++ b/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller_plugin.xml @@ -0,0 +1,7 @@ + + + + The uwrt arm cartesian controller consumes end-effector linear and angular velocities and produces corresponding joint velocities + + + \ No newline at end of file From cb10a38830f3def47eea7935f5d8285ed17782a8 Mon Sep 17 00:00:00 2001 From: younes Date: Sun, 6 Feb 2022 23:39:28 -0500 Subject: [PATCH 2/3] code compiles --- .github/workflows/ci.yaml | 1 + uwrt_arm_cartesian_controller/CMakeLists.txt | 44 ++++- .../uwrt_arm_cartesian_controller.hpp | 85 +++++----- .../visibility_control.hpp | 23 +-- uwrt_arm_cartesian_controller/package.xml | 24 ++- .../src/uwrt_arm_cartesian_controller.cpp | 150 +++++++++--------- windows_amd64_upstream_dependencies.repos | 6 + 7 files changed, 183 insertions(+), 150 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7736ef4f..187d5dc2 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -15,6 +15,7 @@ env: uwrt_mars_rover_drivetrain uwrt_mars_rover_drivetrain_description uwrt_mars_rover_drivetrain_hw + uwrt_arm_cartesian_controller ROS_DISTRO: galactic IMAGE: "rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-desktop-latest" diff --git a/uwrt_arm_cartesian_controller/CMakeLists.txt b/uwrt_arm_cartesian_controller/CMakeLists.txt index 9f6d6ad4..712e5f89 100644 --- a/uwrt_arm_cartesian_controller/CMakeLists.txt +++ b/uwrt_arm_cartesian_controller/CMakeLists.txt @@ -17,8 +17,10 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(orocos_kdl REQUIRED) +find_package(realtime_tools REQUIRED) + add_library(uwrt_arm_cartesian_controller SHARED @@ -32,12 +34,13 @@ ament_target_dependencies(uwrt_arm_cartesian_controller pluginlib rclcpp_lifecycle rcutils - realtime_tools geometry_msgs + orocos_kdl + realtime_tools ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(uwrt_arm_cartesian_controller PRIVATE "UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_DLL") +target_compile_definitions(uwrt_arm_cartesian_controller PRIVATE "UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_LIBRARY") pluginlib_export_plugin_description_file(controller_interface uwrt_arm_cartesian_controller_plugin.xml) install( @@ -80,13 +83,46 @@ if(FALSE) # younes todo add unit testing like in the forwarding controller examp ) endif() +if (BUILD_TESTING) + # Force generation of compile_commands.json for clang-tidy + set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "") + + # clang-format + find_package(ament_cmake_clang_format REQUIRED) + ament_clang_format( + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-format + ) + + # clang-tidy + find_package(ament_cmake_clang_tidy REQUIRED) + ament_clang_tidy( + ${CMAKE_BINARY_DIR} + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-tidy + ) + + # cppcheck + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + + # flake8 + find_package(ament_cmake_flake8 REQUIRED) + ament_flake8( + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.flake8 + ) + + # xmllint + find_package(ament_cmake_xmllint REQUIRED) + ament_xmllint() +endif () + ament_export_dependencies( controller_interface hardware_interface rclcpp rclcpp_lifecycle - realtime_tools geometry_msgs + orocos_kdl + realtime_tools ) ament_export_include_directories( include diff --git a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp index a9016252..ef514bab 100644 --- a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp +++ b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp @@ -1,67 +1,68 @@ #pragma once +#include + #include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "kdl/chainiksolver.hpp" +#include "kdl/chainiksolvervel_pinv.hpp" +#include "kdl/tree.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "uwrt_arm_cartesian_controller/visibility_control.hpp" -#include "kdl/tree.hpp" -#include "kdl/chainiksolver.hpp" -#include "kdl/chainiksolvervel_pinv.hpp" -#include +namespace uwrt_arm_cartesian_controller { +using TwistStamped = geometry_msgs::msg::TwistStamped; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -namespace uwrt_arm_cartesian_controller -{ - using TwistStamped = geometry_msgs::msg::TwistStamped; - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class UWRTCartesianController : public controller_interface::ControllerInterface { + public: + UWRTCartesianController(); - class UWRTCartesianController : public controller_interface::ControllerInterface - { - public: - UWRTCartesianController(); + ~UWRTCartesianController(); - ~UWRTCartesianController(); + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_init() override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; - UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + protected: + rclcpp::Subscription::SharedPtr m_cart_command_sub; - protected: - rclcpp::Subscription::SharedPtr m_cart_command_sub; - // realtime_tools::RealtimeBuffer> m_cart_command_buffer; // younes todo dont this buffer. just always use the latest cmd just like aidan did. initialize the twist to 0 vector + std::vector m_joint_names; + std::string m_cart_command_topic, m_robot_description, m_root_name, m_tip_name; - std::vector m_joint_names; - std::string m_interface_name, m_cart_command_topic, m_robot_description, m_root_name, m_tip_name; + private: + bool parse_params(); + TwistStamped get_null_twist() const; + // younes todo need realtime buffer bc subscirber runs in different thread and the copy isnt atomic - private: - bool parse_params(); - TwistStamped get_null_twist() const; + KDL::Tree m_kdl_tree; + KDL::Chain m_kdl_chain; + std::unique_ptr m_ik_solver; - KDL::Tree m_kdl_tree; - KDL::Chain m_kdl_chain; - std::unique_ptr m_ik_solver; + realtime_tools::RealtimeBuffer> m_rt_buffer; - TwistStamped m_last_twist; - }; + static constexpr auto COMMAND_INTERFACE = "velocity"; + static constexpr auto STATE_INTERFACE = "position"; +}; -} // namespace uwrt_arm_cartesian_controller +} // namespace uwrt_arm_cartesian_controller diff --git a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp index dc684eeb..08223c48 100644 --- a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp +++ b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp @@ -1,24 +1,3 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - #pragma once // This logic was borrowed (then namespaced) from the examples on the gcc wiki: @@ -32,7 +11,7 @@ #define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __declspec(dllexport) #define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT __declspec(dllimport) #endif -#ifdef UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_DLL +#ifdef UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_LIBRARY #define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT #else #define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT diff --git a/uwrt_arm_cartesian_controller/package.xml b/uwrt_arm_cartesian_controller/package.xml index 17c145e0..de1d2340 100644 --- a/uwrt_arm_cartesian_controller/package.xml +++ b/uwrt_arm_cartesian_controller/package.xml @@ -2,15 +2,31 @@ uwrt_arm_cartesian_controller - 0.0.0 - TODO: Package description - younes - TODO: License declaration + 1.0.0 + The uwrt_arm_cartesian_controller converts velocity commands expressed in cartesian coordinates to joint velocities + Younes Reda + + MIT ament_cmake + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + orocos_kdl + geometry_msgs + + pluginlib + ament_lint_auto ament_lint_common + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_xmllint ament_cmake diff --git a/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp index 3836f320..a514f69a 100644 --- a/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp +++ b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp @@ -8,14 +8,13 @@ namespace uwrt_arm_cartesian_controller { - UWRTCartesianController::UWRTCartesianController() : m_cart_command_sub(nullptr), m_ik_solver(nullptr) {} + UWRTCartesianController::UWRTCartesianController() : m_cart_command_sub(nullptr), m_ik_solver(nullptr), m_rt_buffer(nullptr) {} CallbackReturn UWRTCartesianController::on_init() { try { auto_declare>("joint_names", std::vector()); - auto_declare("interface_name", ""); auto_declare("end_effector_velocity_topic", ""); auto_declare("robot_description", ""); auto_declare("root_name", ""); @@ -39,7 +38,7 @@ namespace uwrt_arm_cartesian_controller } // Generate tree from urdf - if (!kdl_parser::treeFromParam(m_robot_description, m_kdl_tree)) + if (!kdl_parser::treeFromFile(m_robot_description, m_kdl_tree)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from robot description"); return CallbackReturn::ERROR; @@ -47,35 +46,37 @@ namespace uwrt_arm_cartesian_controller // Populate the KDL chain const bool chain_generated_successfully = m_kdl_tree.getChain(m_root_name, m_tip_name, m_kdl_chain); - std::stringstream tree_info = "\tTree has " << m_kdl_tree.getNrOfJoints() << " joints \n\tThe joint names are:"; + std::stringstream tree_info; + tree_info <<"Tree has " << m_kdl_tree.getNrOfJoints() << " joints" << std::endl; + tree_info << "The joint names are: " << std::endl; for (auto it = m_kdl_chain.segments.begin(); it != m_kdl_chain.segments.end(); ++it) { - tree_info << "\n\t\t" << it->getJoint().getName()); + tree_info << it->getJoint().getName() << std::endl; } - tree_info << "\n\tTree has " << m_kdl_tree.getNrOfSegments() << " segments" - << "\n\tThe segments are:"; + tree_info << "Tree has " << m_kdl_tree.getNrOfSegments() << " segments" << std::endl; + tree_info << "The segments are:" << std::endl; KDL::SegmentMap segment_map = m_kdl_tree.getSegments(); for (auto it = segment_map.begin(); it != segment_map.end(); it++) { - tree_info << "\n\t\t" << it->first); + tree_info << it->first << std::endl; } if (!chain_generated_successfully) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get KDL chain from tree: \n\t" << m_root_name << " --> " << m_tip_name); - RCLCPP_ERROR(get_node()->get_logger(), tree_info.str()); + RCLCPP_ERROR(get_node()->get_logger(), ("Failed to get KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); + RCLCPP_ERROR(get_node()->get_logger(), tree_info.str().c_str()); return CallbackReturn::ERROR; } - RCLCPP_INFO(get_node()->get_logger(), "Successfully got KDL chain from tree: \n\t" << m_root_name << " --> " << m_tip_name); - RCLCPP_INFO(get_node()->get_logger(), tree_info.str()); + RCLCPP_INFO(get_node()->get_logger(), ("Successfully got KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); + RCLCPP_INFO(get_node()->get_logger(), tree_info.str().c_str()); m_ik_solver.reset(new KDL::ChainIkSolverVel_pinv(m_kdl_chain)); m_cart_command_sub = get_node()->create_subscription( m_cart_command_topic, rclcpp::SystemDefaultsQoS(), [this](const TwistStamped::SharedPtr msg) - { m_last_twist = *msg; }); // younes todo any chance msg could be null? + { m_rt_buffer.writeFromNonRT(msg); }); // younes todo any chance msg could be null? RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -84,13 +85,12 @@ namespace uwrt_arm_cartesian_controller controller_interface::InterfaceConfiguration UWRTCartesianController::command_interface_configuration() const { - // younes todo better understand this. what is command_interfaces_? controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto &joint : m_joint_names) { - command_interfaces_config.names.push_back(joint + "/" + m_interface_name); + command_interfaces_config.names.push_back(joint + "/" + COMMAND_INTERFACE); } return command_interfaces_config; @@ -99,8 +99,15 @@ namespace uwrt_arm_cartesian_controller controller_interface::InterfaceConfiguration UWRTCartesianController::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto &joint : m_joint_names) + { + state_interfaces_config.names.push_back(joint + "/" + STATE_INTERFACE); + } + + return state_interfaces_config; } CallbackReturn UWRTCartesianController::on_activate( @@ -108,20 +115,26 @@ namespace uwrt_arm_cartesian_controller { // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" parameter - std::vector> ordered_interfaces; - if ( - !controller_interface::get_ordered_interfaces( - command_interfaces_, m_joint_names, m_interface_name, ordered_interfaces) || + /* younes todo do i need this std::vector> ordered_interfaces; + if (!controller_interface::get_ordered_interfaces(command_interfaces_, m_joint_names, COMMAND_INTERFACE, ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu position command interfaces, got %zu", m_joint_names.size(), + node_->get_logger(), "Expected %zu velocity command interfaces, got %zu", m_joint_names.size(), ordered_interfaces.size()); return CallbackReturn::ERROR; + }*/ + + if (command_interfaces_.size() != m_joint_names.size() || state_interfaces_.size() != m_joint_names.size()) + { + RCLCPP_ERROR(node_->get_logger(), "Expected %zu velocity command interfaces, got %zu. Expected %zu position state interfaces, got %zu.", m_joint_names.size(), + command_interfaces_.size(), m_joint_names.size(), state_interfaces_.size()); + return CallbackReturn::ERROR; + } // reset command if a command came through callback when controller was inactive - m_last_twist = get_null_twist(); + m_rt_buffer.reset(); return CallbackReturn::SUCCESS; } @@ -134,58 +147,57 @@ namespace uwrt_arm_cartesian_controller command_interface.set_value(0.0); } - m_last_twist = get_null_twist(); + m_rt_buffer.reset(); return CallbackReturn::SUCCESS; } controller_interface::return_type UWRTCartesianController::update(const rclcpp::Time &time, const rclcpp::Duration & /*period*/) { - // expired command - if (time - m_last_twist.header.stamp > rclcpp::Duration(5, 0)) // younes todo make this a param + auto send_zero_joint_velocities = [this]() { - KDL::SetToZero(m_cart_vel); // younes todo this var doesnt exist and its a geometry msg not a kdl type - for (uint8_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interface.set_value(0.0); - } + for (auto &command_interface : command_interfaces_) + { + command_interface.set_value(0.0); + } + }; + const auto end_effector_cart_vel = m_rt_buffer.readFromRT(); + // no command available or expired command + if(!end_effector_cart_vel || !*end_effector_cart_vel || (time - (*end_effector_cart_vel)->header.stamp) > rclcpp::Duration(5, 0)) + { + // younes todo make the duration a param + send_zero_joint_velocities(); } else { - KDL::JntArray joint_positions(command_interfaces_.size()); + KDL::JntArray joint_positions(state_interfaces_.size()); + for (uint8_t i = 0; i < state_interfaces_.size(); ++i) + { + joint_positions(i) = state_interfaces_.at(i).get_value(); + } KDL::Twist end_effector_velocity = KDL::Twist::Zero(); - end_effector_velocity(0) = m_last_twist.twist.linear.x; - end_effector_velocity(1) = m_last_twist.twist.linear.y; - end_effector_velocity(2) = m_last_twist.twist.linear.z; - end_effector_velocity(3) = m_last_twist.twist.angular.x; - end_effector_velocity(4) = m_last_twist.twist.angular.y; - end_effector_velocity(5) = m_last_twist.twist.angular.z; + end_effector_velocity(0) = (*end_effector_cart_vel)->twist.linear.x; + end_effector_velocity(1) = (*end_effector_cart_vel)->twist.linear.y; + end_effector_velocity(2) = (*end_effector_cart_vel)->twist.linear.z; + end_effector_velocity(3) = (*end_effector_cart_vel)->twist.angular.x; + end_effector_velocity(4) = (*end_effector_cart_vel)->twist.angular.y; + end_effector_velocity(5) = (*end_effector_cart_vel)->twist.angular.z; - KDL::JntArray joint_velocities(command_interfaces_.size(); - - for (uint8_t i = 0; i < command_interfaces_.size(); ++i) - { - joint_positions(i) = command_interafces_.at(i).getPosition(); - } + KDL::JntArray joint_velocities(command_interfaces_.size()); if (m_ik_solver->CartToJnt(joint_positions, end_effector_velocity, joint_velocities) < 0) { RCLCPP_ERROR(get_node()->get_logger(), "Unable to compute joint velocities for given end-effector velocity"); - KDL::SetToZero(m_cart_vel); // younes todo var doesnt exist anymroe - for (uint8_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interface.set_value(0.0); - } + send_zero_joint_velocities(); return controller_interface::return_type::ERROR; } for (uint8_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_.at(i).setCommand(joint_velocities(i)); // younes todo use set_values + command_interfaces_.at(i).set_value(joint_velocities(i)); } } - return controller_interface::return_type::OK; } @@ -200,41 +212,23 @@ namespace uwrt_arm_cartesian_controller } // Loop over string params - const std::array, std::string>, 5> string_params = {{m_interface_name, "interface_name"}, - {m_cart_command_topic, "end_effector_velocity_topic"}, - {m_robot_description, "robot_description"}, - {m_root_name, "root_name"}, - {m_tip_name, "tip_name"}}; + const std::unordered_map string_params = {{&m_cart_command_topic, "end_effector_velocity_topic"}, + {&m_robot_description, "robot_description"}, + {&m_root_name, "root_name"}, + {&m_tip_name, "tip_name"}}; for (auto &string_param : string_params) - { - if (string_param.first.empty()) + { + *(string_param.first) = node_->get_parameter(string_param.second).as_string(); + if (string_param.first->empty()) { - string_param.first = node_->get_parameter(string_param.second).as_string(); - if (string_param.first.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), string_param.second + " parameter was empty"); - return false; - } + RCLCPP_ERROR(get_node()->get_logger(), (string_param.second + " parameter was empty").c_str()); + return false; } } return true; } - TwistStamped UWRTCartesianController::get_null_twist() const - { - TwistStamped ret; - ret.twist.linear.x = 0; - ret.twist.linear.y = 0; - ret.twist.linear.z = 0; - ret.twist.angular.x = 0; - ret.twist.angular.y = 0; - ret.twist.angular.z = 0; - ret.header.stamp = rclcpp::Time::now(); - // younes todo whats the name of the arm's base frame ret.header.frame_id = "foo"; - return ret; - } - } // namespace uwrt_arm_cartesian_controller #include "pluginlib/class_list_macros.hpp" diff --git a/windows_amd64_upstream_dependencies.repos b/windows_amd64_upstream_dependencies.repos index 4b3f8e75..50ed4132 100644 --- a/windows_amd64_upstream_dependencies.repos +++ b/windows_amd64_upstream_dependencies.repos @@ -35,3 +35,9 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: galactic-devel + + # KDL Library + orocos/orocos_kinematics_dynamics: + type: git + url: https://github.com/orocos/orocos_kinematics_dynamics.git + version: release-1.5 From 252b36ce6e91de0a75ec8ae6ff95753e60b9beeb Mon Sep 17 00:00:00 2001 From: younes Date: Wed, 9 Feb 2022 15:20:30 -0500 Subject: [PATCH 3/3] fix formatting --- .../uwrt_arm_cartesian_controller.hpp | 22 +- .../src/uwrt_arm_cartesian_controller.cpp | 397 +++++++++--------- 2 files changed, 209 insertions(+), 210 deletions(-) diff --git a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp index ef514bab..e139e928 100644 --- a/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp +++ b/uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp @@ -13,12 +13,14 @@ #include "realtime_tools/realtime_buffer.h" #include "uwrt_arm_cartesian_controller/visibility_control.hpp" -namespace uwrt_arm_cartesian_controller { +namespace uwrt_arm_cartesian_controller +{ using TwistStamped = geometry_msgs::msg::TwistStamped; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class UWRTCartesianController : public controller_interface::ControllerInterface { - public: +class UWRTCartesianController : public controller_interface::ControllerInterface +{ +public: UWRTCartesianController(); ~UWRTCartesianController(); @@ -33,27 +35,27 @@ class UWRTCartesianController : public controller_interface::ControllerInterface CallbackReturn on_init() override; UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC - controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; - protected: +protected: rclcpp::Subscription::SharedPtr m_cart_command_sub; std::vector m_joint_names; std::string m_cart_command_topic, m_robot_description, m_root_name, m_tip_name; - private: +private: bool parse_params(); TwistStamped get_null_twist() const; - // younes todo need realtime buffer bc subscirber runs in different thread and the copy isnt atomic KDL::Tree m_kdl_tree; KDL::Chain m_kdl_chain; diff --git a/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp index a514f69a..a0e35eff 100644 --- a/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp +++ b/uwrt_arm_cartesian_controller/src/uwrt_arm_cartesian_controller.cpp @@ -1,121 +1,119 @@ #include "uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp" -#include "controller_interface/helpers.hpp" - -#include "kdl_parser/kdl_parser.hpp" -#include #include +#include + +#include "controller_interface/helpers.hpp" +#include "kdl_parser/kdl_parser.hpp" namespace uwrt_arm_cartesian_controller { - UWRTCartesianController::UWRTCartesianController() : m_cart_command_sub(nullptr), m_ik_solver(nullptr), m_rt_buffer(nullptr) {} - - CallbackReturn UWRTCartesianController::on_init() - { - try - { - auto_declare>("joint_names", std::vector()); - auto_declare("end_effector_velocity_topic", ""); - auto_declare("robot_description", ""); - auto_declare("root_name", ""); - auto_declare("tip_name", ""); - } - catch (const std::exception &e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; - } - - CallbackReturn UWRTCartesianController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) - { - if (!parse_params()) - { - return CallbackReturn::ERROR; - } - - // Generate tree from urdf - if (!kdl_parser::treeFromFile(m_robot_description, m_kdl_tree)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from robot description"); - return CallbackReturn::ERROR; - } - - // Populate the KDL chain - const bool chain_generated_successfully = m_kdl_tree.getChain(m_root_name, m_tip_name, m_kdl_chain); - std::stringstream tree_info; - tree_info <<"Tree has " << m_kdl_tree.getNrOfJoints() << " joints" << std::endl; - tree_info << "The joint names are: " << std::endl; - for (auto it = m_kdl_chain.segments.begin(); it != m_kdl_chain.segments.end(); ++it) - { - tree_info << it->getJoint().getName() << std::endl; - } - tree_info << "Tree has " << m_kdl_tree.getNrOfSegments() << " segments" << std::endl; - tree_info << "The segments are:" << std::endl; - KDL::SegmentMap segment_map = m_kdl_tree.getSegments(); - for (auto it = segment_map.begin(); it != segment_map.end(); it++) - { - tree_info << it->first << std::endl; - } - - if (!chain_generated_successfully) - { - RCLCPP_ERROR(get_node()->get_logger(), ("Failed to get KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); - RCLCPP_ERROR(get_node()->get_logger(), tree_info.str().c_str()); - return CallbackReturn::ERROR; - } - - RCLCPP_INFO(get_node()->get_logger(), ("Successfully got KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); - RCLCPP_INFO(get_node()->get_logger(), tree_info.str().c_str()); - - m_ik_solver.reset(new KDL::ChainIkSolverVel_pinv(m_kdl_chain)); - - m_cart_command_sub = get_node()->create_subscription( - m_cart_command_topic, rclcpp::SystemDefaultsQoS(), - [this](const TwistStamped::SharedPtr msg) - { m_rt_buffer.writeFromNonRT(msg); }); // younes todo any chance msg could be null? - - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return CallbackReturn::SUCCESS; - } +UWRTCartesianController::UWRTCartesianController() +: m_cart_command_sub(nullptr), m_ik_solver(nullptr), m_rt_buffer(nullptr) +{ +} - controller_interface::InterfaceConfiguration - UWRTCartesianController::command_interface_configuration() const - { - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; +CallbackReturn UWRTCartesianController::on_init() +{ + try { + auto_declare>("joint_names", std::vector()); + auto_declare("end_effector_velocity_topic", ""); + auto_declare("robot_description", ""); + auto_declare("root_name", ""); + auto_declare("tip_name", ""); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UWRTCartesianController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!parse_params()) { + return CallbackReturn::ERROR; + } + + // Generate tree from urdf + if (!kdl_parser::treeFromFile(m_robot_description, m_kdl_tree)) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from robot description"); + return CallbackReturn::ERROR; + } + + // Populate the KDL chain + const bool chain_generated_successfully = + m_kdl_tree.getChain(m_root_name, m_tip_name, m_kdl_chain); + std::stringstream tree_info; + tree_info << "Tree has " << m_kdl_tree.getNrOfJoints() << " joints" << std::endl; + tree_info << "The joint names are: " << std::endl; + for (auto it = m_kdl_chain.segments.begin(); it != m_kdl_chain.segments.end(); ++it) { + tree_info << it->getJoint().getName() << std::endl; + } + tree_info << "Tree has " << m_kdl_tree.getNrOfSegments() << " segments" << std::endl; + tree_info << "The segments are:" << std::endl; + KDL::SegmentMap segment_map = m_kdl_tree.getSegments(); + for (auto it = segment_map.begin(); it != segment_map.end(); it++) { + tree_info << it->first << std::endl; + } + + if (!chain_generated_successfully) { + RCLCPP_ERROR( + get_node()->get_logger(), + ("Failed to get KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); + RCLCPP_ERROR(get_node()->get_logger(), tree_info.str().c_str()); + return CallbackReturn::ERROR; + } + + RCLCPP_INFO( + get_node()->get_logger(), + ("Successfully got KDL chain from tree: " + m_root_name + " --> " + m_tip_name).c_str()); + RCLCPP_INFO(get_node()->get_logger(), tree_info.str().c_str()); + + m_ik_solver.reset(new KDL::ChainIkSolverVel_pinv(m_kdl_chain)); + + m_cart_command_sub = get_node()->create_subscription( + m_cart_command_topic, rclcpp::SystemDefaultsQoS(), [this](const TwistStamped::SharedPtr msg) { + m_rt_buffer.writeFromNonRT(msg); + }); // younes todo any chance msg could be null? + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +UWRTCartesianController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint : m_joint_names) - { - command_interfaces_config.names.push_back(joint + "/" + COMMAND_INTERFACE); - } + for (const auto & joint : m_joint_names) { + command_interfaces_config.names.push_back(joint + "/" + COMMAND_INTERFACE); + } - return command_interfaces_config; - } + return command_interfaces_config; +} - controller_interface::InterfaceConfiguration - UWRTCartesianController::state_interface_configuration() const - { - controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; +controller_interface::InterfaceConfiguration +UWRTCartesianController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint : m_joint_names) - { - state_interfaces_config.names.push_back(joint + "/" + STATE_INTERFACE); - } + for (const auto & joint : m_joint_names) { + state_interfaces_config.names.push_back(joint + "/" + STATE_INTERFACE); + } - return state_interfaces_config; - } + return state_interfaces_config; +} - CallbackReturn UWRTCartesianController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // check if we have all resources defined in the "points" parameter - // also verify that we *only* have the resources defined in the "points" parameter - /* younes todo do i need this std::vector> ordered_interfaces; +CallbackReturn UWRTCartesianController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + /* younes todo do i need this std::vector> ordered_interfaces; if (!controller_interface::get_ordered_interfaces(command_interfaces_, m_joint_names, COMMAND_INTERFACE, ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { @@ -125,113 +123,112 @@ namespace uwrt_arm_cartesian_controller return CallbackReturn::ERROR; }*/ - if (command_interfaces_.size() != m_joint_names.size() || state_interfaces_.size() != m_joint_names.size()) - { - RCLCPP_ERROR(node_->get_logger(), "Expected %zu velocity command interfaces, got %zu. Expected %zu position state interfaces, got %zu.", m_joint_names.size(), - command_interfaces_.size(), m_joint_names.size(), state_interfaces_.size()); - return CallbackReturn::ERROR; + if ( + command_interfaces_.size() != m_joint_names.size() || + state_interfaces_.size() != m_joint_names.size()) { + RCLCPP_ERROR( + node_->get_logger(), + "Expected %zu velocity command interfaces, got %zu. Expected %zu position state interfaces, " + "got %zu.", + m_joint_names.size(), command_interfaces_.size(), m_joint_names.size(), + state_interfaces_.size()); + return CallbackReturn::ERROR; + } + + // reset command if a command came through callback when controller was inactive + m_rt_buffer.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UWRTCartesianController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (auto & command_interface : command_interfaces_) { + command_interface.set_value(0.0); + } - } + m_rt_buffer.reset(); - // reset command if a command came through callback when controller was inactive - m_rt_buffer.reset(); + return CallbackReturn::SUCCESS; +} - return CallbackReturn::SUCCESS; +controller_interface::return_type UWRTCartesianController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto send_zero_joint_velocities = [this]() { + for (auto & command_interface : command_interfaces_) { + command_interface.set_value(0.0); } - - CallbackReturn UWRTCartesianController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - for (auto &command_interface : command_interfaces_) - { - command_interface.set_value(0.0); - } - - m_rt_buffer.reset(); - - return CallbackReturn::SUCCESS; + }; + const auto end_effector_cart_vel = m_rt_buffer.readFromRT(); + // no command available or expired command + if ( + !end_effector_cart_vel || !*end_effector_cart_vel || + (time - (*end_effector_cart_vel)->header.stamp) > rclcpp::Duration(5, 0)) { + // younes todo make the duration a param + send_zero_joint_velocities(); + } else { + KDL::JntArray joint_positions(state_interfaces_.size()); + for (uint8_t i = 0; i < state_interfaces_.size(); ++i) { + joint_positions(i) = state_interfaces_.at(i).get_value(); } - controller_interface::return_type UWRTCartesianController::update(const rclcpp::Time &time, const rclcpp::Duration & /*period*/) - { - auto send_zero_joint_velocities = [this]() - { - for (auto &command_interface : command_interfaces_) - { - command_interface.set_value(0.0); - } - }; - const auto end_effector_cart_vel = m_rt_buffer.readFromRT(); - // no command available or expired command - if(!end_effector_cart_vel || !*end_effector_cart_vel || (time - (*end_effector_cart_vel)->header.stamp) > rclcpp::Duration(5, 0)) - { - // younes todo make the duration a param - send_zero_joint_velocities(); - } - else - { - KDL::JntArray joint_positions(state_interfaces_.size()); - for (uint8_t i = 0; i < state_interfaces_.size(); ++i) - { - joint_positions(i) = state_interfaces_.at(i).get_value(); - } - - KDL::Twist end_effector_velocity = KDL::Twist::Zero(); - end_effector_velocity(0) = (*end_effector_cart_vel)->twist.linear.x; - end_effector_velocity(1) = (*end_effector_cart_vel)->twist.linear.y; - end_effector_velocity(2) = (*end_effector_cart_vel)->twist.linear.z; - end_effector_velocity(3) = (*end_effector_cart_vel)->twist.angular.x; - end_effector_velocity(4) = (*end_effector_cart_vel)->twist.angular.y; - end_effector_velocity(5) = (*end_effector_cart_vel)->twist.angular.z; - - KDL::JntArray joint_velocities(command_interfaces_.size()); - - if (m_ik_solver->CartToJnt(joint_positions, end_effector_velocity, joint_velocities) < 0) - { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to compute joint velocities for given end-effector velocity"); - send_zero_joint_velocities(); - return controller_interface::return_type::ERROR; - } - - for (uint8_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interfaces_.at(i).set_value(joint_velocities(i)); - } - } - return controller_interface::return_type::OK; + KDL::Twist end_effector_velocity = KDL::Twist::Zero(); + end_effector_velocity(0) = (*end_effector_cart_vel)->twist.linear.x; + end_effector_velocity(1) = (*end_effector_cart_vel)->twist.linear.y; + end_effector_velocity(2) = (*end_effector_cart_vel)->twist.linear.z; + end_effector_velocity(3) = (*end_effector_cart_vel)->twist.angular.x; + end_effector_velocity(4) = (*end_effector_cart_vel)->twist.angular.y; + end_effector_velocity(5) = (*end_effector_cart_vel)->twist.angular.z; + + KDL::JntArray joint_velocities(command_interfaces_.size()); + + if (m_ik_solver->CartToJnt(joint_positions, end_effector_velocity, joint_velocities) < 0) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Unable to compute joint velocities for given end-effector velocity"); + send_zero_joint_velocities(); + return controller_interface::return_type::ERROR; } - bool UWRTCartesianController::parse_params() - { - m_joint_names = node_->get_parameter("joints").as_string_array(); + for (uint8_t i = 0; i < command_interfaces_.size(); ++i) { + command_interfaces_.at(i).set_value(joint_velocities(i)); + } + } + return controller_interface::return_type::OK; +} - if (m_joint_names.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return false; - } - - // Loop over string params - const std::unordered_map string_params = {{&m_cart_command_topic, "end_effector_velocity_topic"}, - {&m_robot_description, "robot_description"}, - {&m_root_name, "root_name"}, - {&m_tip_name, "tip_name"}}; - for (auto &string_param : string_params) - { - *(string_param.first) = node_->get_parameter(string_param.second).as_string(); - if (string_param.first->empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), (string_param.second + " parameter was empty").c_str()); - return false; - } - } - - return true; +bool UWRTCartesianController::parse_params() +{ + m_joint_names = node_->get_parameter("joints").as_string_array(); + + if (m_joint_names.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return false; + } + + // Loop over string params + const std::unordered_map string_params = { + {&m_cart_command_topic, "end_effector_velocity_topic"}, + {&m_robot_description, "robot_description"}, + {&m_root_name, "root_name"}, + {&m_tip_name, "tip_name"}}; + for (auto & string_param : string_params) { + *(string_param.first) = node_->get_parameter(string_param.second).as_string(); + if (string_param.first->empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), (string_param.second + " parameter was empty").c_str()); + return false; } + } + + return true; +} -} // namespace uwrt_arm_cartesian_controller +} // namespace uwrt_arm_cartesian_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - uwrt_arm_cartesian_controller::UWRTCartesianController, controller_interface::ControllerInterface) \ No newline at end of file + uwrt_arm_cartesian_controller::UWRTCartesianController, controller_interface::ControllerInterface) \ No newline at end of file