Skip to content

Show graphically direction where the robot is heading on displayed image #376

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Jun 10, 2022
Merged

Show graphically direction where the robot is heading on displayed image #376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a6023a7
Show graphically direction where the robot is heading on displayed image
sergiopaniego Jun 10, 2022
aa65c99
Show directon for keras brains
sergiopaniego Jun 10, 2022
0b6d0ba
Generalized function
sergiopaniego Jun 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 16 additions & 3 deletions behavior_metrics/brains/f1/brain_f1_keras_opencv_dataset.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,13 +67,23 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
print("- Models path: " + PRETRAINED_MODELS)
print("- Model: " + str(model))

def update_frame(self, frame_id, data):
def update_frame(self, frame_id, data, angular_speed=None):
"""Update the information to be shown in one of the GUI's frames.

Arguments:
frame_id {str} -- Id of the frame that will represent the data
data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
"""
if angular_speed:
import math
x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0] # ancho, alto
length = 200
angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
x2 = int(x1 - length * math.cos(angle))
y2 = int(y1 - length * math.sin(angle))

line_thickness = 2
cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
self.handler.update_frame(frame_id, data)

def execute(self):
Expand All @@ -83,12 +93,13 @@ def execute(self):

image = self.camera.getImage().data
# image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
base_image = image

if self.cont == 1:
self.first_image = image

image = self.handler.transform_image(image, self.config['ImageTranform'])
self.update_frame('frame_0', image)
#self.update_frame('frame_0', image)

try:
if self.config['ImageCropped']:
Expand Down Expand Up @@ -129,6 +140,8 @@ def execute(self):
self.previous_v = prediction[0][0]
self.previous_w = prediction[0][1]

self.update_frame('frame_0', base_image, prediction_w)

# GradCAM from image
i = np.argmax(prediction[0])
cam = GradCAM(self.net, i)
Expand All @@ -138,4 +151,4 @@ def execute(self):
self.update_frame('frame_1', output)

except Exception as err:
print(err)
print(err)
28 changes: 20 additions & 8 deletions behavior_metrics/brains/f1/brain_f1_keras_seq_3_opencv_dataset.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
self.third_image = []

if self.config['GPU'] is False:
os.environ["CUDA_VISIBLE_DEVICES"]="-1"
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"

self.gpu_inference = True if tf.test.gpu_device_name() else False

Expand All @@ -72,13 +72,23 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
print("- Models path: " + PRETRAINED_MODELS)
print("- Model: " + str(model))

def update_frame(self, frame_id, data):
def update_frame(self, frame_id, data, angular_speed=None):
"""Update the information to be shown in one of the GUI's frames.

Arguments:
frame_id {str} -- Id of the frame that will represent the data
data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
"""
if angular_speed:
import math
x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0] # ancho, alto
length = 200
angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
x2 = int(x1 - length * math.cos(angle))
y2 = int(y1 - length * math.sin(angle))

line_thickness = 2
cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
self.handler.update_frame(frame_id, data)

def check_center(self, position_x):
Expand Down Expand Up @@ -115,17 +125,18 @@ def execute(self):
'''

image = self.camera.getImage().data
base_image = image
if self.cont == 1:
self.first_image = image
image = self.handler.transform_image(image,self.config['ImageTranform'])
image = self.handler.transform_image(image, self.config['ImageTranform'])
try:
if self.config['ImageCropped']:
image = image[240:480, 0:640]
if 'ImageSize' in self.config:
img = cv2.resize(image, (self.config['ImageSize'][0], self.config['ImageSize'][1]))
else:
img = image
self.update_frame('frame_0', img)
# sself.update_frame('frame_0', img)
if self.config['ImageNormalized']:
AUGMENTATIONS_TEST = Compose([
Normalize()
Expand Down Expand Up @@ -170,21 +181,22 @@ def execute(self):
start_time = time.time()
prediction = self.net.predict(img)
self.inference_times.append(time.time() - start_time)
#prediction = prediction[0]
if self.config['PredictionsNormalized']:
prediction_v = prediction[0][0]*(24 - (6.5)) + (6.5)
prediction_w = prediction[0][1]*(7.1 - (-7.1)) + (-7.1)
prediction_v = prediction[0][0] * (24 - (6.5)) + (6.5)
prediction_w = prediction[0][1] * (7.1 - (-7.1)) + (-7.1)
else:
prediction_v = prediction[0][0]
prediction_w = prediction[0][1]
if prediction_w != '' and prediction_w != '':
self.motors.sendV(prediction_v)
self.motors.sendW(prediction_w)

self.update_frame('frame_0', base_image, prediction_w)

if self.previous_v != None:
a = np.array((prediction[0][0], prediction[0][1]))
b = np.array((self.previous_v, self.previous_w))
distance = np.linalg.norm(a-b)
distance = np.linalg.norm(a - b)
self.suddenness_distance.append(distance)
self.previous_v = prediction[0][0]
self.previous_w = prediction[0][1]
Expand Down
29 changes: 22 additions & 7 deletions behavior_metrics/brains/f1/brain_f1_opencv.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ def __init__(self, sensors, actuators, handler, config=None):
self.cont = 0
self.iteration = 0

#self.previous_timestamp = 0
#self.previous_image = 0
# self.previous_timestamp = 0
# self.previous_image = 0

self.previous_v = None
self.previous_w = None
Expand All @@ -66,7 +66,23 @@ def __init__(self, sensors, actuators, handler, config=None):
'''
time.sleep(2)

def update_frame(self, frame_id, data):
def update_frame(self, frame_id, data, angular_speed=None):
"""Update the information to be shown in one of the GUI's frames.

Arguments:
frame_id {str} -- Id of the frame that will represent the data
data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
"""
if angular_speed:
import math
x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0] # ancho, alto
length = 200
angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
x2 = int(x1 - length * math.cos(angle))
y2 = int(y1 - length * math.sin(angle))

line_thickness = 2
cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
self.handler.update_frame(frame_id, data)

def collinear3(self, x1, y1, x2, y2, x3, y3):
Expand Down Expand Up @@ -115,8 +131,8 @@ def execute(self):
self.previous_timestamp = timestamp
if (timestamp - self.previous_timestamp >= 0.085):
self.previous_image = self.camera.getImage().data
image = self.previous_image
'''
#image = self.previous_image

image = self.camera.getImage().data
if image.shape == (3, 3, 3):
Expand Down Expand Up @@ -144,9 +160,6 @@ def execute(self):
image_mask = cv2.inRange(image_hsv, red_lower, red_upper)
# image_eroded = cv2.erode(image_mask, kernel, iterations=3)

# show image in gui -> frame_0
self.update_frame('frame_0', image)

rows, cols = image_mask.shape
rows = rows - 1 # para evitar desbordamiento

Expand Down Expand Up @@ -202,6 +215,8 @@ def execute(self):
self.motors.sendW(w)
self.motors.sendV(v)

self.update_frame('frame_0', image, w)

v = np.interp(np.array([v]), (6.5, 24), (0, 1))[0]
w = np.interp(np.array([w]), (-7.1, 7.1), (0, 1))[0]
if self.previous_v != None:
Expand Down