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