class twoWallFollow:
def __init__(self):
rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=10)
self.publisher = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=10)
self.rangeStarted = False
self.drive_msg = AckermannDriveStamped()
self.pid = PIDController(rospy.Time.now(), PID_KP,PID_KI,PID_KD)
self.publish()
def laser_callback(self, msg):
ranges = map(lambda x: int(x>1.5), msg.ranges)
ranges= ranges[180:901]
self.rangeStarted = False
#print ranges
currStart = 0
currEnd = 0
currLen = 0
maxLen = 0
maxEnd = 0
maxStart = 0
for i in range(len(ranges)):
if ranges[i]:
#print "empty"
if self.rangeStarted:
pass
else:
currStart = i
#print currStart
#print "started Range at" + str(currStart)
self.rangeStarted = True
else:
#print self.rangeStarted
if self.rangeStarted:
currEnd = i
#print currEnd
currLen = currEnd - currStart
#print currLen
if currLen > maxLen:
print "this"
maxLen = currLen
maxStart = currStart
maxEnd = currEnd
self.rangeStarted = False
else:
continue
targetPoint = (maxEnd + maxStart)/2 + 180 if maxEnd != 0 else 540
print targetPoint
print maxLen
error = (targetPoint * msg.angle_increment) + msg.angle_min
steer_angle = self.pid.update(error, rospy.Time.now())
print steer_angle
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.speed = 2.0 if maxLen >= 115 else -1.0
self.drive_msg.drive.steering_angle = steer_angle
def publish(self):
while not rospy.is_shutdown():
self.publisher.publish(self.drive_msg)
rospy.Rate(8).sleep()
class pidDriver:
def __init__(self, d_des, speed, dist):
self.drive_cmd = AckermannDriveStamped()
self.drive_cmd.drive.speed = speed
self.error = 0 # initializes the running error counter
self.d_des = d_des # takes desired dist from wall
self.dist = dist
self.scan1 = []
self.scan2 = []
self.sideFlag = 0
rospy.init_node("wall_PID", anonymous=False)
self.pid = PIDController(rospy.Time.now().to_sec(), 0.1,0.0, 0.07)#0.00000001,0.12)
#init the pub/subs
self.drive_pub = rospy.Publisher("/vesc/ackermann_cmd_mux/input/teleop", AckermannDriveStamped, queue_size=5)
rospy.Subscriber("left_or_right", Int32, self.leftRight)
rospy.Subscriber("/scan", LaserScan, self.callback)
# START PUB FUNCTION
self.publisher()
def leftRight(self, msg):
self.sideFlag = msg
print str(self.sideFlag)
def callback(self,msg):
self.scan1 = []
self.scan2 = []
if self.sideFlag == 1 or self.sideFlag == 0:
for i in range(175, 186):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(215, 226):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
if self.sideFlag == 2:
for i in range(895, 906):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1/len(scan1) # average of scan1
meanD_y = total2/len(scan2) # average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
pidVal = pidVal/abs(pidVal) * min(1.0, abs(pidVal)) if pidVal!=0 else 0
# print pidVal
self.drive_cmd.drive.steering_angle = pidVal
def publisher(self):
while not rospy.is_shutdown():
self.drive_pub.publish(self.drive_cmd)
rospy.Rate(10).sleep()
class pidDriver:
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0,0.0, 0.0)#0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow", Float32, queue_size = 1)
# START PUB FUNCTION
def flag(self, msg):
self.start = msg.data
def sideChange(self, msg):
self.sideFlag = msg.data
def callback(self,msg):
self.scan1 = []
self.scan2 = []
for i in range(895, 906):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1/len(scan1) if len(scan1) > 0 else 0# average of scan1
meanD_y = total2/len(scan2) if len(scan2) > 0 else 0# average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
self.pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
self.pidVal = self.pidVal/abs(self.pidVal) * min(1.0, abs(self.pidVal)) if self.pidVal!=0 else 0
self.publisher()
def publisher(self):
print("published angle")
self.angle_pub.publish(self.pidVal)
class lane_controller(object):
def __init__(self):
self.node_name = rospy.get_name()
self.lane_reading = None
self.pub_counter = 0
# Initialize PID parameters
P_d = -0.8
I_d = -0.02
D_d = -0.01
P_theta = -0.8
I_theta = -0.02
D_theta = -0.01
# Initialize PID controller
self.pid_d = PIDController(P_d, I_d, D_d)
self.pid_theta = PIDController(P_theta, I_theta, D_theta)
# Setup parameters
self.setGains()
# Publicaiton
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_debug = rospy.Publisher("~debug", Pose2D, queue_size=1)
# Subscriptions
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbPose,
queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
self.gains_timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.getGains_event)
rospy.loginfo("[%s] Initialized " % (rospy.get_name()))
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def setGains(self):
v_bar = 0.5 # nominal speed, 0.5m/s
k_theta = -2.0
k_d = -(k_theta**2) / (4.0 * v_bar)
theta_thres = math.pi / 6
d_thres = math.fabs(k_theta / k_d) * theta_thres
d_offset = 0.0
self.v_bar = self.setupParameter("~v_bar", v_bar) # Linear velocity
# FIXME: AC aug'17: are these inverted?
self.k_d = self.setupParameter("~k_d", k_theta) # P gain for theta
self.k_theta = self.setupParameter("~k_theta", k_d) # P gain for d
self.d_thres = self.setupParameter("~d_thres",
theta_thres) # Cap for error in d
self.theta_thres = self.setupParameter("~theta_thres",
d_thres) # Maximum desire theta
self.d_offset = self.setupParameter(
"~d_offset",
d_offset) # a configurable offset from the lane position
def getGains_event(self, event):
v_bar = rospy.get_param("~v_bar")
k_d = rospy.get_param("~k_d")
k_theta = rospy.get_param("~k_theta")
d_thres = rospy.get_param("~d_thres")
theta_thres = rospy.get_param("~theta_thres")
d_offset = rospy.get_param("~d_offset")
params_old = (self.v_bar, self.k_d, self.k_theta, self.d_thres,
self.theta_thres, self.d_offset)
params_new = (v_bar, k_d, k_theta, d_thres, theta_thres, d_offset)
if params_old != params_new:
rospy.loginfo("[%s] Gains changed." % (self.node_name))
rospy.loginfo(
"old gains, v_var %f, k_d %f, k_theta %f, theta_thres %f, d_thres %f, d_offset %f"
% (params_old))
rospy.loginfo(
"new gains, v_var %f, k_d %f, k_theta %f, theta_thres %f, d_thres %f, d_offset %f"
% (params_new))
self.v_bar = v_bar
self.k_d = k_d
self.k_theta = k_theta
self.d_thres = d_thres
self.theta_thres = theta_thres
self.d_offset = d_offset
def custom_shutdown(self):
rospy.loginfo("[%s] Shutting down..." % self.node_name)
# Stop listening
self.sub_lane_reading.unregister()
# Send stop command
car_control_msg = Twist2DStamped()
car_control_msg.v = 0.0
car_control_msg.omega = 0.0
self.publishCmd(car_control_msg)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" % self.node_name)
def publishCmd(self, car_cmd_msg):
#wheels_cmd_msg = WheelsCmdStamped()
#wheels_cmd_msg.header.stamp = stamp
#speed_gain = 1.0
#steer_gain = 0.5
#vel_left = (speed_gain*speed - steer_gain*steering)
#vel_right = (speed_gain*speed + steer_gain*steering)
#wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
#wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
self.pub_car_cmd.publish(car_cmd_msg)
#self.pub_wheels_cmd.publish(wheels_cmd_msg)
def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
# Calculating the delay image processing took
timestamp_now = rospy.Time.now()
image_delay_stamp = timestamp_now - self.lane_reading.header.stamp
# delay from taking the image until now in seconds
image_delay = image_delay_stamp.secs + image_delay_stamp.nsecs / 1e9
cross_track_err = lane_pose_msg.d - self.d_offset
heading_err = lane_pose_msg.phi
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(
cross_track_err) * self.d_thres
car_control_msg.omega = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
# self.pub_.publish(car_control_msg)
# Testing PID controller
debug = False
# Debug
if debug:
debug_msg = Pose2D()
# Construct message according to original P control
debug_msg.x = self.v_bar
debug_msg.y = 0
debug_msg.theta = self.k_d * cross_track_err + self.k_theta * heading_err
# Publish original control message
self.pub_debug.publish(debug_msg)
return
# Update controller
ctl_d = self.pid_d.update(cross_track_err)
ctl_theta = self.pid_theta.update(heading_err)
ctl_v = self.v_bar
ctl_omega = self.k_d * ctl_d + self.k_theta * ctl_theta
# Construct message according to PID controller
debug_msg = Pose2D()
debug_msg.x = ctl_v
debug_msg.theta = ctl_omega
self.pub_debug.publish(debug_msg)
# publish car command
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = ctl_v
car_control_msg.omega = ctl_omega
self.publishCmd(car_control_msg)
class pidDriver:
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0, 0.0,
0.0) #0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow",
Float32,
queue_size=1)
# START PUB FUNCTION
def flag(self, msg):
self.start = msg.data
def sideChange(self, msg):
self.sideFlag = msg.data
def callback(self, msg):
self.scan1 = []
self.scan2 = []
for i in range(895, 906):
self.scan1.append(
msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(
msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1 / len(scan1) if len(
scan1) > 0 else 0 # average of scan1
meanD_y = total2 / len(scan2) if len(
scan2) > 0 else 0 # average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
self.pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
self.pidVal = self.pidVal / abs(self.pidVal) * min(
1.0, abs(self.pidVal)) if self.pidVal != 0 else 0
self.publisher()
def publisher(self):
print("published angle")
self.angle_pub.publish(self.pidVal)
