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 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 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 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 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)
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'): command.shift_gears = Control.FORWARD
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)
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 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:
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'
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'
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 # if key == 'k': throttle 0, steering 0, gear NEUTRAL, braking +
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
''' 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) if __name__ == '__main__': rospy.init_node('PRIUS_CMD', anonymous=True) ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/prius') rospy.Subscriber("pid_output", Twist, pid_callback) rospy.Subscriber("base_pose_ground_truth", Odometry, vel_callback) pub = rospy.Publisher(ackermann_cmd_topic, Control, queue_size=10) rospy.spin()