collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Ricky ChenTesting lower camera position6ac3dce

simulation
3.7 KiB168 linesraw
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):
12	e = float(2.718281828459)
13	random_value = 10
14
15	num_readings = 400
16	laser_frequency = 40
17	x = float(220) # 1 meter = 220 pixels
18
19	pixels_to_meters = 1/x
20
21	current_time = rospy.Time.now()
22	rows = image.shape[0]
23	cols = image.shape[1]
24
25	scan = LaserScan()
26
27	scan.header.stamp = current_time
28	scan.header.frame_id = 'lane_laser_frame'
29	scan.angle_min = -1.57
30	scan.angle_max = 1.57
31	scan.angle_increment = 3.14 / num_readings
32	scan.time_increment = 0.0003018708
33	scan.range_min = 0.0
34	scan.range_max = 100.0
35
36	scan.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	
57	previous_pixel_col = 0
58	col = previous_pixel_col
59
60	first_row = True
61	obstacle_found = False
62
63	row = 0
64
65	x = 0
66
67	condition_1 = True
68	condition_2 = True
69	condition_3 = True
70
71	while(row < rows):
72		# Counter variables for when obstacle is not found
73
74		col_counter = 1
75		sign = 1
76
77		# Main Loop
78
79		while(not first_row and col < cols and col_counter <= 20):
80			#print(row)		# Debugging
81			#print(col)		# Debugging
82			if(image[row,col] >= 100):		# 255 == White
83				obstacle_found = True
84				scan.ranges.append(col*pixels_to_meters)
85				previous_pixel_col = col
86				break
87			else:
88				col = previous_pixel_col + sign * col_counter
89				sign = sign * -1
90
91				if(col_counter == 20):		# End of range
92					scan.ranges.append(scan.range_max)
93			if(sign == 1):
94			    col_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
107		while(col < cols and first_row):
108			if(image[row,col] >= 100):
109				previous_pixel_col = col
110				first_row = False			# Set first_row condition such that this loop never runs after first row
111				break
112			col += 1
113		row += 1
114
115	scan_pub.publish(scan)
116
117def image_callback(msg):
118	# Set timer
119	last_time = time.time()
120
121	# Set img to msg data
122	img = msg
123
124	# Convert ROS image to CV format
125	cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="passthrough")
126
127	# Get scan data
128	append_data_list(cv_image)
129
130	# If 'q' is detected, quit
131	if(chr(cv2.waitKey(3)&255) == 'q'):
132		rospy.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:
165	rospy.spin()
166except KeyboardInterrupt:
167	print("Shutting down")
168