def shutdown(self):
print("shutdown!")
self.shutdown_ = True
msg = NormalizedSpeedCommand()
msg.value = 0
self.pub_speed.publish(msg)
rospy.sleep(1)
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):
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):
#set speed to 0
print("shutdown!")
self.publishing = True
msgs = NormalizedSpeedCommand()
self.vel_pub.publish(msgs)
rospy.sleep(1)
def shutdown(self):
#set speed to 0
print("shutdown!")
self.publishing = True
msgs = NormalizedSpeedCommand()
self.vel_pub.publish(msgs)
rospy.sleep(1)
np.savetxt('test_new_39.txt', self.test_data)
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(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 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)
#!/usr/bin/env python 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
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 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
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)
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 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)
def stop_driving():
pub_speed.publish(NormalizedSpeedCommand())
rospy.sleep(1)
rospy.signal_shutdown('stop')
print('stop driving')
print('close the plot to stop the program!')