Skip to content

Commit

Permalink
pre-commit changes
Browse files Browse the repository at this point in the history
  • Loading branch information
rakeshv24 committed Aug 23, 2024
1 parent 22621e1 commit a2f83e3
Show file tree
Hide file tree
Showing 3 changed files with 28 additions and 25 deletions.
29 changes: 15 additions & 14 deletions docking_control/src/actag_code.py
Original file line number Diff line number Diff line change
@@ -1,25 +1,26 @@
#!/usr/bin/env python3.12

from actag import AcTag

# import rospy


class AcTagDetection:
def __init__(self):
# Initialize the AcTag Detector
self.detector = AcTag(
min_range=0.1,
max_range=1.5,
horizontal_aperture=1.0472,
tag_family="AcTag24h10",
tag_size=0.130628571428644,
quads_use_same_random_vals=True,
)
def __init__(self):
# Initialize the AcTag Detector
self.detector = AcTag(
min_range=0.1,
max_range=1.5,
horizontal_aperture=1.0472,
tag_family="AcTag24h10",
tag_size=0.130628571428644,
quads_use_same_random_vals=True,
)

def detect_tags(self, sonar_image):
# Detect AcTags in the sonar image
detected_tags = self.detector.detect_tags(sonar_image)
return detected_tags
def detect_tags(self, sonar_image):
# Detect AcTags in the sonar image
detected_tags = self.detector.detect_tags(sonar_image)
return detected_tags


# if __name__ == "__main__":
Expand Down
14 changes: 7 additions & 7 deletions docking_control/src/actag_detect.py
Original file line number Diff line number Diff line change
@@ -1,13 +1,15 @@
#!/usr/bin/env python3.10

import yaml

# from actag_code import AcTagDetection
from actag import AcTag
import rospy
import os
import cv2
import math
import numpy as np

# import video as video
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
Expand All @@ -23,7 +25,6 @@
# Helper function that parses AcTag detection data
# and prints the results
def parse_and_print_results(detected_tags):

num_detected_tags = len(detected_tags)
print("Number of Detected Tags: ", num_detected_tags)

Expand All @@ -37,32 +38,31 @@ def parse_and_print_results(detected_tags):

# This helper function draws the detected tags onto the image
def visualize_decoded_tags(my_sonar_image, detected_tags):

output_image = cv2.cvtColor(my_sonar_image, cv2.COLOR_GRAY2RGB)
for detected_tag in detected_tags:
# Extract corner points
corner_locs = detected_tag[1]
ptA = (corner_locs[0][0], corner_locs[0][1])
ptB = (corner_locs[1][0], corner_locs[1][1])
ptC = (corner_locs[2][0], corner_locs[2][1])
ptD = (corner_locs[3][0], corner_locs[3][1])
pdf = (corner_locs[3][0], corner_locs[3][1])

# Reverse x and y to get the correct orientation with cv2.imshow()
ptA = (ptA[1], ptA[0])
ptB = (ptB[1], ptB[0])
ptC = (ptC[1], ptC[0])
ptD = (ptD[1], ptD[0])
pdf = (pdf[1], pdf[0])

# Draw the bounding box of the AcTag Square
color = (0, 255, 0)
cv2.line(output_image, ptA, ptB, color, 1)
cv2.putText(output_image, "1", ptA, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
cv2.line(output_image, ptB, ptC, color, 1)
cv2.putText(output_image, "2", ptB, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
cv2.line(output_image, ptC, ptD, color, 1)
cv2.line(output_image, ptC, pdf, color, 1)
cv2.putText(output_image, "3", ptC, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
cv2.line(output_image, ptD, ptA, color, 1)
cv2.putText(output_image, "4", ptD, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
cv2.line(output_image, pdf, ptA, color, 1)
cv2.putText(output_image, "4", pdf, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)

# Put the Tag ID in the center
center = (int((ptA[0] + ptC[0]) / 2), int((ptA[1] + ptC[1]) / 2))
Expand Down
10 changes: 6 additions & 4 deletions docking_control/utils/compute_hydrodynamics.py
Original file line number Diff line number Diff line change
@@ -1,16 +1,18 @@
import capytaine as cpt
import os
from numpy import inf


cwd = os.path.dirname(__file__)
cpt.set_logging('INFO')
cpt.set_logging("INFO")
mesh_filepath = cwd + "/../model/bluerov2_custom/bluerov2_custom.stl"
mesh = cpt.load_mesh(mesh_filepath, file_format='stl')
mesh = cpt.load_mesh(mesh_filepath, file_format="stl")

# rov = cpt.FloatingBody(mesh=mesh)
# print(rov.dofs.keys())
# problem = cpt.RadiationProblem(body=rov, omega=inf, free_surface=inf, radiating_dof="Heave")
# problem = cpt.RadiationProblem(body=rov,
# omega=inf,
# free_surface=inf,
# radiating_dof="Heave")

# solver = cpt.BEMSolver()
# result = solver.solve(problem)
Expand Down

0 comments on commit a2f83e3

Please sign in to comment.