add gimbal yaw and pitch command ros2 message

docs/development/testing/testing_frameworks.md

@@ -1 +1,112 @@
-# Testing Frameworks
+# Testing Frameworks
+
+## Testing Categories
+Testing is organized into four main categories:
+
+1. **Integration Testing (End-to-End Testing):**  
+   Validates the entire system within its operational environment, whether in simulation or on hardware.
+
+2. **System Testing:**  
+   Tests interactions between system components, such as communication between ROS nodes.
+
+3. **Node Testing:**  
+   Focuses on verifying the functionality of individual nodes, from initialization to execution.
+
+4. **Unit Testing:**  
+   Tests specific functions or business logic to ensure the correctness of the smallest units of code.
+
+---
+
+## Testing Utilities
+Since our autonomy system primarily relies on ROS 2, we use the `colcon test` framework to run tests. Most tests are written in Python using the `pytest` package, as demonstrated in the example below.
+
+---
+
+## Testing Structure
+All tests should be included in a ```tests/``` folder in their respective heirarchy of the architecture. For example, integration testing should on the same level as the ```robot/``` and ```simulation/``` folders, where a node test should reside in the ros package directoy. ```colcon test``` will search through the workspace to find all testing packages, provided they are specified in the Cmake.txt or setup.py files.
+
+## Example Testing Script
+
+Below is an example of a systems test that can give the general structure of a testing script.
+
+```
+import os
+import sys
+import time
+import unittest
+import uuid
+
+import launch
+from launch.launch_service import LaunchService
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+from launch_testing.io_handler import ActiveIoHandler
+import launch_testing_ros
+
+import pytest
+
+import rclpy
+from rclpy.node import Node
+
+import std_msgs.msg
+from std_msgs.msg import String
+import mavros_msgs.srv
+
+import time
+
+@pytest.mark.rostest
+# this is the test descriptioon used to launch the full robot system with launch_robot_headless.yaml
+def generate_test_description():
+    robot_launch_path = 'path/to/launch/file'
+
+    gui_arg = launch.actions.DeclareLaunchArgument('use_gui', default_value='false', description='Whether to launch the GUI')
+
+    robot_launch = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.AnyLaunchDescriptionSource(robot_launch_path),
+                                                            launch_arguments={'use_gui': launch.substitutions.LaunchConfiguration('use_gui')}.items())
+    return (
+        launch.LaunchDescription([
+            gui_arg,
+            robot_launch,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {}
+    )
+
+class TestRobotSystem(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context for the test node
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        rclpy.shutdown()
+
+    def setUp(self):
+        # Create a ROS node for tests
+        self.node = rclpy.create_node('robot_tester_node')
+        self.service_timeout = 2
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def test_set_mode(self):
+        client = self.node.create_client(mavros_msgs.srv.SetMode, '/mavros/set_mode')
+        self.node.get_logger().info("Waiting for service to be available...")
+        accum_time = 0
+        while not client.wait_for_service(timeout_sec=1.0):
+            print('service not available, waiting again...')
+            accum_time += 1
+            if accum_time > self.service_timeout:
+                print('service not available, aborting test...')
+                self.assertTrue(False)
+        request = mavros_msgs.srv.SetMode.Request()
+        request.custom_mode = "GUIDED"
+        print("Sending request to set mode to GUIDED") 
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+        response = future.result()
+        self.assertTrue(response.mode_sent)
+```

robot/ros_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py

@@ -5,6 +5,7 @@
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState
 from transforms3d.euler import quat2euler
+from std_msgs.msg import Float64  # Assuming the desired yaw is published as a Float64
 
 class GimbalStabilizerNode(Node):
     def __init__(self):
@@ -16,13 +17,25 @@ def __init__(self):
         # Subscriber to receive drone odometry
         self.create_subscription(Odometry, '/robot_1/odometry_conversion/odometry', self.odometry_callback, 10)
         self.create_subscription(JointState, '/joint_states', self.joint_callback, 10)
+        self.create_subscription(Float64, '/desired_gimbal_yaw', self.yaw_callback, 10)
+        self.create_subscription(Float64, '/desired_gimbal_pitch', self.pitch_callback, 10)
 
         # Initialize joint state message
         self.joint_command = JointState()
         self.joint_command.name = ["yaw_joint","roll_joint", "pitch_joint"]
         self.joint_command.position = [0.0, 0.0, 0.0]
+        self.desired_yaw = 0.0
+        self.desired_pitch = 0.0
+
+    def yaw_callback(self, msg):
+        self.desired_yaw = msg.data
+        # self.get_logger().info(f"Received desired yaw angle: {self.desired_yaw}")
+
+    def pitch_callback(self, msg):
+        self.desired_pitch = msg.data
 
     def joint_callback(self, msg):
+        self.got_joint_states = True
         # Inverse the drone angles to stabilize the gimbal
         # self.joint_command.position[0] = -roll  # roll joint
         # self.joint_command.position[1] = -pitch  # pitch joint
@@ -33,7 +46,7 @@ def joint_callback(self, msg):
         # self.joint_command.position[0] = -20.0/180*3.14  # yaw joint
         # self.joint_command.position[1] = 10.0/180*3.14  # roll joint
         # self.joint_command.position[2] = 20.0/180*3.14  # pitch joint
-        self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
+        # self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
         # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
@@ -57,11 +70,11 @@ def odometry_callback(self, msg):
         # self.joint_command.position[1] = -pitch  # pitch joint
         # self.joint_command.position[2] = -yaw  # yaw joint
 
-        self.joint_command.position[0] = -0.0/180*3.14  # yaw joint
-        self.joint_command.position[1] = -roll  # roll joint
-        self.joint_command.position[2] = 0.0/180*3.14  # pitch joint
+        self.joint_command.position[0] = -self.desired_yaw/180*3.14  # yaw joint
+        self.joint_command.position[1] = -roll/180*3.14  # roll joint
+        self.joint_command.position[2] = self.desired_pitch/180*3.14  # pitch joint
         self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
-        self.joint_command.velocity = [-1.0, -1.0, -1.0]
+        # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
         self.joint_pub.publish(self.joint_command)