Difference between revisions of "2021FallTeam3"

From MAE/ECE 148 - Introduction to Autonomous Vehicles
Jump to navigation Jump to search
Line 250: Line 250:


=== Donkey Car Deep Learning Autonomous Laps ===
=== Donkey Car Deep Learning Autonomous Laps ===
https://youtu.be/Ar8LHa24EHA
{{#evu:https://youtu.be/Ar8LHa24EHA
|center
}}


=== ROS Autonomous Laps ===
=== ROS Autonomous Laps ===


=== EMO ===
=== EMO ===

Revision as of 20:47, 10 December 2021

Team Members

  • Han Zhao(ECE)
  • Zhetai Zhou(ECE)
  • Felix Koch(MAE)

Project Overview

The objective is to measure another car's speed using a second stationary jetson nano with two webcams plugged into it. Those two cameras will be set up at a given distance (e.g. 3m) and be trained using AI to give a signal when detecting a car passing. With this data and the time we can calculate the velocity and compare it to our speed limit. If the car turns out to be speeding our parked police car will turn on some LED lights and join the track to chase the traffic law violator.

Demo.png

Must Haves

  • Two webcams
  • Two jetson nanos(One can be replaced by a virtual machine)
  • Another RC car for speed trap

Nice to Haves

  • Two lidars which can be used to replace two webcams to measure the speed

Project Video

Project Presentation

Hardware

Our car is pretty basic with one jetson nano, one lidar, one camera, one PWM board to control the throttle and steering, and one dc/dc converter to supply the power.

Cars.jpg

Besides, the hardware provided by the professor, we also buy and solder a speaking to play the sound on our robocar.

Speaker.jpg

Mechanical Design

The major components of the mechanical design include the baseplate, camera mount, and Jetson Nano case.

Baseplate

Camera Mount

Jetson Nano Case

Electrical Design

Schemetics.png

Code

Code For Speed Trap

# OpenCV Python program to detect cars in video frame
# import libraries of python OpenCV
import socket
import cv2
import time
# capture frames from a video
first = False 
second = False
start = 0
end = 0
#First Camera
cap1 = cv2.VideoCapture(0) 
#Second Camera 
cap2 = cv2.VideoCapture(1)
#threshold for first Camera
threshold1 = 7
#Threshold for second Camera
threshold2 = 2
#Distance between two cameras
distance = 2.2
idle = 20
v = 0
speed = False
current = 0
stop = 0
sleep_time = 5
#Speed Limit
speed_limit = 2
#Background for first camera 
background1 = None
#Background for second camera
background2 = None
scale1 = None
scale2 = None
#create a server
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(("192.168.33.74",23878))
s.listen(5)
data = "0"
old_data ="0"
# loop runs if capturing has been initialized.
while True:
  #check connection
  clientsocket, address = s.accept()
  print(f"Connection from {address} has been establish.") 
  while True:
      # reads frames from a video
      ret1, frames1 = cap1.read()
      ret2,frames2 = cap2.read()
      # convert to gray scale of each frames
      gray1 = cv2.cvtColor(frames1, cv2.COLOR_BGR2GRAY)
      gray2 = cv2.cvtColor(frames2, cv2.COLOR_BGR2GRAY)
      if background1 is None:
        background1 = gray1
      if background2 is None:
        background2 = gray2
      substraction1 = cv2.absdiff(background1, gray1)
      substraction2 = cv2.absdiff(background2,gray2)
      sum1 = cv2.sumElems(substraction1)
      sum2 = cv2.sumElems(substraction2)
      if scale1 is None or scale1 == 0:
        scale1 = sum1[0]
      if scale2 is None or scale2 == 0:
        scale2 = sum2[0]
      if scale1 != 0:
        #if the change is greater than the threshould, set the change be true
        if(sum1[0]/scale1 > threshold1 and not first):
          first = True
          #time starts 
          start = time.time()
          print(sum1[0] /scale1)
          print("Car passing in first")
      if scale2 != 0:
        if(sum2[0]/scale2 > threshold2 and not second and first):
          second = True
          #time ends
          end = time.time()
          print(sum2[0] /scale2)
          print("Car passing in second")
      current = time.time()
      #print the speed of the car
      if(first and second and not speed):
        print("speed:")
        v = round(distance/(end-start),2)
        print(str(v) + "m/s")
        speed = True
        stop = time.time()
      
      if(stop != 0 and (current - stop) > sleep_time):
        first = False
        second = False
        stop = 0
        speed = False
      #send the signal to client
      if(old_data != data):
        clientsocket.send(bytes(data,"utf-8"))
        old_data = data
      if(v > speed_limit):
        data = "1"
      time.sleep(0.05)
      cv2.waitKey(1)

cv2.destroyAllWindows()


Code For Robocar

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32, Int32, Int32MultiArray
import socket

LANE_GUIDANCE_NODE_NAME = 'lane_guidance_node'
STEERING_TOPIC_NAME = '/steering'
THROTTLE_TOPIC_NAME = '/throttle'
CENTROID_TOPIC_NAME = '/centroid'
HOST = "<The IP address of Host Machine>"
PORT = <The port number you want>

class PathPlanner:
     def __init__(self):
        # Initialize node and create publishers/subscribers
        self.init_node = rospy.init_node(LANE_GUIDANCE_NODE_NAME, anonymous=False)
        self.camera_subscriber = rospy.Subscriber(CAMERA_TOPIC_NAME, Image, self.live_calibration_values)
        self.steering_publisher = rospy.Publisher(STEERING_TOPIC_NAME, Float32, queue_size=1)
        self.throttle_publisher = rospy.Publisher(THROTTLE_TOPIC_NAME, Float32, queue_size=1)
        self.steering_float = Float32()
        self.throttle_float = Float32()
        self.centroid_subscriber = rospy.Subscriber(CENTROID_TOPIC_NAME, Float32, self.controller)
        self.s = socket.socket(socket.AF_INET, socket.Sock_STREAM)
        self.s.connect((HOST,PORT)) #connect with the host machine
        # Getting ROS parameters set from calibration Node
        self.steering_sensitivity = rospy.get_param('steering_sensitivity')
        self.no_error_throttle = rospy.get_param('no_error_throttle')
        self.error_throttle = rospy.get_param('error_throttle')
        self.error_threshold = rospy.get_param('error_threshold')
        self.zero_throttle = rospy.get_param('zero_throttle')
        self.signal = 0
        # Display Parameters
        rospy.loginfo(
            f'\nsteering_sensitivity: {self.steering_sensitivity}'
            f'\nno_error_throttle: {self.no_error_throttle}'
            f'\nerror_throttle: {self.error_throttle}'
            f'\nerror_threshold: {self.error_threshold}')

    def controller(msg):
        try:
            #recv the msg from host machine, if the msg is 1 start the car
            msg = self.s.recv(1)
            msg = msg.decode("utf-8")
            if msg == "1":
                self.signal = 1
                self.s.close()
            kp = self.steering_sensitivity
            error_x = data.data
            self.get_logger().info(f"{error_x}")
            if error_x <= self.error_threshold:
                throttle_float = self.no_error_throttle
            else:
                throttle_float = self.error_throttle
            steering_float = float(kp * error_x)
            if steering_float < -1.0:
                steering_float = -1.0
            elif steering_float > 1.0:
                steering_float = 1.0
            else:
                pass
            self.steering_float.data = steering_float
            self.throttle_float.data = throttle_float
            self.steering_publisher.publish(self.steering_float)
            self.throttle_publisher.publish(self.throttle_float)
        except KeyboardInterrupt:
            self.throttle_publisher.data = self.zero_throttle
            self.throttle_publisher.publish(self.throttle_float)

def main():
    path_planner = PathPlanner()
    rate = rospy.Rate(15)
    while not rospy.is_shutdown():
        rospy.spin()
        rate.sleep()

if __name__ == '__main__':
    main()

Code for play sound

To play sound, we use the playsound module in python.(p.s if you do not have playsound in your machine, you can install in by running the command "pip install playsound") Besides, we also use the threading in python to make the sound playing in background so we can run the program while playing the sound.

from playground import playsound
import trheading
threading.Thread(target=playsound, args=('police_siren.mp3',),daemon=True).start() 

Demonstration

Donkey Car Deep Learning Autonomous Laps

https://youtu.be/Ar8LHa24EHA {{#evu:https://youtu.be/Ar8LHa24EHA |center }}

ROS Autonomous Laps

EMO