code-iai · mitsav01 · Feb 14, 2025 · Feb 14, 2025 · Feb 21, 2025 · Feb 21, 2025
diff --git 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
diff --git 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
diff --git a/hsr_meshes b/hsr_meshes
diff --git a/hsr_navigation b/hsr_navigation
diff --git 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
@@ -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
@@ -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
@@ -0,0 +1,5 @@
+<library path="hsr_velocity_controller">
+  <class name="hsr_velocity_controller_ns/HsrVelocityController"
+         type="hsr_velocity_controller_ns::HsrVelocityController"
+         base_class_type="controller_interface::ControllerInterface"/>
+</library>
diff --git a/hsr_velocity_controller/hsr_velocity_controller_plugin.xml b/hsr_velocity_controller/hsr_velocity_controller_plugin.xml
+13 −7		CMakeLists.txt
+0 −52		launch/hsr_amcl_map.launch
+104 −0		launch/hsr_amcl_map.launch.py
+0 −5		launch/map.launch
+1 −1		map/suturo_kitchen/final_projectroom_map.yaml
+8 −3		package.xml
+58 −43		src/global_pose_publisher.py