Resolved merge conflict in guidance.py

guidance/guidance_pkg/guidance_pkg/guidance.py

@@ -719,37 +719,47 @@
 import math
 from transforms3d.euler import quat2euler
 
-def normalize_angle(angle):
-    """Normalize angle to be within [-pi, pi]."""
-    return (angle + np.pi) % (2 * np.pi) - np.pi
 
 class GuidanceNode(Node):
+
     def __init__(self):
         super().__init__('guidance_pkg')
 
-        # Publishers
-        self.output_pub = self.create_publisher(LOSGuidance, '/guidance/los', 10)
-        self.ref_pub = self.create_publisher(PoseStamped, '/guidance/reference', 10)
-        self.error_pub = self.create_publisher(Vector3Stamped, '/guidance/errors', 10)
-        self.twist_pub = self.create_publisher(TwistStamped, '/guidance/twist', 10)
+        # Publisher to output the surge velocity, pitch angle, and heading angle
+        self.output_pub = self.create_publisher(Float32MultiArray,
+                                                '/guidance/waypoints', 10)
 
-        # Subscriber to odometry
-        self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10)
+        # Subscriber to receive odometry data from '/nucleus/odom'
+        self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback,
+                                 10)
 
         # Initialize variables
         self.current_position = None
         self.current_velocity = None
         self.current_orientation = None
 
-        # Parameters for guidance algorithm
-        self.U = 1.0    # Desired surge speed (m/s)
-        self.delta = 0.2  # Lookahead distance (m)
+        # Parameters for the guidance algorithm
+        self.U = 1.0  # Desired speed
+        self.delta = 1.2  # Lookahead distance
 
         # Define waypoints
         self.waypoints = [
-            {'x': 5.0, 'y': 5.0, 'z': 8.0},
-            {'x': 10.0, 'y': 5.0, 'z': -5.0},
-            {'x': 15.0, 'y': 0.0, 'z': 0.0},
+            {
+                'x': 5.0,
+                'y': -5.0,
+                'z': -8.0
+            },
+            {
+                'x': 10.0,
+                'y': 5.0,
+                'z': -5.0
+            },
+            {
+                'x': 15.0,
+                'y': 0.0,
+                'z': 0.0
+            },
+            # Add more waypoints as needed
         ]
         self.current_waypoint_index = 0
 
@@ -764,7 +774,12 @@ def odom_callback(self, msg):
         self.current_position = msg.pose.pose
         self.current_velocity = msg.twist.twist
         orientation_q = msg.pose.pose.orientation
-        quaternion = [orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z]
+
+        # Convert quaternion to Euler angles
+        # ROS quaternion format: (x, y, z, w)
+        quaternion = [
+            orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z
+        ]
         roll, pitch, yaw = quat2euler(quaternion)
         self.current_orientation = {'roll': roll, 'pitch': pitch, 'yaw': yaw}
 
@@ -780,25 +795,29 @@ def timer_callback(self):
 
         waypoint = self.waypoints[self.current_waypoint_index]
 
-        # Extract current position and orientation
-        current_x = self.current_position.position.x
-        current_y = self.current_position.position.y
-        current_z = self.current_position.position.z
-        psi = self.current_orientation['yaw']
+        # Extract current position
+        current_x = self.current_position.x
+        current_y = self.current_position.y
+        current_z = self.current_position.z
+
+        # Extract current orientation
+        psi = self.current_orientation['yaw']  # Heading angle
+        theta = self.current_orientation['pitch']  # Pitch angle
 
         # Extract target waypoint position
         target_x = waypoint['x']
         target_y = waypoint['y']
         target_z = waypoint['z']
 
         # Compute distance to the waypoint
-        distance = math.sqrt((target_x - current_x) ** 2 +
-                             (target_y - current_y) ** 2 +
-                             (target_z - current_z) ** 2)
-
-        # Check if waypoint is reached
-        if distance < 0.1:
-            self.get_logger().info(f'Reached waypoint {self.current_waypoint_index}')
+        distance = math.sqrt((target_x - current_x)**2 +
+                             (target_y - current_y)**2 +
+                             (target_z - current_z)**2)
+
+        # Check if the waypoint is reached
+        if distance < 0.5:
+            self.get_logger().info(
+                f'Reached waypoint {self.current_waypoint_index}')
             self.current_waypoint_index += 1
             if self.current_waypoint_index >= len(self.waypoints):
                 self.get_logger().info('All waypoints reached. Shutting down guidance node.')
@@ -834,15 +853,9 @@ def timer_callback(self):
         # Compute pitch angle (gamma)
         gamma = math.atan2(-dz, math.hypot(dx, dy))
 
-        # Compute along-track and cross-track errors
-        # Transform position error into path coordinate frame
-        R = np.array([[math.cos(pi_p), math.sin(pi_p)],
-                      [-math.sin(pi_p), math.cos(pi_p)]])
-        e_pos = np.array([current_x - ref_x, current_y - ref_y])
-        eps = R @ e_pos
-
-        e_x = eps[0]  # Along-track error
-        e_y = eps[1]  # Cross-track error
+        # Compute cross-track error
+        e_ct = -(dx * (current_y - ref_y) - dy *
+                 (current_x - ref_x)) / math.hypot(dx, dy)
 
         # Compute desired course angle
         course_d = pi_p - math.atan2(e_y, self.delta)
@@ -862,44 +875,12 @@ def timer_callback(self):
         output_msg.yaw = heading_d
         self.output_pub.publish(output_msg)
 
-        # Publish reference pose
-        pose_msg = PoseStamped()
-        pose_msg.header.frame_id = "odom"
-        pose_msg.header.stamp = self.get_clock().now().to_msg()
-        pose_msg.pose.position.x = target_x
-        pose_msg.pose.position.y = target_y
-        pose_msg.pose.position.z = target_z
-        self.ref_pub.publish(pose_msg)
-
-        # Publish errors
-        error_msg = Vector3Stamped()
-        error_msg.header.frame_id = "odom"
-        error_msg.header.stamp = self.get_clock().now().to_msg()
-        error_msg.vector.x = e_x
-        error_msg.vector.y = e_y
-        error_msg.vector.z = e_psi
-        self.error_pub.publish(error_msg)
-
-        # Publish desired twist in body frame
-        twist_msg = TwistStamped()
-        twist_msg.header.frame_id = "base_link"
-        twist_msg.header.stamp = self.get_clock().now().to_msg()
-        # Transform desired velocities to body frame
-        u = surge_velocity * math.cos(e_psi)
-        v = surge_velocity * math.sin(e_psi)
-        twist_msg.twist.linear.x = u
-        twist_msg.twist.linear.y = v
-        twist_msg.twist.linear.z = 0.0  # No vertical control as per request
-        twist_msg.twist.angular.x = 0.0
-        twist_msg.twist.angular.y = 0.0
-        twist_msg.twist.angular.z = 0.0
-        self.twist_pub.publish(twist_msg)
-
-        self.get_logger().info("____________________________________________________")
-        self.get_logger().info(f"Current Position: x={current_x:.2f}, y={current_y:.2f}, z={current_z:.2f}")
-        self.get_logger().info(f"Target Waypoint: x={target_x:.2f}, y={target_y:.2f}, z={target_z:.2f}")
-        self.get_logger().info(f"Errors: e_x={e_x:.2f}, e_y={e_y:.2f}, e_psi={e_psi:.2f}")
-        self.get_logger().info(f"Desired Velocities: u={u:.2f}, v={v:.2f}")
+
+def normalize_angle(angle):
+    """
+    Normalize an angle to the range [-π, π].
+    """
+    return (angle + np.pi) % (2 * np.pi) - np.pi
 
 
 def main(args=None):
@@ -909,5 +890,6 @@ def main(args=None):
     node.destroy_node()
     rclpy.shutdown()
 
+
 if __name__ == '__main__':
     main()

guidance/guidance_pkg/setup.py

@@ -8,7 +8,7 @@
     packages=find_packages(exclude=['test']),
     data_files=[
         ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
+         ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],

guidance/guidance_pkg/test/test_copyright.py

@@ -17,7 +17,8 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason='No copyright header has been placed in the generated source file.')
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():