436 task guidance node for autopilot

[AbubakarAliyuBadawi]

No description provided.

[Andeshog]

This is a start. Made some general comments. Will go more in-depth after you make these adjustments.

[kluge7]

You need to fix the changes flagged by the pre-commit hooks! To solve them you need to open a terminal and run:
pip install pre-commit
and then navigate to the vortex-auv directory:
cd vortex-auv
and then run pre-commit hooks on all the files:
pre-commit run --all-files

It will fix some things automatically, but others you have to configure manually! Send me a message if anything is unclear:)

[Andeshog]

General:

• Make README for the package
• Avoid unnecessary comments
• Add a config directory and a yaml file that holds the necessary parameters

[Andeshog]

On a general note, if the comments are just repeating what the code says, then remove them.

[Andeshog]

Clean up the redundant comments in the los_guidance_action_server.py file as well :)

[Andeshog]

Same goes for this, consider using a dataclass instead of dictionary. FilterParams would be a fitting name for the class.

[Andeshog]

I dont feel like the comments add much here. What I meant was to use typehint for the commands input, a more fitting structure (like the state dataclass), and perhaps a quick description in the docstring.

So something like this

def publish_guidance(self, commands: State):
    """Publish the commanded surge velocity, pitch angle, and yaw angle."""
    msg = LOSGuidance()
    msg.surge = commands.surge_vel
    msg.pitch = commands.pitch
    msg.yaw = commands.yaw

Note that in this example I added surge velocity to the State dataclass.

[AbubakarAliyuBadawi]

This change will require me to modify how I created the commands in the process_guidance method as well.

[Andeshog]

Something like this could work

def compute_raw_los_guidance(
        self, current_pos: State, target_pos: State
    ) -> State:
        
        dx = target_pos.x - current_pos.x
        dy = target_pos.y - current_pos.y

        horizontal_distance = np.sqrt(dx**2 + dy**2)
        desired_yaw = np.arctan2(dy, dx)
        yaw_error = self.ssa(desired_yaw - current_pos.yaw)

        depth_error = target_pos.z - current_pos.z
        desired_pitch = self.compute_pitch_command(depth_error, horizontal_distance)

        desired_surge = self.compute_desired_speed(yaw_error, horizontal_distance)

        commands = State(surge_vel=desired_surge, pitch=desired_pitch, yaw=desired_yaw)

        return commands

def apply_reference_filter(self, commands: State) -> State:
        x_dot = self.Ad @ self.x + self.Bd @ commands.as_los_array()
        self.x = self.x + x_dot * self.dt
        return State(surge_vel=self.x[0], pitch=self.x[1], yaw=self.x[2])

def compute_guidance(
        self, current_pos: State, target_pos: State
    ) -> State:
        raw_commands = self.compute_raw_los_guidance(current_pos, target_pos)

        filtered_commands = self.apply_reference_filter(raw_commands)
        filtered_commands.pitch = self.ssa(filtered_commands.pitch)
        filtered_commands.yaw = self.ssa(filtered_commands.yaw)
        return filtered_commands

where the State class is modified to

@dataclass
class State:
    x: float = 0.0
    y: float = 0.0
    z: float = 0.0
    pitch: float = 0.0
    yaw: float = 0.0
    surge_vel: float = 0.0

    def __add__(self, other: "State") -> "State":
        return State(
            x=self.x + other.x,
            y=self.y + other.y,
            z=self.z + other.z,
            pitch=self.pitch + other.pitch,
            yaw=self.yaw + other.yaw,
        )

    def __sub__(self, other: "State") -> "State":
        return State(
            x=self.x - other.x,
            y=self.y - other.y,
            z=self.z - other.z,
            pitch=self.pitch - other.pitch,
            yaw=self.yaw - other.yaw,
        )
    
    def as_los_array(self):
        return np.array([self.surge_vel, self.pitch, self.yaw])

[Andeshog]

If you utilize Point for target (thus, generally for waypoints), this could be simplified to

def publish_reference(self, target: Point):
    ...
    msg.pose.position = target

[AbubakarAliyuBadawi]

If I change this I might need to modify how waypoints are stored and processed, right now they are stored as a numpy array rather than points.

[Andeshog]

Yes, and it is probably better to just do it straight away since waypoints will be sent and received through a topic as Points in the full system.

[AbubakarAliyuBadawi]

Ohh I see.

[Andeshog]

Finish the README

[Andeshog]

These are not needed. You can just access omega and zeta by

filter_params = FilterParameters()
omega = filter_params.omega_diag
zeta = filter_parameters.zeta_diag

[Andeshog]

Now that you have the two dataclasses for the parameters, you can just use the objects directly

def __init__(self, los_params: LOSParameters, filter_params: FilterParameters):
    self.los_params = los_params
    self.filter_params = filter_params
    self.setup_filter_matrices()

[Andeshog]

Since this is a one time call, you could extract omega and zeta at the top

def setup_filter_matrices(self):
        omega = np.diag(self.filter_params.omega_diag)
        zeta = np.diag(self.filter_params.zeta_diag)
        omega_cubed = omega @ omega @ omega
        omega_squared = omega @ omega

        self.Ad = np.zeros((9, 9))
        self.Bd = np.zeros((9, 3))

        self.Ad[0:3, 3:6] = np.eye(3)
        self.Ad[3:6, 6:9] = np.eye(3)
        self.Ad[6:9, 0:3] = -omega_cubed
        self.Ad[6:9, 3:6] = -(2 * zeta + np.eye(3)) @ omega_squared
        self.Ad[6:9, 6:9] = -(2 * zeta + np.eye(3)) @ omega

        self.Bd[6:9, :] = omega_cubed

[Andeshog]

Add the update period dt here as well.

[Andeshog]

Remember to also import the parameter classes.

[Andeshog]

Split this into two methods, where one creates the los_params object and one creates the filter_params object.

[Andeshog]

Adding a

if not self.debug_mode:
    return

self.publish_log...

before the log calls would remove the need for going into all the debug methods when it is not necessary. This means the same statement can be removed from inside the log methods.

[Andeshog]

The treshold should be a parameter. Add it to the config file.

[Andeshog]

Why is the initial state not just the same as the actual state?