diff --git a/.gitmodules b/.gitmodules index a42a890..d6d40fd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,7 @@ [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 + branch = ros2-jazzy [submodule "hsr_navigation"] path = hsr_navigation - url = https://github.com/sunava/hsr_navigation.git + url = https://github.com/sunava/hsr_navigation.git \ No newline at end of file diff --git a/README.md b/README.md index 1da18cb..ee2631a 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/code-iai/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 +``` diff --git a/hsr_description b/hsr_description index ec886fa..22986db 160000 --- a/hsr_description +++ b/hsr_description @@ -1 +1 @@ -Subproject commit ec886faaf19e4c81ebef22f54cb1ad6fb65b22b2 +Subproject commit 22986dbb9bd1ca96334a3604405afe36fc6defa5 diff --git a/hsr_meshes b/hsr_meshes deleted file mode 160000 index 2d83199..0000000 --- a/hsr_meshes +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 2d831996c0f9548164a4df5886104793e557897a diff --git a/hsr_navigation b/hsr_navigation index 466f709..2fb7ab4 160000 --- a/hsr_navigation +++ b/hsr_navigation @@ -1 +1 @@ -Subproject commit 466f7095f0c1b4c0c7b6e18cf185060845e60e18 +Subproject commit 2fb7ab40dbbf466322ba429f75c8e8e22ee75cef diff --git a/hsr_velocity_controller/CMakeLists.txt b/hsr_velocity_controller/CMakeLists.txt index f9d7365..afd65a3 100644 --- a/hsr_velocity_controller/CMakeLists.txt +++ b/hsr_velocity_controller/CMakeLists.txt @@ -1,209 +1,37 @@ -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 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 +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_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} ) -## 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 hsr_velocity_controller -# CATKIN_DEPENDS controller_interface hardware_interface pluginlib roscpp -# DEPENDS system_lib +install( + TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +# Install plugin XML +install(FILES hsr_velocity_controller.xml + DESTINATION share/${PROJECT_NAME} ) -## 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 ## -############# +# Export plugin description +pluginlib_export_plugin_description_file(controller_interface hsr_velocity_controller.xml) -## 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() 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/config/my_controller_realtime_test.yaml b/hsr_velocity_controller/config/my_controller_realtime_test.yaml index 5ed2a1c..de89b2b 100644 --- a/hsr_velocity_controller/config/my_controller_realtime_test.yaml +++ b/hsr_velocity_controller/config/my_controller_realtime_test.yaml @@ -1,20 +1,16 @@ -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 +controller_manager: + ros__parameters: + update_rate: 200 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + realtime_body_controller_real: + type: hsr_velocity_controller_ns/HsrVelocityController + + +realtime_body_controller_real: + ros__parameters: joints: - arm_flex_joint - arm_lift_joint @@ -23,10 +19,7 @@ hsrb: - 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] - - - + 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/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/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 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 100755 index 0000000..b8a67df --- /dev/null +++ b/hsr_velocity_controller/launch/switch_to_velocity_controllers.launch.py @@ -0,0 +1,58 @@ +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(): + + 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', + 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', + ) + + # 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([ + #ros2_control_node, + unspawn_controllers, + velocity_controller_spawner + ]) diff --git a/hsr_velocity_controller/package.xml b/hsr_velocity_controller/package.xml index 13e00b1..9216e80 100644 --- a/hsr_velocity_controller/package.xml +++ b/hsr_velocity_controller/package.xml @@ -1,71 +1,20 @@ - + 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 + rclcpp_lifecycle - - - - 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..cff7032 100644 --- a/hsr_velocity_controller/src/hsr_velocity_controller.cpp +++ b/hsr_velocity_controller/src/hsr_velocity_controller.cpp @@ -1,228 +1,309 @@ #include #include +#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -namespace hsr_velocity_controller_ns{ - - class HsrVelocityController : public controller_interface::Controller - { - bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle &n) - { - // List of controlled joints - std::string param_name = "joints"; - if(!n.getParam(param_name, joint_names_)) - { - ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); - return false; - } - n_joints_ = joint_names_.size(); +namespace hsr_velocity_controller_ns +{ - 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; - } - } +class HsrVelocityController : public controller_interface::ControllerInterface +{ +public: + HsrVelocityController() = default; - commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + 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; + } - 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 ; ilimits->lower << " upper limit " << joints_urdf_[i]->limits->upper); - } - - n.getParam("p_gains", p_gains_); - n.getParam("i_gains", i_gains_); - n.getParam("d_gains", d_gains_); - n.getParam("feedforward_gains", ff_gains_); + 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; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override + { + try { + auto node = get_node(); + + // 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("")); + + // Get joint names parameter + joint_names_ = node->get_parameter("joints").as_string_array(); + + n_joints_ = joint_names_.size(); + + 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)); - old_integrator_ = std::vector(n_joints_); - old_error_ = std::vector(n_joints_); - filtered_vel_ = std::vector(n_joints_); + // Create subscriber + sub_command_ = node->create_subscription( + "command", + rclcpp::SystemDefaultsQoS(), + std::bind(&HsrVelocityController::commandCB, this, std::placeholders::_1) + ); - return true; + // 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; + } + + 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; } + } - void starting(const ros::Time& time){} - void stopping(const ros::Time& time){} - - void update(const ros::Time& time, const ros::Duration& period) - { - std::vector & commands = *commands_buffer_.readFromRT(); - std::vector d(n_joints_+1, 0); - - double dt = period.toNSec() / 1e9; - for(unsigned int i = 0; i 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]; - } - } + // 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; + } + + 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]); + } - if(counter % 10 == 0) - { - if(pub_ && pub_->trylock()) - { - d[n_joints_] = period.toSec(); - pub_->msg_.data = filtered_vel_; - pub_->unlockAndPublish(); - } + for (unsigned int i = 0; i < n_joints_; i++) { + double vel_cmd = commands[i]; + + // Get current state + double current_pos = state_interfaces_[i * 2].get_value(); + double current_vel = state_interfaces_[i * 2 + 1].get_value(); + + // 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); + } + + // Filter the velocity + float alpha = 0.95f; + filtered_vel_[i] = (1.0f - alpha) * filtered_vel_[i] + alpha * current_vel; + + 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; + + 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]); + + // 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; } - counter++; + } 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; } - // 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_; - - - private: - ros::Subscriber sub_command_; - std::unique_ptr > pub_; - - void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) - { - 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!"); - return; - } - commands_buffer_.writeFromNonRT(msg->data); + 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; + + // 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); } + } + } + + // Publish controller state periodically + if (counter % 10 == 0 && pub_ && pub_->trylock()) { + pub_->msg_.data = filtered_vel_; + pub_->unlockAndPublish(); + } + counter++; + + 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); + } + + // 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]); + } + } + + 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 - }; - PLUGINLIB_EXPORT_CLASS(hsr_velocity_controller_ns::HsrVelocityController, controller_interface::ControllerBase) -} +#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/iai_hsr_bringup/CMakeLists.txt b/iai_hsr_bringup/CMakeLists.txt index b09bb80..0c4ca6a 100644 --- a/iai_hsr_bringup/CMakeLists.txt +++ b/iai_hsr_bringup/CMakeLists.txt @@ -1,202 +1,15 @@ -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_package(hsr_description REQUIRED) +find_package(hsr_navigation REQUIRED) +#find_package(hsr_meshes 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..ec74281 --- /dev/null +++ b/iai_hsr_bringup/launch/hsr.launch.py @@ -0,0 +1,112 @@ +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(): + + # === 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('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' + ]) + + 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', + name='controller_manager', # important for spawner + parameters=[ + robot_description, + controller_yaml_file, + {'use_sim_time': False} + ], + output='screen', + condition=IfCondition(LaunchConfiguration('velocity_controller')), + ) + + # Spawn joint_state_broadcaster + 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')) + ) + + # Spawner for velocity controller + velocity_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['realtime_body_controller_real', '--controller-manager', '/controller_manager'], + output='screen', + ) + + # Optional apartment map + 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')) + ) + + return LaunchDescription([ + velocity_controller_arg, + apartment_map_arg, + robot_state_publisher_node, + joint_state_gui_node, + ros2_control_node, + joint_state_broadcaster_spawner, + velocity_controller_spawner, + rviz_node, + apartment_map_launch + ]) diff --git a/iai_hsr_bringup/package.xml b/iai_hsr_bringup/package.xml index e486198..32007f4 100644 --- a/iai_hsr_bringup/package.xml +++ b/iai_hsr_bringup/package.xml @@ -1,59 +1,32 @@ - + 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 + hsr_description + hsr_navigation - - - - - - - - - - + + ament_cmake + + rosidl_default_runtime - - - - - - - - - - - - - - - - - - - - - catkin - - - - - + ament_cmake - + \ No newline at end of file