Improved Kinematics

bags/joy_recording/README.md

@@ -0,0 +1,45 @@
+# Joystick Recording 
+
+This is a recording of a joystick. It makes the following moves:
+
+1. Left Stick 
+    1. Up and back
+    1. Down and back
+    1. Left and back 
+    1. Right and back 
+    1. Slow circle 
+1. Right Stick 
+    1. Up and back
+    1. Down and back
+    1. Left and back 
+    1. Right and back 
+    1. Slow circle 
+1. Left trigger pull and release
+1. Right trigger pull and release
+1. Left trigger pull and release
+1. Right trigger pull and release
+
+The total play time is about 52 seconds. 
+
+## Using this Bag 
+
+The bag replaces the need for a real joystick. Test the `thrust` node by running the `input_xbox_one` node without the `joy` node. This bag works in place of the `joy` node. 
+
+Start the input node: 
+
+```console
+$ ros2 run seahawk input_xbox_one
+```
+
+Start the thrust node: 
+
+```console
+$ ros2 run seahawk thrust
+```
+
+Play the bagfile: 
+
+```console
+$ ros2 bag play bags/joy_recording/
+```
+

bags/joy_recording/metadata.yaml

@@ -0,0 +1,26 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 52245814145
+  starting_time:
+    nanoseconds_since_epoch: 1704409072014791615
+  message_count: 3583
+  topics_with_message_count:
+    - topic_metadata:
+        name: /joy
+        type: sensor_msgs/msg/Joy
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 3583
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_2024_01_04-14_57_51_0.db3
+  files:
+    - path: rosbag2_2024_01_04-14_57_51_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1704409072014791615
+      duration:
+        nanoseconds: 52245814145
+      message_count: 3583

src/seahawk/launch/kinematics_viz.launch.py

@@ -20,7 +20,7 @@ def generate_launch_description():
                 default_value=model_path
         ),
         Node(
-            package='seahawk_deck',
+            package='seahawk',
             executable='rviz_markers',
             name='rviz_markers'
         ),
@@ -29,7 +29,7 @@ def generate_launch_description():
             package='rviz2',
             executable='rviz2',
             name='rviz2',
-            arguments=['-d', os.path.join(get_package_share_path('seahawk_deck'), 'rviz/debug_kinematics.rviz')]
+            arguments=['-d', os.path.join(get_package_share_path('seahawk'), 'rviz/debug_kinematics.rviz')]
         ),
 
         Node(

src/seahawk/seahawk_deck/input_xbox_one.py

@@ -50,7 +50,7 @@ def __init__(self):
         self.declare_parameter('angular_y_scale', 0.5) # Pitch
         self.declare_parameter('angular_z_scale', 0.5) # Yaw
         self.subscription = self.create_subscription(Joy, 'joy', self._callback, 10)
-        self.twist_pub = self.create_publisher(Twist, 'drive/twist', 10)
+        self.twist_pub = self.create_publisher(Twist, 'desired_twist', 10)
         self.claw_pub = self.create_publisher(Int8MultiArray, 'claw_control', 10)
         self.cam_servo_pub = self.create_publisher(Float32, 'camera_control', 10)
         self.claw_grab = False

src/seahawk/seahawk_deck/rviz_markers.py

@@ -21,21 +21,21 @@ class MarkerMaker(Node):
     TOP_P = 47*math.pi/36
 
     MOTORS = [
-        (( MOTOR_X, -MOTOR_Y, BOT_Z), (0,0,5*math.pi/4)),  
+        (( MOTOR_X,  MOTOR_Y, TOP_Z), (0,TOP_P,math.pi/4)),
         (( MOTOR_X, -MOTOR_Y, TOP_Z), (0,TOP_P,7*math.pi/4)),
-        ((-MOTOR_X, -MOTOR_Y, BOT_Z), (0,0,7*math.pi/4)),
-        ((-MOTOR_X, -MOTOR_Y, TOP_Z), (0,TOP_P,5*math.pi/4)),
-        ((-MOTOR_X,  MOTOR_Y, BOT_Z), (0,0,math.pi/4)),
         ((-MOTOR_X,  MOTOR_Y, TOP_Z), (0,TOP_P,3*math.pi/4)),
+        ((-MOTOR_X, -MOTOR_Y, TOP_Z), (0,TOP_P,5*math.pi/4)),
         (( MOTOR_X,  MOTOR_Y, BOT_Z), (0,0,3*math.pi/4)),
-        (( MOTOR_X,  MOTOR_Y, TOP_Z), (0,TOP_P,math.pi/4)),
+        (( MOTOR_X, -MOTOR_Y, BOT_Z), (0,0,5*math.pi/4)),     
+        ((-MOTOR_X,  MOTOR_Y, BOT_Z), (0,0,math.pi/4)),
+        ((-MOTOR_X, -MOTOR_Y, BOT_Z), (0,0,7*math.pi/4)),
     ]
 
     def __init__(self):
         """Initialize this node"""
         super().__init__('marker_maker')
         self.marker_pub = self.create_publisher(MarkerArray, 'drive/motors_debug', 10)
-        self.subscription = self.create_subscription(Float32MultiArray, 'drive/motors', self._callback, 10)
+        self.subscription = self.create_subscription(Float32MultiArray, 'motor_values', self._callback, 10)
 
         self.markers = MarkerArray()
         self.markers.markers = [ Marker() for x in range(8*3) ]   
@@ -98,7 +98,7 @@ def _callback(self, motor_msg):
         """
         self.get_logger().info(f"Motor Message: {motor_msg.data}")
         for i, motor in enumerate(motor_msg.data):
-            self.arrows[i].scale.x = -motor
+            self.arrows[i].scale.x = -motor / 4
             self.labels[i].text = f"{i}:{round(motor,2)}"
             if motor < -1 or motor > 1:
                 self.arrows[i].color.r = 1.0