2828from tf .transformations import euler_from_quaternion , quaternion_from_euler
2929import time
3030
31- class ReturnValue (object ):
32- def __init__ (self , name ):
33- self .name = name
34-
35- def retun_val (self , stats , data_1 , data_2 , data_3 ):
36- self .stats = stats
37- self .data_1 = data_1
38- self .data_2 = data_2
39- self .data_3 = data_3
40-
4131def scan_parking_spot ():
42- stats = False
32+ scan_done = False
4333 intensity_index = []
4434 index_count = []
4535 spot_angle_index = []
4636 minimun_scan_angle = 30
4737 maximun_scan_angle = 330
4838 intensity_threshold = 100
49-
39+ center_angle = 0
40+ start_angle = 0
41+ end_angle = 0
5042 for i in range (360 ):
5143 if i >= minimun_scan_angle and i < maximun_scan_angle :
5244 spot_intensity = msg .intensities [i ] ** 2 * msg .ranges [i ] / 100000
@@ -62,13 +54,14 @@ def scan_parking_spot():
6254 if abs (i - index_count [int (len (index_count ) / 2 )]) < 20 :
6355 spot_angle_index .append (i )
6456 if len (spot_angle_index ) > 10 :
65- stats = True
57+ scan_done = True
58+ center_angle = spot_angle_index [int (len (spot_angle_index ) / 2 )]
59+ start_angle = spot_angle_index [2 ]
60+ end_angle = spot_angle_index [- 3 ]
61+
6662 else :
67- stats = False
68- center_angle = spot_angle_index [int (len (spot_angle_index ) / 2 )]
69- start_angle = spot_angle_index [2 ]
70- end_angle = spot_angle_index [- 3 ]
71- spot_angle .retun_val (stats , center_angle , start_angle , end_angle )
63+ scan_done = False
64+ return scan_done , center_angle , start_angle , end_angle
7265
7366def quaternion ():
7467 quaternion = (
@@ -104,41 +97,42 @@ def get_point(start_angle_distance):
10497 else :
10598 x = distance * cos (angle ) * - 1
10699 y = distance * sin (angle ) * - 1
107- return x , y
108100
109- def finding_spot_position ():
101+ return [x , y ]
102+
103+ def finding_spot_position (center_angle , start_angle , end_angle ):
110104 print ("scan parking spot done!" )
111- stats = False
112- start_angle_distance = get_angle_distance (spot_angle . data_1 )
113- center_angle_distance = get_angle_distance (spot_angle . data_2 )
114- end_angle_distance = get_angle_distance (spot_angle . data_3 )
105+ fining_spot = False
106+ start_angle_distance = get_angle_distance (start_angle )
107+ center_angle_distance = get_angle_distance (center_angle )
108+ end_angle_distance = get_angle_distance (end_angle )
115109
116110 if start_angle_distance [1 ] != 0 and center_angle_distance [1 ] != 0 and end_angle_distance [1 ] != 0 :
117111 print ("calibration......" )
118112 start_point = get_point (start_angle_distance )
119113 center_point = get_point (center_angle_distance )
120114 end_point = get_point (end_angle_distance )
121- stats = True
115+ fining_spot = True
122116 else :
123- stats = False
117+ fining_spot = False
124118 print ("wrong scan!!" )
125119
126- return spot_point . retun_val ( stats , start_point , center_point , end_point )
120+ return fining_spot , start_point , center_point , end_point
127121
128122def rotate_origin_only (x , y , radians ):
129123 xx = x * cos (radians ) + y * sin (radians )
130124 yy = - x * sin (radians ) + y * cos (radians )
131125 return xx , yy
132126
133- def scan_spot_filter (msg ):
127+ def scan_spot_filter (msg , center_angle , start_angle , end_angle ):
134128 scan_spot_pub = rospy .Publisher ("/scan_spot" , LaserScan , queue_size = 1 )
135129 scan_spot = msg
136130 scan_spot_list = list (scan_spot .intensities )
137131 for i in range (360 ):
138132 scan_spot_list [i ] = 0
139- scan_spot_list [spot_angle . data_1 ] = msg .ranges [spot_angle . data_1 ] + 10000
140- scan_spot_list [spot_angle . data_2 ] = msg .ranges [spot_angle . data_2 ] + 10000
141- scan_spot_list [spot_angle . data_3 ] = msg .ranges [spot_angle . data_3 ] + 10000
133+ scan_spot_list [start_angle ] = msg .ranges [start_angle ] + 10000
134+ scan_spot_list [center_angle ] = msg .ranges [center_angle ] + 10000
135+ scan_spot_list [end_angle ] = msg .ranges [end_angle ] + 10000
142136 scan_spot .intensities = tuple (scan_spot_list )
143137 scan_spot_pub .publish (scan_spot )
144138
@@ -148,26 +142,26 @@ def scan_spot_filter(msg):
148142 reset_pub = rospy .Publisher ('/reset' , Empty , queue_size = 1 )
149143 msg = LaserScan ()
150144 r = rospy .Rate (10 )
151- spot_angle = ReturnValue ('spot_angle' )
152- spot_point = ReturnValue ('spot_point' )
153145 step = 0
154146 twist = Twist ()
155147 reset = Empty ()
148+
156149 while not rospy .is_shutdown ():
157150 msg = rospy .wait_for_message ("/scan" , LaserScan )
158151 odom = rospy .wait_for_message ("/odom" , Odometry )
159152 yaw = quaternion ()
160- scan_parking_spot ()
153+ scan_done , center_angle , start_angle , end_angle = scan_parking_spot ()
154+
161155 if step == 0 :
162- if spot_angle . stats == True :
163- finding_spot_position ()
164- if spot_point . stats == True :
165- theta = np .arctan2 (spot_point . data_1 [1 ] - spot_point . data_3 [1 ], spot_point . data_1 [0 ] - spot_point . data_3 [0 ])
156+ if scan_done == True :
157+ fining_spot , start_point , center_point , end_point = finding_spot_position (center_angle , start_angle , end_angle )
158+ if fining_spot == True :
159+ theta = np .arctan2 (start_point [1 ] - end_point [1 ], start_point [0 ] - end_point [0 ])
166160 print ("=================================" )
167161 print ("| | x | y |" )
168- print ('| start | {0:>10.3f}| {1:>10.3f}|' .format (spot_point . data_1 [0 ], spot_point . data_1 [1 ]))
169- print ('| center | {0:>10.3f}| {1:>10.3f}|' .format (spot_point . data_2 [0 ], spot_point . data_2 [1 ]))
170- print ('| end | {0:>10.3f}| {1:>10.3f}|' .format (spot_point . data_3 [0 ], spot_point . data_3 [1 ]))
162+ print ('| start | {0:>10.3f}| {1:>10.3f}|' .format (start_point [0 ], start_point [1 ]))
163+ print ('| center | {0:>10.3f}| {1:>10.3f}|' .format (center_point [0 ], center_point [1 ]))
164+ print ('| end | {0:>10.3f}| {1:>10.3f}|' .format (end_point [0 ], end_point [1 ]))
171165 print ("=================================" )
172166 print ('| theta | {0:.2f} deg' .format (np .rad2deg (theta )))
173167 print ('| yaw | {0:.2f} deg' .format (np .rad2deg (yaw )))
@@ -176,6 +170,7 @@ def scan_spot_filter(msg):
176170 step = 1
177171 else :
178172 print ("Fail finding parking spot." )
173+
179174 elif step == 1 :
180175 init_yaw = yaw
181176 yaw = theta + yaw
@@ -190,7 +185,7 @@ def scan_spot_filter(msg):
190185 time .sleep (1 )
191186 reset_pub .publish (reset )
192187 time .sleep (3 )
193- rotation_point = rotate_origin_only (spot_point . data_2 [0 ], spot_point . data_2 [1 ], - (pi / 2 - init_yaw ))
188+ rotation_point = rotate_origin_only (center_point [0 ], center_point [1 ], - (pi / 2 - init_yaw ))
194189 step = 2
195190 else :
196191 if theta - init_yaw < - 0.1 :
@@ -203,8 +198,9 @@ def scan_spot_filter(msg):
203198 time .sleep (1 )
204199 reset_pub .publish (reset )
205200 time .sleep (3 )
206- rotation_point = rotate_origin_only (spot_point . data_2 [0 ], spot_point . data_2 [1 ], - (pi / 2 - init_yaw ))
201+ rotation_point = rotate_origin_only (center_point [0 ], center_point [1 ], - (pi / 2 - init_yaw ))
207202 step = 2
203+
208204 elif step == 2 :
209205 if abs (odom .pose .pose .position .x - (rotation_point [1 ])) > 0.02 :
210206 if odom .pose .pose .position .x > (rotation_point [1 ]):
@@ -217,6 +213,7 @@ def scan_spot_filter(msg):
217213 twist .linear .x = 0.0
218214 twist .angular .z = 0.0
219215 step = 3
216+
220217 elif step == 3 :
221218 if yaw > - pi / 2 :
222219 twist .linear .x = 0.0
@@ -225,6 +222,7 @@ def scan_spot_filter(msg):
225222 twist .linear .x = 0.0
226223 twist .angular .z = 0.0
227224 step = 4
225+
228226 elif step == 4 :
229227 ranges = []
230228 for i in range (150 , 210 ):
@@ -240,4 +238,4 @@ def scan_spot_filter(msg):
240238 cmd_pub .publish (twist )
241239 sys .exit ()
242240 cmd_pub .publish (twist )
243- scan_spot_filter (msg )
241+ scan_spot_filter (msg , center_angle , start_angle , end_angle )
0 commit comments