def main(): rospy.on_shutdown(offhook) global angle global velocity key = '' while key != ord('q'): key = stdscr.getch() stdscr.refresh() if key == curses.KEY_LEFT and angle > -100 + increment: angle -= increment elif key == curses.KEY_RIGHT and angle < 100 - increment: angle += increment elif key == curses.KEY_UP and velocity < 100 + increment: velocity += increment elif key == curses.KEY_DOWN and velocity > -100 + increment: velocity -= increment stdscr.addstr(0, 0, "angle: ") stdscr.addstr(0, 7, '%3.2f' % angle) stdscr.addstr(1, 0, "velocity: ") stdscr.addstr(1, 10, '%3.2f' % velocity) message = drive_param() message.velocity = velocity message.angle = angle control_drive_parameters.publish(message) curses.endwin()
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() if (data.pid_vel < 15): #safety speed msg.velocity = data.pid_vel else: msg.velocity = 10 sw = data.pid_switch error = data.pid_error * 4 #** *** #scale the error if (sw == 0): #turning kp = 20 kd = 0.1 else: #straight kp = 14 kd = 0.09 angle_p = error * kp angle_d = kd * (error - prev_error) angle = angle_p + angle_d prev_error = error msg.angle = angle pub.publish(msg)
def offhook(): pub = rospy.Publisher('control_drive_parameters', drive_param, queue_size=10) msg = drive_param() msg.velocity = 0 msg.angle = 0 pub.publish(msg)
def stop(): rospy.init_node('stop_node', anonymous=True) pub = rospy.Publisher('control/drive_parameters', drive_param, queue_size=10) rate = rospy.Rate(10) msg = drive_param() msg.velocity = 0 msg.angle = 0 while not rospy.is_shutdown(): pub.publish(msg) rate.sleep()
def circle(): rospy.init_node('control_straight', anonymous=True) pub = rospy.Publisher('control_drive_parameters', drive_param, queue_size=10) rospy.on_shutdown(offhook) rate = rospy.Rate(25) while not rospy.is_shutdown(): msg = drive_param() msg.velocity = minimum_velocity msg.angle = 0 + miscalibration_angle pub.publish(msg) rate.sleep()
def callback(message): print "[#callback]: received message" data = np.array(map(ord, message.data), dtype=np.uint8) image = data.reshape(message.height, message.width, 3) # BGR -> RGB image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) image = image[188:, 0:672, 0:3] image = cv2.resize(image, (320, 160)) angle = float(model.predict(image, batch_size=1)) message = drive_param() message.velocity = 27 message.angle = angle control_drive_parameters.publish(message)
def control(self, data): ## Your code goes here # 1. Scale the error # 2. Apply the PID equation on error # 3. Make sure the error is within bounds msg = drive_param() msg.velocity = data.pid_vel error = data.pid_error angle_p = error * self.kp angle_d = self.kd * (error - self.prev_error) angle = angle_p + angle_d self.prev_error = error msg.angle = angle if not self.go: msg.velocity = 0.0 self.pub.publish(msg) rospy.loginfo("\nvel: %.0lf\ngo: %i", msg.velocity, self.go)
def callback(msg): steering_angle = msg.drive.steering_angle # velocity = (msg.drive.speed + 1.14) / 0.11 velocity = range_map(msg.drive.speed, -0.5, 0.5, -20, 20) print( '[vugc1_ackermann_drive_to_drive_param]: msg.drive.speed={}, msg.drive.steering_angle={}' .format(msg.drive.speed, msg.drive.steering_angle)) print('degrees={}'.format(math.degrees(steering_angle))) # angle = range_map(math.degrees(steering_angle), -1*angle_max, angle_max, -1*angle_max, angle_max) angle = -sign(velocity) * math.degrees( steering_angle) # hacky solution to correct steering angle issues print( '[vugc1_ackermann_drive_to_drive_param]: velocity={}, angle={}'.format( velocity, angle)) parameters = drive_param() parameters.velocity = velocity parameters.angle = angle control_drive_parameters.publish(parameters)
def callback(msg): steering_angle = msg.drive.steering_angle # velocity = (msg.drive.speed + 1.14) / 0.11 velocity = range_map(msg.drive.speed, -0.5, 0.5, -20, 20) print( '[vugc1_ackermann_drive_to_drive_param]: msg.drive.speed={}, msg.drive.steering_angle={}' .format(msg.drive.speed, msg.drive.steering_angle)) angle = range_map(steering_angle, -math.pi / 2.0, math.pi / 2.0, -1 * angle_max, angle_max) # angle = range_map(math.degrees(steering_angle), -40, 40, -100, 100) angle *= -1 print( '[vugc1_ackermann_drive_to_drive_param]: velocity={}, angle={}'.format( velocity, angle)) parameters = drive_param() parameters.velocity = velocity parameters.angle = angle control_drive_parameters.publish(parameters)
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 = data.pid_vel error = data.pid_error angle_p = error * kp angle_d = kd * (error - prev_error) angle = angle_p + angle_d prev_error = error msg.angle = angle pub.publish(msg)
def offhook(): control_drive_parameters = rospy.Publisher('control_drive_parameters', drive_param, queue_size=10) message = drive_param() message.velocity = 0 message.angle = 0 control_drive_parameters.publish(message)
def callback(data): global prev_angle_error global prev_dist_error global sum_angle_error global sum_dist_error rospy.on_shutdown(offhook) #angle_range = math.degrees(data.angle_max - data.angle_min) angle_range = data.angle_max - data.angle_min print('angle ranges', angle_range) cur_dist_error = 0 cur_angle_error = 0 forward = 0.5 * angle_range actual_dists = [0, 0, 0] if mode == 'centering': right = 0.4 * angle_range left = 0.6 * angle_range angles = [forward, left, right] print('angles', angles) actual_dists = getrange(data, angles) cur_angle_error = actual_dists[2] - actual_dists[1] else: angle1 = math.radians(45) angle2 = math.radians(60) angles = [forward, angle1, angle2] actual_dists = getrange(data, angles) swing = math.radians(angle2 - angle1) alpha = math.atan2( (actual_dists[2] * math.cos(swing)) - actual_dists[1], actual_dists[2] * math.sin(swing)) AB = actual_dists[1] * math.cos(alpha) AC = 1 CD = AB + (AC * math.sin(alpha)) cur_angle_error = -1 * (CD - side_target_dist) cur_dist_error = actual_dists[0] - fwd_target_dist print('distances: [%s]' % ', '.join(map(str, actual_dists))) sum_dist_error, dif_dist_error = calculateerror(cur_dist_error, prev_dist_error, sum_dist_error) sum_angle_error, dif_angle_error = calculateerror(cur_angle_error, prev_angle_error, sum_angle_error) speed = calculateresponse(cur_dist_error, sum_dist_error, dif_dist_error, 'speed') angle = calculateresponse(cur_angle_error, sum_angle_error, dif_dist_error, 'angle') prev_dist_error = cur_dist_error prev_angle_error = cur_angle_error speed = max(min(speed, max_speed), -1 * max_speed) if speed < 0: speed *= 10 #min max angle = max(min(angle, max_angle), -1 * max_angle) msg = drive_param() msg.velocity = speed msg.angle = angle print('speed: %f' % speed) print('angle: %f' % angle) pub.publish(msg)
def offhook(): msg = drive_param() msg.velocity = 0 msg.angle = 0 pub.publish(msg)
# signal.alarm(0) if key == curses.KEY_UP: forward = forward + 0.5 stdscr.addstr(2, 20, "Up ") stdscr.addstr(2, 25, '%.2f' % forward) stdscr.addstr(5, 20, " ") elif key == curses.KEY_DOWN: forward = forward - 0.5 stdscr.addstr(2, 20, "Down") stdscr.addstr(2, 25, '%.2f' % forward) stdscr.addstr(5, 20, " ") if key == curses.KEY_LEFT: left = left - 5 stdscr.addstr(3, 20, "left") stdscr.addstr(3, 25, '%.2f' % left) stdscr.addstr(5, 20, " ") elif key == curses.KEY_RIGHT: left = left + 5 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()