Skip to content

Commit

Permalink
Merge pull request #99 from AutomecUA/dev
Browse files Browse the repository at this point in the history
Bug fixes
  • Loading branch information
manuelgitgomes authored Nov 2, 2021
2 parents 657e8d9 + 6e583a7 commit 446e298
Show file tree
Hide file tree
Showing 11 changed files with 79 additions and 87 deletions.
2 changes: 1 addition & 1 deletion physical_bringup/launch/modules/driving_cnn1.launch
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<arg name="twist_cmd_topic" default="/android_input_dir" />
<arg name="vel_cmd_topic" default="android_input_vel" />
<arg name="model" default="" />
<arg name="signal_cmd_topic" default="/signal_vel"/>
<arg name="signal_cmd_topic" default=""/>

<include file="$(find cnn)/launch/driving1.launch">
<arg name="image_raw_topic" value="$(arg image_raw_topic)" />
Expand Down
2 changes: 1 addition & 1 deletion physical_bringup/launch/modules/driving_cnn2a.launch
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<arg name="twist_cmd_topic" default="/android_input_dir" />
<arg name="vel_cmd_topic" default="android_input_vel" />
<arg name="model" default="" />
<arg name="signal_cmd_topic" default="/signal_vel"/>
<arg name="signal_cmd_topic" default=""/>

<include file="$(find cnn)/launch/driving2a.launch">
<arg name="image_raw_topic" value="$(arg image_raw_topic)" />
Expand Down
Original file line number Diff line number Diff line change
@@ -1,16 +1,10 @@
<launch>
<arg name="image_raw_topic" default="/real_camera"/>
<arg name="vel_cmd_topic" default="/android_input_vel"/>
<arg name="twist_linear_x" default="1"/>
<arg name="signal_cmd_topic" default="/signal_vel"/>
<arg name="twist_cmd_topic" default=""/>

<include file="$(find signal_recognition)/launch/signal_panel_recognition.launch">
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
<arg name="twist_cmd_topic" value="$(arg twist_cmd_topic)"/>
<arg name="image_raw_topic" value="$(arg image_raw_topic)"/>
<arg name="signal_cmd_topic" value="$(arg signal_cmd_topic)"/>
<arg name="twist_linear_x" value="$(arg twist_linear_x)"/>
</include>

</launch>
17 changes: 17 additions & 0 deletions physical_bringup/launch/signal_usbcam.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<launch>
<arg name="camera_topic" default="/real_camera"/>
<arg name="int_camera_id" default="2"/>
<arg name="signal_cmd_topic" default="/signal_vel"/>

<include file="$(find physical_bringup)/launch/modules/signal_panel_recognition.launch">
<arg name="signal_cmd_topic" value="$(arg signal_cmd_topic)"/>
<arg name="image_raw_topic" value="$(arg camera_topic)"/>
</include>

<include file="$(find physical_bringup)/launch/modules/lane_camera.launch">
<arg name="camera_topic" value="$(arg camera_topic)"/>
<arg name="int_camera_id" value="$(arg int_camera_id)"/>
</include>


</launch>
4 changes: 0 additions & 4 deletions physical_bringup/launch/traxxas_drive1.launch
Original file line number Diff line number Diff line change
@@ -1,14 +1,11 @@
<launch>

<arg name="model" default="" />
<arg name="twist_linear_x" default="1" />

<arg name="twist_dir_topic" default="/android_input_dir"/>
<arg name="int_dir_topic" default="/pub_dir"/>
<arg name="int_vel_topic" default="/pub_vel"/>
<arg name="int_vel_max" default="108"/>
<arg name="vel_cmd_topic" default="/android_input_vel"/>

<arg name="camera_topic" default="/real_camera"/>
<arg name="int_camera_id" default="2"/>

Expand All @@ -18,7 +15,6 @@

<include file="$(find physical_bringup)/launch/modules/driving_cnn1.launch" >
<arg name="model" value="$(arg model)"/>
<arg name="twist_linear_x" value="$(arg twist_linear_x)"/>
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
</include>

Expand Down
5 changes: 0 additions & 5 deletions physical_bringup/launch/traxxas_drive2a.launch
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
<launch>

<arg name="model" default="" />
<arg name="float_cmd_topic" default="" />
<arg name="twist_linear_x" default="1" />

<arg name="twist_dir_topic" default="/android_input_dir"/>
<arg name="int_dir_topic" default="/pub_dir"/>
<arg name="int_vel_topic" default="/pub_vel"/>
Expand All @@ -19,8 +16,6 @@

<include file="$(find physical_bringup)/launch/modules/driving_cnn2a.launch" >
<arg name="model" value="$(arg model)"/>
<arg name="float_cmd_topic" value="$(arg float_cmd_topic)"/>
<arg name="twist_linear_x" value="$(arg twist_linear_x)"/>
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
</include>

Expand Down
5 changes: 0 additions & 5 deletions physical_bringup/launch/traxxas_drive2a_kinect.launch
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
<launch>

<arg name="model" default="" />
<arg name="float_cmd_topic" default="" />
<arg name="twist_linear_x" default="1" />

<arg name="twist_dir_topic" default="/android_input_dir"/>
<arg name="int_dir_topic" default="/pub_dir"/>
<arg name="int_vel_topic" default="/pub_vel"/>
Expand All @@ -18,8 +15,6 @@

<include file="$(find physical_bringup)/launch/modules/driving_cnn2a.launch" >
<arg name="model" value="$(arg model)"/>
<arg name="float_cmd_topic" value="$(arg float_cmd_topic)"/>
<arg name="twist_linear_x" value="$(arg twist_linear_x)"/>
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
<arg name="image_raw_topic" value="$(arg image_raw_topic)"/>
</include>
Expand Down
1 change: 0 additions & 1 deletion physical_bringup/launch/traxxas_drive_signal1.launch
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@
</include>

<include file="$(find physical_bringup)/launch/modules/signal_panel_recognition.launch">
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
<arg name="signal_cmd_topic" value="$(arg signal_cmd_topic)"/>
<arg name="image_raw_topic" value="$(arg camera_topic)"/>
</include>
Expand Down
1 change: 0 additions & 1 deletion physical_bringup/launch/traxxas_drive_signal2a.launch
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@
</include>

<include file="$(find physical_bringup)/launch/modules/signal_panel_recognition.launch">
<arg name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
<arg name="signal_cmd_topic" value="$(arg signal_cmd_topic)"/>
<arg name="image_raw_topic" value="$(arg camera_topic)"/>
</include>
Expand Down
6 changes: 0 additions & 6 deletions signal_recognition/launch/signal_panel_recognition.launch
Original file line number Diff line number Diff line change
@@ -1,16 +1,10 @@
<launch>
<arg name="image_raw_topic" default="/real_camera"/>
<arg name="vel_cmd_topic" default="/android_input_vel"/>
<arg name="twist_cmd_topic" default="/vel_cmd"/>
<arg name="signal_cmd_topic" default="/signal_vel"/>
<arg name="twist_linear_x" default="1"/>

<node name="signal_panel_recognition" pkg="signal_recognition" type="signal_panel_recognition.py" output="screen">
<param name="vel_cmd_topic" value="$(arg vel_cmd_topic)"/>
<param name="image_raw_topic" value="$(arg image_raw_topic)"/>
<param name="twist_cmd_topic" value="$(arg twist_cmd_topic)"/>
<param name="signal_cmd_topic" value="$(arg signal_cmd_topic)"/>
<param name="twist_linear_x" value="$(arg twist_linear_x)"/>
</node>

</launch>
117 changes: 60 additions & 57 deletions signal_recognition/scripts/signal_panel_recognition.py
Original file line number Diff line number Diff line change
@@ -1,35 +1,24 @@
#!/usr/bin/env python3

# Imports
#import argparse
import cv2
from csv import writer
#import copy
import numpy as np
import rospy
from std_msgs.msg import Bool
from geometry_msgs.msg._Twist import Twist
from sensor_msgs.msg._Image import Image
from cv_bridge.core import CvBridge
from datetime import datetime
#from tensorflow.keras.models import load_model
import pathlib
#import os
#import string
from datetime import datetime
import pandas as pd
import signal
import sys
import os


global img_rbg
global bridge
global begin_img

# not used by now
def preProcess(img):
img = cv2.cvtColor(img, cv2.COLOR_RGB2YUV)
img = cv2.GaussianBlur(img, (3, 3), 0)
img = cv2.resize(img, (200, 66))
img = img/255

return img


def message_RGB_ReceivedCallback(message):
global img_rbg
Expand All @@ -41,14 +30,25 @@ def message_RGB_ReceivedCallback(message):
begin_img = True


def main():
def signal_handler(sig, frame):
global signal_log
global log_path

rospy.loginfo('You pressed Ctrl+C!')
curr_time = datetime.now()
time_str = str(curr_time.year) + '_' + str(curr_time.month) + '_' + str(curr_time.day) + '__' + str(
curr_time.hour) + '_' + str(curr_time.minute)
signal_log.to_csv(log_path + '/signal_log_' + time_str + '.csv', mode='a', index=False, header=False)
sys.exit(0)

def main():
global signal_log
global log_path
# PARAMETERS__________________________________________________________________

# Import Parameters
scale_import = 0.1 # The scale of the first image, related to the imported one.
N_red = 2 # Number of piramidizations to apply to each image.
factor_red = 0.8

# Font Parameters
subtitle_offset = -10
Expand All @@ -62,13 +62,9 @@ def main():
line_thickness = 3

# Detection Parameters

scale_cap = 0.4
detection_threshold = 0.85

# Initial velocity
vel = 0

# ______________________________________________________________________________

# Images to import and Images Info
Expand All @@ -90,25 +86,36 @@ def main():
global begin_img
begin_img = False
velbool = False
count_stop = 0
count_start = 0
count_max = 5

#twist = Twist()

# Init Node
rospy.init_node('ml_driving', anonymous=False)

# Get parameters
image_raw_topic = rospy.get_param('~image_raw_topic', '/ackermann_vehicle/camera2/rgb/image_raw')
twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel')
image_raw_topic = rospy.get_param('~image_raw_topic', '/ackermann_vehicle/camera2/rgb/image_raw')
signal_cmd_topic = rospy.get_param('~signal_cmd_topic', '/signal_vel')
twist_linear_x = rospy.get_param('~twist_linear_x', 0.5)

# Create publishers
#if twist_cmd_topic!= "":
#pubtwist = rospy.Publisher(twist_cmd_topic, Twist, queue_size=10)

#if vel_cmd_topic!= "":
pubbool = rospy.Publisher(signal_cmd_topic, Bool, queue_size=10)

# Define path for .csv
s = str(pathlib.Path(__file__).parent.absolute())
log_path = s + '/log/'
rospy.loginfo(log_path)

# If the path does not exist, create it
if not os.path.exists(log_path):
os.makedirs(log_path)

# Create pandas dataframe
signal_log = pd.DataFrame(columns=['Time', 'Signal', 'Resolution'])

# set handler on termination
signal.signal(signal.SIGINT, signal_handler)

# ______________________________________________________________________________

path = str(pathlib.Path(__file__).parent.absolute())
Expand Down Expand Up @@ -205,7 +212,6 @@ def main():
for name in dict_images.keys():
for key in dict_images[name]['images']:
dict_images[name]['images'][key] = cv2.GaussianBlur(dict_images[name]['images'][key], (3, 3), 0)
#cv2.imshow(name + ' ' + key, dict_images[name]['images'][key])

# ______________________________________________________________________________

Expand All @@ -218,18 +224,15 @@ def main():
Image, message_RGB_ReceivedCallback)


rate = rospy.Rate(10)
rate = rospy.Rate(30)

while not rospy.is_shutdown():

if begin_img == False:
continue

#resized_ = preProcess(img_rbg)

width_frame = img_rbg.shape[1]
height_frame = img_rbg.shape[0]
#default_dim = (width_frame, height_frame)
reduced_dim = (int(width_frame * scale_cap), int(height_frame * scale_cap))
frame = cv2.resize(img_rbg, reduced_dim)

Expand All @@ -244,11 +247,6 @@ def main():
max_key = ''


##cv2.imshow('Robot View Processed', resized_)
#cv2.imshow('Robot View', img_rbg)
#cv2.imshow('Robot View used', frame)
#cv2.waitKey(1)

# For each image:
for name in dict_images.keys():
for key in dict_images[name]['images']:
Expand All @@ -264,22 +262,29 @@ def main():
max_name = name
max_key = key

#print (max_res, max_key)

# Write log files
curr_time = datetime.now()
time_str = str(curr_time.year) + '_' + str(curr_time.month) + '_' + str(curr_time.day) + '__' + str(
curr_time.hour) + '_' + str(curr_time.minute) + '_' + str(curr_time.second) + '__' + str(
curr_time.microsecond)
# add image, angle and velocity to the signal_log pandas
max_res_round = round(max_res, 3)
print(max_res_round)
row = pd.DataFrame([[time_str, max_name, max_res_round]], columns=['Time', 'Signal', 'Resolution'])
signal_log = signal_log.append(row, ignore_index=True)

if max_res > detection_threshold:

max_width = int(dict_images[max_name]['images'][max_key].shape[1] / scale_cap)
max_height = int(dict_images[max_name]['images'][max_key].shape[0] / scale_cap)
max_dim = (max_width, max_height)

for pt in zip(*max_loc[::-1]):
pt = tuple(int(pti / scale_cap) for pti in pt)
cv2.rectangle(frame, pt, (pt[0] + max_width, pt[1] + max_height),
dict_colors.get(dict_images[max_name]['color']), line_thickness)
text = 'Detected: ' + max_name + ' ' + max_key + ' > ' + dict_images[max_name]['type'] + ': ' + \
dict_images[max_name]['title']

#print(text)

origin = (pt[0], pt[1] + subtitle_offset)
origin_2 = (0, height_frame + subtitle_2_offset)
Expand All @@ -292,21 +297,19 @@ def main():
# Defining and publishing the velocity of the car in regards to the signal seen
if max_name == "pForward":
velbool = True
count_start = count_start + 1
count_stop = 0
elif max_name == "pStop":
velbool = False
count_stop = count_stop + 1
count_start = 0

if count_stop >= count_max or count_start >= count_max:
pubbool.publish(velbool)

# Send twist or bool
#if twist_cmd_topic != "":
# twist.linear.x = vel
# twist.linear.y = 0
# twist.linear.z = 0
# twist.angular.x = 0
# twist.angular.y = 0
# twist.angular.z = 0
# pubtwist.publish(twist)

#if vel_cmd_topic != "":
pubbool.publish(velbool)
else:
count_stop = 0
count_start = 0

# Show image
cv2.imshow("Frame", img_rbg)
Expand Down

0 comments on commit 446e298

Please sign in to comment.