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 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 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 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 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 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 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 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):
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 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'
import time
from prius_msgs.msg import Control
rospy.init_node('car_demo')
pub = rospy.Publisher('prius', Control, queue_size=20)
rate = rospy.Rate(2)
command = Control()
rospy.loginfo('start')
while not rospy.is_shutdown():
current_key = raw_input('use keyboard strokes to move car')
if(current_key == 'w'):
command.shift_gears = Control.FORWARD
command.throttle = 1.0
command.brake =0.0
command.steer= 0.0
pub.publish(command)
if(current_key == 's'):
command.shift_gears = Control.REVERSE
command.throttle = 1.0
command.brake =0.0
command.steer= 0.0
pub.publish(command)
if(current_key == 'a'):
command.shift_gears = Control.FORWARD
command.throttle = 0.5
command.brake =0.0
command.steer= 0.8
pub.publish(command)
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)
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
left = 0
forward = 0
pub.publish(command)
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'
pub = rospy.Publisher('prius', Control, queue_size=1)
rospy.init_node('teleop_twist_keyboard')
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
status = 0
command = Control()
try:
print(msg)
print(vels(speed, turn))
while (1):
#command = Control()
key = getKey()
# if key == 'i': throttle +, steering 0, gear FORWARD, braking 0
if key == 'i':
command.throttle = speed
command.steer = 0.0
command.shift_gears = Control.FORWARD
command.brake = 0.0
# if key == 'u': throttle +, steering +, gear FORWARD, braking 0
elif key == 'u':
command.throttle = speed
command.steer = turn
command.shift_gears = Control.FORWARD
command.brake = 0.0
# if key == 'o': throttle +, steering -, gear FORWARD, braking 0
elif key == 'o':
command.throttle = speed
command.steer = -turn
command.shift_gears = Control.FORWARD
command.brake = 0.0
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 debug:
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 debug:
print "TV Moving!"
PID_dist = distance_control (distance_error) #determines de control action for the distance
lat_compare, long_compare, heading_compare, speed_compare = 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
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, heading_SV)
else:
print "Distance < MINIMUM_distance"
#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'
def SV_info(data):
global longitude_SV
global latitude_SV
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'
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
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
pub.publish(prius_vel)