436 task guidance node for autopilot #502

AbubakarAliyuBadawi · 2024-11-10T19:40:09Z

No description provided.

…ilot' into 436-task-guidance-node-for-autopilot

* ✨ feat: added the submodule for the perception * ci: update clang-formatter and add python-formatter * ci: explicitly checkout branch for black-format commit * Committing black-format changes * chore(pylint): add .pylintrc configuration file * refactor: fix pylint warning W1510 * refactor: fix pylint warning C0116 * refactor: fix pylint warning C0411 * refactor: fix pylint warning R1731 * refactor: fix pylint warning R1705 * refactor: fix pylint warning W1514 * refactor: fix pylint warning C0200 * refactor: fix pylint warning W0611 * refactor: fix pylint warning W0702 * refactor: fix pylint warning C0121 * refactor: fix pylint warning W0107 * refactor: fix pylint warning R1714 * refactor: fix pylint warning C0201 * refactor: fix pylint warning C0303 and W0613 * refactor: fix pylint warning W0621 * refactor: fix pylint warning R0402 * refactor: fix pylint warning W0212 * chore(pylint): update rule adjustments and formatting * style: format code with black * style: format imports with isort * ci: add isort and pylint to Python pipeline * Committing black-format changes * chore: add pyproject.toml for project configuration * ci: update order of pipeline jobs * ci: update pylint job and pylint rules * ci: update which python versions pylint test * ci: update python pipeline to only run on pull_request * refactor: fix pylint warning C0103 * chore: update linting and project configuration rules in .pylintrc and pyproject.toml * Rename files to snake_case * ci: add pipeline for grammar in comments * refactor: fix spelling mistakes in comments * ci: update job codespell_fix to continue on error * refactor: fix spelling mistakes in comments * ci: update pipeline codespell to only have one job * refactor: fix variable naming issues and adjust imports as per review * Committing codespell fixes * refactor: spelling correction * ci: update CI file and job names for consistency * ci: update CI file for more clarity * chore: add codespell configuration and ignore list * chore: add type hints and return types * ci: add mypy.ini for type checking configuration * ci: add mypy type checking workflow * refactor: format code with black * refactor: format all yaml files using prettier * ci: add pipeline for yaml formatting using prettier * ci: update yaml formatting pipeline to run on pull request * refactor: remove deprecated typing * refactor: fix import sorting * ci: remove faulty ci pipelines and combine python pipelines into single file * fix(security): replace subprocess call with safer alternative to remove shell=true * feat: add pre-commit hooks for black, isort, and codespell * ci: update python pipeline to use latest versions * refactor: apply code formatting fixes via pre-commit hooks * ci: split python ci/cd pipelines into multiple files * ci: add ci pipeline for checking style clang-format * refactor: remove unused config file for clang-format * refactor: remove duplicate file * ci: add ci pipeline that tests that codebase can build * refactor: update ci-build pipeline to only run on pull request * refactor: update clang-format config * refactor: apply clang-format * feat: update pipeline to build and run tests * refactor: disable linting checks when running colcon test * refactor: update .pre-commit-config file (#480) * ci: add semver.yaml pipeline and .releaserc configuration * ci: add semver.yaml pipeline and .releaserc configuration * fix: semver pipeline runs when push to main --------- Co-authored-by: Yauhen <[email protected]> Co-authored-by: Black Robot <[email protected]> Co-authored-by: Codespell Robot <[email protected]> Co-authored-by: Sondre Haugen <[email protected]>

…guidance-node-for-autopilot

…node-for-autopilot

Andeshog

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

guidance/guidance_los/guidance_los/los_guidance_action_server.py

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

guidance/guidance_los/package.xml

guidance/guidance_los/setup.py

guidance/guidance_los/guidance_los/los_guidance_action_server.py

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:)

…texntnu/vortex-auv into 436-task-guidance-node-for-autopilot

…e-for-autopilot

for more information, see https://pre-commit.ci

…e default

Andeshog · 2025-01-28T10:10:30Z

guidance/guidance_los/README.md

+```latex
+[x_e^p]    [ cos(π_v)   0   sin(π_v)] [cos(π_h) -sin(π_h) 0] [x^n]
+[y_e^p] = [    0       1       0    ] [sin(π_h)  cos(π_h) 0] [y^n]
+[z_e^p]    [-sin(π_v)   0   cos(π_v)] [   0        0      1] [z^n]


what you can do here instead is use math instead of latex
$$\begin{bmatrix} x_e^p \\\ y_e^p \\\ z_e^p \end{bmatrix} = \begin{bmatrix} \cos( \pi_v) & 0 & \sin(\pi_v) \\\ 0 & 1 & 0 \\\ -\sin(\pi_v) & 0 & \cos(\pi_v) \end{bmatrix} \cdots$$
which is written like

\begin{bmatrix} x_e^p \\ y_e^p \\ z_e^p \end{bmatrix} = \begin{bmatrix} \cos( \pi_v) & 0 & \sin(\pi_v) \\ 0 & 1 & 0 \\ -\sin(\pi_v) & 0 & \cos(\pi_v) \end{bmatrix} \cdots

Talhanc

Maybe look at comments, remove some and add some more

Talhanc · 2025-02-05T21:01:25Z

guidance/guidance_los/config/los_guidance_params.yaml

+    topics:
+      publishers:
+        los_commands: '/guidance/los'
+        debug:
+          reference: '/guidance/debug/reference'
+          errors: '/guidance/debug/errors'
+          logs: '/guidance/debug/logs'
+      subscribers:
+        odometry: '/orca/odom'
+        pose: '/dvl/pose'
+        twist: '/dvl/twist'


This is now in orca.yaml, recommend adding from there 😄

Talhanc · 2025-02-05T21:02:03Z

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

+from dataclasses import dataclass, field
+
+import numpy as np


Maybe not have space here?

Talhanc · 2025-02-05T21:04:56Z

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

+@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])
+
+    def as_pos_array(self):
+        return np.array([self.x, self.y, self.z])
+


This is already in vortex_utils, cough anders cough, maybe look at that.
If you want to keep this maybe rename surge_vel to u to be more correct with marine standard notations

Talhanc · 2025-02-05T21:07:01Z

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

+    lookahead_distance_min: float = 2.0
+    lookahead_distance_max: float = 8.0
+    lookahead_distance_factor: float = 1.0


why is max so high? maybe use 3 as max or else you will get the guidance algorithm looking too far ahead and causing a swing too fast.

Talhanc · 2025-02-05T21:07:31Z

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

+    nominal_speed: float = 1.0
+    min_speed: float = 0.1


Orca has max speed of 0.3 m/s, maybe set this down

Talhanc · 2025-02-05T21:45:03Z

guidance/guidance_los/scripts/los_guidance_action_server.py

+        self.u = msg.twist.twist.linear.x
+        self.v = msg.twist.twist.linear.y
+        self.w = msg.twist.twist.linear.z


name consistence, u or surge_vel

Talhanc · 2025-02-05T21:46:37Z

guidance/guidance_los/scripts/los_guidance_action_server.py

+        beta_c = self.guidance_calculator.calculate_beta_c(
+            self.u, self.v, self.w, self.phi, self.pitch, alpha_c
+            self.state.twist.linear_x,
+            self.state.twist.linear_y,
+            self.state.twist.linear_z,
+            self.state.pose.roll,
+            self.state.pose.pitch,
+            alpha_c,
+        )


isnt self.state.twist.linear_x, and self.u the same? and alpha_c is here two times

Talhanc · 2025-02-05T21:49:39Z

guidance/guidance_los/scripts/los_guidance_action_server.py

+        yaw_error = ssa(psi_d - self.state.pose.yaw)
+        desired_surge = self.guidance_calculator.compute_desired_speed(
+            yaw_error, self.norm, crosstrack_y
+        )
+        unfiltered_commands = State(
+            surge_vel=self.desired_vel, pitch=theta_d, yaw=psi_d
+        )
+        filtered_commands = self.guidance_calculator.apply_reference_filter(
+            unfiltered_commands
+        )
+        return unfiltered_commands
+
+        unfiltered_commands = State()
+        unfiltered_commands.twist.linear_x = desired_surge
+        unfiltered_commands.pose.pitch = theta_d
+        unfiltered_commands.pose.yaw = psi_d
+
+        filtered_commands = self.guidance_calculator.apply_reference_filter(
+            unfiltered_commands
+        )
+        return filtered_commands


There is something going on here, but idk what it is. Maybe comment?
btw there are two returns here 😄

Talhanc · 2025-02-05T21:51:16Z

guidance/guidance_los/scripts/los_guidance_action_server.py

+        msg.surge = commands.surge_vel
+        msg.pitch = commands.pitch
+        msg.yaw = commands.yaw
+        self.guidance_cmd_pub.publish(msg)


Surge_vel or u, you choose

Talhanc · 2025-02-05T21:52:56Z

guidance/guidance_los/scripts/los_guidance_action_server.py

+
+    try:
+        executor.spin()
+    except Exception as e:
+        action_server.get_logger().error(f'Error: {str(e)}')
+    finally:
+        action_server.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+


you can remove the try, if it dosent runt we start again 🤝

for more information, see https://pre-commit.ci

…texntnu/vortex-auv into 436-task-guidance-node-for-autopilot

…omorrow

for more information, see https://pre-commit.ci

AbubakarAliyuBadawi and others added 23 commits October 9, 2024 18:51

Modified guidance algorithim

917d5bf

Automated autoyapf fixes

cd4d076

added guidance action server

7845070

Resolved merge conflict in guidance.py

7157c5c

Automated autoyapf fixes

d7dd424

Remove unwanted folder

cb2e138

Merge remote-tracking branch 'origin/436-task-guidance-node-for-autop…

0e8585a

…ilot' into 436-task-guidance-node-for-autopilot

Fix errors of no executable

cd7f490

trying to resolve merge

29c1ad3

Automated autoyapf fixes

07261ee

Made changes to guidance_action_server and guidance_los files

1fb8b90

update guidance action server

d694e89

Automated autoyapf fixes

a007e80

Delete unnecessary files.

05ff163

modification of the guidance

f44e105

Fixed auto-pilot

9ca5b31

Automated autoyapf fixes

15fed69

intermediate commit

46455e6

Merge remote-tracking branch 'origin/485-task-auv-gui' into 436-task-…

9e7d7ba

…guidance-node-for-autopilot

feat: create guidance action server

fc4780a

Merge remote-tracking branch 'origin/develop' into 436-task-guidance-…

79a48b2

…node-for-autopilot

Merge remote-tracking branch 'origin/develop' into 436-task-guidance-…

acc38cb

…node-for-autopilot

AbubakarAliyuBadawi requested a review from Andeshog November 10, 2024 19:40

AbubakarAliyuBadawi self-assigned this Nov 10, 2024

AbubakarAliyuBadawi requested a review from kluge7 November 10, 2024 19:40

Andeshog requested changes Nov 10, 2024

View reviewed changes

chrstrom reviewed Nov 11, 2024

View reviewed changes

guidance/guidance_los/guidance_los/los_guidance_action_server.py Outdated Show resolved Hide resolved

kluge7 requested changes Nov 11, 2024

View reviewed changes

kluge7 linked an issue Nov 11, 2024 that may be closed by this pull request

[TASK] Guidance node for autopilot #436

Open

3 tasks

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from 7efa0ce to 9467bfc Compare January 15, 2025 15:23

Andeshog and others added 6 commits January 22, 2025 21:29

Merge branch '436-task-guidance-node-for-autopilot' of github.com:vor…

c0a40b1

…texntnu/vortex-auv into 436-task-guidance-node-for-autopilot

Merge remote-tracking branch 'origin/main' into 436-task-guidance-nod…

e8ab1f4

…e-for-autopilot

[pre-commit.ci] auto fixes from pre-commit.com hooks

1c3a504

for more information, see https://pre-commit.ci

💚 ci(workflows): removed deprecated workflow and updated .repos to us…

a210bea

…e default

integrate vortex utils

aa1fc7c

fixed unknown bug

6d23e15

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from d2ad008 to 6d23e15 Compare January 26, 2025 17:59

vortexntnu deleted a comment from Andeshog Jan 26, 2025

kluge7 force-pushed the main branch from c793a8a to 05d6431 Compare January 26, 2025 19:07

Andeshog reviewed Jan 28, 2025

View reviewed changes

Talhanc requested changes Feb 5, 2025

View reviewed changes

rectify vortex util

1b8e018

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from 6324465 to 1b8e018 Compare February 6, 2025 20:27

pre-commit-ci bot and others added 7 commits February 6, 2025 20:28

[pre-commit.ci] auto fixes from pre-commit.com hooks

a656d6c

for more information, see https://pre-commit.ci

reduce deviation error

68a021b

remove try and except

5ae12ee

Merge branch '436-task-guidance-node-for-autopilot' of github.com:vor…

bd9d5b4

…texntnu/vortex-auv into 436-task-guidance-node-for-autopilot

improve readme

c4d8530

added more equations in the readme

94775da

add picture in the readme---no-verify

0500bca

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from e821c91 to 0500bca Compare February 6, 2025 23:51

there is something wrong with waypoint switching, I will check that t…

3bf7576

…omorrow

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from 8c3b34d to 3bf7576 Compare February 7, 2025 00:04

AbubakarAliyuBadawi added 2 commits February 9, 2025 04:58

fixed some guidance equations

cf8cd89

fixed 2D los

76795e0

AbubakarAliyuBadawi force-pushed the 436-task-guidance-node-for-autopilot branch from 7a2e900 to 76795e0 Compare February 9, 2025 05:34

[pre-commit.ci] auto fixes from pre-commit.com hooks

6886022

for more information, see https://pre-commit.ci

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

436 task guidance node for autopilot #502

436 task guidance node for autopilot #502

AbubakarAliyuBadawi commented Nov 10, 2024

Andeshog left a comment

kluge7 left a comment

Andeshog Jan 28, 2025 •

edited

Loading

Talhanc left a comment

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

Talhanc Feb 5, 2025

AbubakarAliyuBadawi Feb 6, 2025

436 task guidance node for autopilot #502

Are you sure you want to change the base?

436 task guidance node for autopilot #502

Conversation

AbubakarAliyuBadawi commented Nov 10, 2024

Andeshog left a comment

Choose a reason for hiding this comment

kluge7 left a comment

Choose a reason for hiding this comment

Andeshog Jan 28, 2025 • edited Loading

Choose a reason for hiding this comment

Talhanc left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Andeshog Jan 28, 2025 •

edited

Loading