# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
import rospy
from cv_bridge import CvBridge

class LaneDetection:
    def __init__(self):
        # Points for Bird Eye View Transformation
        # Upper Left, Upper Right, Lower Left, Lower Right Point
        self.pts1 = np.float32([[140, 355], [502, 355], [0, 460], [640, 460]])
        self.pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])

        self.low_yellow = np.array([0, 100, 30])  # 
        self.high_yellow = np.array([100, 230, 255])  # 

        #self.low_white = np.array([0, 130, 0]) # Lower HLS threshold for Detect White Line
        #self.high_white = np.array([255, 255, 255]) # Higher HLS threshold for Detect White Line

        # Start Image Processing
        rospy.init_node("steering_node") # start node
        self.cvbridge = CvBridge() # CvBridge for transformations between cv image and ros image
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.convertImg)
        self.steering_pub = rospy.Publisher("/wecar/steer", Float32, queue_size=5)
        rospy.spin()

    def convertImg(self, img):
        frame = self.cvbridge.imgmsg_to_cv2(img, "bgr8") # Convert ROS Image to Cv2 Image
        frame = cv2.resize(frame, dsize=(640, 480), interpolation=cv2.INTER_AREA) # Resize Image
        bird_eye_image = self.bird_eye(frame) # Bird Eye View Transformation
        colored_image = self.color_detect(bird_eye_image) # 
        self.sliding_list_left = self.sliding_left(colored_image)
        self.sliding_list_right = self.sliding_right(colored_image)
        self.steering(frame, self.sliding_list_left, self.sliding_list_right)
        self.visualize(frame, bird_eye_image, colored_image)

    def bird_eye(self, frame):
        matrix = cv2.getPerspectiveTransform(self.pts1, self.pts2)
        result = cv2.warpPerspective(frame, matrix, (640, 480))
        return result

    def color_detect(self, img):
        hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)

        ''' Detection White Lineb '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        #return mask_white
        
        ''' Detection Yellow Line '''
        mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        return mask_yellow

        ''' Detection White & Yellow Line '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        # mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        # mask = cv2.bitwise_or(mask_white, mask_yellow)
        # return mask

    def sliding_left(self, img):
        left_list = []
        '''
        row: starting from y=179 to y=460, moving by 40
        col: starting from x=19 to y=300, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(19, int(img.shape[1]/2) - 20, 5):
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = [] 
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        left_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return left_list
    
    def sliding_right(self, img):
        right_list = []
        '''
        row: starting from x=179 pts1 = np.float32([[100, 240], [540, 240], [0, 380], [640, 380]]) #for driving
        pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])to y=560, moving by 40
        col: starting from x=320 to y=620, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(int(img.shape[1]/2), img.shape[1] - 20, 5): 
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = []
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        right_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return right_list
    
    def steering(self, frame, sliding_list_left, sliding_list_right):
        x_left = []
        x_right = []
        image_width = 320
        if len(sliding_list_left) == 0:
            left_distance = 0
        else:
            left_distance = image_width - sliding_list_left[0][1] #should be positive

        if len(sliding_list_right) == 0:
            right_distance = 0
        else:
            right_distance = image_width - sliding_list_right[0][1] #should be negative

        for i in range(0, len(sliding_list_left)): 
            x_left.append(sliding_list_left[i][0])
        
        left_diff_arr = np.diff(x_left)
        divider = len(left_diff_arr)
        if divider == 0:
            divider = 1
        left_diff_sum = np.sum(left_diff_arr)
        left_avg = int(left_diff_sum/divider)


        #remove inappropriate cases 
        for i in range(0, len(left_diff_arr)): 
            if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
                self.sliding_list_left = []
                left_diff_sum = 0

        #extract x coordinates from sliding_list_right
        for i in range(0, len(sliding_list_right)): 
            x_right.append(sliding_list_right[i][0])
            
        right_diff_arr = np.diff(x_right)
        divider = len(right_diff_arr)
        if divider == 0:
            divider = 1
        right_diff_sum = np.sum(right_diff_arr)
        right_avg = int(right_diff_sum/divider)

        #remove inappropriate cases
        for i in range(0, len(right_diff_arr)): 
            if (right_diff_arr[i] > (right_avg +80)) or (right_diff_arr[i] < (right_avg - 80)): #the value 80 can be modified
                self.sliding_list_right = []
                right_diff_sum = 0
        
        #publish average values divided by 100
        dist_th = 160
        if left_distance == 0 and right_distance != 0:
            dist_steer = left_distance - dist_th 
        elif right_distance == 0 and left_distance != 0:
            dist_steer = right_distance + dist_th
        elif right_distance == 0 and left_distance == 0:
            dist_steer = 0
        else:
            dist_steer = right_distance + left_distance
        avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
        if avg_val > 100:
            avg_val = 100
        elif avg_val < -100:
            avg_val = -100
        avg_val /= 100
        avg_val *= 15
        # print("avg_val : {}".format(avg_val * 0.8))
        # print("dist_steer : {}".format(dist_steer * 0.003))
        self.steering_pub.publish(avg_val*1.4 + dist_steer * 0.015)
    
        return frame #return frame is for visualization    

    def visualize(self, original, BEV, Binary):
        #visualization

        # 1. Visualize Bird Eye Points
        arr1 = map(tuple, self.pts1)
        for i in arr1:
            cv2.circle(original, i, 5,(255,255,0), -1)

        # 2. Visualize Result of Sliding Window
        for i in self.sliding_list_left:
            cv2.circle(BEV, i, 5,(255,255,0), -1)
        for i in self.sliding_list_right:
            cv2.circle(BEV, i, 5,(255,255,0), -1)

        # 3. Visualize Images for monitoring
        cv2.imshow("frame", original)
        cv2.imshow("bird_eye_and_colored_image", BEV)
        cv2.imshow("colored_image", Binary)
        cv2.waitKey(1)

if __name__ == '__main__':
    LD = LaneDetection()

# data[].attributes.attachment_link.readable_text
#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from time import sleep
from std_msgs.msg import Float32, Bool, UInt16
from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
import math

class move_wecar:
    def __init__(self):
        rospy.init_node('control_wecar', anonymous=True)
        self.steer = rospy.Subscriber(
            "/wecar/steer",
            Float32,
            self.run
        )
        self.ackermann_pub = rospy.Publisher(
            '/vesc/high_level/ackermann_cmd_mux/input/nav_1',
            AckermannDriveStamped,
            queue_size=5
        )
	
        self.sub_dflag = rospy.Subscriber(
            '/tflag',
            UInt16,
            callback=self.flag_change
        )

        # self.sub_dflag = rospy.Subscriber(
        #     '/traffic2',
        #     String,
        #     callback=self.flag_change
        # )
	
        self.sub_lflag = rospy.Subscriber(
            '/lidar_flag',
            Bool,
            callback=self.e_stop
        )

        self.c_steer = 0
        self.c_speed = 0

        self.t_flag = True
        self.l_flag = True

    def run(self, data):
        steer = data.data
        if steer == -99:
            steer = 0
        if steer == 0:
            position_value = 0.00
            speed_value = 2.0
        else:
            position_value = -steer / 180 * math.pi * 1.3
            speed_value = 1.5
        if not self.t_flag or not self.l_flag:
            #self.cycle = 1
            print("t_flag = {0}".format(self.t_flag))
            print("l_flag = {0}".format(self.l_flag))
            print("Stop!")
            sleep(0.5)
            position_value = 0.0
            speed_value = 0.0
	
        self.c_speed = speed_value
        self.c_steer = position_value

        self.ackermann_pub.publish(self.make_msg())

    def e_stop(self, _flag):
        self.l_flag = _flag.data

    def flag_change(self, _b):
        if _b.data == 0:
            self.t_flag = False
        else:
            self.t_flag = True
        

    def make_msg(self):
        drive_msg_stamped = AckermannDriveStamped()
        drive_msg = AckermannDrive()
        drive_msg.speed = self.c_speed
        drive_msg.steering_angle = self.c_steer * 1.0
        drive_msg.acceleration = 0
        drive_msg.jerk = 0
        drive_msg.steering_angle_velocity = 0
        drive_msg_stamped.drive = drive_msg
	
        return drive_msg_stamped


if __name__ == '__main__':
    MoveCar = move_wecar()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("program down")

# data[].attributes.attachment_link.readable_text
#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from time import sleep
from std_msgs.msg import Float32, Bool, UInt16
from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
import math

class move_wecar:
    def __init__(self):
        rospy.init_node('control_wecar', anonymous=True)
        self.steer = rospy.Subscriber(
            "/wecar/steer",
            Float32,
            self.run
        )
        self.ackermann_pub = rospy.Publisher(
            '/vesc/high_level/ackermann_cmd_mux/input/nav_1',
            AckermannDriveStamped,
            queue_size=5
        )
	
        self.sub_dflag = rospy.Subscriber(
            '/tflag',
            UInt16,
            callback=self.flag_change
        )

        # self.sub_dflag = rospy.Subscriber(
        #     '/traffic2',
        #     String,
        #     callback=self.flag_change
        # )
	
        self.sub_lflag = rospy.Subscriber(
            '/lidar_flag',
            Bool,
            callback=self.e_stop
        )

        self.c_steer = 0
        self.c_speed = 0

        self.t_flag = True
        self.l_flag = True

    def run(self, data):
        steer = data.data
        if steer == -99:
            steer = 0
        if steer == 0:
            position_value = 0.00
            speed_value = 2.0
        else:
            position_value = -steer / 180 * math.pi * 1.6
            speed_value = 1.8
        if not self.t_flag or not self.l_flag:
            #self.cycle = 1
            print("t_flag = {0}".format(self.t_flag))
            print("l_flag = {0}".format(self.l_flag))
            print("Stop!")
            sleep(0.5)
            position_value = 0.0
            speed_value = 0.00
	
        self.c_speed = speed_value
        self.c_steer = position_value

        self.ackermann_pub.publish(self.make_msg())

    def e_stop(self, _flag):
        self.l_flag = _flag.data

    def flag_change(self, _b):
        if _b.data == 0:
            self.t_flag = False
        else:
            self.t_flag = True
        

    def make_msg(self):
        drive_msg_stamped = AckermannDriveStamped()
        drive_msg = AckermannDrive()
        drive_msg.speed = self.c_speed
        drive_msg.steering_angle = self.c_steer * 1.0
        drive_msg.acceleration = 0
        drive_msg.jerk = 0
        drive_msg.steering_angle_velocity = 0
        drive_msg_stamped.drive = drive_msg
	
        return drive_msg_stamped


if __name__ == '__main__':
    MoveCar = move_wecar()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("program down")

# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
import rospy
from cv_bridge import CvBridge

class LaneDetection:
    def __init__(self):
        # Points for Bird Eye View Transformation
        # Upper Left, Upper Right, Lower Left, Lower Right Point
        self.pts1 = np.float32([[140, 355], [502, 355], [0, 460], [640, 460]])
        self.pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])

        self.low_yellow = np.array([0, 100, 30])  # 
        self.high_yellow = np.array([100, 230, 255])  # 

        #self.low_white = np.array([0, 130, 0]) # Lower HLS threshold for Detect White Line
        #self.high_white = np.array([255, 255, 255]) # Higher HLS threshold for Detect White Line

        # Start Image Processing
        rospy.init_node("steering_node") # start node
        self.cvbridge = CvBridge() # CvBridge for transformations between cv image and ros image
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.convertImg)
        self.steering_pub = rospy.Publisher("/wecar/steer", Float32, queue_size=5)
        rospy.spin()

    def convertImg(self, img):
        frame = self.cvbridge.imgmsg_to_cv2(img, "bgr8") # Convert ROS Image to Cv2 Image
        frame = cv2.resize(frame, dsize=(640, 480), interpolation=cv2.INTER_AREA) # Resize Image
        bird_eye_image = self.bird_eye(frame) # Bird Eye View Transformation
        colored_image = self.color_detect(bird_eye_image) # 
        self.sliding_list_left = self.sliding_left(colored_image)
        self.sliding_list_right = self.sliding_right(colored_image)
        self.steering(frame, self.sliding_list_left, self.sliding_list_right)
        self.visualize(frame, bird_eye_image, colored_image)

    def bird_eye(self, frame):
        matrix = cv2.getPerspectiveTransform(self.pts1, self.pts2)
        result = cv2.warpPerspective(frame, matrix, (640, 480))
        return result

    def color_detect(self, img):
        hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)

        ''' Detection White Lineb '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        #return mask_white
        
        ''' Detection Yellow Line '''
        mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        return mask_yellow

        ''' Detection White & Yellow Line '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        # mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        # mask = cv2.bitwise_or(mask_white, mask_yellow)
        # return mask

    def sliding_left(self, img):
        left_list = []
        '''
        row: starting from y=179 to y=460, moving by 40
        col: starting from x=19 to y=300, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(19, int(img.shape[1]/2) - 20, 5):
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = [] 
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        left_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return left_list
    
    def sliding_right(self, img):
        right_list = []
        '''
        row: starting from x=179 pts1 = np.float32([[100, 240], [540, 240], [0, 380], [640, 380]]) #for driving
        pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])to y=560, moving by 40
        col: starting from x=320 to y=620, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(int(img.shape[1]/2), img.shape[1] - 20, 5): 
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = []
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        right_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return right_list
    
    def steering(self, frame, sliding_list_left, sliding_list_right):
        x_left = []
        x_right = []
        image_width = 320
        if len(sliding_list_left) == 0:
            left_distance = 0
        else:
            left_distance = image_width - sliding_list_left[0][1] #should be positive

        if len(sliding_list_right) == 0:
            right_distance = 0
        else:
            right_distance = image_width - sliding_list_right[0][1] #should be negative

        for i in range(0, len(sliding_list_left)): 
            x_left.append(sliding_list_left[i][0])
        
        left_diff_arr = np.diff(x_left)
        divider = len(left_diff_arr)
        if divider == 0:
            divider = 1
        left_diff_sum = np.sum(left_diff_arr)
        left_avg = int(left_diff_sum/divider)


        #remove inappropriate cases 
        for i in range(0, len(left_diff_arr)): 
            if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
                self.sliding_list_left = []
                left_diff_sum = 0

        #extract x coordinates from sliding_list_right
        for i in range(0, len(sliding_list_right)): 
            x_right.append(sliding_list_right[i][0])
            
        right_diff_arr = np.diff(x_right)
        divider = len(right_diff_arr)
        if divider == 0:
            divider = 1
        right_diff_sum = np.sum(right_diff_arr)
        right_avg = int(right_diff_sum/divider)

        #remove inappropriate cases
        for i in range(0, len(right_diff_arr)): 
            if (right_diff_arr[i] > (right_avg +80)) or (right_diff_arr[i] < (right_avg - 80)): #the value 80 can be modified
                self.sliding_list_right = []
                right_diff_sum = 0
        
        #publish average values divided by 100
        dist_th = 160
        if left_distance == 0 and right_distance != 0:
            dist_steer = left_distance - (dist_th - 40) 
        elif right_distance == 0 and left_distance != 0:
            dist_steer = right_distance + (dist_th - 40)
        elif right_distance == 0 and left_distance == 0:
            dist_steer = 0
        else:
            dist_steer = right_distance + left_distance
        avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
        if avg_val > 100:
            avg_val = 100
        elif avg_val < -100:
            avg_val = -100
        avg_val /= 100
        avg_val *= 15
        # print("avg_val : {}".format(avg_val * 0.8))
        # print("dist_steer : {}".format(dist_steer * 0.003))
        self.steering_pub.publish(avg_val*1.4 + dist_steer * 0.02)
    
        return frame #return frame is for visualization    

    def visualize(self, original, BEV, Binary):
        #visualization

        # 1. Visualize Bird Eye Points
        arr1 = map(tuple, self.pts1)
        for i in arr1:
            cv2.circle(original, i, 5,(255,255,0), -1)

        # 2. Visualize Result of Sliding Window
        for i in self.sliding_list_left:
            cv2.circle(BEV, i, 5,(255,255,0), -1)
        for i in self.sliding_list_right:
            cv2.circle(BEV, i, 5,(255,255,0), -1)

        # 3. Visualize Images for monitoring
        cv2.imshow("frame", original)
        cv2.imshow("bird_eye_and_colored_image", BEV)
        cv2.imshow("colored_image", Binary)
        cv2.waitKey(1)

if __name__ == '__main__':
    LD = LaneDetection()

# data[].attributes.attachment_link.title
8주차_캠퍼스로버_정산서

# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool
import math

class ObjectDetector:
    def __init__(self):
        self.DISTANCE_TH = 0.8
        rospy.init_node('object_detector', anonymous=False)
        self.sub_ls = rospy.Subscriber(
            '/scan',
            LaserScan,
            callback=self.callback
        )
        self.pub_flag = rospy.Publisher(
            "/lidar_flag",
            Bool,
            queue_size=3
        )

    def callback(self, _ls):
        cnt = 0
        for i in range(len(_ls.ranges)):
            angle = _ls.angle_min + i * _ls.angle_increment
            # print(angle)
            if -20.0 * math.pi / 180.0 <= angle <= 20.0 * math.pi / 180.0:
                if _ls.ranges[i] < self.DISTANCE_TH:
                    cnt += 1

        print(cnt)
        if cnt >= 5:
            self.pub_flag.publish(False)
        else:
            self.pub_flag.publish(True)

def run():
    od = ObjectDetector()
    rospy.spin()

if __name__ == "__main__":
    run()

# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
import rospy
from cv_bridge import CvBridge

class LaneDetection:
    def __init__(self):
        # Points for Bird Eye View Transformation
        # Upper Left, Upper Right, Lower Left, Lower Right Point
        self.pts1 = np.float32([[140, 355], [502, 355], [0, 460], [640, 460]])
        self.pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])

        self.low_yellow = np.array([0, 100, 30])  # 
        self.high_yellow = np.array([100, 230, 255])  # 

        #self.low_white = np.array([0, 130, 0]) # Lower HLS threshold for Detect White Line
        #self.high_white = np.array([255, 255, 255]) # Higher HLS threshold for Detect White Line

        # Start Image Processing
        rospy.init_node("steering_node") # start node
        self.cvbridge = CvBridge() # CvBridge for transformations between cv image and ros image
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.convertImg)
        self.steering_pub = rospy.Publisher("/wecar/steer", Float32, queue_size=5)
        rospy.spin()

    def convertImg(self, img):
        frame = self.cvbridge.imgmsg_to_cv2(img, "bgr8") # Convert ROS Image to Cv2 Image
        frame = cv2.resize(frame, dsize=(640, 480), interpolation=cv2.INTER_AREA) # Resize Image
        bird_eye_image = self.bird_eye(frame) # Bird Eye View Transformation
        colored_image = self.color_detect(bird_eye_image) # 
        self.sliding_list_left = self.sliding_left(colored_image)
        self.sliding_list_right = self.sliding_right(colored_image)
        self.steering(frame, self.sliding_list_left, self.sliding_list_right)
        self.visualize(frame, bird_eye_image, colored_image)

    def bird_eye(self, frame):
        matrix = cv2.getPerspectiveTransform(self.pts1, self.pts2)
        result = cv2.warpPerspective(frame, matrix, (640, 480))
        return result

    def color_detect(self, img):
        hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)

        ''' Detection White Lineb '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        #return mask_white
        
        ''' Detection Yellow Line '''
        mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        return mask_yellow

        ''' Detection White & Yellow Line '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        # mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        # mask = cv2.bitwise_or(mask_white, mask_yellow)
        # return mask

    def sliding_left(self, img):
        left_list = []
        '''
        row: starting from y=179 to y=460, moving by 40
        col: starting from x=19 to y=300, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(19, int(img.shape[1]/2) - 20, 5):
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = [] 
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        left_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return left_list
    
    def sliding_right(self, img):
        right_list = []
        '''
        row: starting from x=179 pts1 = np.float32([[100, 240], [540, 240], [0, 380], [640, 380]]) #for driving
        pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])to y=560, moving by 40
        col: starting from x=320 to y=620, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(int(img.shape[1]/2), img.shape[1] - 20, 5): 
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = []
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        right_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return right_list
    
    def steering(self, frame, sliding_list_left, sliding_list_right):
        x_left = []
        x_right = []
        image_width = 320
        if len(sliding_list_left) == 0:
            left_distance = 0
        else:
            left_distance = image_width - sliding_list_left[0][1] #should be positive

        if len(sliding_list_right) == 0:
            right_distance = 0
        else:
            right_distance = image_width - sliding_list_right[0][1] #should be negative

        for i in range(0, len(sliding_list_left)): 
            x_left.append(sliding_list_left[i][0])
        
        left_diff_arr = np.diff(x_left)
        divider = len(left_diff_arr)
        if divider == 0:
            divider = 1
        left_diff_sum = np.sum(left_diff_arr)
        left_avg = int(left_diff_sum/divider)


        #remove inappropriate cases 
        for i in range(0, len(left_diff_arr)): 
            if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
                self.sliding_list_left = []
                left_diff_sum = 0

        #extract x coordinates from sliding_list_right
        for i in range(0, len(sliding_list_right)): 
            x_right.append(sliding_list_right[i][0])
            
        right_diff_arr = np.diff(x_right)
        divider = len(right_diff_arr)
        if divider == 0:
            divider = 1
        right_diff_sum = np.sum(right_diff_arr)
        right_avg = int(right_diff_sum/divider)

        #remove inappropriate cases
        for i in range(0, len(right_diff_arr)): 
            if (right_diff_arr[i] > (right_avg +80)) or (right_diff_arr[i] < (right_avg - 80)): #the value 80 can be modified
                self.sliding_list_right = []
                right_diff_sum = 0
        
        #publish average values divided by 100
        dist_th = 160
        if left_distance == 0 and right_distance != 0:
            dist_steer = left_distance - (dist_th - 40) 
        elif right_distance == 0 and left_distance != 0:
            dist_steer = right_distance + (dist_th - 40)
        elif right_distance == 0 and left_distance == 0:
            dist_steer = 0
        else:
            dist_steer = right_distance + left_distance
        avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
        if avg_val > 100:
            avg_val = 100
        elif avg_val < -100:
            avg_val = -100
        avg_val /= 100
        avg_val *= 15
        # print("avg_val : {}".format(avg_val * 0.8))
        # print("dist_steer : {}".format(dist_steer * 0.003))
        self.steering_pub.publish(avg_val*1.4 + dist_steer * 0.02)
    
        return frame #return frame is for visualization    

    def visualize(self, original, BEV, Binary):
        #visualization

        # 1. Visualize Bird Eye Points
        arr1 = map(tuple, self.pts1)
        for i in arr1:
            cv2.circle(original, i, 5,(255,255,0), -1)

        # 2. Visualize Result of Sliding Window
        for i in self.sliding_list_left:
            cv2.circle(BEV, i, 5,(255,255,0), -1)
        for i in self.sliding_list_right:
            cv2.circle(BEV, i, 5,(255,255,0), -1)

        # 3. Visualize Images for monitoring
        cv2.imshow("frame", original)
        cv2.imshow("bird_eye_and_colored_image", BEV)
        cv2.imshow("colored_image", Binary)
        cv2.waitKey(1)

if __name__ == '__main__':
    LD = LaneDetection()

# data[].attributes.attachment_link.readable_text
#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from time import sleep
from std_msgs.msg import Float32, Bool, UInt16
from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
import math

class move_wecar:
    def __init__(self):
        rospy.init_node('control_wecar', anonymous=True)
        self.steer = rospy.Subscriber(
            "/wecar/steer",
            Float32,
            self.run
        )
        self.ackermann_pub = rospy.Publisher(
            '/vesc/high_level/ackermann_cmd_mux/input/nav_1',
            AckermannDriveStamped,
            queue_size=5
        )
	
        self.sub_dflag = rospy.Subscriber(
            '/tflag',
            UInt16,
            callback=self.flag_change
        )

        # self.sub_dflag = rospy.Subscriber(
        #     '/traffic2',
        #     String,
        #     callback=self.flag_change
        # )
	
        self.sub_lflag = rospy.Subscriber(
            '/lidar_flag',
            Bool,
            callback=self.e_stop
        )

        self.c_steer = 0
        self.c_speed = 0

        self.t_flag = True
        self.l_flag = True

    def run(self, data):
        steer = data.data
        if steer == -99:
            steer = 0
        if steer == 0:
            position_value = 0.00
            speed_value = 2.0
        else:
            position_value = -steer / 180 * math.pi * 1.3
            speed_value = 1.7
        if not self.t_flag or not self.l_flag:
            #self.cycle = 1
            print("t_flag = {0}".format(self.t_flag))
            print("l_flag = {0}".format(self.l_flag))
            print("Stop!")
            sleep(0.5)
            position_value = 0.0
            speed_value = 0.0
	
        self.c_speed = speed_value
        self.c_steer = position_value

        self.ackermann_pub.publish(self.make_msg())

    def e_stop(self, _flag):
        self.l_flag = _flag.data

    def flag_change(self, _b):
        if _b.data == 0:
            self.t_flag = False
        else:
            self.t_flag = True
        

    def make_msg(self):
        drive_msg_stamped = AckermannDriveStamped()
        drive_msg = AckermannDrive()
        drive_msg.speed = self.c_speed
        drive_msg.steering_angle = self.c_steer * 1.0
        drive_msg.acceleration = 0
        drive_msg.jerk = 0
        drive_msg.steering_angle_velocity = 0
        drive_msg_stamped.drive = drive_msg
	
        return drive_msg_stamped


if __name__ == '__main__':
    MoveCar = move_wecar()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("program down")

# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
import rospy
from cv_bridge import CvBridge

class LaneDetection:
    def __init__(self):
        # Points for Bird Eye View Transformation
        # Upper Left, Upper Right, Lower Left, Lower Right Point
        self.pts1 = np.float32([[175, 240], [465, 240], [0, 380], [640, 380]])
        self.pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])

        self.low_yellow = np.array([0, 100, 30]) # Lower HLS threshold for Detect Yellow Line
        self.high_yellow = np.array([100, 230, 255]) # Higher HLS threshold for Detect Yellow Line
        self.low_white = np.array([0, 130, 0]) # Lower HLS threshold for Detect White Line
        self.high_white = np.array([255, 255, 255]) # Higher HLS threshold for Detect White Line

        # Start Image Processing
        rospy.init_node("steering_node") # start node
        self.cvbridge = CvBridge() # CvBridge for transformations between cv image and ros image
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.convertImg)
        self.steering_pub = rospy.Publisher("/wecar/steer", Float32, queue_size=5)
        rospy.spin()

    def convertImg(self, img):
        frame = self.cvbridge.imgmsg_to_cv2(img, "bgr8") # Convert ROS Image to Cv2 Image
        frame = cv2.resize(frame, dsize=(640, 480), interpolation=cv2.INTER_AREA) # Resize Image
        bird_eye_image = self.bird_eye(frame) # Bird Eye View Transformation
        colored_image = self.color_detect(bird_eye_image) # 
        self.sliding_list_left = self.sliding_left(colored_image)
        self.sliding_list_right = self.sliding_right(colored_image)
        self.steering(frame, self.sliding_list_left, self.sliding_list_right)
        self.visualize(frame, bird_eye_image, colored_image)

    def bird_eye(self, frame):
        matrix = cv2.getPerspectiveTransform(self.pts1, self.pts2)
        result = cv2.warpPerspective(frame, matrix, (640, 480))
        return result

    def color_detect(self, img):
        hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)

        ''' Detection White Lineb '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        #return mask_white
        
        ''' Detection Yellow Line '''
        mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        return mask_yellow

        ''' Detection White & Yellow Line '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        # mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        # mask = cv2.bitwise_or(mask_white, mask_yellow)
        # return mask

    def sliding_left(self, img):
        left_list = []
        '''
        row: starting from y=179 to y=460, moving by 40
        col: starting from x=19 to y=300, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(19, int(img.shape[1]/2) - 20, 5):
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = [] 
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        left_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return left_list
    
    def sliding_right(self, img):
        right_list = []
        '''
        row: starting from x=179 pts1 = np.float32([[100, 240], [540, 240], [0, 380], [640, 380]]) #for driving
        pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])to y=560, moving by 40
        col: starting from x=320 to y=620, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(int(img.shape[1]/2), img.shape[1] - 20, 5): 
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = []
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        right_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return right_list
    
    def steering(self, frame, sliding_list_left, sliding_list_right):
        x_left = []
        x_right = []
        image_width = 320
        if len(sliding_list_left) == 0:
            left_distance = 0
        else:
            left_distance = image_width - sliding_list_left[0][1] #should be positive

        if len(sliding_list_right) == 0:
            right_distance = 0
        else:
            right_distance = image_width - sliding_list_right[0][1] #should be negative

        for i in range(0, len(sliding_list_left)): 
            x_left.append(sliding_list_left[i][0])
        
        left_diff_arr = np.diff(x_left)
        divider = len(left_diff_arr)
        if divider == 0:
            divider = 1
        left_diff_sum = np.sum(left_diff_arr)
        left_avg = int(left_diff_sum/divider)


        #remove inappropriate cases 
        for i in range(0, len(left_diff_arr)): 
            if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
                self.sliding_list_left = []
                left_diff_sum = 0

        #extract x coordinates from sliding_list_right
        for i in range(0, len(sliding_list_right)): 
            x_right.append(sliding_list_right[i][0])
            
        right_diff_arr = np.diff(x_right)
        divider = len(right_diff_arr)
        if divider == 0:
            divider = 1
        right_diff_sum = np.sum(right_diff_arr)
        right_avg = int(right_diff_sum/divider)

        #remove inappropriate cases
        for i in range(0, len(right_diff_arr)): 
            if (right_diff_arr[i] > (right_avg +80)) or (right_diff_arr[i] < (right_avg - 80)): #the value 80 can be modified
                self.sliding_list_right = []
                right_diff_sum = 0
        
        #publish average values divided by 100
        dist_th = 160
        if left_distance == 0 and right_distance != 0:
            dist_steer = left_distance - dist_th 
        elif right_distance == 0 and left_distance != 0:
            dist_steer = right_distance + dist_th
        elif right_distance == 0 and left_distance == 0:
            dist_steer = 0
        else:
            dist_steer = right_distance + left_distance
        avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
        if avg_val > 100:
            avg_val = 100
        elif avg_val < -100:
            avg_val = -100
        avg_val /= 100
        avg_val *= 15
        # print("avg_val : {}".format(avg_val * 0.8))
        # print("dist_steer : {}".format(dist_steer * 0.003))
        self.steering_pub.publish(avg_val*1.4 + dist_steer * 0.03)
    
        return frame #return frame is for visualization    

    def visualize(self, original, BEV, Binary):
        #visualization

        # 1. Visualize Bird Eye Points
        arr1 = map(tuple, self.pts1)
        for i in arr1:
            cv2.circle(original, i, 5,(255,255,0), -1)

        # 2. Visualize Result of Sliding Window
        for i in self.sliding_list_left:
            cv2.circle(BEV, i, 5,(255,255,0), -1)
        for i in self.sliding_list_right:
            cv2.circle(BEV, i, 5,(255,255,0), -1)

        # 3. Visualize Images for monitoring
        cv2.imshow("frame", original)
        cv2.imshow("bird_eye_and_colored_image", BEV)
        cv2.imshow("colored_image", Binary)
        cv2.waitKey(1)

if __name__ == '__main__':
    LD = LaneDetection()

# data[].attributes.attachment_link.readable_text
#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from time import sleep
from std_msgs.msg import Float32, Bool, UInt16
from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
import math

class move_wecar:
    def __init__(self):
        rospy.init_node('control_wecar', anonymous=True)
        self.steer = rospy.Subscriber(
            "/wecar/steer",
            Float32,
            self.run
        )
        self.ackermann_pub = rospy.Publisher(
            '/vesc/high_level/ackermann_cmd_mux/input/nav_1',
            AckermannDriveStamped,
            queue_size=5
        )
	
        self.sub_dflag = rospy.Subscriber(
            '/tflag',
            UInt16,
            callback=self.flag_change
        )

        # self.sub_dflag = rospy.Subscriber(
        #     '/traffic2',
        #     String,
        #     callback=self.flag_change
        # )
	
        self.sub_lflag = rospy.Subscriber(
            '/lidar_flag',
            Bool,
            callback=self.e_stop
        )

        self.c_steer = 0
        self.c_speed = 0

        self.t_flag = True
        self.l_flag = True

    def run(self, data):
        steer = data.data
        if steer == -99:
            steer = 0
        if steer == 0:
            position_value = 0.00
            speed_value = 0.8
        else:
            position_value = -steer / 180 * math.pi * 1.0
            speed_value = 0.80
        if not self.t_flag or not self.l_flag:
            #self.cycle = 1
            print("t_flag = {0}".format(self.t_flag))
            print("l_flag = {0}".format(self.l_flag))
            print("Stop!")
            sleep(0.5)
            position_value = 0.0
            speed_value = 0.0
	
        self.c_speed = speed_value
        self.c_steer = position_value

        self.ackermann_pub.publish(self.make_msg())

    def e_stop(self, _flag):
        self.l_flag = _flag.data

    def flag_change(self, _b):
        if _b.data == 0:
            self.t_flag = False
        else:
            self.t_flag = True
        

    def make_msg(self):
        drive_msg_stamped = AckermannDriveStamped()
        drive_msg = AckermannDrive()
        drive_msg.speed = self.c_speed
        drive_msg.steering_angle = self.c_steer * 1.0
        drive_msg.acceleration = 0
        drive_msg.jerk = 0
        drive_msg.steering_angle_velocity = 0
        drive_msg_stamped.drive = drive_msg
	
        return drive_msg_stamped


if __name__ == '__main__':
    MoveCar = move_wecar()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("program down")

# data[].attributes.body
문제 1. 카메라 각도의 문제 기준점이 명확하지 않고 수시로 바뀔 수 있음
문제 2. 직선구간에서도 커브 구간이라 인식하여 지그재그로 차가 움직이는 상황이 연출됨
문제 3. 커브를 다 돌고 난 뒤 핸들은 너무 늦게 풀음 => 이탈의 원인
문제 4, 버드아이 시점이 좁음

문제 1 해결방안 : 줄자로 위카와 버드아이시점에서 보이는 가장 가까운 점의 길이를 측정 => 각도 고정
문제 2 해결방안 : 현재 control파일에서는 직선 아님 곡선으로만 나뉘어져 있는데 elif문을 추가하여 길의 곡률이 심하지 않을 경우도 직선으로 인식하게끔 설정함 + 이 경우의 steering값도 찾아야 함
문제 3. 해결방한 모르겠음
문제 4. 내일 실험을 통해 개선할 예정

수정 사항 1. steering파일에서 원래 왼쪽 차선이 없으면 좌회전
오른 쪽 차선이 없으면 우회전으로 코딩 된 곳을 한쪽 차선이 안보이더라도 직진으로 인식하게끔 바꿈
because 버드아이의 시점이 좁아서 양쪽 차선을 온전히 인식할 때도 있고 한쪽만 인식하는 경우도 있음

수정 사항 2. 버드아이에 필요한 4개의 점을 바꿈 => 또 바꿔볼 예정

수정 사항 3. 왼쪽 차선이 없을경우 왼쪽으로 커브돌도록 하였지만 오른 쪽 차선을 기준으로 일정 구간 거리를 유지하는 식으로 바꿈

의문점 : 수정사항1 에서 3가지 경우 모두 직선으로 인식하게끔 설정했는데 그럼 control 파일에서 커브구간은 실행이 되면 안됨 근데 실행이 되었음
의문점에 대한 가설 1. 수정사항1 의 3가지 조건 외에도 커브를 인식하는 방법이 따로 있음
의문점에 대한 가설 2. 커브를 인식하지 않고 직진으로 인식한 상태이지만 수정사항3 에 의하여 커브를 돌았음

# data[].attributes.body
-권한 부여.
Terminal 0
sudo chmod 666 /dev/tty*

-모든 터미널 닫고 새 터미널 열어서
따로 실행-
Terminal 1
roslaunch wecarteleop.launch

-Step 2. 카메라 키기
Terminal 2
roslaunch lane_detection one_lane.launch

-Step 3. 속도 보기
Terminal 3

# data[].attributes.attachment_link.title
캠퍼스로버_5주차_정산서 (1)

# data[].attributes.attachment_link.readable_text
# lower 증가 q=파랑 w=초록 e=빨강
# lower 감소 a=파랑 s=초록 d=빨강

# upper 증가 r=파랑 t=초록 y=빨강
# upper 감소 f=파랑 g=초록 h=빨강

# 실행 종료=x t= 간격

import numpy as np
import cv2
from time import sleep


def main():
    
    lB=0
    lG=100
    lR=30
    
#======================
    uB=100
    uG=230
    uR=255
    t=3
    kk=False
    
    image = cv2.imread('./1.png')
    
    while kk==False:
        lower = np.array([lB, lG, lR])    
        upper = np.array([uB, uG, uR])    
        result = bgrExtraction(image, lower, upper)
        cv2.imshow('test', result)
        sleep(1)
        
        while True:
            
            key = cv2.waitKey(1)&0xff
            if key == ord('q'):
                lB+=t
                break
            if key == ord('w'):
                lG+=t
                break
            if key == ord('e'):
                lR+=t
                break
            if key == ord('a'):
                lB-=t
                break
            if key == ord('s'):
                lG-=t
                break
            if key == ord('d'):
                lR-=t
                break
            if key == ord('r'):
                uB+=t
                break
            if key == ord('t'):
                uG+=t
                break
            if key == ord('y'):
                uR+=t
                break
            if key == ord('f'):
                uB-=t
                break
            if key == ord('g'):
                uG-=t
                break
            if key == ord('h'):
                uR-=t
                break
            if key == ord('x'):
                print(lB,lG,lR,uB,uG,uR)
                kk=True
                break

        cv2.destroyAllWindows()
    


def bgrExtraction(image, bgrLower, bgrUpper):
    img_mask = cv2.inRange(image, bgrLower, bgrUpper) 
    result = cv2.bitwise_and(image, image, mask=img_mask) 
    return result


if __name__ == '__main__':
    main()

# data[].attributes.headline
정산서 작성 시 주의 사항!

# data[].attributes.subject
정산서 작성 시 주의 사항!

# data[].attributes.body
정산서 쓸 때 주의 할 점
1. 모든 글씨 검정색 + 기울임 X
2. 정산서 순서는 1.융합동아리 정산서 ->활동내용 -> 영수증 -> 회의록 -> 서명록 -> 사전자료
(영수증 회의록, 서명록은 같은 일자 끼리 모아서 정렬)
3. 금번정산액은 누적해서 쓰는 것이 아님
4. 첫번 째 장 맨 밑에 있는 붙임 항목중 0장인 것은 삭제
5. 왼쪽 상단의 서직은 모두 삭제

# data[].attributes.body
사양 확인 가능

# data[].attributes.attachment_link.readable_text
WeGo 자율주행 플랫폼 UGV (Unmanned Ground Vehicle)

최근 자율 주행로봇은 무인 방역시스템, 순찰 기능을 탑재한 자율 주행 보안 시스템, 과학 및 연구 목적형 로봇, 탐사로봇, 물류이송 및 기타 분야에 광범위하게 적용되고 있습니다.

또한 위고에서는 최근 납품 이후에도 발생할 수 있는 문제를 위하여 기술 지원도 팀을 별도로 운영하여 문제가 없이 사후 관리를 효율적으로 운영을 하고 있습니다.

기업체에서 바로 적용이 가능한 로봇의 컨설팅과 시스템 엔지니어링도 제공하고 있으니 언제든지 저희 엔지니어들과 상담하여 주십시오.

# data[].attributes.attachment_link.description
WeGo 자율주행 플랫폼 UGV (Unmanned Ground Vehicle) 최근 자율 주행로봇은 무인 방역시스템, 순찰 기능을 탑재한 자율 주행 보안 시스템, 과학 및 연구 목적형 로봇, 탐사로봇, 물류이송 및 기타 분야에 광범위하게 적용되고 있습니다. 또한 위고에서는 최근 납품 이후에도 발생할 수 있는 문제를 위하여 기술 지원도 팀을 별도로 운영하여 문제가 없이 사후 관리를 효율적으로 운영을 하고 있습니다.

# data[].attributes.attachment_link.title
캠퍼스로버_4주차_정산서_황석주 수정본

# data[].attributes.attachment_link.title
캠퍼스로버_4주차_정산서 

# data[].attributes.attachment_link.title
NoMachine - Free Remote Desktop for Everybody

# data[].attributes.attachment_link.description
Connect to your remote computer at the speed of light. NoMachine is the fastest remote desktop you have ever tried. Control any PC or Mac and start working on it as if it was right in front of you. Free for personal use.

# data[].attributes.attachment_link.readable_text
#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from time import sleep
from std_msgs.msg import Float32, Bool, UInt16
from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
import math

class move_wecar:
    def __init__(self):
        rospy.init_node('control_wecar', anonymous=True)
        self.steer = rospy.Subscriber(
            "/wecar/steer",
            Float32,
            self.run
        )
        self.ackermann_pub = rospy.Publisher(
            '/vesc/high_level/ackermann_cmd_mux/input/nav_1',
            AckermannDriveStamped,
            queue_size=5
        )
	
        self.sub_dflag = rospy.Subscriber(
            '/tflag',
            UInt16,
            callback=self.flag_change
        )

        # self.sub_dflag = rospy.Subscriber(
        #     '/traffic2',
        #     String,
        #     callback=self.flag_change
        # )
	
        self.sub_lflag = rospy.Subscriber(
            '/lidar_flag',
            Bool,
            callback=self.e_stop
        )

        self.c_steer = 0
        self.c_speed = 0

        self.t_flag = True
        self.l_flag = True

    def run(self, data):
        steer = data.data
        if steer == -99:
            steer = 0
        if steer == 0:
            position_value = 0.00
            speed_value = 2.0
        else:
            position_value = -steer / 180 * math.pi * 1.3
            speed_value = 1.7
        if not self.t_flag or not self.l_flag:
            #self.cycle = 1
            print("t_flag = {0}".format(self.t_flag))
            print("l_flag = {0}".format(self.l_flag))
            print("Stop!")
            sleep(0.5)
            position_value = 0.0
            speed_value = 0.0
	
        self.c_speed = speed_value
        self.c_steer = position_value

        self.ackermann_pub.publish(self.make_msg())

    def e_stop(self, _flag):
        self.l_flag = _flag.data

    def flag_change(self, _b):
        if _b.data == 0:
            self.t_flag = False
        else:
            self.t_flag = True
        

    def make_msg(self):
        drive_msg_stamped = AckermannDriveStamped()
        drive_msg = AckermannDrive()
        drive_msg.speed = self.c_speed
        drive_msg.steering_angle = self.c_steer * 1.0
        drive_msg.acceleration = 0
        drive_msg.jerk = 0
        drive_msg.steering_angle_velocity = 0
        drive_msg_stamped.drive = drive_msg
	
        return drive_msg_stamped


if __name__ == '__main__':
    MoveCar = move_wecar()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("program down")

# data[].attributes.attachment_link.title
2023 자율주행 모빌리티 융합동아리 운영 지침 (4)

# data[].attributes.attachment_link.readable_text
#! /usr/bin/env python
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
import rospy
from cv_bridge import CvBridge

class LaneDetection:
    def __init__(self):
        # Points for Bird Eye View Transformation
        # Upper Left, Upper Right, Lower Left, Lower Right Point
        self.pts1 = np.float32([[110, 355], [550, 355], [0, 450], [640, 450]])
        self.pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])

        self.low_yellow = np.array([0, 100, 30])  # 
        self.high_yellow = np.array([100, 230, 255])  # 

        #self.low_white = np.array([0, 130, 0]) # Lower HLS threshold for Detect White Line
        #self.high_white = np.array([255, 255, 255]) # Higher HLS threshold for Detect White Line

        # Start Image Processing
        rospy.init_node("steering_node") # start node
        self.cvbridge = CvBridge() # CvBridge for transformations between cv image and ros image
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.convertImg)
        self.steering_pub = rospy.Publisher("/wecar/steer", Float32, queue_size=5)
        rospy.spin()

    def convertImg(self, img):
        frame = self.cvbridge.imgmsg_to_cv2(img, "bgr8") # Convert ROS Image to Cv2 Image
        frame = cv2.resize(frame, dsize=(640, 480), interpolation=cv2.INTER_AREA) # Resize Image
        bird_eye_image = self.bird_eye(frame) # Bird Eye View Transformation
        colored_image = self.color_detect(bird_eye_image) # 
        self.sliding_list_left = self.sliding_left(colored_image)
        self.sliding_list_right = self.sliding_right(colored_image)
        self.steering(frame, self.sliding_list_left, self.sliding_list_right)
        self.visualize(frame, bird_eye_image, colored_image)

    def bird_eye(self, frame):
        matrix = cv2.getPerspectiveTransform(self.pts1, self.pts2)
        result = cv2.warpPerspective(frame, matrix, (640, 480))
        return result

    def color_detect(self, img):
        hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)

        ''' Detection White Lineb '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        #return mask_white
        
        ''' Detection Yellow Line '''
        mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        return mask_yellow

        ''' Detection White & Yellow Line '''
        # mask_white = cv2.inRange(hls, self.low_white, self.high_white)
        # mask_yellow = cv2.inRange(hls, self.low_yellow, self.high_yellow)
        # mask = cv2.bitwise_or(mask_white, mask_yellow)
        # return mask

    def sliding_left(self, img):
        left_list = []
        '''
        row: starting from y=179 to y=460, moving by 40
        col: starting from x=19 to y=300, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(19, int(img.shape[1]/2) - 20, 5):
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = [] 
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        left_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return left_list
    
    def sliding_right(self, img):
        right_list = []
        '''
        row: starting from x=179 pts1 = np.float32([[100, 240], [540, 240], [0, 380], [640, 380]]) #for driving
        pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])to y=560, moving by 40
        col: starting from x=320 to y=620, moving by 5
        '''
        for j in range(179, img.shape[0] - 20, 40): 
            j_list = []
            for i in range(int(img.shape[1]/2), img.shape[1] - 20, 5): 
                num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
                if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
                    j_list.append(i)
            try:
                len_list = []
                #cluster if a gap between elements in the list is over 5
                result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1) 
                for k in range(0, len(result)):
                    len_list.append(len(result[k])) #append the lengths of each cluster
                largest_integer = max(len_list)
            
                for l in range(0, len(result)):
                    if len(result[l]) == largest_integer: 
                        avg = int(np.sum(result[l]) / len(result[l])) #average
                        right_list.append((avg, j)) #avg points of left side 
            except:
                continue
        return right_list
    
    def steering(self, frame, sliding_list_left, sliding_list_right):
        x_left = []
        x_right = []
        image_width = 320
        if len(sliding_list_left) == 0:
            left_distance = 0
        else:
            left_distance = image_width - sliding_list_left[0][1] #should be positive

        if len(sliding_list_right) == 0:
            right_distance = 0
        else:
            right_distance = image_width - sliding_list_right[0][1] #should be negative

        for i in range(0, len(sliding_list_left)): 
            x_left.append(sliding_list_left[i][0])
        
        left_diff_arr = np.diff(x_left)
        divider = len(left_diff_arr)
        if divider == 0:
            divider = 1
        left_diff_sum = np.sum(left_diff_arr)
        left_avg = int(left_diff_sum/divider)


        #remove inappropriate cases 
        for i in range(0, len(left_diff_arr)): 
            if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
                self.sliding_list_left = []
                left_diff_sum = 0

        #extract x coordinates from sliding_list_right
        for i in range(0, len(sliding_list_right)): 
            x_right.append(sliding_list_right[i][0])
            
        right_diff_arr = np.diff(x_right)
        divider = len(right_diff_arr)
        if divider == 0:
            divider = 1
        right_diff_sum = np.sum(right_diff_arr)
        right_avg = int(right_diff_sum/divider)

        #remove inappropriate cases
        for i in range(0, len(right_diff_arr)): 
            if (right_diff_arr[i] > (right_avg +80)) or (right_diff_arr[i] < (right_avg - 80)): #the value 80 can be modified
                self.sliding_list_right = []
                right_diff_sum = 0
        
        #publish average values divided by 100
        dist_th = 160
        if left_distance == 0 and right_distance != 0:
            dist_steer = left_distance - dist_th
        elif right_distance == 0 and left_distance != 0:
            dist_steer = right_distance + dist_th
        elif right_distance == 0 and left_distance == 0:
            dist_steer = 0
        else:
            dist_steer = right_distance + left_distance
        avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
        if avg_val > 100:
            avg_val = 100
        elif avg_val < -100:
            avg_val = -100
        avg_val /= 100
        avg_val *= 15
        # print("avg_val : {}".format(avg_val * 0.8))
        # print("dist_steer : {}".format(dist_steer * 0.003))
        self.steering_pub.publish(avg_val*1.4 + dist_steer * 0.010)
    
        return frame #return frame is for visualization    

    def visualize(self, original, BEV, Binary):
        #visualization

        # 1. Visualize Bird Eye Points
        arr1 = map(tuple, self.pts1)
        for i in arr1:
            cv2.circle(original, i, 5,(255,255,0), -1)

        # 2. Visualize Result of Sliding Window
        for i in self.sliding_list_left:
            cv2.circle(BEV, i, 5,(255,255,0), -1)
        for i in self.sliding_list_right:
            cv2.circle(BEV, i, 5,(255,255,0), -1)

        # 3. Visualize Images for monitoring
        cv2.imshow("frame", original)
        cv2.imshow("bird_eye_and_colored_image", BEV)
        cv2.imshow("colored_image", Binary)
        cv2.waitKey(1)

if __name__ == '__main__':
    LD = LaneDetection()