def forward(self, throttle, steer_angle):
"""forward generates a forward-moving Control msg with throttle and steering angle
:param throttle: throttle in range [-1.0, 1.0]
:param steer_angle: steering angle in range [-1.0, 1.0]
if throttle < 0.0 then apply brakes
"""
assert steer_angle >= -1.0 and steer_angle <= 1.0, "Require steer angle to be in range [-1.0, 1.0]: {0}".format(
steer_angle)
brake = 0.0
if throttle < 0.0:
brake = abs(throttle)
throttle = 0.0
assert brake >= 0.0 and brake <= 1.0, "Brake must be in range [0.0, 1.0]: {0}".format(
brake)
assert throttle >= 0.0 and throttle <= 1.0, "Throttle must be in range [0.0, 1.0]: {0}".format(
throttle)
msg = Control()
self.populate_header(msg)
msg.throttle = throttle # throttle as given
msg.steer = steer_angle # steering angle as given
msg.brake = brake # braking amount
msg.shift_gears = 2 # forward gearing
return msg
def vel_and_angle(data):
global speed_real #chamada para a variavel global
global count
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo atualizado da leitura
msg.header.frame_id = "base_link"
msg.steer = data.angle * ANG_TO_RAD #angulacao desejada para as rodas (0-30) GRAUS
print "----START-vel_and_angle----------"
print "vel=", data.velocity
print "speed_real=", speed_real
#A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
msg.throttle, msg.brake = calc_throttle_brake(
data.velocity, speed_real) #funcao que calcul aceleracao e frenagem
# if count < 100:
# print "count: ",count
# msg.throttle = 1.0
# msg.brake = 0.0
# else:
# print "count: ",count
# msg.throttle = 0.0
# msg.brake = 0.0
#print "Throttle=",msg.throttle
#print "brake=",msg.brake
#print "angle=",data.angle
#print "----END-vel_and_angle----------"
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def handle_incoming_car(msg):
control = Control()
control.shift_gears = 2
print('Receiving command', msg)
args = msg.split(',')
speed_level = int(args[7])
x = float(args[5])
y = float(args[6])
deg = xy2deg(x, y)
delta = 15
if (45 + delta) <= deg <= (135 - delta): # forward
control.throttle = 0.2 * speed_level
elif (225 + delta) <= deg <= (315 - delta): #backward
control.brake = 1.0
elif (135 - delta) <= deg <= (225 + delta): #left
control.throttle = 0.2 * speed_level
control.steer = 0.2 * speed_level
if deg >= 180:
control.shift_gears = 3
else: #right
control.throttle = 0.2 * speed_level
control.steer = -0.2 * speed_level
if 270 <= deg <= 360:
control.shift_gears = 3
vel_pub.publish(control)
def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber("/prius/front_camera/image_raw",
Image, self.camera_callback)
self.cmd_vel_pub = rospy.Publisher('prius', Control, queue_size=20)
self.twist = Control()
def drive(self):
"""
return control object
"""
if self.sign_detected == (0,0):
rospy.loginfo("No sign detected, should be normal operation")
# in the case where there is no sign, set normal values
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle= self.normal_throttle,brake= self.normal_brake, steer=0 ,shift_gears=self.normal_gears)
else:
rospy.loginfo("sign detected")
area = self.sign_detected[1]
matches = self.sign_detected[0]
if self.control.brake == 1.0 and
if matches>0:
# end all be all
rospy.loginfo("Many matches, braking hard")
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
else:
# handle the case where there is a sign
rospy.loginfo("No matches, but area")
if area > self.area_threshold_cut_throttle:
rospy.loginfo("cutting throttle")
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=0 , steer=0 ,shift_gears=self.normal_gears)
elif area > self.area_threshold_apply_brakes:
rospy.loginfo("applying brake")
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake= 1.0, steer=0 ,shift_gears=self.normal_gears )
self.prius_control_publisher.publish(self.control)
def sign_callback(self, msg):
"""
grab sign positions
"""
self.sign_detected = (msg.matches, msg.area)
def stop_the_fucking_car(self):
rospy.loginfo("stopping the f*****g car")
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
self.prius_control_publisher.publish(self.control)
def run(self):
r = rospy.Rate(5)
while not rospy.is_shutdown():
self.drive()
r.sleep()
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global correct_v
global correct_yaw
global v
global yaw
global last_recorded_vel
global last_recorded_ang
global kp
global ki
global kd
global dt
global integral
global last_error
prius_vel = Control()
target_vel = data.linear.x
print("Command Velocity: "),
if (abs(target_vel) < 0.2):
target_vel = 0
print("0.0")
if (target_vel >= 0.2):
prius_vel.shift_gears = 2
print("+"),
print(target_vel)
if (target_vel <= -0.2):
prius_vel.shift_gears = 3
print(target_vel)
integral = integral + ki * (target_vel - last_recorded_vel)
integral = min(3, max(-3, integral))
v = kp * (target_vel - last_recorded_vel) + integral + kd * (
target_vel - last_recorded_vel - last_error)
v = min(1, max(-1, v))
last_error = (target_vel - last_recorded_vel)
#v = data.linear.x + correct_v
yaw = data.angular.z + correct_yaw
steering = convert_trans_rot_vel_to_steering_angle(v, yaw, wheelbase)
if (target_vel <= -0.2):
v = v * -1.0
# msg = AckermannDriveStamped()
# msg.header.stamp = rospy.Time.now()
# msg.header.frame_id = frame_id
# msg.drive.steering_angle = steering
# msg.drive.speed = v
prius_vel.throttle = v
prius_vel.brake = 0
prius_vel.steer = steering / 3.14
if (prius_vel.throttle < 0):
prius_vel.brake = 1
#pub.publish(msg)
pub.publish(prius_vel)
def vehicle_general_control():
#TV data
global longitude_TV
global latitude_TV
global heading_TV
global speed_TV #chamada para a variavel global
global steering_TV #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_TV #percentual do acelerador (0 - 1)
global brake_TV #percentual do freio (0 - 1)
#SV data
global longitude_SV
global latitude_SV
global heading_SV
global speed_SV #chamada para a variavel global
global steering_SV #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_SV #percentual do acelerador (0 - 1)
global brake_SV #percentual do freio (0 - 1)
global publication_topic
global erro_vel
pub = rospy.Publisher(publication_topic, Control,
queue_size=10) #topico de controle do carro
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo SVizado da leitura
msg.header.frame_id = "base_link"
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-30) GRAUS
#calcula distancia
distance = ((longitude_TV - longitude_SV)**2 +
(latitude_TV - latitude_SV)**2)**0.5
#print "Distance", distance
maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME
distance_error = distance - maximum_distance
print "------------------------------------------------------"
#ajusta velocidade e aceleracao
if (speed_TV >= EDGE_LOW_SPEED and distance_error >= 0.0):
print "TV moving"
#acao_velocidade = controla_velocidade (speed_TV, speed_SV) #m/s
acao_velocidade = distance_control(speed_TV, speed_SV, distance_error)
msg.steer = direction_control(heading_TV, heading_SV)
msg.throttle, msg.brake = calc_throttle_brake(acao_velocidade,
speed_TV)
else:
print "TV stopped"
msg.throttle = 0.0
msg.brake = 1.0
#print "throttle", msg.throttle
#print "brake", msg.brake
#Atualiza topico carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def controla_veiculo():
#dados do lider
global longitude_leader
global latitude_leader
global heading_leader
global speed_leader #chamada para a variavel global
global steering_leader #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_leader #percentual do acelerador (0 - 1)
global brake_leader #percentual do freio (0 - 1)
#dados do veiculo
global longitude_atual
global latitude_atual
global heading_atual
global speed_atual #chamada para a variavel global
global steering_atual #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_atual #percentual do acelerador (0 - 1)
global brake_atual #percentual do freio (0 - 1)
global publication_topic
global erro_vel
pub = rospy.Publisher(publication_topic, Control,
queue_size=10) #topico de controle do carro
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo atualizado da leitura
msg.header.frame_id = "base_link"
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-30) GRAUS
#calcula distancia
distance = ((longitude_leader - longitude_atual)**2 +
(latitude_leader - latitude_atual)**2)**0.5
#print "Distance", distance
maximum_distance = MINIMUM_DISTANCE + speed_leader * 2
#ajusta velocidade e aceleracao
if (speed_leader >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE):
print "Leader moving"
acao_velocidade = controla_velocidade(speed_leader, speed_atual) #m/s
msg.throttle, msg.brake = calc_throttle_brake(acao_velocidade,
speed_leader)
else:
print "Leader stopped"
msg.throttle = 0.0
msg.brake = 1.0
print "throttle", msg.throttle
print "brake", msg.brake
#ajusta heading
#atualiza topico carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def vel_and_angle(data):
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo atualizado da leitura
msg.header.frame_id = "base_link"
msg.throttle = data.velocity #aceleracao desejada (0-1)
#msg.brake = data.brake #frenagem desejada (0-1)
msg.steer = data.angle #angulacao desejada para as rodas (0-30) GRAUS OU RADIANOS?
if data.velocity == 0:
msg.brake = 1
print "vel=", data.velocity
print "angle=", data.angle
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def vel_and_angle(data):
global speed_real #chamada para a variavel global
global count
global car_name_TV
var_print = OFF
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo atualizado da leitura
msg.header.frame_id = "base_link"
msg.steer = data.angle * ANG_TO_RAD #angulacao desejada para as rodas (0-30) GRAUS
if var_print:
print "----START-vel_and_angle----------"
print "vel: ", data.velocity
print "speed_real: ", speed_real
print "data_angle: ", data.angle
print "steer: ", data.angle * ANG_TO_RAD
if (data.angle * ANG_TO_RAD) > 0.05 and REDUCED_SPEED:
#A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
msg.throttle, msg.brake = calc_throttle_brake(
VEL_FRENAGEM, speed_real) #funcao que calcul aceleracao e frenagem
else:
#A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
msg.throttle, msg.brake = calc_throttle_brake(
data.velocity,
speed_real) #funcao que calcul aceleracao e frenagem
# if count < 100:
# print "count: ",count
# msg.throttle = 1.0
# msg.brake = 0.0
# else:
# print "count: ",count
# msg.throttle = 0.0
# msg.brake = 0.0
#print "Throttle=",msg.throttle
#print "brake=",msg.brake
#print "angle=",data.angle
#print "----END-vel_and_angle----------"
pub = rospy.Publisher(car_name_TV + 'prius', Control,
queue_size=10) #topico de controle do carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def sendCtrlCmdsToGazebo(self):
"""
Send control commands to Gazebo, which updates the robot state according to its inertial model.
"""
command_msg = Control()
command_msg.header.stamp = rospy.get_rostime()
command_msg.throttle = self.throttle
command_msg.brake = 0.0
command_msg.shift_gears = Control.FORWARD
steer = self.steering
command_msg.steer = steer
self.control_pub.publish(command_msg)
position_msg = PointStamped()
position_msg.header.stamp = rospy.get_rostime()
position_msg.point.x = self.lin_position
self.linear_position_pub.publish(position_msg)
return
def control_test():
pub = rospy.Publisher("/prius", Control, queue_size=1)
rospy.init_node('sim_control', anonymous=True)
rate = rospy.Rate(10) # 10hz
command = Control()
command.header.stamp = rospy.Time.now()
command.throttle = 0.2
command.brake = 0
command.steer = 0.0
command.shift_gears = Control.NO_COMMAND
# while not rospy.is_shutdown():
for i in range(10):
pub.publish(command)
rate.sleep()
for i in range(10):
command.brake = 0.5
pub.publish(command)
rate.sleep()
def vel_and_angle(data): global speed_real #chamada para a variavel global msg = Control() #Define msg como um dado do tipo Control -> comando de controle do veiculo msg.header.stamp = rospy.Time.now(); #tempo atualizado da leitura msg.header.frame_id = "base_link"; #msg.throttle = data.velocity #aceleracao desejada (0-1) #msg.brake = data.brake #frenagem desejada (0-1) msg.steer = data.angle #angulacao desejada para as rodas (0-30) GRAUS OU RADIANOS? print "------------------------------------------" print "vel=", data.velocity print "speed_real=", speed_real """ erro_vel = data.velocity/3.6 - speed_real print "erro_vel=", erro_vel erro_vel_norm = (erro_vel - MIN_SPEED) / (MAX_SPEED - MIN_SPEED) print "erro_vel_norm=", erro_vel_norm accel_control = min(erro_vel_norm,1.0) print "accel_control=", accel_control accel_control = max(accel_control,-1.0) print "accel_control=", accel_control if(abs(accel_control) > 0.05): msg.throttle = max(0.0,accel_control) msg.brake = max(0.0,-accel_control) elif (data.velocity == 0.0): msg.throttle = 0.0 msg.brake = 1.0 """ msg.throttle, msg.brake = calc_throttle_brake (data.velocity, speed_real) #if data.velocity == 0: # msg.brake = 1 print "Throttle=",msg.throttle print "brake=",msg.brake #print "angle=",data.angle pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
def prius_pub(data):
'''
publishes the velocity and steering angle
published on topic : ackermann_cmd_topic
'''
global prius_vel
prius_vel = Control()
if (data.linear.x > 0):
prius_vel.throttle = data.linear.x / 100
prius_vel.brake = 0
print("acc")
print(prius_vel.throttle)
if (data.linear.x < 0):
prius_vel.brake = -data.linear.x / 100
prius_vel.throttle = 0
print("brake")
print(prius_vel.brake)
prius_vel.steer = data.angular.z / 30
pub.publish(prius_vel)
def callback(self, message):
rospy.logdebug("joy_translater received axes %s", message.axes)
command = Control()
command.header = message.header
if message.axes[THROTTLE_AXIS] >= 0:
command.throttle = message.axes[THROTTLE_AXIS]
command.brake = 0.0
else:
command.brake = message.axes[THROTTLE_AXIS] * -1
command.throttle = 0.0
if message.buttons[3]:
command.shift_gears = Control.FORWARD
elif message.buttons[1]:
command.shift_gears = Control.NEUTRAL
elif message.buttons[0]:
command.shift_gears = Control.REVERSE
else:
command.shift_gears = Control.NO_COMMAND
command.steer = message.axes[STEERING_AXIS]
self.last_published = message
self.pub.publish(command)
def callback(self, message):
throttle_value = message.linear.x
gears_value = message.linear.z
steering_value = message.angular.z
rospy.logdebug("joy_translater received Throttle value " +
str(throttle_value))
rospy.logdebug("joy_translater received Gears value " +
str(gears_value))
rospy.logdebug("joy_translater received Steering value " +
str(steering_value))
command = Control()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
command.header = header
if throttle_value >= 0:
command.throttle = throttle_value
command.brake = 0.0
else:
command.brake = throttle_value * -1
command.throttle = 0.0
if gears_value > 0.0:
command.shift_gears = Control.FORWARD
elif gears_value == 0.0:
command.shift_gears = Control.NEUTRAL
elif gears_value < 0.0:
command.shift_gears = Control.REVERSE
else:
command.shift_gears = Control.NO_COMMAND
command.steer = steering_value
self.last_published = message
self.pub.publish(command)
def callback_path(data):
'''
calculates target path point and the steering angle
:param data [Path]
:param ep [float]
:param cp [int]
'''
global ep # min distance
global cp # index of closest point
global ep_max
global ep_sum
global ep_avg
global n
global cp1
global path_length
cross_err = Twist()
x_p = data
# calculate minimum distance
calc_path_length(x_p)
distances = []
for i in range(len(x_p.poses)):
a = x_p.poses[i]
distances += [dist(a, x_bot, y_bot)]
ep = min(distances)
ep1 = ep
if (ep > ep_max):
ep_max = ep
n = n + 1
ep_sum = ep_sum + ep
ep_avg = ep_sum / n
cp = distances.index(ep)
cp1 = cp
cross2 = [(x_bot - data.poses[cp1].pose.position.x),
(y_bot - data.poses[cp1].pose.position.y)]
cross = [math.cos(yaw), math.sin(yaw)]
cross_prod = cross[0] * cross2[1] - cross[1] * cross2[0]
if (cross_prod > 0):
ep1 = -ep1
print 'ep_sum: ', ep_sum
print 'ep_avg: ', ep_avg
cross_err.linear.x = ep1
cross_err.angular.x = ep_max
cross_err.angular.y = ep_avg
print 'old index:', cp
# calculate index of target point on path
cmd = Twist()
cmd1 = Twist()
prius_vel = Control()
L = 0
Lf = k * max_vel + d_lookahead
while Lf > L and (cp + 1) < len(x_p.poses):
dx = data.poses[cp + 1].pose.position.x - \
data.poses[cp].pose.position.x
dy = data.poses[cp + 1].pose.position.y - \
data.poses[cp].pose.position.y
L += math.sqrt(dx ** 2 + dy ** 2)
cp = cp + 1
print len(x_p.poses)
print 'new index is:', cp
goal_point = [x_p.poses[cp].pose.position.x,
x_p.poses[cp].pose.position.y]
print 'current goal is:', goal_point
error = [goal_point[0] - x_bot, goal_point[1] - y_bot]
print error
steer_angle = pure_pursuit(goal_point)
siny = +2.0 * (x_p.poses[cp].pose.orientation.w *
x_p.poses[cp].pose.orientation.z +
x_p.poses[cp].pose.orientation.x *
x_p.poses[cp].pose.orientation.y)
cosy = +1.0 - 2.0 * (x_p.poses[cp].pose.orientation.y *
x_p.poses[cp].pose.orientation.y +
x_p.poses[cp].pose.orientation.z *
x_p.poses[cp].pose.orientation.z)
steer_path = math.atan2(siny, cosy)
steer_err = (yaw - steer_path)
cross_err.linear.y = (-1) * (yaw - steer_path)
print "steer_angle :", steer_angle * 180 / math.pi
cmd.angular.z = min(30, max(-30, steer_angle * 180 / math.pi))
cmd.linear.y = math.sqrt(error[0]**2 + error[1]**2)
print 'omega:', cmd.angular.z
cross_err.linear.z = path_length[cp]
#r = rospy.Rate(100)
# while not rospy.is_shutdown():
pub1.publish(cmd)
pub2.publish(cross_err)
# r.sleep()
print "cmd published"
# print (ep)
print x_p.poses[cp].pose.orientation
def vehicle_general_control():
#TV data
global longitude_TV
global latitude_TV
global heading_TV
global speed_TV #chamada para a variavel global
global steering_TV #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_TV #percentual do acelerador (0 - 1)
global brake_TV #percentual do freio (0 - 1)
global speed_TV_reference
#SV data
global longitude_SV
global latitude_SV
global heading_SV
global speed_SV #chamada para a variavel global
global steering_SV #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
global throttle_SV #percentual do acelerador (0 - 1)
global brake_SV #percentual do freio (0 - 1)
global theta_error
global publication_error_topic
global publication_topic
global erro_vel
global TV_position_vector #vector to store TV positions to be followed
global mode
global incremental_controller
global TV_error_vector
global TV_PID_error
global TV_THETA_error
debug = ON
pub = rospy.Publisher(publication_topic, Control, queue_size=10) #topico de controle do carro
msg = Control() #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now(); #tempo atualizado da leitura
msg.header.frame_id = "base_link";
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-20) GRAUS
#print "------------------------------------------------------"
#calcula distancia
distance = ((longitude_TV - longitude_SV)**2 + (latitude_TV - latitude_SV)**2)**0.5 #real distance between TV and SV
if abs(speed_TV - speed_TV_reference) > 1:
speed_TV_reference = speed_TV
maximum_distance = MINIMUM_DISTANCE + speed_TV_reference * SAFETY_TIME #desisred distance
#maximum_distance = 10
distance_error = distance - maximum_distance #error between desired ditance and real distance
if abs(distance_error) < 2.0:
distance_error = 0.0
elif distance_error > 0:
distance_error = distance_error - 2.0
else:
distance_error = distance_error + 2.0
pid_error = distance_error
if debug:
#print "TV 1: ", longitude_TV, latitude_TV, heading_TV, speed_TV
#print "SV: ", longitude_SV, latitude_SV, heading_SV, speed_SV
print "Distance", distance
print "maximum_distance: ", maximum_distance
print "Distance Error: ", distance_error
if (TV_position_vector[0,0]<=0.1 and TV_position_vector[0,1]<=0.1): #cleaning the first position of the vector
TV_position_vector = np.append(TV_position_vector,[[latitude_TV, longitude_TV, heading_TV, speed_TV]],axis=0) #Numpy vector que armazena posicoes de TV
TV_position_vector = TV_position_vector[1:,:] #delete the first line with empty values
if (incremental_controller):
TV_error_vector = np.append(TV_error_vector,[[TV_PID_error, TV_THETA_error]],axis=0)
TV_error_vector = TV_error_vector[1:,:]
else:
TV_position_vector = np.append(TV_position_vector,[[latitude_TV, longitude_TV, heading_TV, speed_TV]],axis=0) #Numpy vector que armazena posicoes de TV
if (incremental_controller):
TV_error_vector = np.append(TV_error_vector,[[TV_PID_error, TV_THETA_error]],axis=0)
#if debug:
#print "TV_position_vector shape: ", TV_position_vector.shape
#print "Speed_TV: ", speed_TV
#print TV_position_vector
safety_dist = ((speed_SV*3.6)*(speed_SV*3.6))/(250*0.75) #determine the safety distance considering a Mi = 1.0(coeficiente de atrito)
if debug:
print "Safety Distance: ", safety_dist
#ajusta velocidade e aceleracao
if brake_TV == 1.0:
msg.throttle, msg.brake = 0.0 , 0.1 #determine throttle and brake
msg.steer = steering_SV
if debug:
print "TV Brake "
#elif ((distance < safety_dist and speed_SV > 0.5) or distance < MINIMUM_DISTANCE):
elif (distance < MINIMUM_DISTANCE):
msg.throttle, msg.brake = 0.0 , 0.7 #determine throttle and brake
msg.steer = steering_SV
if debug:
print "Safety Distance: ", safety_dist
print "Distance Error: ", distance_error
#print "maximun distance: ", maximum_distance
elif (speed_TV >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE): #TV is moving and the distance is significative
if (distance < safety_dist and speed_SV > 0.5):
distance_error = distance - safety_dist
if incremental_controller:
PID_dist = distance_control (distance_error + TV_PID_error) #determines de control action for the distance
else:
PID_dist = distance_control (distance_error) #determines de control action for the distance
lat_compare, long_compare, heading_compare, speed_compare, TV_dist_acum, TV_head_acum = compare_lost_position() #compares the SV_position with the vector of positions
new_speed_SV = speed_compare + PID_dist #defines the new speed for the SV
# if debug:
# print "TV Moving!"
# print "speed_compare: ", speed_compare
# print "new Speed_SV: ", new_speed_SV
msg.throttle, msg.brake = calc_throttle_brake(new_speed_SV,speed_SV) #determine throttle and brake
msg.steer = direction_control (lat_compare, long_compare, heading_compare + TV_head_acum, heading_SV)
# if (new_speed_SV > speed_TV * 1.5) and distance < maximum_distance:
# new_speed_SV = max(50.0/3.6,speed_TV)
# print "REDUCED SPEED"
# print "new Speed_SV: ", new_speed_SV
# msg.throttle, msg.brake = calc_throttle_brake(new_speed_SV,speed_SV) #determine throttle and brake
else:
msg.throttle, msg.brake = 0.0 , 0.5 #determine throttle and brake
msg.steer = steering_SV
if debug:
if speed_TV < EDGE_LOW_SPEED:
print "Speed < EDGE_LOW_SPEED"
elif distance <= MINIMUM_DISTANCE:
print "Distance < MINIMUM_distance"
else:
print "ERROR!!!"
# print "throttle: ", msg.throttle
# print "brake: ", msg.brake
# print "steer: ", msg.steer
if debug:
print "------------------------------"
#Atualiza topico carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
#publishing error messages
pub_error = rospy.Publisher(publication_error_topic, error_control, queue_size=1) #parametros de erro para controle e impressao
msg_error = error_control()
msg_error.pid_error_value = pid_error
msg_error.theta_error_value = theta_error
pub_error.publish(msg_error)
left = 0
stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)
rospy.init_node('keyboard_talker', anonymous=True)
self.pub = rospy.Publisher('car_1/prius', Control, queue_size=1)
stdscr.refresh()
key = ''
while key != ord('q'):
key = stdscr.getch()
stdscr.refresh()
command = Control()
# fill in the conditions to increment/decrement throttle/steer
if key == curses.KEY_UP:
command.throttle = message.axes[THROTTLE_AXIS]
command.brake = 0.0
command.shift_gears = Control.FORWARD
elif key == curses.KEY_DOWN:
forward = forward - 1
elif key == curses.KEY_LEFT:
left = left + 1
elif key == curses.KEY_RIGHT:
left = left - 1
elif key == ord(' '):
# this key will center the steer and throttle
def stop_the_fucking_car(self):
rospy.loginfo("stopping the f*****g car")
self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
self.prius_control_publisher.publish(self.control)
def callback_feedback(data):
'''
Assigns the position of the robot to global variables from odometry and
calculates target path point and the steering angle
:param x_bot [float]
:param y_bot [float]
:param yaw [float]
:param vel [float]
:param data [Path]
:param ep [float]
:param cp [int]
'''
global x_bot
global y_bot
global yaw
global vel
global ep # min distance
global cp # index of closest point
global ep_max
global ep_sum
global ep_avg
global n
global cp1
global path_length
global x_p
x_bot = data.pose.pose.position.x
y_bot = data.pose.pose.position.y
# quarternion to euler conversion
siny = +2.0 * (
data.pose.pose.orientation.w * data.pose.pose.orientation.z +
data.pose.pose.orientation.x * data.pose.pose.orientation.y)
cosy = +1.0 - 2.0 * (
data.pose.pose.orientation.y * data.pose.pose.orientation.y +
data.pose.pose.orientation.z * data.pose.pose.orientation.z)
yaw = math.atan2(siny, cosy) # yaw in radians
# printing the odometry readings
print("x of car:", str(x_bot))
print('y of car:', str(y_bot))
print('angle of car:', str(yaw))
print('vel of car:', str(data.twist.twist.linear.x),
str(data.twist.twist.linear.y), str(data.twist.twist.linear.z))
print('c')
cross_err = Twist()
calc_path_length(x_p)
data1 = x_p
distances = []
for i in range(len(x_p.poses)):
a = x_p.poses[i]
distances += [dist(a, x_bot, y_bot)]
ep = min(distances)
ep1 = ep
if (ep > ep_max):
ep_max = ep
n = n + 1
ep_sum = ep_sum + ep
ep_avg = ep_sum / n
cp = distances.index(ep)
cp1 = cp
cross2 = [(x_bot - data1.poses[cp1].pose.position.x),
(y_bot - data1.poses[cp1].pose.position.y)]
cross = [math.cos(yaw), math.sin(yaw)]
cross_prod = cross[0] * cross2[1] - cross[1] * cross2[0]
if (cross_prod > 0):
ep1 = -ep1
print('ep_sum: ', str(ep_sum))
print('ep_avg: ', str(ep_avg))
cross_err.linear.x = ep1
cross_err.angular.x = ep_max
cross_err.angular.y = ep_avg
print('old index:', str(cp))
# calculate index of target point on path
cmd = Twist()
cmd1 = Twist()
prius_vel = Control()
L = 0
Lf = k * max_vel + d_lookahead
while Lf > L and (cp + 1) < len(x_p.poses):
dx = data1.poses[cp + 1].pose.position.x - \
data1.poses[cp].pose.position.x
dy = data1.poses[cp + 1].pose.position.y - \
data1.poses[cp].pose.position.y
L += math.sqrt(dx**2 + dy**2)
cp = cp + 1
print(len(x_p.poses))
print('new index is:', str(cp))
goal_point = [x_p.poses[cp].pose.position.x, x_p.poses[cp].pose.position.y]
print('current goal is:', str(goal_point))
error = [goal_point[0] - x_bot, goal_point[1] - y_bot]
print(error)
steer_angle = pure_pursuit(goal_point)
siny = +2.0 * (
x_p.poses[cp].pose.orientation.w * x_p.poses[cp].pose.orientation.z +
x_p.poses[cp].pose.orientation.x * x_p.poses[cp].pose.orientation.y)
cosy = +1.0 - 2.0 * (
x_p.poses[cp].pose.orientation.y * x_p.poses[cp].pose.orientation.y +
x_p.poses[cp].pose.orientation.z * x_p.poses[cp].pose.orientation.z)
steer_path = math.atan2(siny, cosy)
steer_err = (yaw - steer_path)
cross_err.linear.y = (-1) * (yaw - steer_path)
print("steer_angle :", str(steer_angle * 180 / math.pi))
cmd.angular.z = min(30, max(-30, steer_angle * 180 / math.pi))
cmd.linear.y = math.sqrt(error[0]**2 + error[1]**2)
print('omega:', str(cmd.angular.z))
cross_err.linear.z = path_length[cp]
pub1.publish(cmd)
pub2.publish(cross_err)
print("cmd published")
# print (ep)
print(x_p.poses[cp].pose.orientation)
def vehicle_general_control():
#TV data
global longitude_TV
global latitude_TV
global heading_TV
global speed_TV #chamada para a variavel global
global steering_TV #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_TV #percentual do acelerador (0 - 1)
global brake_TV #percentual do freio (0 - 1)
#SV data
global longitude_SV
global latitude_SV
global heading_SV
global speed_SV #chamada para a variavel global
global steering_SV #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
global throttle_SV #percentual do acelerador (0 - 1)
global brake_SV #percentual do freio (0 - 1)
global publication_topic
global erro_vel
global TV_position_vector
var_print = 1
pub = rospy.Publisher(publication_topic, Control,
queue_size=10) #topico de controle do carro
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo SVizado da leitura
msg.header.frame_id = "base_link"
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-30) GRAUS
print "------------------------------------------------------"
#calcula distancia
distance = (
(longitude_TV - longitude_SV)**2 +
(latitude_TV - latitude_SV)**2)**0.5 #real distance between TV and SV
maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME
distance_error = distance - maximum_distance
if var_print:
print "Distance", distance
print "maximum_distance: ", maximum_distance
print "Distance Error: ", distance_error
TV_position_vector = np.append(
TV_position_vector,
[[latitude_TV, longitude_TV, heading_TV, speed_TV]],
axis=0) #Numpy vector que armazena posicoes de TV
if var_print:
print "TV_position_vector shape: ", TV_position_vector.shape
#print TV_position_vector
#ajusta velocidade e aceleracao
if (speed_TV >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE):
if var_print:
print "TV Moving!"
PID_dist = distance_control(
distance_error) #determines de control action for the distance
heading_compare, speed_compare = compare_position(
) #compares the SV_position with the vector of positions
new_speed_SV = speed_compare + PID_dist #defines the new speed for the SV
msg.throttle, msg.brake = calc_throttle_brake(
new_speed_SV, speed_SV) #determine throttle and brake
if (heading_compare <> heading_SV):
msg.steer = direction_control(heading_compare, heading_SV)
#print "throttle", msg.throttle
#print "brake", msg.brake
#Atualiza topico carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
data.pose.pose.orientation.z)
yaw = math.atan2(siny, cosy)
last_recorded_vel = data.twist.twist.linear.x * \
math.cos(yaw) + data.twist.twist.linear.y * math.sin(yaw)
act_vel_can = last_recorded_vel
#print("Real Vel %f" %act_vel_can)
def pid_callback(data):
'''
for subscribing to topic "pid_output"
sets velocity using geometry_msgs:Twist
'''
global prius_vel
global act_vel_can
prius_vel = Control()
if(data.linear.x > 0):
prius_vel.throttle = data.linear.x / 100
prius_vel.brake = 0
print ("acc")
print (prius_vel.throttle)
if(data.linear.x < 0):
prius_vel.brake = -data.linear.x / 100
prius_vel.throttle = 0
print ("brake")
print (prius_vel.brake)
prius_vel.steer = data.angular.z / 30
print "steering:", prius_vel.steer
def vehicle_general_control():
#TV data
global longitude_TV
global latitude_TV
global heading_TV
global speed_TV #chamada para a variavel global
global steering_TV #steering (angulacao das rodas do veiculo em relacao ao veiculo)
global throttle_TV #percentual do acelerador (0 - 1)
global brake_TV #percentual do freio (0 - 1)
#SV data
global longitude_SV
global latitude_SV
global heading_SV
global speed_SV #chamada para a variavel global
global steering_SV #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
global throttle_SV #percentual do acelerador (0 - 1)
global brake_SV #percentual do freio (0 - 1)
global publication_topic
global erro_vel
global TV_position_vector
var_print = 1
pub = rospy.Publisher(publication_topic, Control,
queue_size=10) #topico de controle do carro
msg = Control(
) #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now()
#tempo atualizado da leitura
msg.header.frame_id = "base_link"
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-20) GRAUS
print "------------------------------------------------------"
#calcula distancia
distance = (
(longitude_TV - longitude_SV)**2 +
(latitude_TV - latitude_SV)**2)**0.5 #real distance between TV and SV
maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME #desisred distance
distance_error = distance - maximum_distance #error between desired ditance and real distance
if var_print:
print "Distance", distance
print "maximum_distance: ", maximum_distance
print "Distance Error: ", distance_error
if (TV_position_vector[0, 0] <= 0.1 and TV_position_vector[0, 1] <=
0.1): #cleaning the first position of the vector
TV_position_vector = np.append(
TV_position_vector,
[[latitude_TV, longitude_TV, heading_TV, speed_TV]],
axis=0) #Numpy vector que armazena posicoes de TV
TV_position_vector = TV_position_vector[
1:, :] #delete the first line with empty values
else:
TV_position_vector = np.append(
TV_position_vector,
[[latitude_TV, longitude_TV, heading_TV, speed_TV]],
axis=0) #Numpy vector que armazena posicoes de TV
if var_print:
print "TV_position_vector shape: ", TV_position_vector.shape
#print TV_position_vector
#ajusta velocidade e aceleracao
if (speed_TV >= EDGE_LOW_SPEED and distance >=
MINIMUM_DISTANCE): #TV is moving and the distance is significative
if var_print:
print "TV Moving!"
PID_dist = distance_control(
distance_error) #determines de control action for the distance
heading_compare, speed_compare = compare_position(
) #compares the SV_position with the vector of positions
if mode == NORMAL_MODE: #NORMAL MODE (platoon)
if var_print:
"NORMAL MODE"
new_speed_SV = speed_compare + PID_dist #defines the new speed for the SV
if var_print:
print "new_speed_SV: ", new_speed_SV
msg.throttle, msg.brake = calc_throttle_brake(
new_speed_SV, speed_SV) #determine throttle and brake
if (heading_compare <> heading_SV):
msg.steer = direction_control(heading_compare, heading_SV)
else: #LOST MODE (OUT OF REACH)
if var_print:
"LOST MODE ON!!!"
heading_compare, speed_compare = compare_lost_position(
) #compares the SV_position with the TV_position
new_speed_SV = speed_compare + PID_dist
if var_print:
print "new_speed_SV: ", new_speed_SV
msg.throttle, msg.brake = calc_throttle_brake(
new_speed_SV, speed_SV)
if (heading_compare <> heading_SV):
msg.steer = direction_control(heading_compare, heading_SV)
else:
print "Distance < MINIMUM_distance"
#TODO in order to test, force the steer to be the same as the TV
#msg.steer = steering_TV
print "throttle: ", msg.throttle
print "brake: ", msg.brake
print "steer: ", msg.steer
#Atualiza topico carro
pub.publish(msg) #publica a mensagem desejada no topico 'car1/prius'
global longitude_SV
global latitude_SV
global heading_SV
global speed_SV #chamada para a variavel global
global steering_SV #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
global throttle_SV #percentual do acelerador (0 - 1)
global brake_SV #percentual do freio (0 - 1)
global publication_topic
global erro_vel
global TV_position_vector #vector to store TV positions to be followed
debug = ON
pub = rospy.Publisher(publication_topic, Control, queue_size=10) #topico de controle do carro
msg = Control() #Define msg como um dado do tipo Control -> comando de controle do veiculo
msg.header.stamp = rospy.Time.now(); #tempo atualizado da leitura
msg.header.frame_id = "base_link";
msg.steer = 0 * ANG_TO_RAD #angulacao desejada para as rodas (0-20) GRAUS
print "------------------------------------------------------"
#calcula distancia
distance = ((longitude_TV - longitude_SV)**2 + (latitude_TV - latitude_SV)**2)**0.5 #real distance between TV and SV
maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME #desisred distance
distance_error = distance - maximum_distance #error between desired ditance and real distance
if debug:
# print "Distance", distance
# print "maximum_distance: ", maximum_distance
print "Distance Error: ", distance_error
if (TV_position_vector[0,0]<=0.1 and TV_position_vector[0,1]<=0.1): #cleaning the first position of the vector
def joystick_callback(self, message):
command = Control()
# Set message header
command.header = message.header
# Get steering value
command.steer = message.axes[self._STEERING_AXIS]
# Set GUI steering value
self._steering_scrollbar_signal.emit(
int(message.axes[self._STEERING_AXIS] * -100))
# Get cruise control state
if message.buttons[self._CIRCLE] and self._circle_flag:
self._circle_flag = False
self._cruise_control_state = not self._cruise_control_state
self._cruise_radiobutton_signal.emit(self._cruise_control_state)
elif not message.buttons[self._CIRCLE]:
self._circle_flag = True
# Increment cruise control speed
if message.buttons[self._R1] and self._R1_flag:
self._R1_flag = False
if self._cruise_control_speed < self._MAX_SPEED:
self._cruise_control_speed += 1
self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
elif not message.buttons[self._R1]:
self._R1_flag = True
# Decrement cruise control speed
if message.buttons[self._L1] and self._L1_flag:
self._L1_flag = False
if self._cruise_control_speed > 0:
self._cruise_control_speed -= 1
self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
elif not message.buttons[self._L1]:
self._L1_flag = True
# If cruise control was on then
if self._cruise_control_state:
command.shift_gears = Control.FORWARD
self._gears_lineedit_signal.emit('D')
# Calculate the safe distance which prevents collision
safe_velocity = np.sqrt(
2 * self._AMAX * self._closest_distance) * 3.6
# Get the minimum of the safe distance or the speed desired by the driver
desired_velocity = min(self._cruise_control_speed, safe_velocity)
# PID loop
t_current = time.time()
dt = t_current - self._t_previous
v_current_error = desired_velocity - self._current_velocity
self._v_total_error += v_current_error * dt
v_error_rate = (v_current_error - self._v_previous_error) / dt
p_throttle = self._KP * v_current_error
i_throttle = self._KI * self._v_total_error
d_throttle = self._KD * v_error_rate
longitudinal_output = p_throttle + i_throttle + d_throttle
if longitudinal_output >= 0:
command.throttle = np.fmax(np.fmin(longitudinal_output, 1.0),
0.0)
command.brake = 0.0
self._throttle_scrollbar_signal.emit(
int(command.throttle * -100))
else:
command.throttle = 0.0
command.brake = np.fmax(np.fmin(-longitudinal_output, 1.0),
0.0)
self._throttle_scrollbar_signal.emit(int(command.brake * 100))
self._v_previous_error = v_current_error
self._t_previous = t_current
else:
# Reset variables
self._v_total_error = 0
self._v_previous_error = 0
self._t_previous = 0
self._closest_distance = float('inf')
# Get throttle/breaking value
if message.axes[self._THROTTLE_AXIS] >= 0:
command.throttle = message.axes[self._THROTTLE_AXIS]
command.brake = 0.0
else:
command.brake = message.axes[self._THROTTLE_AXIS] * -1
command.throttle = 0.0
# Set GUI throttle value
self._throttle_scrollbar_signal.emit(
int(message.axes[self._THROTTLE_AXIS] * -100))
# Get gears value
if message.buttons[self._TRIANGLE]:
command.shift_gears = Control.FORWARD
self._gears_lineedit_signal.emit('D')
elif message.buttons[self._CROSS]:
command.shift_gears = Control.NEUTRAL
self._gears_lineedit_signal.emit('N')
elif message.buttons[self._SQUARE]:
command.shift_gears = Control.REVERSE
self._gears_lineedit_signal.emit('R')
else:
command.shift_gears = Control.NO_COMMAND
self._t_previous = time.time()
# Send control message
try:
self._prius_publisher.publish(command)
rospy.loginfo("Published commands")
except Exceptionrospy.ROSInterruptException as e:
pass
self._last_published = message
def __init__(self):
# Initiate variables
self.control_type = rospy.get_param(
'~lat_control', 'stanley') # Default is stanley controller
rospy.loginfo("Setting up Control Node")
rospy.loginfo("Lateral Control Type: %s", self.control_type)
rospy.sleep(1)
self.goalMarker = Marker()
self.freq = 1 / 30.0 # 30 hz
self.velocity = 0
self.angular_velocity = 0
self.xpos = 0
self.ypos = 0
self.roll = 0
self.yaw = 0
self.prev_the = 0
self.prev_e_lat = 0
self.pitch = 0
self.steer_cmd = 0.0
self.waypoint_path = Path()
self.waypoint_path_cubic_spline = Path()
self.path_changed = True
self.prev_cmd = 0.0
# Publishers
self.goalMarkerPub = rospy.Publisher("~goal", Marker, queue_size=10)
self.cmdPub = rospy.Publisher(
"/prius",
Control,
latch=True,
)
# Subscribers
rospy.Subscriber("/base_pose_ground_truth", Odometry,
self.odomCallback)
rospy.Subscriber("/waypoints", Path, self.waypointCallback)
rospy.sleep(1)
# Setup Controller
lat_control = purePursuit.PurePursuit(path=self.waypoint_path)
speed_control = SpeedPID.SpeedPID(path=self.waypoint_path,
target_speed=3)
r = rospy.Rate(1 / self.freq)
# Control Loop
while not rospy.is_shutdown():
lat_control.update(self) # Update Control
speed_control.update(self, target_speed=3)
# Build Command
self.prius_command = Control()
self.prius_command.steer = np.clip(lat_control.getSteeringCmd(),
-1, 1)
self.prius_command.throttle = np.clip(
speed_control.get_pedal_cmds()[0], 0, 1)
self.prius_command.brake = np.clip(
speed_control.get_pedal_cmds()[1], 0, 1)
# Update Throttle
self.prius_command.shift_gears = 2
self.cmdPub.publish(self.prius_command)
rospy.loginfo("Steer CMD %f, Throttle CMD %f, Brake CMD %f",
self.prius_command.steer,
self.prius_command.throttle,
self.prius_command.brake)
gx, gy = lat_control.getGoalPoint()
self.genGoalMarker(gx, gy)
self.goalMarkerPub.publish(self.goalMarker)
r.sleep()