From 29252ae33b61031492dde0b9f2b3947f4b0d284a Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 14 Feb 2025 03:27:52 +0100 Subject: [PATCH 01/12] overall modification to make it ros2-jazzy compatible --- hsr_description | 2 +- hsr_meshes | 2 +- hsr_navigation | 2 +- hsr_velocity_controller/CMakeLists.txt | 215 +---------- .../config/my_controller_realtime_test.yaml | 59 ++- .../hsr_velocity_controller_plugin.xml | 4 - .../switch_to_velocity_controllers.launch | 16 - .../switch_to_velocity_controllers.launch.py | 36 ++ hsr_velocity_controller/package.xml | 78 +--- .../src/hsr_velocity_controller.cpp | 340 ++++++++---------- iai_hsr_bringup/CMakeLists.txt | 204 +---------- iai_hsr_bringup/launch/hsr.launch | 20 -- iai_hsr_bringup/launch/hsr.launch.py | 47 +++ iai_hsr_bringup/package.xml | 58 +-- 14 files changed, 321 insertions(+), 762 deletions(-) delete mode 100644 hsr_velocity_controller/hsr_velocity_controller_plugin.xml delete mode 100644 hsr_velocity_controller/launch/switch_to_velocity_controllers.launch create mode 100644 hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py delete mode 100644 iai_hsr_bringup/launch/hsr.launch create mode 100644 iai_hsr_bringup/launch/hsr.launch.py diff --git a/hsr_description b/hsr_description index ec886fa..53fee8e 160000 --- a/hsr_description +++ b/hsr_description @@ -1 +1 @@ -Subproject commit ec886faaf19e4c81ebef22f54cb1ad6fb65b22b2 +Subproject commit 53fee8e4bb6f7fc0ca18aebceb6b1522f17dedd2 diff --git a/hsr_meshes b/hsr_meshes index 2d83199..648f690 160000 --- a/hsr_meshes +++ b/hsr_meshes @@ -1 +1 @@ -Subproject commit 2d831996c0f9548164a4df5886104793e557897a +Subproject commit 648f690ec04a3719582c3b3caed8193ac7d68c5b diff --git a/hsr_navigation b/hsr_navigation index 466f709..ea96068 160000 --- a/hsr_navigation +++ b/hsr_navigation @@ -1 +1 @@ -Subproject commit 466f7095f0c1b4c0c7b6e18cf185060845e60e18 +Subproject commit ea96068bd43bae83e3d1e8ffb4f856bcd70ef225 diff --git a/hsr_velocity_controller/CMakeLists.txt b/hsr_velocity_controller/CMakeLists.txt index f9d7365..cbec596 100644 --- a/hsr_velocity_controller/CMakeLists.txt +++ b/hsr_velocity_controller/CMakeLists.txt @@ -1,209 +1,28 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(hsr_velocity_controller) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(std_msgs REQUIRED) +find_package(realtime_tools REQUIRED) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - controller_interface - hardware_interface - pluginlib - roscpp -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) +add_library(${PROJECT_NAME} SHARED src/hsr_velocity_controller.cpp) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES hsr_velocity_controller -# CATKIN_DEPENDS controller_interface hardware_interface pluginlib roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install( + TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION lib ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/hsr_velocity_controller.cpp -# ) -add_library(${PROJECT_NAME} src/hsr_velocity_controller.cpp) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/hsr_velocity_controller_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html - install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_hsr_velocity_controller.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +# Export package +ament_export_dependencies(rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() \ No newline at end of file diff --git a/hsr_velocity_controller/config/my_controller_realtime_test.yaml b/hsr_velocity_controller/config/my_controller_realtime_test.yaml index 5ed2a1c..dc39ce0 100644 --- a/hsr_velocity_controller/config/my_controller_realtime_test.yaml +++ b/hsr_velocity_controller/config/my_controller_realtime_test.yaml @@ -1,32 +1,31 @@ hsrb: - realtime_body_controller_gazebo: - type: hsr_velocity_controller/HsrVelocityController - joints: - - arm_flex_joint - - arm_lift_joint - - arm_roll_joint - - wrist_flex_joint - - wrist_roll_joint - - head_pan_joint - - head_tilt_joint - p_gains: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - i_gains: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] - d_gains: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] - feedforward_gains: [1, 1, 1, 1, 1, 1, 1] - realtime_body_controller_real: - type: hsr_velocity_controller/HsrVelocityController - joints: - - arm_flex_joint - - arm_lift_joint - - arm_roll_joint - - wrist_flex_joint - - wrist_roll_joint - - head_pan_joint - - head_tilt_joint - p_gains: [ 0.1, 0.05, 0.0, 0.01, 0.01, 0.01, 0.01 ] - i_gains: [ 0, 0, 0, 0, 0, 0, 0 ] - d_gains: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] - feedforward_gains: [1, 1, 1, 0.5, 0.1, 1, 1] - - + ros__parameters: + realtime_body_controller_gazebo: + type: hsr_velocity_controller/HsrVelocityController + joints: + - arm_flex_joint + - arm_lift_joint + - arm_roll_joint + - wrist_flex_joint + - wrist_roll_joint + - head_pan_joint + - head_tilt_joint + p_gains: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + i_gains: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + d_gains: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + feedforward_gains: [1, 1, 1, 1, 1, 1, 1] + realtime_body_controller_real: + type: hsr_velocity_controller/HsrVelocityController + joints: + - arm_flex_joint + - arm_lift_joint + - arm_roll_joint + - wrist_flex_joint + - wrist_roll_joint + - head_pan_joint + - head_tilt_joint + p_gains: [0.1, 0.05, 0.0, 0.01, 0.01, 0.01, 0.01] + i_gains: [0, 0, 0, 0, 0, 0, 0] + d_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + feedforward_gains: [1, 1, 1, 0.5, 0.1, 1, 1] diff --git a/hsr_velocity_controller/hsr_velocity_controller_plugin.xml b/hsr_velocity_controller/hsr_velocity_controller_plugin.xml deleted file mode 100644 index fb450db..0000000 --- a/hsr_velocity_controller/hsr_velocity_controller_plugin.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch deleted file mode 100644 index 4366bda..0000000 --- a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - diff --git a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py new file mode 100644 index 0000000..80c4989 --- /dev/null +++ b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py @@ -0,0 +1,36 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Path to the controller configuration file + controller_config = os.path.join( + get_package_share_directory("hsr_velocity_controller"), + "config", + "my_controller_realtime_test.yaml" + ) + + # Unspawner node (disabling trajectory controllers) + controller_unspawner = Node( + package="controller_manager", + executable="spawner", + arguments=["arm_trajectory_controller", "head_trajectory_controller"], + namespace="/hsrb", + output="screen" + ) + + # Spawner for the realtime controller + realtime_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["realtime_body_controller_real", "--controller-manager", "/hsrb/controller_manager"], + namespace="/hsrb", + output="screen" + ) + + return LaunchDescription([ + controller_unspawner, + realtime_controller_spawner + ]) diff --git a/hsr_velocity_controller/package.xml b/hsr_velocity_controller/package.xml index 13e00b1..dc6fb53 100644 --- a/hsr_velocity_controller/package.xml +++ b/hsr_velocity_controller/package.xml @@ -1,71 +1,19 @@ - + hsr_velocity_controller - 0.0.0 - The hsr_velocity_controller package + 0.1.0 + HSR Velocity Controller for ROS2 + Your Name + Apache-2.0 + + rclcpp + controller_interface + hardware_interface + pluginlib + std_msgs + realtime_tools - - - - huerkamp - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - controller_interface - hardware_interface - pluginlib - roscpp - controller_interface - hardware_interface - pluginlib - roscpp - controller_interface - hardware_interface - pluginlib - roscpp - - - - - + ament_cmake diff --git a/hsr_velocity_controller/src/hsr_velocity_controller.cpp b/hsr_velocity_controller/src/hsr_velocity_controller.cpp index bfe26d0..31f4ede 100644 --- a/hsr_velocity_controller/src/hsr_velocity_controller.cpp +++ b/hsr_velocity_controller/src/hsr_velocity_controller.cpp @@ -1,228 +1,196 @@ #include #include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace hsr_velocity_controller_ns{ - - class HsrVelocityController : public controller_interface::Controller +#include + +#include "rclcpp/rclcpp.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "urdf/model.h" +#include +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" + +namespace hsr_velocity_controller_ns +{ + class HsrVelocityController : public controller_interface::ControllerInterface { - bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle &n) + public: + HsrVelocityController() : controller_interface::ControllerInterface() {} + + controller_interface::CallbackReturn on_init() override { - // List of controlled joints - std::string param_name = "joints"; - if(!n.getParam(param_name, joint_names_)) + try { - ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); - return false; + node_ = std::make_shared("hsr_velocity_controller"); } - n_joints_ = joint_names_.size(); - - if(n_joints_ == 0){ - ROS_ERROR_STREAM("List of joint names is empty."); - return false; - } - for(unsigned int i=0; igetHandle(joint_names_[i])); - } - catch (const hardware_interface::HardwareInterfaceException& e) - { - ROS_ERROR_STREAM("Exception thrown: " << e.what()); - return false; - } + RCLCPP_ERROR(rclcpp::get_logger("HsrVelocityController"), "Exception: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; } - commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + return controller_interface::CallbackReturn::SUCCESS; + } - sub_command_ = n.subscribe("command", 1, &HsrVelocityController::commandCB, this); - // pub_ = n.advertise("controller_state", 1); - pub_.reset(new realtime_tools::RealtimePublisher(n, "controller_state", 1)); - urdf::Model urdf; - if (!urdf.initParamWithNodeHandle("robot_description", n)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } - - counter = 0; - js_ = std::vector(n_joints_); - joints_urdf_ = std::vector(n_joints_); - for(unsigned int i ; ideclare_parameter("joints", std::vector()); + joint_names_ = node_->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) { - js_[i] = joints_[i].getPosition(); - joints_urdf_[i] = urdf.getJoint(joint_names_[i]); - ROS_ERROR_STREAM("Joint " << joint_names_[i] << " lower limit " << joints_urdf_[i]->limits->lower << " upper limit " << joints_urdf_[i]->limits->upper); + RCLCPP_ERROR(node_->get_logger(), "No joints specified in parameter file!"); + return controller_interface::CallbackReturn::ERROR; } - - n.getParam("p_gains", p_gains_); - n.getParam("i_gains", i_gains_); - n.getParam("d_gains", d_gains_); - n.getParam("feedforward_gains", ff_gains_); - old_integrator_ = std::vector(n_joints_); - old_error_ = std::vector(n_joints_); - filtered_vel_ = std::vector(n_joints_); + n_joints_ = joint_names_.size(); + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); - return true; + // Initialize vectors + old_integrator_.resize(n_joints_, 0.0); + old_error_.resize(n_joints_, 0.0); + filtered_vel_.resize(n_joints_, 0.0); + p_gains_.resize(n_joints_, 0.0); + i_gains_.resize(n_joints_, 0.0); + d_gains_.resize(n_joints_, 0.0); + ff_gains_.resize(n_joints_, 0.0); + + // Create subscriber + sub_command_ = node_->create_subscription( + "command", 1, std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1)); + + // Create real-time publisher + pub_ = std::make_unique>( + node_->create_publisher("controller_state", 1)); + + RCLCPP_INFO(node_->get_logger(), "HsrVelocityController Configured."); + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (size_t i = 0; i < n_joints_; i++) + { + joint_handles_.push_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION)); + } + return controller_interface::CallbackReturn::SUCCESS; } - void starting(const ros::Time& time){} - void stopping(const ros::Time& time){} + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) override + { + return controller_interface::CallbackReturn::SUCCESS; + } - void update(const ros::Time& time, const ros::Duration& period) + controller_interface::return_type update( + const rclcpp::Time & /*time*/, const rclcpp::Duration &period) override { - std::vector & commands = *commands_buffer_.readFromRT(); - std::vector d(n_joints_+1, 0); - - double dt = period.toNSec() / 1e9; - for(unsigned int i = 0; i &commands = *commands_buffer_.readFromRT(); + double dt = period.seconds(); + + for (size_t i = 0; i < n_joints_; i++) { double vel_cmd = commands[i]; - // filter the velocity - float alpha = 0.95f; - filtered_vel_[i] = (1.0f - alpha) * filtered_vel_[i] + alpha * joints_[i].getVelocity(); - if(vel_cmd == 0.0) - { - js_[i] = joints_[i].getPosition(); - old_integrator_[i] = 0.0; - old_error_[i] = 0.0; + double current_pos; - }else + // Correct way to get joint position + if (!joint_handles_[i].get_value(current_pos)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to get value for joint %s", joint_names_[i].c_str()); + continue; + } + + double error = vel_cmd - filtered_vel_[i]; + double new_integrator = old_integrator_[i] + error * dt; + + double next_pos = current_pos + + vel_cmd * dt * ff_gains_[i] + + error * p_gains_[i] + + new_integrator * i_gains_[i] + + d_gains_[i] / dt * (error - old_error_[i]); + + // Correct way to set joint position + if (!joint_handles_[i].set_value(next_pos)) { - double error = (vel_cmd - filtered_vel_[i]); - double new_integrator = old_integrator_[i] + error * dt; - - double next_pos = joints_[i].getPosition() + vel_cmd * dt * ff_gains_[i] - + error * p_gains_[i] - + new_integrator * i_gains_[i] - + d_gains_[i]/dt * (error - old_error_[i]); - - // clamp the output by the joint limits and disable further integration integration - if(next_pos > joints_urdf_[i]->limits->upper) - { - next_pos = joints_urdf_[i]->limits->upper; - if(error < 0) - { - old_integrator_[i] = new_integrator; - } - } - else if(next_pos < joints_urdf_[i]->limits->lower) - { - next_pos = joints_urdf_[i]->limits->lower; - if(error > 0) - { - old_integrator_[i] = new_integrator; - } - } - else - { - old_integrator_[i] = new_integrator; - } - - // TODO: test that - // js_[i] = next_pos; // Currently not needed, could be used agin on the real robot - joints_[i].setCommand(next_pos); - old_error_[i] = error; - - d[i] = vel_cmd - filtered_vel_[i]; + RCLCPP_ERROR(node_->get_logger(), "Failed to set value for joint %s", joint_names_[i].c_str()); } + + old_error_[i] = error; + old_integrator_[i] = new_integrator; } - if(counter % 10 == 0) + if (counter % 10 == 0) { - if(pub_ && pub_->trylock()) + if (pub_ && pub_->trylock()) { - d[n_joints_] = period.toSec(); pub_->msg_.data = filtered_vel_; pub_->unlockAndPublish(); } } - counter++; - } - // void update_adaptive(const ros::Time& time, const ros::Duration& period) - // { - // ROS_ERROR_STREAM("Wrong Function"); - // std::vector & commands = *commands_buffer_.readFromRT(); - // // for(unsigned int i=0; itrylock()) - // // { - // // std_msgs::Float64MultiArray msg; - // // std::vector d(5, 0); - // // d[0] = vel_cmd; - // // d[1] = vel_diff; - // // d[2] = vel_curr; - // // d[3] = vel_gain; - // // d[4] = next_pos; - // // pub_->msg_.data = d; - // // pub_->msg_.header.stamp = ros::Time::now(); - // // pub_.publish(msg); - - // // counter = 0; - // // } - // // counter = counter + 1; - // } - - std::vector< std::string > joint_names_; - std::vector< hardware_interface::JointHandle > joints_; - realtime_tools::RealtimeBuffer > commands_buffer_; - unsigned int n_joints_; - unsigned int counter; - std::vector js_ ; - std::vector old_integrator_ ; - std::vector old_error_ ; - std::vector filtered_vel_ ; - std::vector p_gains_; - std::vector i_gains_; - std::vector d_gains_; - std::vector ff_gains_; - std::vector joints_urdf_; + counter++; + return controller_interface::return_type::OK; + } + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) + { + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); + } + return config; + } - private: - ros::Subscriber sub_command_; - std::unique_ptr > pub_; + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) + { + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + return config; + } - void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) + private: + void commandCB(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { - if(msg->data.size()!=n_joints_) + if (msg->data.size() != n_joints_) { - ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + RCLCPP_ERROR(node_->get_logger(), "Command size mismatch: %zu (expected %u)", + msg->data.size(), n_joints_); return; } - commands_buffer_.writeFromNonRT(msg->data); + commands_buffer_.writeFromNonRT(msg->data); } + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr sub_command_; + std::unique_ptr> pub_; + + std::vector joint_names_; + std::vector joint_handles_; + unsigned int n_joints_; + unsigned int counter = 0; + std::vector old_integrator_; + std::vector old_error_; + std::vector filtered_vel_; + std::vector p_gains_; + std::vector i_gains_; + std::vector d_gains_; + std::vector ff_gains_; + + realtime_tools::RealtimeBuffer> commands_buffer_; }; - PLUGINLIB_EXPORT_CLASS(hsr_velocity_controller_ns::HsrVelocityController, controller_interface::ControllerBase) -} + +} // namespace hsr_velocity_controller_ns + +PLUGINLIB_EXPORT_CLASS(hsr_velocity_controller_ns::HsrVelocityController, controller_interface::ControllerInterface) diff --git a/iai_hsr_bringup/CMakeLists.txt b/iai_hsr_bringup/CMakeLists.txt index b09bb80..9753c10 100644 --- a/iai_hsr_bringup/CMakeLists.txt +++ b/iai_hsr_bringup/CMakeLists.txt @@ -1,202 +1,12 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(iai_hsr_bringup) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +# Find ament and dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES iai_hsr_bringup -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/iai_hsr_bringup.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/iai_hsr_bringup_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_iai_hsr_bringup.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() \ No newline at end of file diff --git a/iai_hsr_bringup/launch/hsr.launch b/iai_hsr_bringup/launch/hsr.launch deleted file mode 100644 index f6e575b..0000000 --- a/iai_hsr_bringup/launch/hsr.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py new file mode 100644 index 0000000..14969ae --- /dev/null +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -0,0 +1,47 @@ +import os +from ament_index_python.packages import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import os + +def generate_launch_description(): + # Declare launch arguments + return LaunchDescription([ + DeclareLaunchArgument('velocity_controller', default_value='False', description='Activate velocity controller'), + DeclareLaunchArgument('apartment_map', default_value='True', description='Load apartment map'), + + # Include upload_hsrb.launch from hsr_description + IncludeLaunchDescription( + launch_description_source=[os.path.join( + get_package_share_directory('hsr_description'), 'launch', 'upload_hsrb.launch')], + launch_arguments={}.items() + ), + + # Robot state publisher node + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_pub', + respawn=True, + output='screen', + namespace='hsrb/robot_state' + ), + + # Conditionally include the apartment map launch file + IncludeLaunchDescription( + launch_description_source=[os.path.join( + get_package_share_directory('hsr_navigation'), 'launch', 'hsr_map.launch')], + condition=IfCondition(LaunchConfiguration('apartment_map')) + ), + + # Conditionally include the velocity controller launch file + IncludeLaunchDescription( + launch_description_source=[os.path.join( + get_package_share_directory('hsr_velocity_controller'), 'launch', 'switch_to_velocity_controllers.launch')], + condition=IfCondition(LaunchConfiguration('velocity_controller')) + ), + ]) diff --git a/iai_hsr_bringup/package.xml b/iai_hsr_bringup/package.xml index e486198..6792c85 100644 --- a/iai_hsr_bringup/package.xml +++ b/iai_hsr_bringup/package.xml @@ -1,59 +1,31 @@ - + iai_hsr_bringup 0.0.0 The iai_hsr_bringup package - - - toya - - - - TODO + + rclcpp + sensor_msgs + geometry_msgs + std_msgs + tf2_ros + robot_state_publisher + ros2_control + ros2_controllers - - - - - - - - - - + + ament_cmake + + rosidl_default_runtime - - - - - - - - - - - - - - - - - - - - - catkin - - - - + ament_cmake From 9a1fe6d281d746901897c581518893f607d51d35 Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 14 Feb 2025 12:37:48 +0100 Subject: [PATCH 02/12] corrected common errors --- hsr_velocity_controller/CMakeLists.txt | 7 ++++--- .../launch/switch_to_velocity_controllers.launch.py | 0 iai_hsr_bringup/launch/hsr.launch.py | 6 +++--- 3 files changed, 7 insertions(+), 6 deletions(-) mode change 100644 => 100755 hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py diff --git a/hsr_velocity_controller/CMakeLists.txt b/hsr_velocity_controller/CMakeLists.txt index cbec596..04d5a6c 100644 --- a/hsr_velocity_controller/CMakeLists.txt +++ b/hsr_velocity_controller/CMakeLists.txt @@ -9,13 +9,14 @@ find_package(pluginlib REQUIRED) find_package(std_msgs REQUIRED) find_package(realtime_tools REQUIRED) - - add_library(${PROJECT_NAME} SHARED src/hsr_velocity_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) - +# Install directories (if there are specific resources to install) +install(DIRECTORY launch config src + DESTINATION share/${PROJECT_NAME} +) install( TARGETS ${PROJECT_NAME} diff --git a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py old mode 100644 new mode 100755 diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index 14969ae..886eb98 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): # Include upload_hsrb.launch from hsr_description IncludeLaunchDescription( launch_description_source=[os.path.join( - get_package_share_directory('hsr_description'), 'launch', 'upload_hsrb.launch')], + get_package_share_directory('hsr_description'), 'launch', 'hsrb4s_fake_joints.launch.py')], launch_arguments={}.items() ), @@ -34,14 +34,14 @@ def generate_launch_description(): # Conditionally include the apartment map launch file IncludeLaunchDescription( launch_description_source=[os.path.join( - get_package_share_directory('hsr_navigation'), 'launch', 'hsr_map.launch')], + get_package_share_directory('hsr_navigation'), 'launch', 'hsr_amcl_map.launch.py')], condition=IfCondition(LaunchConfiguration('apartment_map')) ), # Conditionally include the velocity controller launch file IncludeLaunchDescription( launch_description_source=[os.path.join( - get_package_share_directory('hsr_velocity_controller'), 'launch', 'switch_to_velocity_controllers.launch')], + get_package_share_directory('hsr_velocity_controller'), 'launch', 'switch_to_velocity_controllers.launch.py')], condition=IfCondition(LaunchConfiguration('velocity_controller')) ), ]) From 060b1771475ab5c8a98385779cfb0f2469f6d47b Mon Sep 17 00:00:00 2001 From: Mitesh Savsaviya Date: Fri, 21 Feb 2025 11:15:01 +0100 Subject: [PATCH 03/12] modified meshes package --- hsr_description | 2 +- hsr_meshes | 1 - iai_hsr_bringup/CMakeLists.txt | 3 +++ iai_hsr_bringup/launch/hsr.launch.py | 2 +- iai_hsr_bringup/package.xml | 5 +++-- 5 files changed, 8 insertions(+), 5 deletions(-) delete mode 160000 hsr_meshes diff --git a/hsr_description b/hsr_description index 53fee8e..f868a7d 160000 --- a/hsr_description +++ b/hsr_description @@ -1 +1 @@ -Subproject commit 53fee8e4bb6f7fc0ca18aebceb6b1522f17dedd2 +Subproject commit f868a7d02b1234a6596810faf9eff090ab3c64a6 diff --git a/hsr_meshes b/hsr_meshes deleted file mode 160000 index 648f690..0000000 --- a/hsr_meshes +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 648f690ec04a3719582c3b3caed8193ac7d68c5b diff --git a/iai_hsr_bringup/CMakeLists.txt b/iai_hsr_bringup/CMakeLists.txt index 9753c10..0c4ca6a 100644 --- a/iai_hsr_bringup/CMakeLists.txt +++ b/iai_hsr_bringup/CMakeLists.txt @@ -4,6 +4,9 @@ project(iai_hsr_bringup) # Find ament and dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(hsr_description REQUIRED) +find_package(hsr_navigation REQUIRED) +#find_package(hsr_meshes REQUIRED) install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index 886eb98..d42ffea 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): # Include upload_hsrb.launch from hsr_description IncludeLaunchDescription( launch_description_source=[os.path.join( - get_package_share_directory('hsr_description'), 'launch', 'hsrb4s_fake_joints.launch.py')], + get_package_share_directory('hsr_description'), 'launch', 'hsrb4s.launch.py')], launch_arguments={}.items() ), diff --git a/iai_hsr_bringup/package.xml b/iai_hsr_bringup/package.xml index 6792c85..32007f4 100644 --- a/iai_hsr_bringup/package.xml +++ b/iai_hsr_bringup/package.xml @@ -17,6 +17,8 @@ robot_state_publisher ros2_control ros2_controllers + hsr_description + hsr_navigation ament_cmake @@ -24,8 +26,7 @@ rosidl_default_runtime - ament_cmake - + \ No newline at end of file From a1f22166452caecda743ece4226b48354dd3203d Mon Sep 17 00:00:00 2001 From: Mitesh Savsaviya Date: Fri, 21 Feb 2025 11:23:36 +0100 Subject: [PATCH 04/12] changed submodules --- .gitmodules | 7 ++----- hsr_navigation | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/.gitmodules b/.gitmodules index a42a890..bc2d1fc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,6 @@ [submodule "hsr_description"] path = hsr_description - url = https://github.com/code-iai/hsr_description.git -[submodule "hsr_meshes"] - path = hsr_meshes - url = https://github.com/ToyotaResearchInstitute/hsr_meshes.git + url = https://github.com/mitsav01/hsr_description.git [submodule "hsr_navigation"] path = hsr_navigation - url = https://github.com/sunava/hsr_navigation.git + url = https://github.com/mitsav01/hsr_navigation.git diff --git a/hsr_navigation b/hsr_navigation index ea96068..2fb7ab4 160000 --- a/hsr_navigation +++ b/hsr_navigation @@ -1 +1 @@ -Subproject commit ea96068bd43bae83e3d1e8ffb4f856bcd70ef225 +Subproject commit 2fb7ab40dbbf466322ba429f75c8e8e22ee75cef From 54d789272bb387d221c159c7de1937f17edcfefd Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 24 Oct 2025 03:07:29 +0200 Subject: [PATCH 05/12] jvc_working --- hsr_velocity_controller/CMakeLists.txt | 12 +- .../config/my_controller_realtime_test.yaml | 50 ++-- .../hsr_velocity_controller.xml | 5 + .../switch_to_velocity_controllers.launch.py | 38 +-- .../src/hsr_velocity_controller.cpp | 267 ++++++++---------- hsrb_description | 1 + iai_hsr_bringup/CMakeLists.txt | 2 +- iai_hsr_bringup/launch/hsr.launch.py | 135 ++++++--- iai_hsr_bringup/package.xml | 2 +- 9 files changed, 279 insertions(+), 233 deletions(-) create mode 100644 hsr_velocity_controller/hsr_velocity_controller.xml create mode 160000 hsrb_description diff --git a/hsr_velocity_controller/CMakeLists.txt b/hsr_velocity_controller/CMakeLists.txt index 04d5a6c..2b6cb6a 100644 --- a/hsr_velocity_controller/CMakeLists.txt +++ b/hsr_velocity_controller/CMakeLists.txt @@ -13,7 +13,7 @@ add_library(${PROJECT_NAME} SHARED src/hsr_velocity_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) -# Install directories (if there are specific resources to install) +# Install directories install(DIRECTORY launch config src DESTINATION share/${PROJECT_NAME} ) @@ -23,7 +23,15 @@ install( LIBRARY DESTINATION lib ) +# Install plugin XML +install(FILES hsr_velocity_controller.xml + DESTINATION share/${PROJECT_NAME} +) + +# Export plugin description +pluginlib_export_plugin_description_file(controller_interface hsr_velocity_controller.xml) + # Export package ament_export_dependencies(rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) -ament_package() \ No newline at end of file +ament_package() diff --git a/hsr_velocity_controller/config/my_controller_realtime_test.yaml b/hsr_velocity_controller/config/my_controller_realtime_test.yaml index dc39ce0..de89b2b 100644 --- a/hsr_velocity_controller/config/my_controller_realtime_test.yaml +++ b/hsr_velocity_controller/config/my_controller_realtime_test.yaml @@ -1,31 +1,25 @@ -hsrb: +controller_manager: ros__parameters: - realtime_body_controller_gazebo: - type: hsr_velocity_controller/HsrVelocityController - joints: - - arm_flex_joint - - arm_lift_joint - - arm_roll_joint - - wrist_flex_joint - - wrist_roll_joint - - head_pan_joint - - head_tilt_joint - p_gains: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - i_gains: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] - d_gains: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] - feedforward_gains: [1, 1, 1, 1, 1, 1, 1] + update_rate: 200 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster realtime_body_controller_real: - type: hsr_velocity_controller/HsrVelocityController - joints: - - arm_flex_joint - - arm_lift_joint - - arm_roll_joint - - wrist_flex_joint - - wrist_roll_joint - - head_pan_joint - - head_tilt_joint - p_gains: [0.1, 0.05, 0.0, 0.01, 0.01, 0.01, 0.01] - i_gains: [0, 0, 0, 0, 0, 0, 0] - d_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - feedforward_gains: [1, 1, 1, 0.5, 0.1, 1, 1] + type: hsr_velocity_controller_ns/HsrVelocityController + + +realtime_body_controller_real: + ros__parameters: + joints: + - arm_flex_joint + - arm_lift_joint + - arm_roll_joint + - wrist_flex_joint + - wrist_roll_joint + - head_pan_joint + - head_tilt_joint + p_gains: [0.1, 0.05, 0.0, 0.01, 0.01, 0.01, 0.01] + i_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + d_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + feedforward_gains: [1.0, 1.0, 1.0, 0.5, 0.1, 1.0, 1.0] diff --git a/hsr_velocity_controller/hsr_velocity_controller.xml b/hsr_velocity_controller/hsr_velocity_controller.xml new file mode 100644 index 0000000..8a91481 --- /dev/null +++ b/hsr_velocity_controller/hsr_velocity_controller.xml @@ -0,0 +1,5 @@ + + + diff --git a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py index 80c4989..dc38388 100755 --- a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py +++ b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py @@ -3,34 +3,34 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory - def generate_launch_description(): - # Path to the controller configuration file - controller_config = os.path.join( + controller_yaml = os.path.join( get_package_share_directory("hsr_velocity_controller"), "config", "my_controller_realtime_test.yaml" ) - # Unspawner node (disabling trajectory controllers) - controller_unspawner = Node( - package="controller_manager", - executable="spawner", - arguments=["arm_trajectory_controller", "head_trajectory_controller"], - namespace="/hsrb", - output="screen" + # ros2_control node + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[{ + 'robot_description': os.path.join( + get_package_share_directory('hsrb_description'), + 'robots', 'hsrb4s.urdf.xacro') + }, controller_yaml], + output='screen', ) - # Spawner for the realtime controller - realtime_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["realtime_body_controller_real", "--controller-manager", "/hsrb/controller_manager"], - namespace="/hsrb", - output="screen" + # Spawner for velocity controller + velocity_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['realtime_body_controller_real', '--controller-manager', '/controller_manager'], + output='screen', ) return LaunchDescription([ - controller_unspawner, - realtime_controller_spawner + ros2_control_node, + velocity_controller_spawner ]) diff --git a/hsr_velocity_controller/src/hsr_velocity_controller.cpp b/hsr_velocity_controller/src/hsr_velocity_controller.cpp index 31f4ede..559daf4 100644 --- a/hsr_velocity_controller/src/hsr_velocity_controller.cpp +++ b/hsr_velocity_controller/src/hsr_velocity_controller.cpp @@ -8,188 +8,167 @@ #include "controller_interface/controller_interface.hpp" #include "std_msgs/msg/float64_multi_array.hpp" #include "pluginlib/class_list_macros.hpp" -#include "urdf/model.h" -#include #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" namespace hsr_velocity_controller_ns { - class HsrVelocityController : public controller_interface::ControllerInterface +class HsrVelocityController : public controller_interface::ControllerInterface +{ +public: + HsrVelocityController() : controller_interface::ControllerInterface() {} + + controller_interface::CallbackReturn on_init() override { - public: - HsrVelocityController() : controller_interface::ControllerInterface() {} + // No need to create a new node; get_node() provides lifecycle node + return controller_interface::CallbackReturn::SUCCESS; + } - controller_interface::CallbackReturn on_init() override + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + // Get parameters from launch/YAML + joint_names_ = get_node()->declare_parameter>("joints"); + p_gains_ = get_node()->declare_parameter>("p_gains"); + i_gains_ = get_node()->declare_parameter>("i_gains"); + d_gains_ = get_node()->declare_parameter>("d_gains"); + ff_gains_ = get_node()->declare_parameter>("feedforward_gains"); + + if (joint_names_.empty()) { - try - { - node_ = std::make_shared("hsr_velocity_controller"); - } - catch (const std::exception &e) - { - RCLCPP_ERROR(rclcpp::get_logger("HsrVelocityController"), "Exception: %s", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; + RCLCPP_ERROR(get_node()->get_logger(), "No joints specified in parameter file!"); + return controller_interface::CallbackReturn::ERROR; } - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) override - { - node_->declare_parameter("joints", std::vector()); - joint_names_ = node_->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(node_->get_logger(), "No joints specified in parameter file!"); - return controller_interface::CallbackReturn::ERROR; - } + n_joints_ = joint_names_.size(); + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); - n_joints_ = joint_names_.size(); - commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + // Initialize internal vectors + old_integrator_.resize(n_joints_, 0.0); + old_error_.resize(n_joints_, 0.0); + filtered_vel_.resize(n_joints_, 0.0); - // Initialize vectors - old_integrator_.resize(n_joints_, 0.0); - old_error_.resize(n_joints_, 0.0); - filtered_vel_.resize(n_joints_, 0.0); - p_gains_.resize(n_joints_, 0.0); - i_gains_.resize(n_joints_, 0.0); - d_gains_.resize(n_joints_, 0.0); - ff_gains_.resize(n_joints_, 0.0); + // Subscriber + sub_command_ = get_node()->create_subscription( + "command", 1, std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1)); - // Create subscriber - sub_command_ = node_->create_subscription( - "command", 1, std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1)); + // Real-time publisher + pub_ = std::make_unique>( + get_node()->create_publisher("controller_state", 1)); - // Create real-time publisher - pub_ = std::make_unique>( - node_->create_publisher("controller_state", 1)); - - RCLCPP_INFO(node_->get_logger(), "HsrVelocityController Configured."); - return controller_interface::CallbackReturn::SUCCESS; - } + RCLCPP_INFO(get_node()->get_logger(), "HsrVelocityController Configured."); + return controller_interface::CallbackReturn::SUCCESS; + } - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) override + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + { + // Initialize command interfaces + joint_handles_.clear(); + for (const auto &joint_name : joint_names_) { - for (size_t i = 0; i < n_joints_; i++) - { - joint_handles_.push_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_POSITION)); - } - return controller_interface::CallbackReturn::SUCCESS; + joint_handles_.push_back(hardware_interface::CommandInterface(joint_name, hardware_interface::HW_IF_POSITION)); } + return controller_interface::CallbackReturn::SUCCESS; + } - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) override - { - return controller_interface::CallbackReturn::SUCCESS; - } + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) override + { + return controller_interface::CallbackReturn::SUCCESS; + } - 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 + { + std::vector &commands = *commands_buffer_.readFromRT(); + double dt = period.seconds(); + + for (size_t i = 0; i < n_joints_; i++) { - std::vector &commands = *commands_buffer_.readFromRT(); - double dt = period.seconds(); + double vel_cmd = commands[i]; + double current_pos; - for (size_t i = 0; i < n_joints_; i++) + if (!joint_handles_[i].get_value(current_pos)) { - double vel_cmd = commands[i]; - double current_pos; - - // Correct way to get joint position - if (!joint_handles_[i].get_value(current_pos)) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to get value for joint %s", joint_names_[i].c_str()); - continue; - } - - double error = vel_cmd - filtered_vel_[i]; - double new_integrator = old_integrator_[i] + error * dt; - - double next_pos = current_pos + - vel_cmd * dt * ff_gains_[i] + - error * p_gains_[i] + - new_integrator * i_gains_[i] + - d_gains_[i] / dt * (error - old_error_[i]); - - // Correct way to set joint position - if (!joint_handles_[i].set_value(next_pos)) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to set value for joint %s", joint_names_[i].c_str()); - } - - old_error_[i] = error; - old_integrator_[i] = new_integrator; + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get value for joint %s", joint_names_[i].c_str()); + continue; } - if (counter % 10 == 0) + double error = vel_cmd - filtered_vel_[i]; + double new_integrator = old_integrator_[i] + error * dt; + + double next_pos = current_pos + + vel_cmd * dt * ff_gains_[i] + + error * p_gains_[i] + + new_integrator * i_gains_[i] + + d_gains_[i] / dt * (error - old_error_[i]); + + if (!joint_handles_[i].set_value(next_pos)) { - if (pub_ && pub_->trylock()) - { - pub_->msg_.data = filtered_vel_; - pub_->unlockAndPublish(); - } + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set value for joint %s", joint_names_[i].c_str()); } - counter++; - return controller_interface::return_type::OK; + old_error_[i] = error; + old_integrator_[i] = new_integrator; } - controller_interface::InterfaceConfiguration command_interface_configuration() const override + // Publish controller state at lower frequency + if (counter % 10 == 0 && pub_ && pub_->trylock()) { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) - { - config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); - } - return config; + pub_->msg_.data = filtered_vel_; + pub_->unlockAndPublish(); } - controller_interface::InterfaceConfiguration state_interface_configuration() const override + counter++; + return controller_interface::return_type::OK; + } + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) - { - config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); - } - return config; + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); } + return config; + } - private: - void commandCB(const std_msgs::msg::Float64MultiArray::SharedPtr msg) + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { - if (msg->data.size() != n_joints_) - { - RCLCPP_ERROR(node_->get_logger(), "Command size mismatch: %zu (expected %u)", - msg->data.size(), n_joints_); - return; - } - commands_buffer_.writeFromNonRT(msg->data); + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); } + return config; + } - rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr sub_command_; - std::unique_ptr> pub_; - - std::vector joint_names_; - std::vector joint_handles_; - unsigned int n_joints_; - unsigned int counter = 0; - std::vector old_integrator_; - std::vector old_error_; - std::vector filtered_vel_; - std::vector p_gains_; - std::vector i_gains_; - std::vector d_gains_; - std::vector ff_gains_; - - realtime_tools::RealtimeBuffer> commands_buffer_; - }; +private: + void commandCB(const std_msgs::msg::Float64MultiArray::SharedPtr msg) + { + if (msg->data.size() != n_joints_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Command size mismatch: %zu (expected %u)", msg->data.size(), n_joints_); + return; + } + commands_buffer_.writeFromNonRT(msg->data); + } + + std::vector joint_names_; + std::vector joint_handles_; + unsigned int n_joints_; + unsigned int counter = 0; + std::vector old_integrator_; + std::vector old_error_; + std::vector filtered_vel_; + std::vector p_gains_; + std::vector i_gains_; + std::vector d_gains_; + std::vector ff_gains_; + + rclcpp::Subscription::SharedPtr sub_command_; + std::unique_ptr> pub_; + realtime_tools::RealtimeBuffer> commands_buffer_; +}; } // namespace hsr_velocity_controller_ns diff --git a/hsrb_description b/hsrb_description new file mode 160000 index 0000000..886c290 --- /dev/null +++ b/hsrb_description @@ -0,0 +1 @@ +Subproject commit 886c290800f12061e4c6e0dc3d7f74e74eb9918e diff --git a/iai_hsr_bringup/CMakeLists.txt b/iai_hsr_bringup/CMakeLists.txt index 0c4ca6a..7a924ab 100644 --- a/iai_hsr_bringup/CMakeLists.txt +++ b/iai_hsr_bringup/CMakeLists.txt @@ -4,7 +4,7 @@ project(iai_hsr_bringup) # Find ament and dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(hsr_description REQUIRED) +find_package(hsrb_description REQUIRED) find_package(hsr_navigation REQUIRED) #find_package(hsr_meshes REQUIRED) diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index d42ffea..0cdaa0a 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -1,47 +1,106 @@ import os from ament_index_python.packages import get_package_share_directory -import launch from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, LogInfo, IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable from launch_ros.actions import Node -import os +from launch.launch_description_sources import PythonLaunchDescriptionSource + def generate_launch_description(): - # Declare launch arguments - return LaunchDescription([ - DeclareLaunchArgument('velocity_controller', default_value='False', description='Activate velocity controller'), - DeclareLaunchArgument('apartment_map', default_value='True', description='Load apartment map'), - - # Include upload_hsrb.launch from hsr_description - IncludeLaunchDescription( - launch_description_source=[os.path.join( - get_package_share_directory('hsr_description'), 'launch', 'hsrb4s.launch.py')], - launch_arguments={}.items() - ), - - # Robot state publisher node - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_pub', - respawn=True, - output='screen', - namespace='hsrb/robot_state' - ), - # Conditionally include the apartment map launch file - IncludeLaunchDescription( - launch_description_source=[os.path.join( - get_package_share_directory('hsr_navigation'), 'launch', 'hsr_amcl_map.launch.py')], - condition=IfCondition(LaunchConfiguration('apartment_map')) - ), + # === Launch Arguments === + velocity_controller_arg = DeclareLaunchArgument( + 'velocity_controller', + default_value='False', + description='Use ros2_control for velocity-based control (instead of GUI)' + ) + + apartment_map_arg = DeclareLaunchArgument( + 'apartment_map', + default_value='False', + description='Load apartment navigation stack' + ) + + description_file = LaunchConfiguration('description_file', default='hsrb4s.urdf.xacro') + + # === Robot Description === + robot_description_content = Command( + [PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', + PathJoinSubstitution([get_package_share_directory('hsrb_description'), 'robots', description_file])] + ) + robot_description = {'robot_description': robot_description_content} - # Conditionally include the velocity controller launch file - IncludeLaunchDescription( - launch_description_source=[os.path.join( - get_package_share_directory('hsr_velocity_controller'), 'launch', 'switch_to_velocity_controllers.launch.py')], - condition=IfCondition(LaunchConfiguration('velocity_controller')) + rviz_config_file = PathJoinSubstitution([get_package_share_directory('hsrb_description'), 'launch', 'display.rviz']) + + # === YAML file path === + controller_yaml_file = os.path.join( + get_package_share_directory('hsr_velocity_controller'), + 'config', 'my_controller_realtime_test.yaml' + ) + + # === Nodes === + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[robot_description] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file] + ) + + joint_state_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + condition=UnlessCondition(LaunchConfiguration('velocity_controller')), + ) + + # === ros2_control Node === + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ + robot_description, + controller_yaml_file, # Load controller config + {'use_sim_time': False} # Add any other required parameters + ], + output='screen', + condition=IfCondition(LaunchConfiguration('velocity_controller')), + ) + + # === Controller Spawner === + velocity_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'realtime_body_controller_real', + '--controller-manager', '/controller_manager' + ], + output='screen', + condition=IfCondition(LaunchConfiguration('velocity_controller')), + ) + + # === Optional Apartment Map Launch === + apartment_map_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('hsr_navigation'), 'launch', 'hsr_amcl_map.launch.py') ), - ]) + condition=IfCondition(LaunchConfiguration('apartment_map')) + ) + + # === Launch Description === + return LaunchDescription([ + velocity_controller_arg, + apartment_map_arg, + robot_state_publisher_node, + joint_state_gui_node, + ros2_control_node, + rviz_node, + velocity_controller_spawner, + apartment_map_launch + ]) \ No newline at end of file diff --git a/iai_hsr_bringup/package.xml b/iai_hsr_bringup/package.xml index 32007f4..d56ef69 100644 --- a/iai_hsr_bringup/package.xml +++ b/iai_hsr_bringup/package.xml @@ -17,7 +17,7 @@ robot_state_publisher ros2_control ros2_controllers - hsr_description + hsrb_description hsr_navigation From 9ca8daa72e40632e2bb9a70e3b00f71b2235c9cb Mon Sep 17 00:00:00 2001 From: Mitesh Savsaviya <135125057+mitsav01@users.noreply.github.com> Date: Fri, 24 Oct 2025 03:17:54 +0200 Subject: [PATCH 06/12] Revise README for HSR Velocity Controller details Updated README to include details about HSR velocity controller, installation instructions, and usage. --- README.md | 50 +++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 47 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 1da18cb..ea018d1 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,48 @@ -# iai_hsr -Installation, configuration, and launch files specific to IAI's HSR robot. +# HSR Velocity Controller for ROS 2 Jazzy -Please make sure to clone this repo with `git clone --recurse-submodules` to get the submodules automatically checked out. +This package provides a velocity controller for the HSR robot using ROS 2 Jazzy and `ros2_control`. The controller is implemented in C++ and can be used to command the robot's joints in real time. + +--- + +## Features + +- Velocity control of multiple joints +- Real-time command handling +- Publishes controller state for monitoring +- Compatible with ROS 2 Jazzy and `ros2_control` + +--- + +## Installation + +Clone the repository into your ROS 2 workspace: + +```bash +cd ~/ros2_ws/src +git clone https://github.com/mitsav01/iai_hsr.git --recursive +``` + +--- + +## Build the workspace +```bash +cd ~/ros2_ws +colcon build --symlink-install +source install/setup.bash +``` + +## Visualise the robot + +Use the provided launch file to start the robot: + +```bash +ros2 launch iai_hsr_bringup hsr.launch.py +``` + +## Launching the controller + +Use the provided launch file to start the robot and controller: + +```bash +ros2 launch iai_hsr_bringup hsr.launch.py velocity_controller:=True +``` From ee64dff39929a705883b0b82eb5dfc416cc95a63 Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 24 Oct 2025 13:06:45 +0200 Subject: [PATCH 07/12] Update submodule references and enhance README with installation and usage instructions --- .gitmodules | 6 +++--- README.md | 50 ++++++++++++++++++++++++++++++++++++++++++++++--- hsr_description | 1 - 3 files changed, 50 insertions(+), 7 deletions(-) delete mode 160000 hsr_description diff --git a/.gitmodules b/.gitmodules index bc2d1fc..c2b66ac 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ -[submodule "hsr_description"] - path = hsr_description - url = https://github.com/mitsav01/hsr_description.git +[submodule "hsrb_description"] + path = hsrb_description + url = https://github.com/mitsav01/hsrb_description.git [submodule "hsr_navigation"] path = hsr_navigation url = https://github.com/mitsav01/hsr_navigation.git diff --git a/README.md b/README.md index 1da18cb..83e3ce2 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,48 @@ -# iai_hsr -Installation, configuration, and launch files specific to IAI's HSR robot. +# HSR Velocity Controller for ROS 2 Jazzy -Please make sure to clone this repo with `git clone --recurse-submodules` to get the submodules automatically checked out. +This package provides a velocity controller for the HSR robot using ROS 2 Jazzy and `ros2_control`. The controller is implemented in C++ and can be used to command the robot's joints in real time. + +--- + +## Features + +- Velocity control of multiple joints +- Real-time command handling +- Publishes controller state for monitoring +- Compatible with ROS 2 Jazzy and `ros2_control` + +--- + +## Installation + +Clone the repository into your ROS 2 workspace: + +```bash +cd ~/ros2_ws/src +git clone https://github.com/mitsav01/iai_hsr.git --recursive +``` + +--- + +## Build the workspace +```bash +cd ~/ros2_ws +colcon build --symlink-install +source install/setup.bash +``` + +## Visualise the robot + +Use the provided launch file to start the robot: + +```bash +ros2 launch iai_hsr_bringup hsr.launch.py +``` + +## Launching the controller + +Use the provided launch file to start the robot and controller: + +```bash +ros2 launch iai_hsr_bringup hsr.launch.py velocity_controller:=True +``` \ No newline at end of file diff --git a/hsr_description b/hsr_description deleted file mode 160000 index f868a7d..0000000 --- a/hsr_description +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f868a7d02b1234a6596810faf9eff090ab3c64a6 From 2b65f556f3e74146c77ce168b0f602a4ce652d0f Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 24 Oct 2025 13:14:31 +0200 Subject: [PATCH 08/12] Set hsrb_description submodule to track humble branch --- .gitmodules | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index c2b66ac..9ad1367 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,8 @@ [submodule "hsrb_description"] - path = hsrb_description - url = https://github.com/mitsav01/hsrb_description.git + path = hsrb_description + url = https://github.com/mitsav01/hsrb_description.git + branch = humble + [submodule "hsr_navigation"] - path = hsr_navigation - url = https://github.com/mitsav01/hsr_navigation.git + path = hsr_navigation + url = https://github.com/mitsav01/hsr_navigation.git From affc8412d06551915336be8b9f2eb48f311d284b Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 24 Oct 2025 13:16:29 +0200 Subject: [PATCH 09/12] updated modules --- hsrb_description | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hsrb_description b/hsrb_description index 886c290..953733a 160000 --- a/hsrb_description +++ b/hsrb_description @@ -1 +1 @@ -Subproject commit 886c290800f12061e4c6e0dc3d7f74e74eb9918e +Subproject commit 953733a63b0ec41f16706fceb3b7116c9bd9ac21 From b493d1a377fa2071a6e23bb6912754443b5d22de Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Wed, 29 Oct 2025 21:23:54 +0100 Subject: [PATCH 10/12] Refactor submodule references and update package dependencies for hsr_description --- .gitmodules | 13 +- README.md | 2 +- hsr_description | 1 + hsr_velocity_controller/CMakeLists.txt | 10 +- hsr_velocity_controller/package.xml | 1 + .../src/hsr_velocity_controller.cpp | 392 ++++++++++++------ hsrb_description | 1 - iai_hsr_bringup/CMakeLists.txt | 2 +- iai_hsr_bringup/launch/hsr.launch.py | 29 +- iai_hsr_bringup/package.xml | 2 +- 10 files changed, 293 insertions(+), 160 deletions(-) create mode 160000 hsr_description delete mode 160000 hsrb_description diff --git a/.gitmodules b/.gitmodules index 9ad1367..d6d40fd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,8 +1,7 @@ -[submodule "hsrb_description"] - path = hsrb_description - url = https://github.com/mitsav01/hsrb_description.git - branch = humble - +[submodule "hsr_description"] + path = hsr_description + url = https://github.com/code-iai/hsr_description.git + branch = ros2-jazzy [submodule "hsr_navigation"] - path = hsr_navigation - url = https://github.com/mitsav01/hsr_navigation.git + path = hsr_navigation + url = https://github.com/sunava/hsr_navigation.git \ No newline at end of file diff --git a/README.md b/README.md index ea018d1..ee2631a 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Clone the repository into your ROS 2 workspace: ```bash cd ~/ros2_ws/src -git clone https://github.com/mitsav01/iai_hsr.git --recursive +git clone https://github.com/code-iai/iai_hsr.git --recursive ``` --- diff --git a/hsr_description b/hsr_description new file mode 160000 index 0000000..22986db --- /dev/null +++ b/hsr_description @@ -0,0 +1 @@ +Subproject commit 22986dbb9bd1ca96334a3604405afe36fc6defa5 diff --git a/hsr_velocity_controller/CMakeLists.txt b/hsr_velocity_controller/CMakeLists.txt index 2b6cb6a..afd65a3 100644 --- a/hsr_velocity_controller/CMakeLists.txt +++ b/hsr_velocity_controller/CMakeLists.txt @@ -8,24 +8,24 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(std_msgs REQUIRED) find_package(realtime_tools REQUIRED) - +find_package(rclcpp_lifecycle REQUIRED) add_library(${PROJECT_NAME} SHARED src/hsr_velocity_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} rclcpp controller_interface hardware_interface pluginlib std_msgs realtime_tools) # Install directories install(DIRECTORY launch config src - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) install( - TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION lib + TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION lib ) # Install plugin XML install(FILES hsr_velocity_controller.xml - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) # Export plugin description diff --git a/hsr_velocity_controller/package.xml b/hsr_velocity_controller/package.xml index dc6fb53..9216e80 100644 --- a/hsr_velocity_controller/package.xml +++ b/hsr_velocity_controller/package.xml @@ -12,6 +12,7 @@ pluginlib std_msgs realtime_tools + rclcpp_lifecycle ament_cmake diff --git a/hsr_velocity_controller/src/hsr_velocity_controller.cpp b/hsr_velocity_controller/src/hsr_velocity_controller.cpp index 559daf4..cff7032 100644 --- a/hsr_velocity_controller/src/hsr_velocity_controller.cpp +++ b/hsr_velocity_controller/src/hsr_velocity_controller.cpp @@ -2,174 +2,308 @@ #include #include -#include "rclcpp/rclcpp.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "controller_interface/controller_interface.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" -#include "pluginlib/class_list_macros.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_publisher.hpp" +#include +#include +#include +#include +#include +#include +#include +#include namespace hsr_velocity_controller_ns { + class HsrVelocityController : public controller_interface::ControllerInterface { public: - HsrVelocityController() : controller_interface::ControllerInterface() {} + HsrVelocityController() = default; - controller_interface::CallbackReturn on_init() override - { - // No need to create a new node; get_node() provides lifecycle node - return controller_interface::CallbackReturn::SUCCESS; + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + config.names.push_back(joint_name + "/position"); } + return config; + } - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - // Get parameters from launch/YAML - joint_names_ = get_node()->declare_parameter>("joints"); - p_gains_ = get_node()->declare_parameter>("p_gains"); - i_gains_ = get_node()->declare_parameter>("i_gains"); - d_gains_ = get_node()->declare_parameter>("d_gains"); - ff_gains_ = get_node()->declare_parameter>("feedforward_gains"); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "No joints specified in parameter file!"); - return controller_interface::CallbackReturn::ERROR; - } + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(joint_names_.size() * 2); + for (const auto& joint_name : joint_names_) { + config.names.push_back(joint_name + "/position"); + config.names.push_back(joint_name + "/velocity"); + } + return config; + } - n_joints_ = joint_names_.size(); - commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override + { + try { + auto node = get_node(); - // Initialize internal vectors - old_integrator_.resize(n_joints_, 0.0); - old_error_.resize(n_joints_, 0.0); - filtered_vel_.resize(n_joints_, 0.0); + // Declare parameters first + node->declare_parameter("joints", std::vector()); + node->declare_parameter("p_gains", std::vector()); + node->declare_parameter("i_gains", std::vector()); + node->declare_parameter("d_gains", std::vector()); + node->declare_parameter("feedforward_gains", std::vector()); + node->declare_parameter("robot_description", std::string("")); - // Subscriber - sub_command_ = get_node()->create_subscription( - "command", 1, std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1)); + // Get joint names parameter + joint_names_ = node->get_parameter("joints").as_string_array(); - // Real-time publisher - pub_ = std::make_unique>( - get_node()->create_publisher("controller_state", 1)); + n_joints_ = joint_names_.size(); - RCLCPP_INFO(get_node()->get_logger(), "HsrVelocityController Configured."); - return controller_interface::CallbackReturn::SUCCESS; - } + if (n_joints_ == 0) { + RCLCPP_ERROR(get_node()->get_logger(), "List of joint names is empty."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_node()->get_logger(), "Controller configured for %zu joints:", n_joints_); + for (size_t i = 0; i < n_joints_; i++) { + RCLCPP_INFO(get_node()->get_logger(), " - %s", joint_names_[i].c_str()); + } + + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + + // Create subscriber + sub_command_ = node->create_subscription( + "command", + rclcpp::SystemDefaultsQoS(), + std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1) + ); + + // Create publisher first, then create realtime publisher + auto publisher = node->create_publisher( + "controller_state", 1); + pub_ = std::make_unique>(publisher); + + // Load URDF + urdf_ = std::make_shared(); + std::string urdf_string = node->get_parameter("robot_description").as_string(); + if (!urdf_->initString(urdf_string)) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override - { - // Initialize command interfaces - joint_handles_.clear(); - for (const auto &joint_name : joint_names_) - { - joint_handles_.push_back(hardware_interface::CommandInterface(joint_name, hardware_interface::HW_IF_POSITION)); + counter = 0; + js_ = std::vector(n_joints_); + joints_urdf_ = std::vector(n_joints_); + + for (unsigned int i = 0; i < n_joints_; i++) { + js_[i] = 0.0; + joints_urdf_[i] = urdf_->getJoint(joint_names_[i]); + if (joints_urdf_[i] && joints_urdf_[i]->limits) { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Joint " << joint_names_[i] << " limits: [" + << joints_urdf_[i]->limits->lower << ", " << joints_urdf_[i]->limits->upper << "]"); + } else { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Joint " << joint_names_[i] << " has no limits defined!"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - return controller_interface::CallbackReturn::SUCCESS; + } + + // Get PID gains with default values + p_gains_ = node->get_parameter("p_gains").as_double_array(); + i_gains_ = node->get_parameter("i_gains").as_double_array(); + d_gains_ = node->get_parameter("d_gains").as_double_array(); + ff_gains_ = node->get_parameter("feedforward_gains").as_double_array(); + + // Check gain sizes + if (p_gains_.size() != n_joints_ || i_gains_.size() != n_joints_ || + d_gains_.size() != n_joints_ || ff_gains_.size() != n_joints_) { + RCLCPP_ERROR(get_node()->get_logger(), "Gain vector sizes do not match number of joints!"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_node()->get_logger(), "Gains loaded successfully:"); + for (size_t i = 0; i < n_joints_; i++) { + RCLCPP_INFO(get_node()->get_logger(), "Joint %zu: P=%.3f, I=%.3f, D=%.3f, FF=%.3f", + i, p_gains_[i], i_gains_[i], d_gains_[i], ff_gains_[i]); + } + + old_integrator_ = std::vector(n_joints_, 0.0); + old_error_ = std::vector(n_joints_, 0.0); + filtered_vel_ = std::vector(n_joints_, 0.0); + + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Exception thrown in init: " << e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) override - { - return controller_interface::CallbackReturn::SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override + { + std::vector& commands = *commands_buffer_.readFromRT(); + + double dt = period.seconds(); + + // Debug: print first command occasionally + if (counter % 100 == 0 && commands[0] != 0.0) { + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Received command: %.3f", commands[0]); } - controller_interface::return_type update(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) override - { - std::vector &commands = *commands_buffer_.readFromRT(); - double dt = period.seconds(); + for (unsigned int i = 0; i < n_joints_; i++) { + double vel_cmd = commands[i]; - for (size_t i = 0; i < n_joints_; i++) - { - double vel_cmd = commands[i]; - double current_pos; + // Get current state + double current_pos = state_interfaces_[i * 2].get_value(); + double current_vel = state_interfaces_[i * 2 + 1].get_value(); - if (!joint_handles_[i].get_value(current_pos)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get value for joint %s", joint_names_[i].c_str()); - continue; - } + // Debug occasionally + if (counter % 100 == 0 && i == 0 && vel_cmd != 0.0) { + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Joint %d: cmd=%.3f, pos=%.3f, vel=%.3f", + i, vel_cmd, current_pos, current_vel); + } - double error = vel_cmd - filtered_vel_[i]; - double new_integrator = old_integrator_[i] + error * dt; + // Filter the velocity + float alpha = 0.95f; + filtered_vel_[i] = (1.0f - alpha) * filtered_vel_[i] + alpha * current_vel; - double next_pos = current_pos + - vel_cmd * dt * ff_gains_[i] + - error * p_gains_[i] + - new_integrator * i_gains_[i] + - d_gains_[i] / dt * (error - old_error_[i]); + if (vel_cmd == 0.0) { + js_[i] = current_pos; + old_integrator_[i] = 0.0; + old_error_[i] = 0.0; + } else { + double error = (vel_cmd - filtered_vel_[i]); + double new_integrator = old_integrator_[i] + error * dt; - if (!joint_handles_[i].set_value(next_pos)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to set value for joint %s", joint_names_[i].c_str()); - } + double next_pos = current_pos + vel_cmd * dt * ff_gains_[i] + + error * p_gains_[i] + + new_integrator * i_gains_[i] + + d_gains_[i] / dt * (error - old_error_[i]); - old_error_[i] = error; + // Clamp the output by the joint limits + if (joints_urdf_[i] && joints_urdf_[i]->limits) { + if (next_pos > joints_urdf_[i]->limits->upper) { + next_pos = joints_urdf_[i]->limits->upper; + if (error < 0) { + old_integrator_[i] = new_integrator; + } + } else if (next_pos < joints_urdf_[i]->limits->lower) { + next_pos = joints_urdf_[i]->limits->lower; + if (error > 0) { + old_integrator_[i] = new_integrator; + } + } else { old_integrator_[i] = new_integrator; + } + } else { + old_integrator_[i] = new_integrator; } - // Publish controller state at lower frequency - if (counter % 10 == 0 && pub_ && pub_->trylock()) - { - pub_->msg_.data = filtered_vel_; - pub_->unlockAndPublish(); + bool success = command_interfaces_[i].set_value(next_pos); + if (!success) { + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Failed to set command for joint %d", i); } + old_error_[i] = error; - counter++; - return controller_interface::return_type::OK; + // Debug output position + if (counter % 100 == 0 && i == 0) { + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Setting joint %d position to: %.3f", i, next_pos); + } + } } - controller_interface::InterfaceConfiguration command_interface_configuration() const override - { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) - { - config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); - } - return config; + // Publish controller state periodically + if (counter % 10 == 0 && pub_ && pub_->trylock()) { + pub_->msg_.data = filtered_vel_; + pub_->unlockAndPublish(); } + counter++; - controller_interface::InterfaceConfiguration state_interface_configuration() const override - { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) - { - config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); - } - return config; + return controller_interface::return_type::OK; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State& previous_state) override + { + RCLCPP_INFO(get_node()->get_logger(), "Configuring controller"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state) override + { + RCLCPP_INFO(get_node()->get_logger(), "Activating controller"); + + // Reset controller state + for (unsigned int i = 0; i < n_joints_; i++) { + double current_pos = state_interfaces_[i * 2].get_value(); + js_[i] = current_pos; + old_integrator_[i] = 0.0; + old_error_[i] = 0.0; + filtered_vel_[i] = 0.0; + + RCLCPP_INFO(get_node()->get_logger(), "Joint %d initial position: %.3f", i, current_pos); } -private: - void commandCB(const std_msgs::msg::Float64MultiArray::SharedPtr msg) - { - if (msg->data.size() != n_joints_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Command size mismatch: %zu (expected %u)", msg->data.size(), n_joints_); - return; - } - commands_buffer_.writeFromNonRT(msg->data); + // Set initial command to current position + for (unsigned int i = 0; i < n_joints_; i++) { + bool success = command_interfaces_[i].set_value(js_[i]); + if (!success) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set initial command for joint %d", i); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Set joint %d initial command to: %.3f", i, js_[i]); + } } - std::vector joint_names_; - std::vector joint_handles_; - unsigned int n_joints_; - unsigned int counter = 0; - std::vector old_integrator_; - std::vector old_error_; - std::vector filtered_vel_; - std::vector p_gains_; - std::vector i_gains_; - std::vector d_gains_; - std::vector ff_gains_; - - rclcpp::Subscription::SharedPtr sub_command_; - std::unique_ptr> pub_; - realtime_tools::RealtimeBuffer> commands_buffer_; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state) override + { + RCLCPP_INFO(get_node()->get_logger(), "Deactivating controller"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + +private: + std::vector joint_names_; + realtime_tools::RealtimeBuffer> commands_buffer_; + unsigned int n_joints_; + unsigned int counter; + std::vector js_; + std::vector old_integrator_; + std::vector old_error_; + std::vector filtered_vel_; + std::vector p_gains_; + std::vector i_gains_; + std::vector d_gains_; + std::vector ff_gains_; + std::vector joints_urdf_; + std::shared_ptr urdf_; + + rclcpp::Subscription::SharedPtr sub_command_; + std::unique_ptr> pub_; + + void commandCB(const std_msgs::msg::Float64MultiArray::SharedPtr msg) + { + if (msg->data.size() != n_joints_) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Dimension of command (" << msg->data.size() + << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + return; + } + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Received command with %zu elements", msg->data.size()); + commands_buffer_.writeFromNonRT(msg->data); + } }; -} // namespace hsr_velocity_controller_ns +} // namespace hsr_velocity_controller_ns -PLUGINLIB_EXPORT_CLASS(hsr_velocity_controller_ns::HsrVelocityController, controller_interface::ControllerInterface) +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + hsr_velocity_controller_ns::HsrVelocityController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/hsrb_description b/hsrb_description deleted file mode 160000 index 953733a..0000000 --- a/hsrb_description +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 953733a63b0ec41f16706fceb3b7116c9bd9ac21 diff --git a/iai_hsr_bringup/CMakeLists.txt b/iai_hsr_bringup/CMakeLists.txt index 7a924ab..0c4ca6a 100644 --- a/iai_hsr_bringup/CMakeLists.txt +++ b/iai_hsr_bringup/CMakeLists.txt @@ -4,7 +4,7 @@ project(iai_hsr_bringup) # Find ament and dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(hsrb_description REQUIRED) +find_package(hsr_description REQUIRED) find_package(hsr_navigation REQUIRED) #find_package(hsr_meshes REQUIRED) diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index 0cdaa0a..9a40c76 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -28,11 +28,11 @@ def generate_launch_description(): # === Robot Description === robot_description_content = Command( [PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', - PathJoinSubstitution([get_package_share_directory('hsrb_description'), 'robots', description_file])] + PathJoinSubstitution([get_package_share_directory('hsr_description'), 'robots', description_file])] ) robot_description = {'robot_description': robot_description_content} - rviz_config_file = PathJoinSubstitution([get_package_share_directory('hsrb_description'), 'launch', 'display.rviz']) + rviz_config_file = PathJoinSubstitution([get_package_share_directory('hsr_description'), 'launch', 'display.rviz']) # === YAML file path === controller_yaml_file = os.path.join( @@ -53,26 +53,25 @@ def generate_launch_description(): arguments=['-d', rviz_config_file] ) - joint_state_gui_node = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui', - condition=UnlessCondition(LaunchConfiguration('velocity_controller')), - ) - # === ros2_control Node === ros2_control_node = Node( package='controller_manager', executable='ros2_control_node', parameters=[ robot_description, - controller_yaml_file, # Load controller config - {'use_sim_time': False} # Add any other required parameters + controller_yaml_file, + {'use_sim_time': False} ], output='screen', condition=IfCondition(LaunchConfiguration('velocity_controller')), ) - + non_actuated_joint_state_pub_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher_non_actuated', + parameters=[{'use_gui': False}], + condition=IfCondition(LaunchConfiguration('velocity_controller')) + ) # === Controller Spawner === velocity_controller_spawner = Node( package='controller_manager', @@ -98,9 +97,9 @@ def generate_launch_description(): velocity_controller_arg, apartment_map_arg, robot_state_publisher_node, - joint_state_gui_node, ros2_control_node, rviz_node, velocity_controller_spawner, - apartment_map_launch - ]) \ No newline at end of file + apartment_map_launch, + non_actuated_joint_state_pub_node + ]) diff --git a/iai_hsr_bringup/package.xml b/iai_hsr_bringup/package.xml index d56ef69..32007f4 100644 --- a/iai_hsr_bringup/package.xml +++ b/iai_hsr_bringup/package.xml @@ -17,7 +17,7 @@ robot_state_publisher ros2_control ros2_controllers - hsrb_description + hsr_description hsr_navigation From 37ae1ef7e0c96ae6034fefa8e5a30f9667e68d09 Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Thu, 30 Oct 2025 13:51:42 +0100 Subject: [PATCH 11/12] added joint_state_broadcaster node --- iai_hsr_bringup/launch/hsr.launch.py | 60 ++++++++++++++++------------ 1 file changed, 34 insertions(+), 26 deletions(-) diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index 9a40c76..528ea9f 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -1,12 +1,11 @@ -import os -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable from launch_ros.actions import Node from launch.launch_description_sources import PythonLaunchDescriptionSource - +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): @@ -26,15 +25,16 @@ def generate_launch_description(): description_file = LaunchConfiguration('description_file', default='hsrb4s.urdf.xacro') # === Robot Description === - robot_description_content = Command( - [PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', - PathJoinSubstitution([get_package_share_directory('hsr_description'), 'robots', description_file])] - ) + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', + PathJoinSubstitution([get_package_share_directory('hsr_description'), 'robots', description_file]) + ]) robot_description = {'robot_description': robot_description_content} - rviz_config_file = PathJoinSubstitution([get_package_share_directory('hsr_description'), 'launch', 'display.rviz']) + rviz_config_file = PathJoinSubstitution([ + get_package_share_directory('hsr_description'), 'launch', 'display.rviz' + ]) - # === YAML file path === controller_yaml_file = os.path.join( get_package_share_directory('hsr_velocity_controller'), 'config', 'my_controller_realtime_test.yaml' @@ -53,10 +53,18 @@ def generate_launch_description(): arguments=['-d', rviz_config_file] ) - # === ros2_control Node === + joint_state_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + condition=UnlessCondition(LaunchConfiguration('velocity_controller')), + ) + + # ros2_control Node ros2_control_node = Node( package='controller_manager', executable='ros2_control_node', + name='controller_manager', # important for spawner parameters=[ robot_description, controller_yaml_file, @@ -65,26 +73,26 @@ def generate_launch_description(): output='screen', condition=IfCondition(LaunchConfiguration('velocity_controller')), ) - non_actuated_joint_state_pub_node = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher_non_actuated', - parameters=[{'use_gui': False}], + + # Spawn joint_state_broadcaster first + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'], + output='screen', condition=IfCondition(LaunchConfiguration('velocity_controller')) ) - # === Controller Spawner === + + # Spawn velocity controller after joint_state_broadcaster velocity_controller_spawner = Node( package='controller_manager', executable='spawner', - arguments=[ - 'realtime_body_controller_real', - '--controller-manager', '/controller_manager' - ], + arguments=['realtime_body_controller_real', '--controller-manager', '/controller_manager'], output='screen', - condition=IfCondition(LaunchConfiguration('velocity_controller')), + condition=IfCondition(LaunchConfiguration('velocity_controller')) ) - # === Optional Apartment Map Launch === + # Optional apartment map apartment_map_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('hsr_navigation'), 'launch', 'hsr_amcl_map.launch.py') @@ -92,14 +100,14 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('apartment_map')) ) - # === Launch Description === return LaunchDescription([ velocity_controller_arg, apartment_map_arg, robot_state_publisher_node, + joint_state_gui_node, ros2_control_node, - rviz_node, + joint_state_broadcaster_spawner, velocity_controller_spawner, - apartment_map_launch, - non_actuated_joint_state_pub_node + rviz_node, + apartment_map_launch ]) From 027120036f9f2451c6eef91e0844d36e9bf7866d Mon Sep 17 00:00:00 2001 From: mitsav01 Date: Fri, 31 Oct 2025 21:43:03 +0100 Subject: [PATCH 12/12] added mock_hardware tutorial and minor changes in launchfiles --- hsr_velocity_controller/README.md | 6 +- hsr_velocity_controller/launch/README.md | 79 +++++++++++++++++++ .../switch_to_velocity_controllers.launch.py | 46 ++++++++--- iai_hsr_bringup/launch/hsr.launch.py | 5 +- 4 files changed, 119 insertions(+), 17 deletions(-) create mode 100644 hsr_velocity_controller/launch/README.md diff --git a/hsr_velocity_controller/README.md b/hsr_velocity_controller/README.md index ff1f708..f1bbc58 100644 --- a/hsr_velocity_controller/README.md +++ b/hsr_velocity_controller/README.md @@ -9,8 +9,10 @@ To have the velocity controller also working on the real HSR do the following st 2. Create an overlay workspace and copy the hsr_velocity_controller package into the src folder of the workspace 3. Build the workspace 4. Activate the overlay workspace in the HSR config files (A deatiled description can be found on hsr.io) -5. After restarting the HSR the command `rosservice call /controller_manager/list_controllers` should also list the hsr_velocity_controller +5. After restarting the HSR the command `ros2 control list_controllers` should also list the hsr_velocity_controller ## Usage Instructions On startup the HSR shoulda always start with is default controllers active. -Use the launchfile `switch_to_velocity_controllers.launch` to deactivate those and activate the velocity controller. +Use the launchfile `switch_to_velocity_controllers.launch.py` to deactivate those and activate the velocity controller. + +> **Note:** Before launching `switch_to_velocity_controllers.launch.py`, ensure that the `/controller_manager` node is already running. diff --git a/hsr_velocity_controller/launch/README.md b/hsr_velocity_controller/launch/README.md new file mode 100644 index 0000000..bb60a6c --- /dev/null +++ b/hsr_velocity_controller/launch/README.md @@ -0,0 +1,79 @@ +ROS2's `mock_componennts` mimics the behaviour of robot in simulation without any physical robot. + +Below is a miny tutorial for this setup. + +# 1. What "Mock Hardware" does: + +The `mock_components/GenericSystem` simulates joints interfaces defined in our URDF. + +- It is a good choice to test controller logic and message flow. + +# 2. Setup Overview: + +*URDF (ros2_control.robot.xacro) + +Inside `` block, under `` tag, ensure this selection exists: + +```xml + + + mock_components/GenericSystem + + + + + +``` +**Note: The joint names should match exactly what velocity controller expects (as seen in its log list)** + +# 3. Controller config (my_controller_realtime_test.yaml) + +Make sure this YAML defines your controller and its joints + +# 4. Launch Mock test + +We can reuse our `hsr.launch.py` + +```bash +ros2 launch iai_hsr_bringup hsr.launch.py velocity_controller:=True +``` + +We will see something like this in logs: + +```bash +[ros2_control_node-2] [INFO] [controller_manager]: Loaded hardware 'hsrb' from plugin 'mock_components/GenericSystem' +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: Controller configured for 7 joints: +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - arm_flex_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - arm_lift_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - arm_roll_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - wrist_flex_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - wrist_roll_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - head_pan_joint +[ros2_control_node-2] [INFO] [realtime_body_controller_real]: - head_tilt_joint +``` + +At this point, our system is running with *mock_hardware* + +# 5. send Commands to the controller + +Our controller subscribes to `/command`(like `std_msgs/msg/Float64MultiArray`), test it using below command in another terminal: + +```bash +ros2 topic pub /command std_msgs/msg/Float64MultiArray \ +"data: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" +``` + +We can observe the movement of robot in `rviz2`. + +# 6. Typical debug checks: + +Issue + +``` +[spawner-3] [WARN] [spawner_realtime_body_controller_real]: Could not contact service /controller_manager/list_controllers +[spawner-3] [INFO] [spawner_realtime_body_controller_real]: waiting for service /controller_manager/list_controllers to become available... +``` + +Solution + +Please make sure `/controller_manager` node is visible. If not, please check if `ros2_control` node is included in the launch file. \ No newline at end of file diff --git a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py index dc38388..b8a67df 100755 --- a/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py +++ b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py @@ -1,24 +1,45 @@ import os from launch import LaunchDescription from launch_ros.actions import Node +from launch.substitutions import Command,LaunchConfiguration, PathJoinSubstitution, FindExecutable from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - controller_yaml = os.path.join( - get_package_share_directory("hsr_velocity_controller"), - "config", - "my_controller_realtime_test.yaml" - ) - # ros2_control node + description_file = LaunchConfiguration('description_file', default='hsrb4s.urdf.xacro') + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', + PathJoinSubstitution([get_package_share_directory('hsr_description'), 'robots', description_file]) + ]) + robot_description = {'robot_description': robot_description_content} + + controller_yaml_file = os.path.join( + get_package_share_directory('hsr_velocity_controller'), + 'config', 'my_controller_realtime_test.yaml' +) + + # ros2_control Node ros2_control_node = Node( package='controller_manager', executable='ros2_control_node', - parameters=[{ - 'robot_description': os.path.join( - get_package_share_directory('hsrb_description'), - 'robots', 'hsrb4s.urdf.xacro') - }, controller_yaml], + name='controller_manager', + parameters=[ + robot_description, + controller_yaml_file, + {'use_sim_time': False} + ], + output='screen' + ) + # Unspawn existing trajectory controllers + unspawn_controllers = Node( + package='controller_manager', + executable='unspawner', + arguments=[ + 'arm_trajectory_controller', + 'head_trajectory_controller', + '-c', '/controller_manager' + ], output='screen', ) @@ -31,6 +52,7 @@ def generate_launch_description(): ) return LaunchDescription([ - ros2_control_node, + #ros2_control_node, + unspawn_controllers, velocity_controller_spawner ]) diff --git a/iai_hsr_bringup/launch/hsr.launch.py b/iai_hsr_bringup/launch/hsr.launch.py index 528ea9f..ec74281 100644 --- a/iai_hsr_bringup/launch/hsr.launch.py +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -74,7 +74,7 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('velocity_controller')), ) - # Spawn joint_state_broadcaster first + # Spawn joint_state_broadcaster joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', @@ -83,13 +83,12 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('velocity_controller')) ) - # Spawn velocity controller after joint_state_broadcaster + # Spawner for velocity controller velocity_controller_spawner = Node( package='controller_manager', executable='spawner', arguments=['realtime_body_controller_real', '--controller-manager', '/controller_manager'], output='screen', - condition=IfCondition(LaunchConfiguration('velocity_controller')) ) # Optional apartment map