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()
