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)