diff --git a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml
index 52e49dc6..fdc04e13 100644
--- a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml
+++ b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml
@@ -33,7 +33,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
diff --git a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml
index 5083a29c..c94b81d5 100644
--- a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml
+++ b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml
@@ -51,7 +51,7 @@ tricycle_controller:
 
     # cmd_vel input
     cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
-    use_stamped_vel: false # Set to True if using TwistStamped.
+    use_stamped_vel: true # Set to True if using TwistStamped.
 
     # Debug
     publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.
diff --git a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp
index a3c117ff..5439073f 100644
--- a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp
+++ b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp
@@ -16,7 +16,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -27,20 +27,22 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("diff_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
-    "/diff_drive_base_controller/cmd_vel_unstamped", 10);
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+    "/tricycle_controller/cmd_vel", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::TwistStamped command;
 
-  command.linear.x = 0.2;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  command.header.stamp = node->now();
 
-  command.angular.x = 0.0;
-  command.angular.y = 0.0;
-  command.angular.z = 0.1;
+  command.twist.linear.x = 0.2;
+  command.twist.linear.y = 0.0;
+  command.twist.linear.z = 0.0;
+
+  command.twist.angular.x = 0.0;
+  command.twist.angular.y = 0.0;
+  command.twist.angular.z = 0.1;
 
   while (1) {
     publisher->publish(command);
diff --git a/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp b/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp
index 9713ff76..1783ce1e 100644
--- a/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp
+++ b/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp
@@ -16,7 +16,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -27,20 +27,22 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("tricycle_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
     "/tricycle_controller/cmd_vel", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::TwistStamped command;
 
-  command.linear.x = 0.2;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  command.header.stamp = node->now();
 
-  command.angular.x = 0.0;
-  command.angular.y = 0.0;
-  command.angular.z = 0.1;
+  command.twist.linear.x = 0.2;
+  command.twist.linear.y = 0.0;
+  command.twist.linear.z = 0.0;
+
+  command.twist.angular.x = 0.0;
+  command.twist.angular.y = 0.0;
+  command.twist.angular.z = 0.1;
 
   while (1) {
     publisher->publish(command);