Replace Twist with TwistStamped (#210)

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
ros-controls · Jan 2, 2024 · 3a44b0b · 3a44b0b
1 parent 5412da5
commit 3a44b0b
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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);