def simple_parking(self): if self.net_image == []: return # rospy.sleep(1.0) self.network.load_weights( '/home/alvarado/catkin_emiliano/src/assigment_publisher_subscriber/src/net1.h5' ) gray = cv2.cvtColor(self.net_image, cv2.COLOR_BGR2GRAY) #bi_gray_max = 255 #bi_gray_min = 70 #ret,thresh1=cv2.threshold(gray, bi_gray_min, bi_gray_max, cv2.THRESH_BINARY) ret2, thresh1 = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) test_images = thresh1 print("Tamanio imagen: {}".format(test_images.shape)) test_images = cv2.resize( test_images, (56, 56), interpolation=cv2.INTER_AREA ) #esto es lo que se cambia para el tamamio de la imagen try: ros_img = self.bridge.cv2_to_imgmsg(test_images) self.img_pub.publish(ros_img) except CvBridgeError as e: print(e) test_images = test_images[0:28, 28:56] ############ imagenes_NN = test_images.reshape((1, 28 * 28)) imagenes_NN = imagenes_NN.astype('float32') / 255 resultado = self.network.predict_classes(imagenes_NN) ########### print("Resultado: {}".format(resultado)) return velocidad = NormalizedSpeedCommand() if resultado == 5: velocidad.value = 0.0 self.vel_pub.publish(velocidad) if resultado == 4: velocidad.value = 0.2 self.vel_pub.publish(velocidad) if resultado == 0: velocidad.value = velocidad.value self.vel_pub.publish(velocidad)
def simple_parking(self): if self.net_image == []: return #here are the uploadings of the weights self.classifier.load_weights( '/home/alvarado/catkin_emiliano/src/assigment_publisher_subscriber/src/net4.h5' ) #here the image is being converted into black and white gray = cv2.cvtColor(self.net_image, cv2.COLOR_BGR2GRAY) ret2, thresh1 = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) test_images = thresh1 print("Tamanio imagen: {}".format(test_images.shape)) test_images = cv2.resize(test_images, (56, 56), interpolation=cv2.INTER_AREA) test_images = test_images[ 0:28, 28: 56] #here image is being resizedinto something the computer can reads try: ros_img = self.bridge.cv2_to_imgmsg(test_images) self.img_pub.publish(ros_img) except CvBridgeError as e: print(e) ############ imagenes_NN = test_images.reshape((1, 28, 28, 1)) imagenes_NN = imagenes_NN.astype('float32') / 255 resultado = self.classifier.predict_classes(imagenes_NN) ########### print("Resultado: {}".format(resultado)) #return #here are set up the conditions for the numbers the net sees. velocidad = NormalizedSpeedCommand() if resultado == 5: velocidad.value = 0.5 self.vel_pub.publish(velocidad) if resultado == 4: velocidad.value = 0.0 self.vel_pub.publish(velocidad) if resultado == 0: velocidad.value = 0.3 self.vel_pub.publish(velocidad)
def shutdown(self): print("shutdown!") self.shutdown_ = True msg = NormalizedSpeedCommand() msg.value = 0 self.pub_speed.publish(msg) rospy.sleep(1)
def callback(data): pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() mensaje = NormalizedSpeedCommand() mensaje.header = h mensaje.value = data.value if data.value < 0: mensaje.value = data.value * -1 if data.value > 0: mensaje.value = data.value * -1 pub.publish(mensaje)
def initialize(): pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=1) steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=1) rospy.Subscriber("/carstate/steering", NormalizedSteeringCommand, queue_size=1) rospy.Subscriber("/carstate/speed", Speed, callback) rospy.Timer(5.0, timer) h = Header() h.stamp = rospy.Time.now() mensaje = NormalizedSpeedCommand() mensaje.header = h mensaje.value = -0.45 pub.publish(mensaje)
def simple_parking(self): # here comes the values I will asign to the topics # definition of message for speed velocidad = NormalizedSpeedCommand() # definition of message for steering direccion = NormalizedSteeringCommand() rospy.sleep(1.0) #here are the values assigned to speed amd steering # first values velocidad.value = -0.2 direccion.value = 0.7 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) # use this values for this period of time rospy.sleep(2.0) # second values velocidad.value = -0.2 direccion.value = -0.7 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) # use this values for this period of time rospy.sleep(1.5) #third values velocidad.value = 0.2 direccion.value = 0.2 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) rospy.sleep(1.0) # for stop, in this program the stop was by using ctrl+c velocidad.value = 0.0 direccion.value = 0.0 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) rospy.sleep(10.0)
def backwards_right(data, date): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.value) pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10) steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() mensaje = NormalizedSpeedCommand() mensaje.header = h mensaje.value = data.value j = std_msgs.msg.Header() j.stamp = rospy.Time.now() mensaje2 = NormalizedSteeringCommand() mensaje2.header = j mensaje2.value = date.value if date.value > 0: mensaje.vlaue = date.value * -1 pub.publish(mensaje)
def callback(self, data): dt = (data.header.stamp - self.last_time).to_sec() # 25hz if dt < 0.04: return self.last_time = data.header.stamp x = data.pose.pose.position.x y = data.pose.pose.position.y orientation_q = data.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) x_index_floor = int(math.floor(x * (100.0 / self.resolution))) y_index_floor = int(math.floor(y * (100.0 / self.resolution))) x_index_ceil = x_index_floor + 1 y_index_ceil = y_index_floor + 1 ceil_ratio_x = x * (100.0 / self.resolution) - x_index_floor ceil_ratio_y = y * (100.0 / self.resolution) - y_index_floor if x_index_floor < 0: x_index_floor = 0 if x_index_floor > self.map_size_x / self.resolution - 1: x_index_floor = self.map_size_x / self.resolution - 1 if y_index_floor < 0: y_index_floor = 0 if y_index_floor > self.map_size_y / self.resolution - 1: y_index_floor = self.map_size_y / self.resolution - 1 if x_index_ceil < 0: x_index_ceil = 0 if x_index_ceil > self.map_size_x / self.resolution - 1: x_index_ceil = self.map_size_x / self.resolution - 1 if y_index_ceil < 0: y_index_ceil = 0 if y_index_ceil > self.map_size_y / self.resolution - 1: y_index_ceil = self.map_size_y / self.resolution - 1 x3_floor, y3_floor = self.matrix[x_index_floor, y_index_floor, :] x3_ceil, y3_ceil = self.matrix[x_index_ceil, y_index_ceil, :] x3 = x3_floor * (1.0 - ceil_ratio_x) + x3_ceil * ceil_ratio_x y3 = y3_floor * (1.0 - ceil_ratio_y) + y3_ceil * ceil_ratio_y f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3 f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3 angle = np.arctan2(f_y, f_x) self.integral_error = self.integral_error + angle * dt steering = self.Kp * angle + self.Kd * ( (angle - self.last_angle) / dt) + self.Ki * self.integral_error self.last_angle = angle if f_x > 0: speed = -self.speed_value else: speed = self.speed_value if steering > 1: steering = 1 if steering < -1: steering = -1 if f_x > 0: speed = max(self.speed_value, speed * ((np.pi / 3) / (abs(steering) + 1))) steerMsg = NormalizedSteeringCommand() steerMsg.value = steering steerMsg.header.stamp = rospy.Time.now() self.pub.publish(steerMsg) if not self.shutdown_: msg = NormalizedSpeedCommand() msg.value = speed msg.header.stamp = rospy.Time.now() self.pub_speed.publish(msg)
import rospy import time import math import numpy as np from autominy_msgs.msg import NormalizedSpeedCommand, NormalizedSteeringCommand, SteeringFeedback, Speed from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseWithCovariance, Pose, Point, PointStamped from visualization_msgs.msg import Marker from std_msgs.msg import Header speed = 0.1 speed_cmd = NormalizedSpeedCommand() steering_cmd = NormalizedSteeringCommand() speed_cmd.value = speed punkt = Odometry() close = Marker() clicked = Marker() lookahead = Marker() isactiv = False steeringvalue = 0.0 gewollterWinkel = 0.0 lastgewollterWinkel = 0.0 kurvenverlangsamung = 1.0 #Muss noch angepasst werden, konnte aber empierisch nicht getan werden, da das gps aus war kp = 4.0 ki = 0.1 kd = 0.2 lasterror = 0.0 ierror = 0.0 derror = 0.0
def PID_steering(self, raw_msgs): #retrieving values from message x = raw_msgs.pose.pose.position.x y = raw_msgs.pose.pose.position.y orientation = raw_msgs.pose.pose.orientation orientation_array = [ orientation.x, orientation.y, orientation.z, orientation.w ] #change the read-out data from Euler to Rad #(roll, pitch, yaw) = (orientation_array) orientation_in_Rad = tf.transformations.euler_from_quaternion( orientation_array) self.yaw = orientation_in_Rad[2] x_ind = np.int(x * (100.0 / self.resolution)) y_ind = np.int(y * (100.0 / self.resolution)) #if abroad, car virtually set on the map if x_ind < 0: x_ind = 0 if x_ind > self.map_size_x / self.resolution - 1: x_ind = self.map_size_x / self.resolution - 1 if y_ind < 0: y_ind = 0 if y_ind > self.map_size_y / self.resolution - 1: y_ind = self.map_size_y / self.resolution - 1 x_map, y_map = self.matrix[x_ind, y_ind] x_car = np.cos(self.yaw) * x_map + np.sin(self.yaw) * y_map y_car = -np.sin(self.yaw) * x_map + np.cos(self.yaw) * y_map #hyperparameters for PID Kp = 3.0 Ki = 0.0 Kd = 0.0 #so the error is the steepness of the correction anlge error = np.arctan2(y_car, x_car) #print(x_ind, y_ind, x_car, y_car) #pseudo integral memory in borders self.aq_error = self.aq_error + error if self.aq_error > 10: self.aq_error = 10 elif self.aq_error < -10: self.aq_error = -10 #time between measurements for delta t current_time = rospy.Time.now() dif_time = (current_time - self.last_time).to_sec() #if dif_time == 0.0: # return #error manipulation with PID PID = Kp * error + Ki * self.aq_error * dif_time + Kd * ( error - self.last_error) / dif_time #print(PID) self.last_time = current_time self.last_error = error #detect min dist direction vel = NormalizedSpeedCommand() excep = False if self.distance > 200 or self.distance < 0: #dis= 400 vel.value = self.speed_value print('No Control') else: try: self.PID_speed() print("Distace Control") print("delta speed is:", self.delta_speed) print("delta_distance is :", self.delta_dis) print("acceleration is :", self.acc) #vel.value = (self.ego_speed / 1.95 + self.acc *0 ) #vel.value = (self.ego_speed / 3 + self.acc / 50 + 0.1) #acc/100 vel.value = (self.ego_speed / 7 + self.acc / 10 + 0.04) #print("tar speed is:" , self.tar_speed) #print("ego speed :" , self.ego_speed) print("new speed is :", vel.value) except: print("Exception") excep = True if vel.value > 0.19: vel.value = 0.19 if PID > 1: PID = 1 elif PID < -1: PID = -1 str_val = NormalizedSteeringCommand() str_val.value = PID #dont start acceleration after shutdown if not self.publishing: self.str_pub.publish(str_val) if not excep: self.vel_pub.publish(vel)
def call_back(self, raw_msgs): #retrieving values from message x = raw_msgs.pose.pose.position.x y = raw_msgs.pose.pose.position.y #self.position.append((x,y)) orientation = raw_msgs.pose.pose.orientation orientation_array = [ orientation.x, orientation.y, orientation.z, orientation.w ] #change the read-out data from Euler to Rad orientation_in_Rad = tf.transformations.euler_from_quaternion( orientation_array) self.yaw = orientation_in_Rad[2] x_ind = np.int(x * (100.0 / self.resolution)) y_ind = np.int(y * (100.0 / self.resolution)) #if abroad, car virtually set on the map if x_ind < 0: x_ind = 0 if x_ind > self.map_size_x / self.resolution - 1: x_ind = self.map_size_x / self.resolution - 1 if y_ind < 0: y_ind = 0 if y_ind > self.map_size_y / self.resolution - 1: y_ind = self.map_size_y / self.resolution - 1 x_map, y_map = self.matrix[x_ind, y_ind] #print(x_ind,y_ind) #if we imagine the odometry as polar coordinates #we have some radius r and a angle phi #so now we combine the steering angle given from the force field alpha and phi #with x = r * (cos(alpha-phi)) and y = (sin(alpha-phi)) #maybe it was phi - alpha in both i wrote it on a paper #formula was given on the assignment sheet x_car = np.cos(self.yaw) * x_map + np.sin(self.yaw) * y_map y_car = -np.sin(self.yaw) * x_map + np.cos(self.yaw) * y_map #hyperparameters for PID Kp = 4.2 Ki = 0.001 Kd = 0.0 #so the error is the steepness of the correction anlge error = np.arctan2(y_car, x_car) #print(x_ind, y_ind, x_car, y_car) #pseudo integral memory in borders self.aq_error = self.aq_error + error if self.aq_error > 10: self.aq_error = 10.0 elif self.aq_error < -10: self.aq_error = -10.0 #time between measurements for delta t current_time = rospy.Time.now() dif_time = (current_time - self.last_time).to_sec() if dif_time == 0.0: return #error manipulation with PI PID = Kp * error + Ki * self.aq_error * dif_time + Kd * ( error - self.last_error) / dif_time #print(PID) self.last_time = current_time self.last_error = error #detect min dist direction vel = NormalizedSpeedCommand() if (x_car < 0): vel.value = -self.speed_value else: vel.value = self.speed_value #if x_car > 0: #reduce speed based on steering #vel.value = max(self.speed_value, vel.value * ((np.pi/3.0)/(abs(PID)+1.0))) #valid steering angle -100<=alpha<=80 if PID > 1: PID = 1.0 elif PID < -1: PID = -1.0 #offset 100 == straight ahead #PID = 180 - (90 + PID) #print("yaw: {0}, vff_angle: {1}".format(np.rad2deg(self.yaw), np.rad2deg(np.arctan(y_map / x_map)))) #print(np.rad2deg(error),UInt8(PID)) str_val = NormalizedSteeringCommand() str_val.value = PID #dont start acceleration after shutdown if not self.publishing: self.str_pub.publish(str_val) self.vel_pub.publish(vel)
def call_back(self, raw_msgs): #retrieving values from message x = raw_msgs.pose.pose.position.x y = raw_msgs.pose.pose.position.y orientation = raw_msgs.pose.pose.orientation orientation_array = [ orientation.x, orientation.y, orientation.z, orientation.w ] #change the read-out data from Euler to Rad #(roll, pitch, yaw) = (orientation_array) orientation_in_Rad = tf.transformations.euler_from_quaternion( orientation_array) self.yaw = orientation_in_Rad[2] x_ind = np.int(x * (100.0 / self.resolution)) y_ind = np.int(y * (100.0 / self.resolution)) #if abroad, car virtually set on the map if x_ind < 0: x_ind = 0 if x_ind > self.map_size_x / self.resolution - 1: x_ind = self.map_size_x / self.resolution - 1 if y_ind < 0: y_ind = 0 if y_ind > self.map_size_y / self.resolution - 1: y_ind = self.map_size_y / self.resolution - 1 x_map, y_map = self.matrix[x_ind, y_ind] x_car = np.cos(self.yaw) * x_map + np.sin(self.yaw) * y_map y_car = -np.sin(self.yaw) * x_map + np.cos(self.yaw) * y_map #hyperparameters for PID Kp = 3.0 Ki = 0.0 Kd = 0.0 #so the error is the steepness of the correction anlge error = np.arctan2(y_car, x_car) #print(x_ind, y_ind, x_car, y_car) #pseudo integral memory in borders self.aq_error = self.aq_error + error if self.aq_error > 10: self.aq_error = 10 elif self.aq_error < -10: self.aq_error = -10 #time between measurements for delta t current_time = rospy.Time.now() dif_time = (current_time - self.last_time).to_sec() if dif_time == 0.0: return #error manipulation with PID PID = Kp * error + Ki * self.aq_error * dif_time + Kd * ( error - self.last_error) / dif_time #print(PID) self.last_time = current_time self.last_error = error #detect min dist direction vel = NormalizedSpeedCommand() if (x_car < 0): vel.value = self.speed_value else: vel.value = -self.speed_value if x_car > 0: #reduce speed based on steering vel.value = max(self.speed_value, vel.value * ((np.pi / 3.0) / (abs(PID) + 1.0))) #valid steering angle -100<=alpha<=80 if PID > 1: PID = 1 elif PID < -1: PID = -1 str_val = NormalizedSteeringCommand() str_val.value = PID #dont start acceleration after shutdown if not self.publishing: self.str_pub.publish(str_val) self.vel_pub.publish(vel)
def PID_call_back(self, raw_msgs): #retrieving values from message x = raw_msgs.pose.pose.position.x y = raw_msgs.pose.pose.position.y orientation = raw_msgs.pose.pose.orientation orientation_array = [ orientation.x, orientation.y, orientation.z, orientation.w ] #change the read-out data from Euler to Rad #(roll, pitch, yaw) = (orientation_array) orientation_in_Rad = tf.transformations.euler_from_quaternion( orientation_array) self.yaw = orientation_in_Rad[2] x_ind = np.int(x * (100.0 / self.resolution)) y_ind = np.int(y * (100.0 / self.resolution)) #if abroad, car virtually set on the map if x_ind < 0: x_ind = 0 if x_ind > self.map_size_x / self.resolution - 1: x_ind = self.map_size_x / self.resolution - 1 if y_ind < 0: y_ind = 0 if y_ind > self.map_size_y / self.resolution - 1: y_ind = self.map_size_y / self.resolution - 1 x_map, y_map = self.matrix[x_ind, y_ind] x_car = np.cos(self.yaw) * x_map + np.sin(self.yaw) * y_map y_car = -np.sin(self.yaw) * x_map + np.cos(self.yaw) * y_map #hyperparameters for PID Kp = 3.0 Ki = 0.00001 Kd = 0.0003 #so the error is the steepness of the correction anlge error = np.arctan2(y_car, x_car) #print(x_ind, y_ind, x_car, y_car) #pseudo integral memory in borders self.aq_error = self.aq_error + error if self.aq_error > 10: self.aq_error = 10 elif self.aq_error < -10: self.aq_error = -10 #time between measurements for delta t current_time = rospy.Time.now() dif_time = (current_time - self.last_time).to_sec() #if dif_time == 0.0: # return #error manipulation with PID PID = Kp * error + Ki * self.aq_error * dif_time + Kd * ( error - self.last_error) / dif_time self.last_time = current_time self.last_error = error #detect min dist direction vel = NormalizedSpeedCommand() excep = False if self.distance > 200 or self.distance < 0: #dis= 400 vel.value = self.speed_value else: try: self.fuzzy_controller() print("ACC is active") print("delta speed is:", self.delta_speed) print("delta_distance is :", self.delta_dis) print("acceleration is :", self.acc) #vel.value = (self.ego_speed / 1.95 + self.acc *0 ) # low level control self.ACC_speed_x = (self.ego_speed / 6.75 + self.acc / 7 + 0.058) self.ACC_speeds.append(self.ACC_speed_x) self.ACC_speed = 0.0 for speed in self.ACC_speeds: self.ACC_speed += speed / float(len(self.ACC_speeds)) vel.value = self.ACC_speed #print("tar speed is:" , self.tar_speed) #print("ego speed :" , self.ego_speed) print("new speed is :", vel.value) except: print("Exception") excep = True if vel.value > 0.19: vel.value = 0.19 #if vel.value < 0.09: # vel.value = 0.1 #if x_car > 0: #reduce speed based on steering #vel.value = max(self.speed_value, vel.value * ((np.pi/3.0)/(abs(PID)+1.0))) #valid steering angle -100<=alpha<=80 if PID > 1: PID = 1 elif PID < -1: PID = -1 str_val = NormalizedSteeringCommand() str_val.value = PID #dont start acceleration after shutdown if not self.publishing: self.str_pub.publish(str_val) if not excep: self.vel_pub.publish(vel)
# pub_forward = rospy.Publisher( # "simple_drive_control/forward", # Float32, # queue_size=10) # rospy.loginfo('Publisher(simple_drive_control/forward') # rospy.sleep(1.0) # publisher_steering.publish(norm_steering_left) # publisher_speed.publish(move_speed) positions_and_ticks = [] steering_angles = [] steering_angles.append(steering_angle_power) drive_command = NormalizedSpeedCommand() drive_command.value = 0.067 stop_command = NormalizedSpeedCommand() stop_command.value = 0.0 steering_cmd = NormalizedSteeringCommand() steering_cmd.value = 1.0 # publisher_steering.publish() publisher_steering.publish(steering_cmd) rospy.loginfo('publisher_steering.publish(steering_cmd={})'.format( steering_cmd.value)) rospy.sleep(0.5) # drive for a while
# "simple_drive_control/forward", # Float32, # queue_size=10) # rospy.loginfo('Publisher(simple_drive_control/forward') # rospy.sleep(1.0) # publisher_steering.publish(norm_steering_left) # publisher_speed.publish(move_speed) positions = [] steering_angles = [] positions.append((x_position, y_position)) steering_angles.append(steering_angle_power) drive_command = NormalizedSpeedCommand() drive_command.value = 0.33 stop_command = NormalizedSpeedCommand() stop_command.value = 0.0 steering_cmd = NormalizedSteeringCommand() steering_cmd.value = 0.0 # publisher_steering.publish() publisher_steering.publish(steering_cmd) rospy.loginfo('publisher_steering.publish(steering_cmd={})'.format( steering_cmd.value)) rospy.sleep(0.5) # drive for a while
def scan_callback(scan_msg): global initial_line, wall_angle, add_pi, last_theta, speed, speed_value, max_y, target_angle, steering_angle_feedback, invert_sign_gamma radius = np.asarray(scan_msg.ranges) angles = np.arange(scan_msg.angle_min, scan_msg.angle_max + scan_msg.angle_increment / 2, scan_msg.angle_increment) mask_fin = np.isfinite(radius) # only consider finite radii if mask_angles: if (abs(target_angle - wall_angle) < np.pi / 4): target_angle = wall_angle # right side of the car angle_spread = np.pi / 4 # size of cone to consider #if Lidar installed inversed! #mask_angle = np.logical_or((-np.pi+target_angle + angle_spread) > angles, angles > (np.pi+target_angle - angle_spread)) #if Lidar installed direct! mask_angle = np.logical_or((target_angle + angle_spread) > angles, angles > (target_angle - angle_spread)) mask = np.logical_and(mask_fin, mask_angle) else: mask = mask_fin masked_angles = angles[mask] masked_radius = radius[mask] # calculate coordinates of our masked values x = np.cos(masked_angles) * masked_radius y = np.sin(masked_angles) * masked_radius X = np.ones((np.size(x), 3)) X[:, 0] = x X[:, 2] = y points = np.column_stack((x, y)) slope, intercept = find_best_params( points) # detect a line in these coordinates # slope, intercept = np.linalg.lstsq(X[:,0:2],X[:,2], rcond=-1)[0] # wall_dist = get_distance((0, 0), slope, intercept) # shortest distance from current position to the wall wall_angle = np.arctan(slope) # angle of the wall if ((wall_angle > 0) and (wall_angle < np.pi / 2)): wall_angle = wall_angle - np.pi / 2 else: wall_angle += np.pi / 2 wall_dist = abs(intercept) / pow( pow(slope, 2) + 1.0, 0.5) # shortest distance from current position to the wall if initial_line is None or scan_msg.header.stamp < initial_line.stamp: initial_line = LineParams(slope, intercept, wall_dist, wall_angle, scan_msg.header.stamp) del turn_radii[:] print 'starting to drive: wall_dist: %.3f, wall_angle: %.1f ' % ( wall_dist, np.rad2deg(wall_angle)) if not manual_mode: steer_msg = SteeringCommand() steer_msg.value = steering_angle pub_steering.publish(steer_msg) rospy.sleep(2.0) speed_msg = NormalizedSpeedCommand() speed = -speed_value speed_msg.value = speed pub_speed.publish(speed_msg) else: theta = angle_diff(initial_line.wall_angle, wall_angle) dist_diff = initial_line.wall_dist - wall_dist abs_theta = abs(theta) t = abs(dist_diff) / np.cos((initial_line.wall_angle + wall_angle) / 2) phi = (np.pi - abs_theta) / 2 time_diff = scan_msg.header.stamp - initial_line.stamp if (abs_theta > 0.06) and (abs_theta < 1.2) and (speed < 0): if (theta < 0): invert_sign_gamma = True else: invert_sign_gamma = False last_theta = theta turn_radius = t * np.sin(phi) / np.sin(abs_theta) time_offset = time_diff.secs + time_diff.nsecs * 1e-9 # reset turn_radii plot if rosbag loops if len(turn_radii) and time_offset < turn_radii[-1][0]: del turn_radii[:] turn_radii.append([theta, turn_radius]) servo_feedback.append(steering_angle_feedback) print( 'current wall dist: %.3f, dist diff: %.3f, angle diff: %.1f, current angle: %.1f, start angle: %.1f, turn radius: %.3f' % (wall_dist, dist_diff, np.rad2deg(theta), np.rad2deg(wall_angle), np.rad2deg( initial_line.wall_angle), turn_radius)) else: turn_radius = np.NAN # dividing by such a small value leads very unexpected results # check if we have been driving long enough: if not manual_mode and time_diff.secs >= drive_duration_max: # pub_speed.publish(0) # np.savez_compressed('angle-%03d-%s.txt' % (steering_angle, strftime('%H-%M-%S', localtime())), turn_radii) # speed = speed_value speed_msg = NormalizedSpeedCommand() speed_msg.value = speed pub_speed.publish(speed_msg) print(steering_angle) if (900 > steering_angle and steering_angle > 2350): if wall_dist < 1.2: if len(turn_radii) > 0: average_r = 0 for r in turn_radii: average_r += r[1] average_r = average_r / float(len(turn_radii)) feedback = np.average(servo_feedback) print('average turn radius: %.3f' % average_r) l = 0.26 # 26cm gamma = np.arcsin(l / average_r) if (invert_sign_gamma == True): gamma = -gamma save_xml(int(steering_angle), average_r, gamma, feedback) else: print "turn radius is nan!!" stop_driving() elif (abs(wall_angle) < 0.1): if (len(turn_radii) > 0): average_r = 0 for r in turn_radii: average_r += r[1] average_r = average_r / float(len(turn_radii)) feedback = np.average(servo_feedback) print('average turn radius: %.3f' % average_r) l = 0.26 # 26cm gamma = np.arcsin(l / average_r) if (invert_sign_gamma == True): gamma = -gamma save_xml(int(steering_angle), average_r, gamma, feedback) else: print "turn radius is nan!!" stop_driving() if plotting: ax_a.cla() ax_a.set_title('Scatter plot of laser scan data') # plot inliers detected by ransac inlier_mask = get_inliers(points, slope, intercept) ax_a.scatter(*points[np.where(inlier_mask)].T, color='r') ax_a.scatter(*points[np.where(np.logical_not(inlier_mask))].T, color='b') # plot any filtered points inv_mask = np.logical_not(mask) other_x = np.cos(angles[inv_mask]) * radius[inv_mask] other_y = np.sin(angles[inv_mask]) * radius[inv_mask] ax_a.scatter(other_x, other_y, color='k') # how many meters to plot in each direction plt_window = 3.5 ax_a.set_xlim([-plt_window, plt_window]) ax_a.set_ylim([-plt_window, plt_window]) line_x = np.linspace(-plt_window, plt_window, 10) line_y = intercept + line_x * slope ax_a.plot(line_x, line_y, color='b') # draw coordinate system ax_a.axvline(0, color='k') ax_a.axhline(0, color='k') ax_b.cla() ax_b.set_title('turn radius over the difference angle') ax_b.plot(*np.array(turn_radii).T, marker='o') # ax_b.plot(*np.array(radius_theta).T, marker='o') if len(turn_radii) > 0 and (max_y < turn_radii[-1][1]): max_y = turn_radii[-1][1] ax_b.set_ylim([0, max_y]) # cut off very large outliers plt.show(block=False) rospy.sleep(0.1) # sleep to avoid threading issues with plotting