use only Twist not TwistStamped

- This removes the TwistStamped. The Stamp information was unused.
- This makes it easier to use with teleop_twist_joy
- In the future we plan to add a TwistStamped version which can be configured

Signed-off-by: Alex Moriarty <[email protected]>

Author: moriarty

README.md

@@ -24,7 +24,7 @@ Controller Manager (`ros2_control`) will try to activate both on request, but th
 ros2 control switch_controllers --activate streaming_controller --deactivate joint_trajectory_controller
 ```
 
-By publishing `geometry_msgs::msg::TwistStamped` to `/streaming_controller/commands`, the commands will be piped to the driver
+By publishing `geometry_msgs::msg::Twist` to `/streaming_controller/commands`, the commands will be piped to the driver
 and streamed to the robot controller.
 
 ## PicknikResetFaultController

picknik_twist_controller/include/picknik_twist_controller/picknik_twist_controller.hpp

@@ -29,13 +29,13 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
 #include "picknik_twist_controller/visibility_control.h"
 #include "realtime_tools/realtime_buffer.h"
 
 namespace picknik_twist_controller
 {
-using CmdType = geometry_msgs::msg::TwistStamped;
+using CmdType = geometry_msgs::msg::Twist;
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 class PicknikTwistController : public controller_interface::ControllerInterface

picknik_twist_controller/src/picknik_twist_controller.cpp

@@ -145,12 +145,12 @@ controller_interface::return_type PicknikTwistController::update(
       command_interfaces_.size());
     return controller_interface::return_type::ERROR;
   }
-  command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
-  command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
-  command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
-  command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
-  command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
-  command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);
+  command_interfaces_[0].set_value((*twist_commands)->linear.x);
+  command_interfaces_[1].set_value((*twist_commands)->linear.y);
+  command_interfaces_[2].set_value((*twist_commands)->linear.z);
+  command_interfaces_[3].set_value((*twist_commands)->angular.x);
+  command_interfaces_[4].set_value((*twist_commands)->angular.y);
+  command_interfaces_[5].set_value((*twist_commands)->angular.z);
 
   return controller_interface::return_type::OK;
 }