From 92d76bd64687ebeb3648d848989174037571492f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Thu, 28 Dec 2023 10:54:31 +0100
Subject: [PATCH] Replace Twist with TwistStamped
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 .../diff_drive_controller_velocity.yaml       |  2 +-
 .../config/tricycle_drive_controller.yaml     |  2 +-
 .../examples/example_diff_drive.cpp           | 22 ++++++++++---------
 .../examples/example_tricycle_drive.cpp       | 20 +++++++++--------
 4 files changed, 25 insertions(+), 21 deletions(-)

diff --git a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml
index 6f3ddab1..1061aea7 100644
--- a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml
+++ b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml
@@ -32,7 +32,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/gz_ros2_control_demos/config/tricycle_drive_controller.yaml b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml
index 17dbb626..b0951649 100644
--- a/gz_ros2_control_demos/config/tricycle_drive_controller.yaml
+++ b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml
@@ -50,7 +50,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/gz_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp
index d77baec5..365ecb59 100644
--- a/gz_ros2_control_demos/examples/example_diff_drive.cpp
+++ b/gz_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>(
+    "/diff_drive_base_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.1;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  command.header.stamp = node->now();
 
-  command.angular.x = 0.1;
-  command.angular.y = 0.0;
-  command.angular.z = 0.0;
+  command.twist.linear.x = 0.1;
+  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/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp
index 9713ff76..1783ce1e 100644
--- a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp
+++ b/gz_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);