Add Teleop and fix velocity transformer

controls/velocity_transformer.py

@@ -2,16 +2,18 @@
 
 import sys
 import rclpy
+from rclpy.node import Node
 from geometry_msgs.msg import Twist
 from sub_interfaces.msg import SubThrusts
 # from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
 
 
-class VelocityTransformer(rclpy.node.Node):
+class VelocityTransformer(Node):
 
     def __init__(self):
-        self.sub = self.create_subscription('/sub/cmd_vel', Twist, self.vel_callback)
-        self.pub = self.create_publisher('/sub/thrusts', SubThrusts, 10)
+        super().__init__('velocity_transformer')
+        self.sub = self.create_subscription(Twist, '/sub/cmd_vel', self.vel_callback, 10)
+        self.pub = self.create_publisher(SubThrusts, '/sub/thrusts', 10)
         # self.thrusters = [rospy.Publisher(f'/sub/thrusters/{i}/input', FloatStamped, queue_size=10) for i in ]
 
     def vel_callback(self, twist):
@@ -51,8 +53,11 @@ def vel_callback(self, twist):
 
         self.pub.publish(msg)
 
-if __name__ == "__main__":
-    rclpy.init(sys.argv)
+def main(args=None):
+    rclpy.init(args=args)
     vel_transform = VelocityTransformer()
     rclpy.spin(vel_transform)
     rclpy.shutdown(vel_transform)
+
+if __name__ == "__main__":
+    main()

launch/robosub_controls.launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='controls',
+            node_namespace='sb',
+            node_executable='velocity_transformer',
+            node_name='velocity_transformer'
+        ),
+        Node(
+            package='teleop_twist_keyboard',
+            node_namespace='sb',
+            node_executable='teleop_twist_keyboard',
+            output='screen',
+            prefix='xterm -e',
+            node_name='teleop_twist_keyboard'
+        ),
+    ])

package.xml

@@ -10,12 +10,14 @@
   <buildtool_depend>ament_python</buildtool_depend>
 
   <build_depend>rclpy</build_depend>
-
+  <build_depend>teleop_twist_keyboard</build_depend>
+
   <exec_depend>rclpy</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>razor_imu_9dof</exec_depend>
   <exec_depend>sub_interfaces</exec_depend>
+  <exec_depend>teleop_twist_keyboard</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

setup.py

@@ -1,5 +1,6 @@
+import os
 from setuptools import setup
-import glob
+from glob import glob
 
 package_name = 'controls'
 
@@ -11,7 +12,7 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name, glob.glob('launch/*.launch*'))
+        (os.path.join('share', package_name), glob('launch/*.launch.py'))
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -22,7 +23,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'init = controls.sub_init:processInput'
+            'init = controls.sub_init:processInput',
+            'velocity_transformer = controls.velocity_transformer:main',
         ],
     },
 )