diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt
index beaa2cf22..fe43b4bba 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt
+++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt
@@ -3,8 +3,40 @@ project(ocs2_legged_robot)
 
 # Generate compile_commands.json for clang tools
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-set(CATKIN_PACKAGE_DEPENDENCIES
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(ocs2_core REQUIRED)
+find_package(ocs2_mpc REQUIRED)
+find_package(ocs2_robotic_tools REQUIRED)
+find_package(ocs2_oc REQUIRED)
+find_package(ocs2_sqp REQUIRED)
+find_package(ocs2_thirdparty REQUIRED)
+find_package(ocs2_ddp REQUIRED)
+find_package(ocs2_msgs REQUIRED)
+find_package(ocs2_pinocchio_interface REQUIRED)
+find_package(ocs2_centroidal_model REQUIRED)
+find_package(ocs2_ros_interfaces REQUIRED)
+find_package(ocs2_robotic_assets REQUIRED)
+find_package(OpenMP REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(urdf REQUIRED)
+find_package(kdl_parser REQUIRED)
+find_package(robot_state_publisher REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
+set(AMENT_PACKAGE_DEPENDENCIES
   ocs2_core
   ocs2_oc
   ocs2_ddp
@@ -14,38 +46,41 @@ set(CATKIN_PACKAGE_DEPENDENCIES
   ocs2_pinocchio_interface
   ocs2_centroidal_model
   ocs2_robotic_assets
+  ocs2_thirdparty
+  ocs2_msgs
+  ocs2_ros_interfaces
+  rclcpp
+  kdl_parser
+  tf2
+  urdf
+  robot_state_publisher
+  tf2_ros
+  tf2_geometry_msgs
+  std_srvs
+  # trajectory_msgs
+  visualization_msgs
+  pinocchio
 )
 
-find_package(catkin REQUIRED COMPONENTS
-  ${CATKIN_PACKAGE_DEPENDENCIES}
-)
 
 find_package(Boost REQUIRED COMPONENTS
   system
   filesystem
+  log
+  log_setup
 )
 
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(pinocchio REQUIRED pinocchio)
+if ( EXISTS "/opt/openrobots/lib/libpinocchio.so")
+else()
+    message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html")
+endif()
 
-###################################
-## catkin specific configuration ##
-###################################
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fopenmp -std=c++11 -Wfatal-errors")
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-    ${EIGEN3_INCLUDE_DIRS}
-  LIBRARIES
-    ${PROJECT_NAME}
-  CATKIN_DEPENDS
-    ${CATKIN_PACKAGE_DEPENDENCIES}
-  DEPENDS
-    Boost
-    pinocchio
-)
+list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/")
+find_package(pinocchio REQUIRED)
 
 ###########
 ## Build ##
@@ -54,7 +89,7 @@ catkin_package(
 # Resolve for the package path at compile time.
 configure_file (
   "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in"
-  "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY
+  "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/package_path.h" @ONLY
 )
 
 set(FLAGS
@@ -99,16 +134,30 @@ add_library(${PROJECT_NAME}
   src/LeggedRobotInterface.cpp
   src/LeggedRobotPreComputation.cpp
 )
-add_dependencies(${PROJECT_NAME}
-  ${catkin_EXPORTED_TARGETS}
+
+set_target_properties(${PROJECT_NAME}
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib"
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
 )
+
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
+  Eigen3::Eigen
   ${Boost_LIBRARIES}
   ${pinocchio_LIBRARIES}
   dl
 )
+
+ament_target_dependencies(
+  ocs2_legged_robot
+  ${AMENT_PACKAGE_DEPENDENCIES}
+)
+
 target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS})
+target_compile_definitions(${PROJECT_NAME} PRIVATE BOOST_LOG_DYN_LINK=true)
 
 #########################
 ###   CLANG TOOLING   ###
@@ -129,36 +178,87 @@ endif(cmake_clang_tools_FOUND)
 ## Install ##
 #############
 
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
+# install(TARGETS ${PROJECT_NAME}
+#   EXPORT export_${PROJECT_NAME}
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+  INCLUDES DESTINATION include
 )
+
+
 install(DIRECTORY include/
   DESTINATION include
 )
+
 install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}
 )
 
+ament_export_include_directories(include)
+
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_export_dependencies(ocs2_core)
+ament_export_dependencies(ocs2_mpc)
+ament_export_dependencies(ocs2_robotic_tools)
+ament_export_dependencies(ocs2_oc)
+ament_export_dependencies(ocs2_thirdparty)
+ament_export_dependencies(ocs2_ddp)
+ament_export_dependencies(ocs2_msgs)
+ament_export_dependencies(ocs2_pinocchio_interface)
+ament_export_dependencies(ocs2_centroidal_model)
+ament_export_dependencies(ocs2_ros_interfaces)
+ament_export_dependencies(tf2)
+ament_export_dependencies(urdf)
+ament_export_dependencies(kdl_parser)
+ament_export_dependencies(robot_state_publisher)
+ament_export_dependencies(visualization_msgs)
+ament_export_dependencies(nav_msgs)
+ament_export_dependencies(std_srvs)
+
 #############
 ## Testing ##
 #############
 
-catkin_add_gtest(${PROJECT_NAME}_test
-  test/AnymalFactoryFunctions.cpp
-  test/constraint/testEndEffectorLinearConstraint.cpp
-  test/constraint/testFrictionConeConstraint.cpp
-  test/constraint/testZeroForceConstraint.cpp
-)
-target_include_directories(${PROJECT_NAME}_test PRIVATE
-  test/include
-  ${PROJECT_BINARY_DIR}/include
-)
-target_link_libraries(${PROJECT_NAME}_test
-  gtest_main
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${Boost_LIBRARIES}
-)
-target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS})
+# if(BUILD_TESTING)
+
+# # Include linting tests
+# find_package(ament_lint_auto REQUIRED)
+# find_package(ament_cmake_gtest REQUIRED)
+# ament_lint_auto_find_test_dependencies()
+
+# # test_time_triggered_rollout
+# ament_add_gtest(${PROJECT_NAME}_test
+#   test/AnymalFactoryFunctions.cpp
+#   test/constraint/testEndEffectorLinearConstraint.cpp
+#   test/constraint/testFrictionConeConstraint.cpp
+#   test/constraint/testZeroForceConstraint.cpp
+# )
+
+# target_link_libraries(${PROJECT_NAME}_test
+#   ${PROJECT_NAME}
+#   ${Boost_LIBRARIES}
+#   gtest_main
+# )
+
+# target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS})
+
+# target_include_directories(${PROJECT_NAME}_test PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test/include>
+#   $<INSTALL_INTERFACE:include>
+# )
+# endif(BUILD_TESTING)
+
+ament_package()
+
diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml
index 08ad586ad..73dde07fc 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml
+++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml
@@ -1,5 +1,5 @@
-<?xml version="1.0"?>
-<package format="2">
+<?xml version="2.0"?>
+<package>
   <name>ocs2_legged_robot</name>
   <version>0.0.1</version>
   <description>The ocs2_legged_robot package</description>
@@ -12,15 +12,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>ocs2_core</depend>
-  <depend>ocs2_oc</depend>
-  <depend>ocs2_ddp</depend>
-  <depend>ocs2_mpc</depend>
-  <depend>ocs2_sqp</depend>
-  <depend>ocs2_robotic_assets</depend>
-  <depend>ocs2_robotic_tools</depend>
-  <depend>ocs2_pinocchio_interface</depend>
-  <depend>ocs2_centroidal_model</depend>
-  <depend>pinocchio</depend>
+  <build_depend>ocs2_core</build_depend>
+  <build_depend>ocs2_oc</build_depend>
+  <build_depend>ocs2_ddp</build_depend>
+  <build_depend>ocs2_mpc</build_depend>
+  <build_depend>ocs2_sqp</build_depend>
+  <build_depend>ocs2_msgs</build_depend>
+  <build_depend>ocs2_robotic_assets</build_depend>
+  <build_depend>ocs2_robotic_tools</build_depend>
+  <build_depend>ocs2_pinocchio_interface</build_depend>
+  <build_depend>ocs2_ros_interfaces</build_depend>
+  <build_depend>ocs2_centroidal_model</build_depend>
+  <build_depend>pinocchio</build_depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt
index 5e29c63f4..1445658da 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt
@@ -3,13 +3,51 @@ project(ocs2_legged_robot_ros)
 
 # Generate compile_commands.json for clang tools
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-set(CATKIN_PACKAGE_DEPENDENCIES
-  roslib
-  cmake_modules
-  tf
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(ocs2_core REQUIRED)
+find_package(ocs2_mpc REQUIRED)
+find_package(ocs2_robotic_tools REQUIRED)
+find_package(ocs2_oc REQUIRED)
+find_package(ocs2_thirdparty REQUIRED)
+find_package(ocs2_sqp REQUIRED)
+find_package(ocs2_ddp REQUIRED)
+find_package(ocs2_msgs REQUIRED)
+find_package(ocs2_pinocchio_interface REQUIRED)
+find_package(ocs2_centroidal_model REQUIRED)
+find_package(ocs2_ros_interfaces REQUIRED)
+find_package(OpenMP REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(urdf REQUIRED)
+find_package(kdl_parser REQUIRED)
+find_package(robot_state_publisher REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(ocs2_robotic_assets REQUIRED)
+find_package(ocs2_legged_robot REQUIRED)
+
+
+
+set(AMENT_PACKAGE_DEPENDENCIES
+  # roslib
+  # cmake_modules
+  tf2
+  tf2_ros
   urdf
   kdl_parser
+  rclcpp
   robot_state_publisher
   ocs2_core
   ocs2_oc
@@ -25,36 +63,24 @@ set(CATKIN_PACKAGE_DEPENDENCIES
   ocs2_legged_robot
 )
 
-find_package(catkin REQUIRED COMPONENTS
-  ${CATKIN_PACKAGE_DEPENDENCIES}
-)
-
 find_package(Boost REQUIRED COMPONENTS
   system
   filesystem
+  log
+  log_setup
 )
 
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(pinocchio REQUIRED pinocchio)
+if ( EXISTS "/opt/openrobots/lib/libpinocchio.so")
+else()
+    message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html")
+endif()
 
-###################################
-## catkin specific configuration ##
-###################################
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fopenmp -std=c++11 -Wfatal-errors")
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-    ${EIGEN3_INCLUDE_DIRS}
-  LIBRARIES
-    ${PROJECT_NAME}
-  CATKIN_DEPENDS
-    ${CATKIN_PACKAGE_DEPENDENCIES}
-  DEPENDS
-    Boost
-    pinocchio
-)
+list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/")
+find_package(pinocchio REQUIRED)
 
 ###########
 ## Build ##
@@ -65,7 +91,6 @@ include_directories(
   ${pinocchio_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
 )
 
 link_directories(
@@ -78,25 +103,50 @@ add_library(${PROJECT_NAME}
   src/gait/GaitReceiver.cpp
   src/visualization/LeggedRobotVisualizer.cpp
 )
-add_dependencies(${PROJECT_NAME}
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+
+set_target_properties(${PROJECT_NAME}
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
 )
+
+
+# TODO: NOT NEEDED?
+# add_dependencies(${PROJECT_NAME}
+#   ${${PROJECT_NAME}_EXPORTED_TARGETS}
+#   ${catkin_EXPORTED_TARGETS}
+# )
+
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${AMENT_PACKAGE_DEPENDENCIES}
+)
+
 target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
 
 # DDP-MPC node for legged robot
 add_executable(legged_robot_ddp_mpc
   src/LeggedRobotDdpMpcNode.cpp
 )
+
+set_target_properties(legged_robot_ddp_mpc
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(legged_robot_ddp_mpc PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_dependencies(legged_robot_ddp_mpc
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
 )
+
 target_link_libraries(legged_robot_ddp_mpc
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
@@ -107,26 +157,42 @@ target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_CXX_FLAGS})
 add_executable(legged_robot_sqp_mpc
   src/LeggedRobotSqpMpcNode.cpp
 )
+set_target_properties(legged_robot_sqp_mpc
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(legged_robot_sqp_mpc PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_dependencies(legged_robot_sqp_mpc
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
 )
+
 target_link_libraries(legged_robot_sqp_mpc
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
-target_compile_options(legged_robot_sqp_mpc  PRIVATE ${OCS2_CXX_FLAGS})
+target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS})
 
 # Dummy node
 add_executable(legged_robot_dummy
   src/LeggedRobotDummyNode.cpp
 )
+set_target_properties(legged_robot_dummy
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(legged_robot_dummy PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_dependencies(legged_robot_dummy
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
 )
+
 target_link_libraries(legged_robot_dummy
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
@@ -137,11 +203,19 @@ target_compile_options(legged_robot_dummy PRIVATE ${OCS2_CXX_FLAGS})
 add_executable(legged_robot_target
   src/LeggedRobotPoseCommandNode.cpp
 )
+set_target_properties(legged_robot_target
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(legged_robot_target PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_dependencies(legged_robot_target
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
 )
+
 target_link_libraries(legged_robot_target
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
@@ -152,11 +226,19 @@ target_compile_options(legged_robot_target PRIVATE ${OCS2_CXX_FLAGS})
 add_executable(legged_robot_gait_command
   src/LeggedRobotGaitCommandNode.cpp
 )
+set_target_properties(legged_robot_gait_command
+  PROPERTIES INSTALL_RPATH "/opt/openrobots/lib")
+
+target_include_directories(legged_robot_gait_command PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_dependencies(legged_robot_gait_command
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
 )
+
 target_link_libraries(legged_robot_gait_command
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
@@ -173,10 +255,10 @@ if(cmake_clang_tools_FOUND)
      TARGETS
        ${PROJECT_NAME}
        legged_robot_ddp_mpc
-       legged_robot_sqp_mpc
-       legged_robot_dummy
-       legged_robot_target
-       legged_robot_gait_command
+      #  legged_robot_sqp_mpc
+      #  legged_robot_dummy
+      #  legged_robot_target
+      #  legged_robot_gait_command
      SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include
      CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
      CF_WERROR
@@ -189,11 +271,13 @@ endif(cmake_clang_tools_FOUND)
 install(DIRECTORY include/
   DESTINATION include
 )
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
+
 install(
   TARGETS
     legged_robot_ddp_mpc
@@ -201,12 +285,34 @@ install(
     legged_robot_dummy
     legged_robot_target
     legged_robot_gait_command
-  RUNTIME DESTINATION bin
-)
-install(DIRECTORY launch rviz
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+  INCLUDES DESTINATION include
 )
 
+install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+
+ament_export_dependencies(ocs2_core)
+ament_export_dependencies(ocs2_mpc)
+ament_export_dependencies(ocs2_robotic_tools)
+ament_export_dependencies(ocs2_oc)
+ament_export_dependencies(ocs2_thirdparty)
+ament_export_dependencies(ocs2_ddp)
+ament_export_dependencies(ocs2_msgs)
+ament_export_dependencies(ocs2_sqp)
+ament_export_dependencies(ocs2_robotic_assets)
+ament_export_dependencies(ocs2_pinocchio_interface)
+ament_export_dependencies(ocs2_centroidal_model)
+ament_export_dependencies(ocs2_ros_interfaces)
+ament_export_dependencies(tf2)
+ament_export_dependencies(urdf)
+ament_export_dependencies(kdl_parser)
+ament_export_dependencies(robot_state_publisher)
+ament_export_dependencies(visualization_msgs)
+ament_export_dependencies(nav_msgs)
+ament_export_dependencies(std_srvs)
+ament_package()
 #############
 ## Testing ##
 #############
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h
index bc399bdb5..56ea817dc 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h
@@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 #include <vector>
 
-#include <ros/ros.h>
-#include <std_msgs/Bool.h>
+#include <rclcpp/rclcpp.hpp>
+#include <ocs2_msgs/msg/mode_schedule.hpp>
 
 #include <ocs2_legged_robot/gait/ModeSequenceTemplate.h>
 
@@ -43,7 +43,7 @@ namespace legged_robot {
 /** This class implements ModeSequence communication using ROS. */
 class GaitKeyboardPublisher {
  public:
-  GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false);
+  GaitKeyboardPublisher(rclcpp::Node::SharedPtr &nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false);
 
   /** Prints the command line interface and responds to user input. Function returns after one user input. */
   void getKeyboardCommand();
@@ -55,7 +55,7 @@ class GaitKeyboardPublisher {
   std::vector<std::string> gaitList_;
   std::map<std::string, ModeSequenceTemplate> gaitMap_;
 
-  ros::Publisher modeSequenceTemplatePublisher_;
+  rclcpp::Publisher<ocs2_msgs::msg::ModeSchedule>::SharedPtr modeSequenceTemplatePublisher_;
 };
 
 }  // namespace legged_robot
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h
index e6327327d..9e37e1abf 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h
@@ -31,10 +31,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <mutex>
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <ocs2_core/Types.h>
-#include <ocs2_msgs/mode_schedule.h>
+#include <ocs2_msgs/msg/mode_schedule.hpp>
+
 #include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
 
 #include <ocs2_legged_robot/gait/GaitSchedule.h>
@@ -45,7 +46,7 @@ namespace ocs2 {
 namespace legged_robot {
 class GaitReceiver : public SolverSynchronizedModule {
  public:
-  GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName);
+  GaitReceiver(rclcpp::Node::SharedPtr &nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName);
 
   void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState,
                     const ReferenceManagerInterface& referenceManager) override;
@@ -53,11 +54,11 @@ class GaitReceiver : public SolverSynchronizedModule {
   void postSolverRun(const PrimalSolution& primalSolution) override{};
 
  private:
-  void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg);
+  void mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::SharedPtr msg);
 
   std::shared_ptr<GaitSchedule> gaitSchedulePtr_;
 
-  ros::Subscriber mpcModeSequenceSubscriber_;
+  rclcpp::Subscription<ocs2_msgs::msg::ModeSchedule>::SharedPtr mpcModeSequenceSubscriber_;
 
   std::mutex receivedGaitMutex_;
   std::atomic_bool gaitUpdated_;
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h
index e4bb1e330..c0cac8f8c 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h
@@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <vector>
 
-#include <ocs2_msgs/mode_schedule.h>
+#include <ocs2_msgs/msg/mode_schedule.hpp>
 
 #include <ocs2_legged_robot/gait/ModeSequenceTemplate.h>
 
@@ -39,17 +39,17 @@ namespace ocs2 {
 namespace legged_robot {
 
 /** Convert mode sequence template to ROS message */
-inline ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) {
-  ocs2_msgs::mode_schedule modeScheduleMsg;
-  modeScheduleMsg.eventTimes.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end());
-  modeScheduleMsg.modeSequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end());
+inline ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) {
+  ocs2_msgs::msg::ModeSchedule modeScheduleMsg;
+  modeScheduleMsg.event_times.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end());
+  modeScheduleMsg.mode_sequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end());
   return modeScheduleMsg;
 }
 
 /** Convert ROS message to mode sequence template */
-inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) {
-  std::vector<scalar_t> switchingTimes(modeScheduleMsg.eventTimes.begin(), modeScheduleMsg.eventTimes.end());
-  std::vector<size_t> modeSequence(modeScheduleMsg.modeSequence.begin(), modeScheduleMsg.modeSequence.end());
+inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) {
+  std::vector<scalar_t> switchingTimes(modeScheduleMsg.event_times.begin(), modeScheduleMsg.event_times.end());
+  std::vector<size_t> modeSequence(modeScheduleMsg.mode_sequence.begin(), modeScheduleMsg.mode_sequence.end());
   return {switchingTimes, modeSequence};
 }
 
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h
index a3c1b9dc6..534394bfb 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h
@@ -29,9 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include <robot_state_publisher/robot_state_publisher.h>
-#include <ros/node_handle.h>
-#include <tf/transform_broadcaster.h>
+#include <robot_state_publisher/robot_state_publisher.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <ocs2_centroidal_model/CentroidalModelInfo.h>
 #include <ocs2_core/Types.h>
@@ -43,7 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace ocs2 {
 namespace legged_robot {
 
-class LeggedRobotVisualizer : public DummyObserver {
+class LeggedRobotVisualizer : public DummyObserver, public robot_state_publisher::RobotStatePublisher {
  public:
   /** Visualization settings (publicly available) */
   std::string frameId_ = "odom";              // Frame name all messages are published in
@@ -63,44 +64,44 @@ class LeggedRobotVisualizer : public DummyObserver {
    * @param maxUpdateFrequency : maximum publish frequency measured in MPC time.
    */
   LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo,
-                        const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle,
-                        scalar_t maxUpdateFrequency = 100.0);
+                        const PinocchioEndEffectorKinematics& endEffectorKinematics, rclcpp::Node::SharedPtr& nodeHandle,
+                        const std::string &urdf_xml, scalar_t maxUpdateFrequency = 100.0);
 
   ~LeggedRobotVisualizer() override = default;
 
   void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override;
 
-  void launchVisualizerNode(ros::NodeHandle& nodeHandle);
+  void launchVisualizerNode(rclcpp::Node::SharedPtr& nodeHandle, const std::string &urdf_xml);
 
   void publishTrajectory(const std::vector<SystemObservation>& system_observation_array, scalar_t speed = 1.0);
 
-  void publishObservation(ros::Time timeStamp, const SystemObservation& observation);
+  void publishObservation(rclcpp::Time timeStamp, const SystemObservation& observation);
 
-  void publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories);
+  void publishDesiredTrajectory(rclcpp::Time timeStamp, const TargetTrajectories& targetTrajectories);
 
-  void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory,
+  void publishOptimizedStateTrajectory(rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory,
                                        const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule);
 
  private:
   LeggedRobotVisualizer(const LeggedRobotVisualizer&) = delete;
-  void publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const;
-  void publishBaseTransform(ros::Time timeStamp, const vector_t& basePose);
-  void publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, const std::vector<vector3_t>& feetPositions,
+  void publishJointTransforms(rclcpp::Time timeStamp, const vector_t& jointAngles);
+  void publishBaseTransform(rclcpp::Time timeStamp, const vector_t& basePose);
+  void publishCartesianMarkers(rclcpp::Time timeStamp, const contact_flag_t& contactFlags, const std::vector<vector3_t>& feetPositions,
                                const std::vector<vector3_t>& feetForces) const;
 
   PinocchioInterface pinocchioInterface_;
   const CentroidalModelInfo centroidalModelInfo_;
   std::unique_ptr<PinocchioEndEffectorKinematics> endEffectorKinematicsPtr_;
 
-  tf::TransformBroadcaster tfBroadcaster_;
-  std::unique_ptr<robot_state_publisher::RobotStatePublisher> robotStatePublisherPtr_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+  rclcpp::Node::SharedPtr nodeHandle_;
 
-  ros::Publisher costDesiredBasePositionPublisher_;
-  std::vector<ros::Publisher> costDesiredFeetPositionPublishers_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr costDesiredBasePositionPublisher_;
+  std::vector<rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr> costDesiredFeetPositionPublishers_;
 
-  ros::Publisher stateOptimizedPublisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr stateOptimizedPublisher_;
 
-  ros::Publisher currentStatePublisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr currentStatePublisher_;
 
   scalar_t lastTime_;
   scalar_t minPublishTimeDifference_;
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py
new file mode 100644
index 000000000..4deb64fad
--- /dev/null
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py
@@ -0,0 +1,102 @@
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch import LaunchDescription, LaunchService
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets')
+    ocs2_legged_robot_dir = get_package_share_directory('ocs2_legged_robot')
+    description_name = "legged_robot_description"
+
+    task_file = os.path.join(ocs2_legged_robot_dir, 'config', 'mpc', 'task.info')
+    reference_file = os.path.join(ocs2_legged_robot_dir, 'config', 'command', 'reference.info')
+    urdf_file = os.path.join(ocs2_robotic_assets_dir, 'resources', 'anymal_c', 'urdf', 'anymal.urdf')
+    gait_command_file = os.path.join(ocs2_legged_robot_dir, 'config', 'command', 'gait.info')
+    robot_name = "legged_robot"
+    config_name = "mpc"
+    use_sim_time = False
+
+    urdf_content = ""
+    with open(urdf_file, 'r') as fp:
+        urdf_content = fp.read()
+
+    robot_description = {"robot_description": urdf_content}
+
+    # only for rviz
+    description_name = "legged_robot_description"
+
+    return LaunchDescription([
+
+        Node(
+            name='legged_robot_ddp_mpc',
+            package='ocs2_legged_robot_ros',
+            executable='legged_robot_ddp_mpc',
+            parameters=[
+                {"use_sim_time": use_sim_time},
+                {"urdf_file": urdf_file},
+                {"task_file": task_file},
+                {"reference_file": reference_file},
+                ],
+            output='screen',
+        ),
+        Node(
+            name='legged_robot_dummy',
+            package='ocs2_legged_robot_ros',
+            executable='legged_robot_dummy',
+            parameters=[
+                robot_description,
+                {"use_sim_time": use_sim_time},
+                {"urdf_file": urdf_file},
+                {"task_file": task_file},
+                {"reference_file": reference_file},
+                ],
+            output='screen',
+            prefix=["gnome-terminal --"],
+        ),
+        Node(
+            name='legged_robot_target',
+            package='ocs2_legged_robot_ros',
+            executable='legged_robot_target',
+            parameters=[
+                {"use_sim_time": use_sim_time},
+                {"reference_file": reference_file},
+                ],
+            output='screen',
+            prefix=["gnome-terminal --"],
+        ),
+        Node(
+            name='legged_robot_gait_command',
+            package='ocs2_legged_robot_ros',
+            executable='legged_robot_gait_command',
+            parameters=[
+                {"use_sim_time": use_sim_time},
+                {"gait_command_file": gait_command_file},
+                ],
+            output='screen',
+            prefix=["gnome-terminal --"],
+        ),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    """Run lifecycle nodes via launch."""
+    ld = generate_launch_description()
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return ls.run()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml
index 9866cb252..46c5f999a 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml
@@ -1,5 +1,5 @@
-<?xml version="1.0"?>
-<package format="2">
+<?xml version="2.0"?>
+<package>
   <name>ocs2_legged_robot_ros</name>
   <version>0.0.1</version>
   <description>The ocs2_legged_robot_ros package</description>
@@ -12,26 +12,30 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>roslib</depend>
-  <depend>tf</depend>
-  <depend>urdf</depend>
-  <depend>kdl_parser</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>cmake_modules</depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>urdf</build_depend>
+  <build_depend>kdl_parser</build_depend>
+  <build_depend>robot_state_publisher</build_depend>
+  <build_depend>cmake_modules</build_depend>
 
-  <depend>ocs2_core</depend>
-  <depend>ocs2_oc</depend>
-  <depend>ocs2_ddp</depend>
-  <depend>ocs2_mpc</depend>
-  <depend>ocs2_sqp</depend>
-  <depend>ocs2_robotic_tools</depend>
-  <depend>ocs2_pinocchio_interface</depend>
-  <depend>ocs2_centroidal_model</depend>
-  <depend>ocs2_robotic_assets</depend>
-  <depend>ocs2_msgs</depend>
-  <depend>ocs2_legged_robot</depend>
-  <depend>ocs2_ros_interfaces</depend>
+  <build_depend>ocs2_core</build_depend>
+  <build_depend>ocs2_oc</build_depend>
+  <build_depend>ocs2_ddp</build_depend>
+  <build_depend>ocs2_mpc</build_depend>
+  <build_depend>ocs2_sqp</build_depend>
+  <build_depend>ocs2_robotic_tools</build_depend>
+  <build_depend>ocs2_pinocchio_interface</build_depend>
+  <build_depend>ocs2_centroidal_model</build_depend>
+  <build_depend>ocs2_robotic_assets</build_depend>
+  <run_depend>ocs2_robotic_assets</run_depend>
+  <build_depend>ocs2_msgs</build_depend>
+  <build_depend>ocs2_legged_robot</build_depend>
+  <build_depend>ocs2_ros_interfaces</build_depend>
 
-  <depend>pinocchio</depend>
+  <build_depend>pinocchio</build_depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp
index b37272d69..294867655 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp
@@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  ******************************************************************************/
 
-#include <ros/init.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <ocs2_ddp/GaussNewtonDDP_MPC.h>
 #include <ocs2_legged_robot/LeggedRobotInterface.h>
@@ -40,17 +39,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 using namespace ocs2;
 using namespace legged_robot;
 
+static auto LOGGER = rclcpp::get_logger("LeggedRobotDDPMpcNode");
+
+auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string &param, std::string &param_value) {
+  if (!node->has_parameter(param))
+  {
+    node->declare_parameter(param, std::string(""));
+  }
+  rclcpp::Parameter parameter;
+  node->get_parameter(param, parameter);
+  param_value = parameter.as_string();
+  RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value);
+};
+
 int main(int argc, char** argv) {
   const std::string robotName = "legged_robot";
 
   // Initialize ros node
-  ros::init(argc, argv, robotName + "_mpc");
-  ros::NodeHandle nodeHandle;
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr nodeHandle = std::make_shared<rclcpp::Node>(robotName + "_mpc");
   // Get node parameters
   std::string taskFile, urdfFile, referenceFile;
-  nodeHandle.getParam("/taskFile", taskFile);
-  nodeHandle.getParam("/referenceFile", referenceFile);
-  nodeHandle.getParam("/urdfFile", urdfFile);
+
+  declareAndGetStringParam(nodeHandle, "task_file", taskFile);
+  declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile);
+  declareAndGetStringParam(nodeHandle, "reference_file", referenceFile);
 
   // Robot interface
   LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp
index 4fc71d2de..f7da1f5eb 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp
@@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
-#include <ros/init.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
 #include <ocs2_legged_robot/LeggedRobotInterface.h>
@@ -40,18 +39,41 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace ocs2;
 using namespace legged_robot;
+static auto LOGGER = rclcpp::get_logger("LeggedRobotDummyNode");
+
+auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string &param, std::string &param_value) {
+  if (!node->has_parameter(param))
+  {
+    node->declare_parameter(param, std::string(""));
+  }
+  rclcpp::Parameter parameter;
+  node->get_parameter(param, parameter);
+  param_value = parameter.as_string();
+  RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value);
+};
+
+auto readFile = [] (const std::string &path, std::string &file_content) {
+  std::ifstream stream( path.c_str() );
+  if (!stream)
+  {
+    RCLCPP_ERROR_STREAM(LOGGER, "File " << path << " does not exist");
+    return;
+  }
+  file_content = std::string((std::istreambuf_iterator<char>(stream)),
+                      std::istreambuf_iterator<char>());
+};
 
 int main(int argc, char** argv) {
   const std::string robotName = "legged_robot";
 
   // Initialize ros node
-  ros::init(argc, argv, robotName + "_mrt");
-  ros::NodeHandle nodeHandle;
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr nodeHandle = std::make_shared<rclcpp::Node>(robotName + "_mrt");
   // Get node parameters
   std::string taskFile, urdfFile, referenceFile;
-  nodeHandle.getParam("/taskFile", taskFile);
-  nodeHandle.getParam("/urdfFile", urdfFile);
-  nodeHandle.getParam("/referenceFile", referenceFile);
+  declareAndGetStringParam(nodeHandle, "task_file", taskFile);
+  declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile);
+  declareAndGetStringParam(nodeHandle, "reference_file", referenceFile);
 
   // Robot interface
   LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);
@@ -61,12 +83,16 @@ int main(int argc, char** argv) {
   mrt.initRollout(&interface.getRollout());
   mrt.launchNodes(nodeHandle);
 
+  std::string urdf_string;
+  readFile(urdfFile, urdf_string);
+
   // Visualization
   CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo());
   PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping,
                                                        interface.modelSettings().contactNames3DoF);
   std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizer(
-      new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle));
+      new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), 
+                                endEffectorKinematics, nodeHandle, urdf_string));
 
   // Dummy legged robot
   MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_,
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp
index dd3892302..7381cb10c 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp
@@ -27,23 +27,35 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
-#include <ros/init.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h"
 
 using namespace ocs2;
 using namespace legged_robot;
+static auto LOGGER = rclcpp::get_logger("LeggedRobotGaitCommandNode");
+
+auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string &param, std::string &param_value) {
+  if (!node->has_parameter(param))
+  {
+    node->declare_parameter(param, std::string(""));
+  }
+  rclcpp::Parameter parameter;
+  node->get_parameter(param, parameter);
+  param_value = parameter.as_string();
+  RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value);
+};
 
 int main(int argc, char* argv[]) {
   const std::string robotName = "legged_robot";
 
   // Initialize ros node
-  ros::init(argc, argv, robotName + "_mpc_mode_schedule");
-  ros::NodeHandle nodeHandle;
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_mpc_mode_schedule");
+
   // Get node parameters
   std::string gaitCommandFile;
-  nodeHandle.getParam("/gaitCommandFile", gaitCommandFile);
+  declareAndGetStringParam(nodeHandle, "gait_command_file", gaitCommandFile);
   std::cerr << "Loading gait file: " << gaitCommandFile << std::endl;
 
   GaitKeyboardPublisher gaitCommand(nodeHandle, gaitCommandFile, robotName, true);
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp
index 1e827ef3e..b4137e3db 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp
@@ -29,8 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <string>
 
-#include <ros/init.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <ocs2_core/Types.h>
 #include <ocs2_core/misc/LoadData.h>
@@ -94,15 +93,28 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar
   return {timeTrajectory, stateTrajectory, inputTrajectory};
 }
 
+static auto LOGGER = rclcpp::get_logger("LeggedRobotPoseCommandNode");
+
+auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string &param, std::string &param_value) {
+  if (!node->has_parameter(param))
+  {
+    node->declare_parameter(param, std::string(""));
+  }
+  rclcpp::Parameter parameter;
+  node->get_parameter(param, parameter);
+  param_value = parameter.as_string();
+  RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value);
+};
+
 int main(int argc, char* argv[]) {
   const std::string robotName = "legged_robot";
 
   // Initialize ros node
-  ::ros::init(argc, argv, robotName + "_target");
-  ::ros::NodeHandle nodeHandle;
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_target");
   // Get node parameters
   std::string referenceFile;
-  nodeHandle.getParam("/referenceFile", referenceFile);
+  declareAndGetStringParam(nodeHandle, "reference_file", referenceFile);
 
   loadData::loadCppDataType(referenceFile, "comHeight", comHeight);
   loadData::loadEigenMatrix(referenceFile, "defaultJointState", defaultJointState);
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp
index 13953b284..26d17a606 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp
@@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  ******************************************************************************/
 
-#include <ros/init.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <ocs2_legged_robot/LeggedRobotInterface.h>
 #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>
@@ -40,17 +39,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 using namespace ocs2;
 using namespace legged_robot;
 
+static auto LOGGER = rclcpp::get_logger("LeggedRobotSQPMPCNode");
+
+auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string &param, std::string &param_value) {
+  if (!node->has_parameter(param))
+  {
+    node->declare_parameter(param, std::string(""));
+  }
+  rclcpp::Parameter parameter;
+  node->get_parameter(param, parameter);
+  param_value = parameter.as_string();
+  RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value);
+};
+
 int main(int argc, char** argv) {
   const std::string robotName = "legged_robot";
 
   // Initialize ros node
-  ros::init(argc, argv, robotName + "_mpc");
-  ros::NodeHandle nodeHandle;
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_mpc");
   // Get node parameters
   std::string taskFile, urdfFile, referenceFile;
-  nodeHandle.getParam("/taskFile", taskFile);
-  nodeHandle.getParam("/urdfFile", urdfFile);
-  nodeHandle.getParam("/referenceFile", referenceFile);
+  declareAndGetStringParam(nodeHandle, "task_file", taskFile);
+  declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile);
+  declareAndGetStringParam(nodeHandle, "reference_file", referenceFile);
 
   // Robot interface
   LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp
index f381c062f..c9784c1d1 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp
@@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ocs2_core/misc/CommandLine.h>
 #include <ocs2_core/misc/LoadData.h>
-#include <ocs2_msgs/mode_schedule.h>
+#include <ocs2_msgs/msg/mode_schedule.hpp>
 
 #include "ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h"
 
@@ -43,18 +43,19 @@ namespace legged_robot {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-GaitKeyboardPublisher::GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName,
+GaitKeyboardPublisher::GaitKeyboardPublisher(rclcpp::Node::SharedPtr &nodeHandle, const std::string& gaitFile, const std::string& robotName,
                                              bool verbose) {
-  ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ...");
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("ocs2_legged_robot_ros"), robotName + "_mpc_mode_schedule node is setting up ...");
   loadData::loadStdVector(gaitFile, "list", gaitList_, verbose);
 
-  modeSequenceTemplatePublisher_ = nodeHandle.advertise<ocs2_msgs::mode_schedule>(robotName + "_mpc_mode_schedule", 1, true);
+
+  modeSequenceTemplatePublisher_ = nodeHandle->create_publisher<ocs2_msgs::msg::ModeSchedule>(robotName + "_mpc_mode_schedule", 1);
 
   gaitMap_.clear();
   for (const auto& gaitName : gaitList_) {
     gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)});
   }
-  ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready.");
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("ocs2_legged_robot_ros"), robotName + "_mpc_mode_schedule command node is ready.");
 }
 
 /******************************************************************************************************/
@@ -64,7 +65,7 @@ void GaitKeyboardPublisher::getKeyboardCommand() {
   const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\"";
   std::cout << commadMsg << ": ";
 
-  auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); };
+  auto shouldTerminate = []() { return !rclcpp::ok(); };
   const auto commandLine = stringToWords(getCommandLineString(shouldTerminate));
 
   if (commandLine.empty()) {
@@ -87,7 +88,7 @@ void GaitKeyboardPublisher::getKeyboardCommand() {
 
   try {
     ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand);
-    modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSequenceTemplate));
+    modeSequenceTemplatePublisher_->publish(createModeSequenceTemplateMsg(modeSequenceTemplate));
   } catch (const std::out_of_range& e) {
     std::cout << "Gait \"" << gaitCommand << "\" not found.\n";
     printGaitList(gaitList_);
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp
index 82c452459..891943522 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp
@@ -37,10 +37,10 @@ namespace legged_robot {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName)
+GaitReceiver::GaitReceiver(rclcpp::Node::SharedPtr &nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName)
     : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) {
-  mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this,
-                                                    ::ros::TransportHints().udp());
+  mpcModeSequenceSubscriber_ = nodeHandle->create_subscription<ocs2_msgs::msg::ModeSchedule>(robotName + "_mpc_mode_schedule", 
+      1, std::bind(&GaitReceiver::mpcModeSequenceCallback, this, std::placeholders::_1));
 }
 
 /******************************************************************************************************/
@@ -61,7 +61,7 @@ void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vec
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) {
+void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::SharedPtr msg) {
   std::lock_guard<std::mutex> lock(receivedGaitMutex_);
   receivedGait_ = readModeSequenceTemplateMsg(*msg);
   gaitUpdated_ = true;
diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp
index 60c6d0c05..67447c31c 100644
--- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp
+++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp
@@ -44,8 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ocs2_legged_robot/gait/MotionPhaseDefinition.h"
 
 // Additional messages not in the helpers file
-#include <geometry_msgs/PoseArray.h>
-#include <visualization_msgs/MarkerArray.h>
+// #include <visualization_msgs/MarkerArray.h>
 
 // URDF related
 #include <urdf/model.h>
@@ -58,41 +57,37 @@ namespace legged_robot {
 /******************************************************************************************************/
 /******************************************************************************************************/
 LeggedRobotVisualizer::LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo,
-                                             const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle,
-                                             scalar_t maxUpdateFrequency)
+                                             const PinocchioEndEffectorKinematics& endEffectorKinematics, rclcpp::Node::SharedPtr& nodeHandle,
+                                             const std::string &urdf_xml, scalar_t maxUpdateFrequency)
     : pinocchioInterface_(std::move(pinocchioInterface)),
       centroidalModelInfo_(std::move(centroidalModelInfo)),
       endEffectorKinematicsPtr_(endEffectorKinematics.clone()),
       lastTime_(std::numeric_limits<scalar_t>::lowest()),
-      minPublishTimeDifference_(1.0 / maxUpdateFrequency) {
+      minPublishTimeDifference_(1.0 / maxUpdateFrequency),
+      robot_state_publisher::RobotStatePublisher(nodeHandle->get_node_options()),
+      nodeHandle_(nodeHandle) {
   endEffectorKinematicsPtr_->setPinocchioInterface(pinocchioInterface_);
-  launchVisualizerNode(nodeHandle);
+  launchVisualizerNode(nodeHandle, urdf_xml);
 };
 
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle) {
-  costDesiredBasePositionPublisher_ = nodeHandle.advertise<visualization_msgs::Marker>("/legged_robot/desiredBaseTrajectory", 1);
+void LeggedRobotVisualizer::launchVisualizerNode(rclcpp::Node::SharedPtr& nodeHandle, const std::string &urdf_xml) {
+  tfBroadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(nodeHandle);
+
+  costDesiredBasePositionPublisher_ = nodeHandle->create_publisher<visualization_msgs::msg::Marker>("/legged_robot/desiredBaseTrajectory", 1);
   costDesiredFeetPositionPublishers_.resize(centroidalModelInfo_.numThreeDofContacts);
-  costDesiredFeetPositionPublishers_[0] = nodeHandle.advertise<visualization_msgs::Marker>("/legged_robot/desiredFeetTrajectory/LF", 1);
-  costDesiredFeetPositionPublishers_[1] = nodeHandle.advertise<visualization_msgs::Marker>("/legged_robot/desiredFeetTrajectory/RF", 1);
-  costDesiredFeetPositionPublishers_[2] = nodeHandle.advertise<visualization_msgs::Marker>("/legged_robot/desiredFeetTrajectory/LH", 1);
-  costDesiredFeetPositionPublishers_[3] = nodeHandle.advertise<visualization_msgs::Marker>("/legged_robot/desiredFeetTrajectory/RH", 1);
-  stateOptimizedPublisher_ = nodeHandle.advertise<visualization_msgs::MarkerArray>("/legged_robot/optimizedStateTrajectory", 1);
-  currentStatePublisher_ = nodeHandle.advertise<visualization_msgs::MarkerArray>("/legged_robot/currentState", 1);
+  costDesiredFeetPositionPublishers_[0] = nodeHandle->create_publisher<visualization_msgs::msg::Marker>("/legged_robot/desiredFeetTrajectory/LF", 1);
+  costDesiredFeetPositionPublishers_[1] = nodeHandle->create_publisher<visualization_msgs::msg::Marker>("/legged_robot/desiredFeetTrajectory/RF", 1);
+  costDesiredFeetPositionPublishers_[2] = nodeHandle->create_publisher<visualization_msgs::msg::Marker>("/legged_robot/desiredFeetTrajectory/LH", 1);
+  costDesiredFeetPositionPublishers_[3] = nodeHandle->create_publisher<visualization_msgs::msg::Marker>("/legged_robot/desiredFeetTrajectory/RH", 1);
+  stateOptimizedPublisher_ = nodeHandle->create_publisher<visualization_msgs::msg::MarkerArray>("/legged_robot/optimizedStateTrajectory", 1);
+  currentStatePublisher_ = nodeHandle->create_publisher<visualization_msgs::msg::MarkerArray>("/legged_robot/currentState", 1);
 
   // Load URDF model
-  urdf::Model urdfModel;
-  if (!urdfModel.initParam("legged_robot_description")) {
-    std::cerr << "[LeggedRobotVisualizer] Could not read URDF from: \"legged_robot_description\"" << std::endl;
-  } else {
-    KDL::Tree kdlTree;
-    kdl_parser::treeFromUrdfModel(urdfModel, kdlTree);
-
-    robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(kdlTree));
-    robotStatePublisherPtr_->publishFixedTransforms(true);
-  }
+  this->setupURDF(urdf_xml);
+  this->publishFixedTransforms();
 }
 
 /******************************************************************************************************/
@@ -105,7 +100,7 @@ void LeggedRobotVisualizer::update(const SystemObservation& observation, const P
     pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(observation.state, centroidalModelInfo_));
     pinocchio::updateFramePlacements(model, data);
 
-    const auto timeStamp = ros::Time::now();
+    const auto timeStamp = nodeHandle_->now();
     publishObservation(timeStamp, observation);
     publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_);
     publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_,
@@ -117,7 +112,7 @@ void LeggedRobotVisualizer::update(const SystemObservation& observation, const P
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishObservation(ros::Time timeStamp, const SystemObservation& observation) {
+void LeggedRobotVisualizer::publishObservation(rclcpp::Time timeStamp, const SystemObservation& observation) {
   // Extract components from state
   const auto basePose = centroidal_model::getBasePose(observation.state, centroidalModelInfo_);
   const auto qJoints = centroidal_model::getJointAngles(observation.state, centroidalModelInfo_);
@@ -138,30 +133,26 @@ void LeggedRobotVisualizer::publishObservation(ros::Time timeStamp, const System
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const {
-  if (robotStatePublisherPtr_ != nullptr) {
-    std::map<std::string, scalar_t> jointPositions{{"LF_HAA", jointAngles[0]}, {"LF_HFE", jointAngles[1]},  {"LF_KFE", jointAngles[2]},
-                                                   {"LH_HAA", jointAngles[3]}, {"LH_HFE", jointAngles[4]},  {"LH_KFE", jointAngles[5]},
-                                                   {"RF_HAA", jointAngles[6]}, {"RF_HFE", jointAngles[7]},  {"RF_KFE", jointAngles[8]},
-                                                   {"RH_HAA", jointAngles[9]}, {"RH_HFE", jointAngles[10]}, {"RH_KFE", jointAngles[11]}};
-    robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp);
-  }
+void LeggedRobotVisualizer::publishJointTransforms(rclcpp::Time timeStamp, const vector_t& jointAngles) {
+  std::map<std::string, scalar_t> jointPositions{{"LF_HAA", jointAngles[0]}, {"LF_HFE", jointAngles[1]},  {"LF_KFE", jointAngles[2]},
+                                                  {"LH_HAA", jointAngles[3]}, {"LH_HFE", jointAngles[4]},  {"LH_KFE", jointAngles[5]},
+                                                  {"RF_HAA", jointAngles[6]}, {"RF_HFE", jointAngles[7]},  {"RF_KFE", jointAngles[8]},
+                                                  {"RH_HAA", jointAngles[9]}, {"RH_HFE", jointAngles[10]}, {"RH_KFE", jointAngles[11]}};
+  this->publishTransforms(jointPositions, timeStamp);
 }
 
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vector_t& basePose) {
-  if (robotStatePublisherPtr_ != nullptr) {
-    geometry_msgs::TransformStamped baseToWorldTransform;
-    baseToWorldTransform.header = getHeaderMsg(frameId_, timeStamp);
-    baseToWorldTransform.child_frame_id = "base";
-
-    const Eigen::Quaternion<scalar_t> q_world_base = getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>()));
-    baseToWorldTransform.transform.rotation = getOrientationMsg(q_world_base);
-    baseToWorldTransform.transform.translation = getVectorMsg(basePose.head<3>());
-    tfBroadcaster_.sendTransform(baseToWorldTransform);
-  }
+void LeggedRobotVisualizer::publishBaseTransform(rclcpp::Time timeStamp, const vector_t& basePose) {
+  geometry_msgs::msg::TransformStamped baseToWorldTransform;
+  baseToWorldTransform.header = getHeaderMsg(frameId_, timeStamp);
+  baseToWorldTransform.child_frame_id = "base";
+
+  const Eigen::Quaternion<scalar_t> q_world_base = getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>()));
+  baseToWorldTransform.transform.rotation = getOrientationMsg(q_world_base);
+  baseToWorldTransform.transform.translation = getVectorMsg(basePose.head<3>());
+  tfBroadcaster_->sendTransform(baseToWorldTransform);
 }
 
 /******************************************************************************************************/
@@ -170,9 +161,10 @@ void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vect
 void LeggedRobotVisualizer::publishTrajectory(const std::vector<SystemObservation>& system_observation_array, scalar_t speed) {
   for (size_t k = 0; k < system_observation_array.size() - 1; k++) {
     scalar_t frameDuration = speed * (system_observation_array[k + 1].time - system_observation_array[k].time);
-    scalar_t publishDuration = timedExecutionInSeconds([&]() { publishObservation(ros::Time::now(), system_observation_array[k]); });
+    scalar_t publishDuration = timedExecutionInSeconds([&]() { publishObservation(nodeHandle_->now(), system_observation_array[k]); });
     if (frameDuration > publishDuration) {
-      ros::WallDuration(frameDuration - publishDuration).sleep();
+      rclcpp::Duration dur(frameDuration - publishDuration);
+      rclcpp::sleep_for(std::chrono::nanoseconds(dur.nanoseconds()));
     }
   }
 }
@@ -180,12 +172,12 @@ void LeggedRobotVisualizer::publishTrajectory(const std::vector<SystemObservatio
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags,
+void LeggedRobotVisualizer::publishCartesianMarkers(rclcpp::Time timeStamp, const contact_flag_t& contactFlags,
                                                     const std::vector<vector3_t>& feetPositions,
                                                     const std::vector<vector3_t>& feetForces) const {
   // Reserve message
   const size_t numberOfCartesianMarkers = 10;
-  visualization_msgs::MarkerArray markerArray;
+  visualization_msgs::msg::MarkerArray markerArray;
   markerArray.markers.reserve(numberOfCartesianMarkers);
 
   // Feet positions and Forces
@@ -208,22 +200,22 @@ void LeggedRobotVisualizer::publishCartesianMarkers(ros::Time timeStamp, const c
   assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end());
 
   // Publish cartesian markers (minus the CoM Pose)
-  currentStatePublisher_.publish(markerArray);
+  currentStatePublisher_->publish(markerArray);
 }
 
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories) {
+void LeggedRobotVisualizer::publishDesiredTrajectory(rclcpp::Time timeStamp, const TargetTrajectories& targetTrajectories) {
   const auto& stateTrajectory = targetTrajectories.stateTrajectory;
   const auto& inputTrajectory = targetTrajectories.inputTrajectory;
 
   // Reserve com messages
-  std::vector<geometry_msgs::Point> desiredBasePositionMsg;
+  std::vector<geometry_msgs::msg::Point> desiredBasePositionMsg;
   desiredBasePositionMsg.reserve(stateTrajectory.size());
 
   // Reserve feet messages
-  feet_array_t<std::vector<geometry_msgs::Point>> desiredFeetPositionMsgs;
+  feet_array_t<std::vector<geometry_msgs::msg::Point>> desiredFeetPositionMsgs;
   for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) {
     desiredFeetPositionMsgs[i].reserve(stateTrajectory.size());
   }
@@ -239,7 +231,7 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const
 
     // Construct base pose msg
     const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_);
-    geometry_msgs::Pose pose;
+    geometry_msgs::msg::Pose pose;
     pose.position = getPointMsg(basePose.head<3>());
 
     // Fill message containers
@@ -253,7 +245,7 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const
 
     const auto feetPositions = endEffectorKinematicsPtr_->getPosition(state);
     for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) {
-      geometry_msgs::Pose footPose;
+      geometry_msgs::msg::Pose footPose;
       footPose.position = getPointMsg(feetPositions[i]);
       desiredFeetPositionMsgs[i].push_back(footPose.position);
     }
@@ -265,30 +257,30 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const
   comLineMsg.id = 0;
 
   // Publish
-  costDesiredBasePositionPublisher_.publish(comLineMsg);
+  costDesiredBasePositionPublisher_->publish(comLineMsg);
   for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) {
     auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), feetColorMap_[i], trajectoryLineWidth_);
     footLineMsg.header = getHeaderMsg(frameId_, timeStamp);
     footLineMsg.id = 0;
-    costDesiredFeetPositionPublishers_[i].publish(footLineMsg);
+    costDesiredFeetPositionPublishers_[i]->publish(footLineMsg);
   }
 }
 
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory,
+void LeggedRobotVisualizer::publishOptimizedStateTrajectory(rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory,
                                                             const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule) {
   if (mpcTimeTrajectory.empty() || mpcStateTrajectory.empty()) {
     return;  // Nothing to publish
   }
 
   // Reserve Feet msg
-  feet_array_t<std::vector<geometry_msgs::Point>> feetMsgs;
-  std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector<geometry_msgs::Point>& v) { v.reserve(mpcStateTrajectory.size()); });
+  feet_array_t<std::vector<geometry_msgs::msg::Point>> feetMsgs;
+  std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector<geometry_msgs::msg::Point>& v) { v.reserve(mpcStateTrajectory.size()); });
 
   // Reserve Com Msg
-  std::vector<geometry_msgs::Point> mpcComPositionMsgs;
+  std::vector<geometry_msgs::msg::Point> mpcComPositionMsgs;
   mpcComPositionMsgs.reserve(mpcStateTrajectory.size());
 
   // Extract Com and Feet from state
@@ -296,7 +288,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp,
     const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_);
 
     // Fill com position and pose msgs
-    geometry_msgs::Pose pose;
+    geometry_msgs::msg::Pose pose;
     pose.position = getPointMsg(basePose.head<3>());
     mpcComPositionMsgs.push_back(pose.position);
 
@@ -314,7 +306,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp,
   });
 
   // Convert feet msgs to Array message
-  visualization_msgs::MarkerArray markerArray;
+  visualization_msgs::msg::MarkerArray markerArray;
   markerArray.markers.reserve(centroidalModelInfo_.numThreeDofContacts +
                               2);  // 1 trajectory per foot + 1 for the future footholds + 1 for the com trajectory
   for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) {
@@ -325,8 +317,8 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp,
   markerArray.markers.back().ns = "CoM Trajectory";
 
   // Future footholds
-  visualization_msgs::Marker sphereList;
-  sphereList.type = visualization_msgs::Marker::SPHERE_LIST;
+  visualization_msgs::msg::Marker sphereList;
+  sphereList.type = visualization_msgs::msg::Marker::SPHERE_LIST;
   sphereList.scale.x = footMarkerDiameter_;
   sphereList.scale.y = footMarkerDiameter_;
   sphereList.scale.z = footMarkerDiameter_;
@@ -362,7 +354,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp,
   assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, timeStamp));
   assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end());
 
-  stateOptimizedPublisher_.publish(markerArray);
+  stateOptimizedPublisher_->publish(markerArray);
 }
 
 }  // namespace legged_robot
diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp
index 6c9aaa9ec..5fd1fcba0 100644
--- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp
+++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp
@@ -252,6 +252,8 @@ Redistribution and use in source and binary forms, with or without
   void MRT_ROS_Interface::launchNodes(rclcpp::Node::SharedPtr& nodeHandle) {
     this->reset();
 
+    node_ = nodeHandle;
+
     // display
     RCLCPP_INFO_STREAM(LOGGER, "MRT node is setting up ...");