collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
6ac3dce
simulation
1#!/usr/bin/env python 2 3import roslib 4import rospy 5import cv2 6from sensor_msgs .msg import LaserScan 7from sensor_msgs .msg import Image 8from cv_bridge import CvBridge ,CvBridgeError 9import time 10 11def append_data_list (image ): 12e = float (2.718281828459 ) 13random_value = 10 14 15num_readings = 400 16laser_frequency = 40 17x = float (220 )# 1 meter = 220 pixels 18 19pixels_to_meters = 1 / x 20 21current_time = rospy .Time .now () 22rows = image .shape [0 ] 23cols = image .shape [1 ] 24 25scan = LaserScan () 26 27scan .header .stamp = current_time 28scan .header .frame_id = 'lane_laser_frame' 29scan .angle_min = - 1.57 30scan .angle_max = 1.57 31scan .angle_increment = 3.14 / num_readings 32scan .time_increment = 0.0003018708 33scan .range_min = 0.0 34scan .range_max = 100.0 35 36scan .ranges = [] 37# scan.intensities = [] 38 39# cv2.imshow('testing', image) 40 41# for i in range(0, rows): 42# for j in range(previous_pixel_col, cols): 43# pixel = image[i,j] 44# #print(pixel[0]) 45# if(pixel == 255): ##### BLUE GREEN RED ENCODING 46# #print("Obstacle found!") 47# scan.ranges.append((j*pixels_to_meters)) 48# previous_pixel_col = j 49# break 50# if(image[i, previous_pixel_col - 1] == 0): 51# direction = 1 52# elif(image[i, previous_pixel_col - 1] == 255): 53# direction = -1 54# if(j == cols - 1): 55# scan.ranges.append(scan.range_max) 56 57previous_pixel_col = 0 58col = previous_pixel_col 59 60first_row = True 61obstacle_found = False 62 63row = 0 64 65x = 0 66 67condition_1 = True 68condition_2 = True 69condition_3 = True 70 71while (row < rows ): 72# Counter variables for when obstacle is not found 73 74col_counter = 1 75sign = 1 76 77# Main Loop 78 79while (not first_row and col < cols and col_counter <= 20 ): 80#print(row) # Debugging 81#print(col) # Debugging 82if (image [row ,col ]>= 100 ):# 255 == White 83obstacle_found = True 84scan .ranges .append (col * pixels_to_meters ) 85previous_pixel_col = col 86break 87else : 88col = previous_pixel_col + sign * col_counter 89sign = sign * - 1 90 91if (col_counter == 20 ):# End of range 92scan .ranges .append (scan .range_max ) 93if (sign == 1 ): 94col_counter += 1 95 96# while(condition_1 and condition_2): 97# while(condition_3): 98# x += 1 99# if(x == 5): 100# condition_2 = False 101# break 102# print(x) 103 104 105#loop to get first obstacle 106 107while (col < cols and first_row ): 108if (image [row ,col ]>= 100 ): 109previous_pixel_col = col 110first_row = False # Set first_row condition such that this loop never runs after first row 111break 112col += 1 113row += 1 114 115scan_pub .publish (scan ) 116 117def image_callback (msg ): 118# Set timer 119last_time = time .time () 120 121# Set img to msg data 122img = msg 123 124# Convert ROS image to CV format 125cv_image = bridge .imgmsg_to_cv2 (img ,desired_encoding = "passthrough" ) 126 127# Get scan data 128append_data_list (cv_image ) 129 130# If 'q' is detected, quit 131if (chr (cv2 .waitKey (3 )& 255 )== 'q' ): 132rospy .signal_shutdown ("Quit detected" ) 133 134# Print FPS 135#print("{} fps".format(1/(time.time() - last_time))) 136 137bridge = CvBridge () 138 139# Init node 140rospy .init_node ('lane_laser_publisher' ) 141 142# Create publisher to publish scan data 143scan_pub = rospy .Publisher ('/mybot/camera1/laser_scan' ,LaserScan ,queue_size = 50 ) 144 145# Create subscriber to log polar image topic 146rospy .Subscriber ("/mybot/camera1/image_log_polar" ,Image ,image_callback ) 147 148##### Putting this here so I don't forget: 149##### Top to down is angle. Left to Right is radius 150 151## Have to scale pixels -> m 152 153### PSEUDOCODE ### 154# for cols in image: 155# last_time = time.time() 156# for rows in image: 157# if found red: 158# append to ranges list as obstacle 159# print(time.time() - last_time) # Only need to do this once to figure out time increments 160 161# obstacle_ranges = [] 162# obstacle_angle = [] 163 164try : 165rospy .spin () 166except KeyboardInterrupt : 167"Shutting down" ) 168