def control(data): global vel_input global kp global kd global OFFSET circle_detected = False if os.path.isfile(path_to_watch): with open(path_to_watch, 'r') as content_file: content = content_file.readlines() for line in content: print "\n\nBEGINNING NEW PROCESSING OF TEXT FILE \n\n" if line not in cache: cache.add(line) print "Should start to turn! " + str(line) # we will actually parse this line in the future # every line has to be unique. So must follow format "# - command" per line circle_detected = True else: print "~/circle.txt does not exist" if circle_detected: print "Circle Detected" # ignore any of the wall distance correction logic, just set angle, velocity and publish angle = -170 msg = drive_param() msg.velocity = vel_input msg.angle = angle print "PUBLISHING MESSAGE: " + str(msg) pub.publish(msg) time.sleep(1) else: print "Circle Not Detected" data.pid_error = data.pid_error + OFFSET angle = data.pid_error * kp if angle > 100: angle = 100 if angle < -100: angle = -100 msg = drive_param() if data.pid_vel == 0: msg.velocity = -8 angle = 0 else: msg.velocity = vel_input msg.angle = angle print "PUBLISHING MESSAGE: " + str(msg) pub.publish(msg)
def control_callback(data): # TODO: Based on the error (data.data), determine the car's required velocity # amd steering angle. e_t1 = data.data global e_t0 u = KP * e_t1 + KD * (e_t1 - e_t0) u = np.rad2deg(u) if u > 30: u = 30 elif u < -30: u = -30 if abs(u) <= 10: vel = 1.5 elif abs(u) <= 20: vel = 1.0 else: vel = 0.5 u = np.deg2rad(u) msg = drive_param() msg.velocity = vel # TODO: implement PID for velocity msg.angle = u # TODO: implement PID for steering angle # print msg pub.publish(msg) e_t0 = data.data
def send_actuation_command(self, pred): #create the drive param message msg = drive_param() msg.angle = 0.0 msg.velocity = 1.0 #get the label label = self.classes[pred[0].argmax()] if (label == "left"): msg.angle = 0.4108652353 elif (label == "right"): msg.angle = -0.4108652353 elif (label == "straight"): msg.angle = 0.0 elif (label == "weak_left"): msg.angle = 0.10179 elif (label == "weak_right"): msg.angle = -0.10179 else: print("error:", label) msg.velocity = 0 msg.angle = 0 self.commands.append(msg.angle) self.times.append(time.time() - self.start_time) self.pub.publish(msg)
def shutdown_car(): vel_input = 0 angle = 0 msg = drive_param() msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error error = scale_factor*data.pid_error # 2. Apply the PID equation on error V0 = kp*error + kd*(prev_error-error) # 3. Make sure the error is within bounds if V0 < max_angle and V0 > -max_angle: angle = V0 elif V0 > max_angle: angle = max_angle elif V0 < -max_angle: angle = -max_angle else: angle = 0 angle+=servo_offset angle=round(angle,2) print("Angle: ", angle) prev_error = V0 ## END msg = drive_param(); if data.pid_vel == 0: msg.velocity = 0; else: msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(self,data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds angle = data.pid_error*kp; if angle > 100: angle = 100 if angle < -100: angle = -100 ## END # do extra processing with the classifier decision if not self.shouldTurn: angle = min(turn_threshold,angle) msg = drive_param(); if(data.pid_vel == 0): msg.velocity = -8 else: msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def laser_callback(laser_data): global throttle global turn global deadman_butt global VMAX global old_velocity min_dist = get_min_dist(laser_data) velocity_ratio = 0.5 * min_dist msg = drive_param() msg.angle = turn * 24 * np.pi / 180 + servo_offset if min_dist < 1: # and throttle * VMAX < -1.5: velocity_ratio = 0.2 * min_dist if velocity_ratio > 1: velocity_ratio = 1 if throttle < 0: print("velocity ratio", velocity_ratio) msg.velocity = velocity_ratio * throttle * VMAX else: msg.velocity = throttle * 0.5 if not deadman_butt: msg.velocity = 0 msg.angle = 0 if msg.velocity < 0: if msg.velocity - old_velocity < -0.05: msg.velocity = old_velocity - 0.05 old_velocity = msg.velocity print "Velocity", msg.velocity print "Angle in Degrees", msg.angle * 180 / np.pi pub.publish(msg)
def callback(data): print " " x_dot = data.linear.x # dot is the representation of velocity (derivative of x) y_dot = data.linear.y theta_dot = data.angular.z # Velocity which car rotates with respect to map # velocity = math.sqrt(x_dot**2 + y_dot**2) velocity = x_dot # angle = math.atan2(WHEELBASE_LENGTH * theta_dot, velocity) angle = theta_dot # angle = -1.0 * angle # Need to flip the sign because left steering is negative print "Computed velocity: ", velocity print "Computed angle: ", angle print "Angle in Degrees", angle*180/np.pi angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max if velocity < 1.0 and velocity >= 0.0: velocity = 1.0 # if angle < 0.035 and angle > -0.035: # 2 degrees # velocity = 3.0 print "Output velocity: ", velocity print "Output angle: ", angle msg = drive_param() msg.velocity = velocity msg.angle = angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd scaling_factor = 10 ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds error = data.pid_error*scaling_factor angle = kp*error+kd*(prev_error - error) #deriv = kd*(error-prev_error) if(angle < -100): angle = -100 if(angle > 100): angle = 100 prev_error = data.pid_error ## END msg = drive_param(); msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds #error = data.pid_error * 10 #angle = kp * error + kd * (prev_error - error) prev_error = data.pid_error angle = prev_error if (angle < -100): angle = -100 if (angle > 100): angle = 100 ## END msg = drive_param() msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control_callback(self, data): #calculate frame time self.currentTime = time.time()#float(data.header.stamp.nsecs) * pow(10, -9) #print(self.currentTime - self.lastTime) #get and store error et = data.pid_error self.storeError(et) self.etInt += self.integralError(et) #weighted pid equation for angle increment ut = KP * (et) + KI * self.etInt + KD * self.derivativeError(et) self.angle += ut #* (self.currentTime - self.lastTime) if np.isnan(self.angle): self.angle = np.nanmean(self.angleWindow) self.angle = max(min(self.angle, SPD_DEC_ANGLE_MAX), -1*SPD_DEC_ANGLE_MAX) #clamp angle between -/+ max angle #store historical data self.lastError = et self.storeAngle(self.angle) self.lastTime = self.currentTime msg = drive_param() msg.angle = self.angle # TODO: implement PID for steering angle msg.velocity = self.angleMaxVelocity(self.angle) * self.velocityMultiple # TODO: implement PID for velocity print("ERROR: ", et) print("ANGLE: ", np.rad2deg(self.angle)) #print("VEL: ", msg.velocity) self.drivePub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds pid_offset = -0.158 angle = (data.pid_error + pid_offset) * kp print("Printing pid_error: ", data.pid_error, "angle: ", angle) #print("Printing angle: %d", angle) if angle > 100: angle = 100 if angle < -100: angle = -100 ## END msg = drive_param() if (data.pid_vel == 0): msg.velocity = -8 else: msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control_callback(msg): global sum_error global prev_error global prev_angle # TODO: Based on the error (msg.data), determine the car's required velocity and steering angle. error = msg.data sum_error = sum_error + error pid_output = KP*error + KI*sum_error + KD*(error - prev_error)/0.025 #TODO: compute pid response for steering angle based on the error prev_error = error angle = pid_output angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max ang_deg = math.degrees(angle) if abs(ang_deg) >= 0.0 and abs(ang_deg) <= 10.0: vel = 1.5 elif abs(ang_deg) >=10 and abs(ang_deg) <= 20.0: vel = 1.0 else: vel = 0.5 msg = drive_param() msg.velocity = vel # TODO: implement PID for velocity msg.angle = angle # TODO: implement PID for steering angle pub.publish(msg) print("Error: {:0.2f}, PID o/p: {:0.2f}, Angle: {:0.2f}, Velocity: {:0.2f}".format(error, pid_output, ang_deg, vel))
def control_callback(data): # TODO: Based on the error (data.data), determine the car's required velocity and steering angle. global errs global err_i global err_i_sum e = data.data errs[0] = errs[1] #shift data up in the array errs[1] = e err_p = e err_d = errs[1] - errs[0] global count if count == 100: err_i = err_i_sum / 100 count = 0 count += 1 err_i_sum += e steer_d = np.clip(KP * err_p + KD * err_d + KI * err_i, -30.0, 30.0) steer = steer_d / 180.0 * np.pi #convert in to radians velocity = np.clip(1 + KPV * err_p + KDV * err_d, 0, 1.0) if np.mod(count, 10) == 0: print("Errors", err_p, err_d, err_i, velocity, steer_d) msg = drive_param() msg.velocity = 1 #TODO: implement PID for velocity 0.3 msg.angle = steer # TODO: implement PID for steering angle pub.publish(msg)
def publish_commands(self, speed_force, angle_force): """ TODO: describe what do :param speed_force: (float) :param angle_force: (float) """ # Convert angle_force to angle command angle_cmd = self.steering_p_gain * angle_force + self.steering_d_gain * ( angle_force - self.last_angle_force) self.last_angle_force = angle_force # Make sure angle command is within steering settings if abs(angle_cmd) > self.max_turn_angle: angle_cmd = (angle_cmd / abs(angle_cmd)) * self.max_turn_angle # Convert speed_force to speed command if speed_force <= 0: # In the case that the car needs to backup, flip the turning angle angle_cmd = -1 * angle_cmd speed_cmd = -1 * self.min_speed else: speed_percentage = speed_force / self.force_offset_x speed_cmd = speed_percentage * (self.max_speed - self.min_speed) + self.min_speed # Publish the command msg = drive_param() msg.angle = angle_cmd msg.velocity = speed_cmd self.pub_drive_param.publish(msg) rospy.loginfo('speed [force, cmd]: [' + str(speed_force) + ', ' + str(speed_cmd) + '] angle [force, cmd]: [' + str(angle_force) + ', ' + str(angle_cmd) + ']') return
def publish_drive_param(json_string): msg = drive_param() msg.steer = json_string['drive']['steer'] msg.throttle = json_string['drive']['throttle'] pub_drive_parameters.publish(msg) logger.log_debug("[ROSMODULE] drive_param message sent") return
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds error = data.pid_error * 10 angle = kp * error + kd *(error - prev_error) if angle < -100: angle = -100 elif angle > 100: angle = 100 prev_error = error ## END msg = drive_param(); msg.velocity = data.pid_vel msg.angle = angle print(angle) print(data.pid_vel) pub.publish(msg)
def control(data): global integral global prev_error print integral velocity = data.pid_vel angle = servo_offset error = data.pid_error if error!=0.0: integral = integral + error control_error = kp*error + kd*(prev_error - error) + ki*integral integral = integral/1.3 print control_error angle = angle - control_error elif error == 0.0: angle = servo_offset prev_error = error print "Control error",control_error print "Velocity",velocity print "Angle",angle msg = drive_param(); msg.velocity = velocity msg.angle = angle pub.publish(msg)
def control(data): global prev_error global kp global kd global angle global prev_time ## Your code goes here # 1. Scale the error error = data.pid_error cur_time = time.time() # 2. Apply the PID equation on error to compute steering v0 = (kp*error) + kd*(prev_error - error)/(cur_time-prev_time) # 3. Make sure the steering value is within bounds for talker.py prev_time = time.time() prev_error = error angle = -v0 angle = angle if -100 < angle < 100 else 100 if angle > 100 else -100 vel_input = data.pid_vel if -100 < data.pid_vel < 100 else 100 if data.pid_vel > 100 else -100 ## END msg = drive_param(); msg.velocity = vel_input msg.angle = angle print "Angle:", msg.angle print "Velocity:", msg.velocity print "prev_error:", error print "-----------------------" pub.publish(msg)
def control_callback(msg): curr_error = msg.data global past_error pid_output = KP*curr_error + KD*(curr_error - past_error)/0.025 past_error = curr_error angle = pid_output angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max # Car starts driving down center direction = rospy.get_param('DIRECTION') vel = rospy.get_param('VELOCITY') if DIRECTION == 'stop': vel = 0.0 angle = 0.0 degree_angle = math.degrees(angle) if 0.0 <= abs(degree_angle) and abs(degree_angle) <= ANGLE_LEVEL_1: vel = vel elif ANGLE_LEVEL_1<= abs(degree_angle) and abs(degree_angle) <= ANGLE_LEVEL_2: vel = SPEED_LEVEL_2 else: vel = SPEED_LEVEL_3 rospy.loginfo('vel %f angle_degrees %f angle_rad %f curr_error %f DIRECTION %s', vel, degree_angle, angle, curr_error, direction) msg = drive_param() msg.velocity = vel msg.angle = angle pub.publish(msg)
def selfdrive_control(data): global START if START == 1: msg_param = drive_param() msg_param.velocity = data.velocity msg_param.angle = data.angle pub_param.publish(msg_param)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds current_error = 10 * data.pid_error angle = kp * current_error + kd * (prev_error - current_error) prev_error = current_error if angle > 100: angle = 100 elif angle < -100: angle = -100 else: angle = angle #vel_input = data.pid_vel ## END msg = drive_param() msg.velocity = vel_input msg.angle = angle print msg pub.publish(msg)
def callback(data): #vertical: left_stick, axes[1] (down: -1.0 -- up: 1.0) #horizontal: right_stick, axes[2] (right: -1.0 -- left: 1.0) if (data.buttons[1]==1): print("braking") horizontal = 0 vertical = -20 stdscr.refresh() stdscr.addstr(1, 25, 'Emergency Stop') stdscr.addstr(2, 25, '%.2f' % vertical) stdscr.addstr(3, 25, '%.2f' % horizontal) else: horizontal = -data.axes[2]*100*scale_hori #Warning... this one changes often vertical = data.axes[1]*100*scale_vert stdscr.refresh() stdscr.addstr(1, 25, ' ') stdscr.addstr(2, 25, '%.2f' % vertical) stdscr.addstr(3, 25, '%.2f' % horizontal) # msg_speed = drive_speed() # msg_speed.speed = vertical # pub_speed.publish(msg_speed) # msg_angle = drive_angle() # msg_angle.angle = horizontal # pub_angle.publish(msg_angle) msg_param = drive_param() msg_param.velocity = vertical msg_param.angle = horizontal pub_param.publish(msg_param)
def stop(): msg = drive_param() msg.steer = 0 msg.throttle = 0 pub_drive_parameters.publish(msg) logger.log_info("[ROSMODULE] Stop signal sent") return
def callback(data): # Note: These following numbered steps below are taken from R. Craig Coulter's paper on pure pursuit. # 1. Determine the current location of the vehicle (we are subscribed to vesc/odom) # Hint: Read up on PoseStamped message type in ROS to determine how to extract x, y, and yaw. # 2. Find the path point closest to the vehicle that is >= 1 lookahead distance from vehicle's current location. # 3. Transform the goal point to vehicle coordinates. # 4. Calculate the curvature = 1/r = 2x/l^2 # The curvature is transformed into steering wheel angle by the vehicle on board controller. # Hint: You may need to flip to negative because for the VESC a right steering angle has a negative value. angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max msg = drive_param() msg.velocity = VELOCITY msg.angle = angle pub.publish(msg)
def control(data): global kp global kd global servo_offset global prev_error global vel_input global mode msg = drive_param() msg.velocity = vel_input if mode == 'wall': pid_error = data.pid_error error = pid_error * kp errordot = kd * (pid_error - prev_error) angle = error + errordot if angle > 100: angle = 100 elif angle < -100: angle = -100 prev_error = pid_error print 'pid_error -> {}\nangle -> {}'.format(pid_error, angle) msg.angle = angle elif mode == 'corner': print 'corner mode, angle 100' msg.angle = 100 pub.publish(msg)
def control(data): global prev_error global integral_error global vel_input global kp global ki global kd # Your code goes here # 1. Scale the error # 2. Apply the PID equation on error to compute steering # 3. Make sure the steering value is within bounds for talker.py msg = drive_param() msg.velocity = vel_input pid_error = data.pid_error error = pid_error * kp errordot = kd * (pid_error - prev_error) angle = error + errordot if angle > 100: angle = 100 elif angle < -100: angle = -100 prev_error = pid_error msg.angle = angle print(msg.angle) print("-------------") print(msg.velocity) pub.publish(msg) msg.angle = angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds angle = data.pid_error*kp; if angle > 100: angle = 100 if angle < -100: angle = -100 ## END msg = drive_param(); if(data.pid_vel == 0): msg.velocity = -8 else: msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds error = data.pid_error * 10 angle = -kp * error - kd * (error - prev_error) angleRad = 180 / angle if angleRad < -0.78: angleRad = -0.78 elif angleRad > 0.78: angleRad = 0.78 prev_error = error ## END msg = drive_param() msg.velocity = data.pid_vel msg.angle = angleRad print(angle) print(data.pid_vel) pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error to compute steering # 3. Make sure the steering value is within bounds for talker.py error = data.pid_error distance_error = kp * error + kd * (prev_error - error) #angle = int(maprange((-10,10),(-100,100), distance_error)) angle = -10 * distance_error #if (angle < 0): #angle = angle * 3 if angle < -100: angle = -100 elif angle > 100: angle = 100 prev_error = error rospy.loginfo(error) rospy.loginfo(distance_error) rospy.loginfo(angle) rospy.loginfo("-------------") ## END msg = drive_param() msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global kp global kd global prev_angle global prev_error global vel_input ## Your code goes here # 1. Scale the error # data.pid_error *= 10 # 2. Apply the PID equation on error to compute steering pid = data.pid_error * kp + kd * (data.pid_error - prev_error) print("pid: " + str(pid)) # 3. Make sure the steering value is within bounds for talker.py angle = pid prev_error = data.pid_error print("angle:" + str(angle)) if angle < -100: angle = -100 if angle > 100: angle = 100 prev_angle = angle msg = drive_param() msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global kp global kd global servo_offset global prev_error global vel_input global mode msg = drive_param(); msg.velocity = vel_input if mode == 'wall': pid_error = data.pid_error error = pid_error * kp errordot = kd * (pid_error - prev_error) angle = error + errordot if angle > 100: angle = 100 elif angle < -100: angle = -100 prev_error = pid_error print 'pid_error {}\nangle {}'.format(pid_error, angle) msg.angle = angle elif mode == 'corner': print 'corner mode, angle 100' msg.angle = 100 pub.publish(msg)
def control_callback(data): # TODO: Based on the error (data.data), determine the car's required velocity # amd steering angle. msg = drive_param() msg.velocity = 0.5 # TODO: implement PID for velocity msg.angle = 20.0 # TODO: implement PID for steering angle pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds ## END msg = drive_param(); msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds error = data.error vel_input = data.velocity v_theta = clamp(kp * error + kd * (prev_error - error)) angle -= v_theta ## END msg = drive_param(); msg.velocity = vel_input msg.angle = angle pub.publish(msg)
def control(data): global integral global prev_error global vel_input global kp global ki global kd global kd_vel global kp_vel velocity = data.pid_vel angle = servo_offset error = 5*data.pid_error print "Error Control",error if error!=0.0: # if abs(error - prev_error)>0.5: # integral = integral + error control_error = kp*error + kd*(prev_error - error)# + ki*integral print "Control", control_error # integral = integral/1.3 # print "Control error",control_error angle = angle + control_error*np.pi/180 # print "Control error",control_error control_error_vel = kp_vel*error + kd_vel*(prev_error - error) # print "Control error velocity",control_error_vel # velocity = velocity - abs(control_error_vel)/10 velocity = velocity - (control_error_vel)/1000 # else: # angle = servo_offset prev_error = error # if angle<-100*np.pi/180: # angle = -100*np.pi/180 # if angle>100*np.pi/180: # angle = 100*np.pi/180 # angle = angle%360 if angle > 30*np.pi/180: angle = 30*np.pi/180 if angle < -30*np.pi/180: angle = -30*np.pi/180 # print "Velocity",velocity print "Angle",angle*180/np.pi msg = drive_param() # velocity = 1 # # if angle > 20*np.pi/180 or angle < -20*np.pi/180: # # velocity = 0.3 if angle >= 10*np.pi/180 or angle <= -10*np.pi/180: velocity = 0.8 if angle > 20*np.pi/180 or angle < -20*np.pi/180: velocity = 0.3 if angle >= -1*np.pi/180 and angle <= 1*np.pi/180: velocity = 2.0 if velocity < 0: velocity = 1 if velocity > 2.5: velocity = 2.5 print "Velocity",velocity print "Angle",angle msg.velocity = velocity msg.angle = angle pub.publish(msg)
stdscr.refresh() # signal.alarm(0) if key == curses.KEY_UP: forward = forward + 0.1; stdscr.addstr(2, 20, "Up ") stdscr.addstr(2, 25, '%.2f' % forward) stdscr.addstr(5, 20, " ") elif key == curses.KEY_DOWN: forward = forward - 0.1; stdscr.addstr(2, 20, "Down") stdscr.addstr(2, 25, '%.2f' % forward) stdscr.addstr(5, 20, " ") if key == curses.KEY_LEFT: left = left - 0.1; stdscr.addstr(3, 20, "left") stdscr.addstr(3, 25, '%.2f' % left) stdscr.addstr(5, 20, " ") elif key == curses.KEY_RIGHT: left = left + 0.1; stdscr.addstr(3, 20, "rgt ") stdscr.addstr(3, 25, '%.2f' % left) stdscr.addstr(5, 20, " ") if key == curses.KEY_DC: left = 0 forward = 0 stdscr.addstr(5, 20, "Stop") msg = drive_param() msg.velocity = forward msg.angle = left pub.publish(msg) curses.endwin()