def test_PIDControl():
target = 50
pidController = PIDController(target, 0.6, 0.1, 0.02)
time = np.linspace(0, 100, 100)
position = [0] # starting position
for i in range(1, len(time)):
velocity = pidController.getFeedback(position[i - 1])
position.append(position[i - 1] + velocity)
pylab.ylim(0, 60)
pylab.plot(time, [target for i in range(100)], "r", time, position, "b.")
pylab.show()
class ObstacleAvoider(object):
""" Class for handling wall following behavior
"""
def __init__(self, targetAngle=0.0, pullForce=10.0, obstacleScaleInverseFactor=1.0):
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._odomSubscriber = rospy.Subscriber('odom', Odometry, self._onOdom)
self._velocityMessage = Twist()
self._pullForce = pullForce
self._obstacleScaleFactor = obstacleScaleInverseFactor
self._currentAngle = None
self._angleController = PIDController(targetAngle, 0.2, 0.001, 0.02)
self._stop = False
def stop():
self._stop = True
@staticmethod
def _filterReadings(data, lowerBound=0.0, upperBound=10.0):
return map(lambda x: x if x > lowerBound and x <= upperBound else float("inf"), data)
@staticmethod
def _odomToAngle(value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value - leftMin) / float(leftSpan)
return rightMin + (valueScaled * rightSpan)
def _sumComponentForces(self, angleDistances):
""" Sum the force components
"""
pullAngle = self._odomToAngle(self._currentAngle, -1.0, 1.0, -3.14, 3.14)
pullX, pullY = math.cos(pullAngle), math.sin(pullAngle)
xComponents = pullX*self._pullForce
yComponents = pullY*self._pullForce
for angle, reading in angleDistances:
if reading == float("inf"):
continue
angle = self._odomToAngle(angle, 0, 359, 0, 2*3.14) - pullAngle
xComponents -= (self._obstacleScaleFactor / reading)*math.cos(angle)
yComponents -= (self._obstacleScaleFactor / reading)*math.sin(angle)
return xComponents, yComponents
def _onOdom(self, msg):
""" Odometry handler
"""
self._currentAngle = msg.pose.pose.orientation.z
def _onScan(self, msg):
if self._currentAngle:
readings = self._filterReadings(msg.ranges)
desiredAngle = self._sumComponentForces(zip(range(360), readings))
desiredAngle = -1.0 * math.atan2(desiredAngle[1],desiredAngle[0])
self._angleController.target = desiredAngle
parallelFeedback = -1.0 * self._angleController.getFeedback(self._currentAngle)
print parallelFeedback
self._velocityMessage = Twist(angular=Vector3(z=20*parallelFeedback), linear=Vector3(x=0.5))
def run(self):
""" main behavior of obstacle avoider class
"""
self._stop = False
rospy.init_node('obstacle_avoider', anonymous=True)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if not self._stop:
self._velocityPublisher.publish(self._velocityMessage)
r.sleep()
class WallFollower(object):
""" Class for handling wall following behavior
"""
def __init__(self, targetDistance=1, targetAngle=90):
self.targetDistance = targetDistance
self.targetAngle = targetAngle
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
# self._velocityPublisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._angleController = PIDController(self.targetAngle, 0.02, 0.0001, 0.00)
self._distanceController = PIDController(self.targetDistance, 0.5, 0.001, 0.00)
self._velocityMessage = Twist()
self._states = {"followWall":self._followWall, "teleop":self._teleop}
self._currentState = "followWall"
self._keyDirection = None
self._stop = False
def stop():
self._stop = True
@property
def state(self):
return self._currentState
@state.setter
def state(self, state):
self._currentState = state
@property
def key(self):
return self._keyDirection
@key.setter
def key(self, key):
self._keyDirection = key
@staticmethod
def _filterReadings(data, lowerBound=0.0, upperBound=10.0):
return map(lambda x: x if x > lowerBound and x <= upperBound else float("inf") , data)
@staticmethod
def _sampleReadings(data, startIndex, endIndex):
""" returns the mean of a sample of readings
startIndex -- Number starting index of sample
endIndex -- Number end index of sample (end index is NOT included in result)
"""
if (endIndex > len(data)):
endIndex = len(data)
validData = [data[i] for i in range(startIndex, endIndex) if data[i] != float("inf")]
dataLength = endIndex - startIndex
if len(validData) == dataLength:
return sum(validData) / float(dataLength)
else:
return None
@staticmethod
def _wallAngle(data):
""" returns the angle of the wall relative to the robot in degrees
"""
if (len(data)):
return data.index(min(data))
def _onScan(self, msg):
""" callback handler for scan event
"""
readings = self._filterReadings(msg.ranges)
angle = self._wallAngle(readings)
distance = self._sampleReadings(readings, angle, angle + 5)
distance = readings[angle]
if distance:
self._states[self._currentState](angle, distance)
def _followWall(self, angle, distance):
""" wall following behavior. Uses two complementary PID controllers to
keep the robot along the wall
"""
parallelFeedback = self._angleController.getFeedback(angle)
distanceFeedback = self._distanceController.getFeedback(distance)
if angle <= 180:
parallelFeedback *= -1
distanceFeedback *= -1
self._velocityMessage.angular.z = parallelFeedback + 2.25 * distanceFeedback
self._velocityMessage.linear.x = 0.35
def _teleop(self, angle, distance):
char = self.key
if char == 'w':
self._velocityMessage = Twist(linear=Vector3(x=0.5))
elif char == 's':
self._velocityMessage = Twist(linear=Vector3(x=-0.5))
if char == 'a':
self._velocityMessage = Twist(angular=Vector3(z=0.5))
elif char == 'd':
self._velocityMessage = Twist(angular=Vector3(z=-0.5))
def run(self):
""" main behavior of wall follower class
"""
self._stop = False
rospy.init_node('wall_follower', anonymous=True)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if not self._stop:
self._velocityPublisher.publish(self._velocityMessage)
r.sleep()
