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 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 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 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 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 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 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 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):
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 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
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)
curses.endwin()
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
# if key == 'k': throttle 0, steering 0, gear NEUTRAL, braking +
elif key == 'k':
command.throttle = 0.0
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)
if(current_key == 'd'):
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)