diff --git a/mouse_teleop/config/mouse_teleop.yaml b/mouse_teleop/config/mouse_teleop.yaml
index 394bbe7..be2ea4a 100644
--- a/mouse_teleop/config/mouse_teleop.yaml
+++ b/mouse_teleop/config/mouse_teleop.yaml
@@ -3,3 +3,4 @@ mouse_teleop:
     frequency: 10.0
     scale    : 1.0
     holonomic: False
+    twist_stamped: True
diff --git a/mouse_teleop/mouse_teleop/mouse_teleop.py b/mouse_teleop/mouse_teleop/mouse_teleop.py
index 53e7151..0b95357 100755
--- a/mouse_teleop/mouse_teleop/mouse_teleop.py
+++ b/mouse_teleop/mouse_teleop/mouse_teleop.py
@@ -41,7 +41,7 @@
 import signal
 import tkinter
 
-from geometry_msgs.msg import TwistStamped, Vector3
+from geometry_msgs.msg import Twist, TwistStamped, Vector3
 import numpy
 import rclpy
 from rclpy.node import Node
@@ -56,9 +56,15 @@ def __init__(self):
         self._frequency = self.declare_parameter('frequency', 0.0).value
         self._scale = self.declare_parameter('scale', 1.0).value
         self._holonomic = self.declare_parameter('holonomic', False).value
+        self._twist_stamped = self.declare_parameter('twist_stamped', True).value
 
         # Create twist publisher:
-        self._pub_cmd = self.create_publisher(TwistStamped, 'mouse_vel', 10)
+        if self._twist_stamped:
+            print("_twist_stamped true!")
+            self._pub_cmd = self.create_publisher(TwistStamped, 'mouse_vel', 10)
+        else:
+            print("_twist_stamped false!")
+            self._pub_cmd = self.create_publisher(Twist, 'mouse_vel', 10)
 
         # Initialize twist components to zero:
         self._v_x = 0.0
@@ -256,7 +262,10 @@ def _send_motion(self):
         twist_stamped.twist.linear = lin
         twist_stamped.twist.angular = ang
 
-        self._pub_cmd.publish(twist_stamped)
+        if self._twist_stamped:
+            self._pub_cmd.publish(twist_stamped)
+        else:
+            self._pub_cmd.publish(twist_stamped.twist)
 
     def _publish_twist(self):
         self._send_motion()
@@ -292,7 +301,6 @@ def _change_to_motion_angular(self, event):
 def main():
     try:
         rclpy.init()
-
         node = MouseTeleop()
 
         node.destroy_node()