def main(x=None,y=None):
global goal
rospy.init_node('astar_alpha_goal_publisher')
goalPub = rospy.Publisher('goal_point',GoalMsg)
naptime = rospy.Rate(RATE)
goal = GoalMsg()
goal.new = True
if(x is None or y is None):
goal.none = True
else:
goal_point = PointMsg()
goal_point.x = x
goal_point.y = y
goal.goal = goal_point
while not rospy.is_shutdown():
goalPub.publish(goal)
goal.new = False
naptime.sleep()
def test_updateState_PosOffsetPosArc(self):
'''
Test the robot following a path starting from the same angle
but with a positive offset and larger radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi / 2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi + math.pi / 2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature - .3
angle = State.getYaw(pathSeg.init_tan_angle)
r = 1 / abs(rhoDes)
startAngle = angle - math.pi / 2.0
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length * (count / (maxIter / 2.0)) * rhoDes
arcAng = startAngle + dAng
point.x = pathSeg.ref_point.x + r * math.cos(arcAng)
point.y = pathSeg.ref_point.y + r * math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegArc(self):
'''
Test the robot following a path starting from the same angle
but with a negative offset and a smaller radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi/2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0,0,3*math.pi/4.0-math.pi/2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = -1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature + .3
angle = State.getYaw(pathSeg.init_tan_angle)
r=1/abs(rhoDes)
startAngle = angle + math.pi/2.0
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length*(count/(maxIter/2.0))*rhoDes
arcAng = startAngle+dAng
point.x = pathSeg.ref_point.x + r*math.cos(arcAng)
point.y = pathSeg.ref_point.y + r*math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_stop(self):
'''
Test the stop method
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 1.5
point = PointMsg()
maxIter = 300
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = pathSeg.seg_length*(count/(maxIter/2.0))*math.cos(math.pi/4.0)
point.y = pathSeg.seg_length*(count/(maxIter/2.0))*math.sin(math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
self.assertEquals(state.v, 0.5)
self.assertEquals(state.o, 1.5)
state.stop()
self.assertEquals(state.v, 0.0)
self.assertEquals(state.o, 0.0)
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def getStartAndEndPoints():
if(currSeg.seg_type == PathSegmentMsg.LINE):
p_s = PointMsg()
p_f = PointMsg()
p_s.x = currSeg.ref_point.x
p_s.y = currSeg.ref_point.y
p_f.x = currSeg.ref_point.x + currSeg.seg_length*cos(getYaw(currSeg.init_tan_angle))
p_f.y = currSeg.ref_point.y + currSeg.seg_length*sin(getYaw(currSeg.init_tan_angle))
return (p_s,p_f)
elif(currSeg.seg_type == PathSegmentMsg.ARC):
return (0.0,0.0) # TODO
elif(currSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE):
return (currSeg.ref_point,currSeg.ref_point) # spins should never move in x,y plane
else:
return (0.0,0.0) # not sure if this is the best default answer
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, 0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0 * state.pathSeg.seg_length * count / maxIter + math.pi / 4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0*state.pathSeg.seg_length*count/maxIter + math.pi/4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2) * 2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi / 4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0 * math.sqrt(2) + 1.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength * (count / (maxIter / 2.0)) * math.cos(
3 * math.pi / 4.0) + 1.0
point.y = actSegLength * (count / (maxIter / 2.0)) * math.sin(
3 * math.pi / 4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def getPoint(self): t = self.getTranslation() p = Point() p.x = t[0] p.y = t[1] p.z = t[2] return p #eof #eoc
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0*math.sqrt(2)+1.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength*(count/(maxIter/2.0))*math.cos(3*math.pi/4.0) + 1.0
point.y = actSegLength*(count/(maxIter/2.0))*math.sin(3*math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25,8.2)
corner2 = (15.75,28.2)
numCells = 100
brush = BrushFire(corner1,corner2,numCells,size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point',GoalMsg,goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,obstaclesCallback)
rospy.Subscriber('map_pos',PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list',PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if(position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x,position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25, 8.2)
corner2 = (15.75, 28.2)
numCells = 100
brush = BrushFire(corner1, corner2, numCells, size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point', GoalMsg, goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,
obstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if (position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x, position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
def getPoint(self):
t = self.getTranslation()
p = Point()
p.x = t[0]
p.y = t[1]
p.z = t[2]
return p
#eof
#eoc
def main(fullList):
global pose
global pathList
rospy.init_node('pathplanner_alpha_dummyNode')
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
dummyPointPub = rospy.Publisher('point_list', PointListMsg)
pathList.new = True
naptime = rospy.Rate(RATE)
#point = PointMsg()
#point.x = pose.pose.position.x
#point.y = pose.pose.position.y
#appendToList(point)
with open(fullList, 'rb') as csvFile:
dialect = csv.Sniffer().sniff(csvFile.read(1024)) # auto detect delimiters
csvFile.seek(0)
reader = csv.reader(csvFile, dialect) # open up a csv reader object with the csv file
for i,row in enumerate(reader):
point1 = PointMsg()
try:
point1.x = float(row[0])
except ValueError:
print "x ValueError"
try:
point1.y = float(row[1])
except ValueError:
print "y ValueError"
appendToList(point1)
while not rospy.is_shutdown():
dummyPointPub.publish(pathList)
pathList.new = False
naptime.sleep()
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x, corner1y)
corner2 = (corner2x, corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1, corner2, numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic, GoalMsg, goalCallback)
rospy.Subscriber(inflatedTopic, GridCellsMsg, inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()
def updateState(self, vel_cmd, point, psi):
"""
Integrates along the desired path vector and projects the actual path
vector onto the desired vector
Inputs
------
vel_cmd is expected to be of type Twist
point is expected to be of type Point
Returns
-------
True if everything went okay
False if an error occurred
"""
dt = 1.0 / 20.0
if (self.pathSeg.seg_type == PathSegmentMsg.LINE):
approx_equal = lambda a, b, t: abs(a - b) < t
# grab the angle of the path
angle = State.getYaw(self.pathSeg.init_tan_angle)
# grab the starting point
p0 = self.pathSeg.ref_point
# calculate another point along the line
p1 = PointMsg()
p1.x = p0.x + self.pathSeg.seg_length * cos(angle)
p1.y = p0.y + self.pathSeg.seg_length * sin(angle)
# line segment
tanVecMag = pow(p0.x - p1.x, 2) + pow(p0.y - p1.y, 2)
# intersection point
u = ((point.x - p0.x) * (p1.x - p0.x) + (point.y - p0.y) *
(p1.y - p0.y)) / tanVecMag
intersect = PointMsg()
intersect.x = point.x + u * cos(angle)
intersect.y = point.y + u * sin(angle)
d = sqrt(pow(intersect.x - p0.x, 2) + pow(intersect.y - p0.y, 2))
# see where this point will lead if followed along the line for a distance d
testX = intersect.x + d * cos(angle)
testY = intersect.y + d * sin(angle)
# if the distance gets smaller then the d should be negative
# if the distance gets bigger then the d should be positive
if (sqrt(pow(testX - p0.x, 2) + pow(testY - p0.y, 2)) < d):
d = -d
self.segDistDone = d / self.pathSeg.seg_length
elif (self.pathSeg.seg_type == PathSegmentMsg.ARC):
self.segDistDone += abs(
(vel_cmd.angular.z * dt) /
(self.pathSeg.seg_length / abs(self.pathSeg.curvature)))
"""
tanAngStart = State.getYaw(self.pathSeg.init_tan_angle)
rho = self.pathSeg.curvature
r = 1/abs(rho)
if(rho >= 0):
startAngle = tanAngStart - pi/2
else:
startAngle = tanAngStart + pi/2
ref_point = self.pathSeg.ref_point
rVec = State.createVector(ref_point, point)
theta = atan2(rVec.y,rVec.x)
phi = startAngle % (2*pi)
if(abs(phi-2*pi) < phi):
phi = phi - 2*pi
posPhi = phi % (2*pi)
posTheta = theta % (2*pi)
# figure out angle halfway between end and start angle
if(rho >= 0):
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = self.pathSeg.seg_length/r
else:
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = -self.pathSeg.seg_length/r
# find theta in terms of starting angle
if(rho >= 0):
if(posTheta > posPhi):
beta = posTheta - posPhi
else:
beta = 2*pi-posPhi+posTheta
else:
if(posTheta < posPhi):
beta = posTheta - posPhi
else:
beta = posTheta-posPhi-(2*pi)
# figure out what region the angle is in
if(rho >= 0):
if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc
alpha = beta
else:
alpha = -(2*pi-beta)
else:
if(beta >= halfAngle-pi and beta <= 0):
alpha = beta;
else:
alpha = beta + (2*pi)
if(rho >= 0):
self.segDistDone = r*alpha/self.pathSeg.seg_length
else:
self.segDistDone = -r*alpha/self.pathSeg.seg_length
"""
elif (self.pathSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE):
rho = self.pathSeg.curvature
phi = State.getYaw(self.pathSeg.init_tan_angle)
posPhi = phi % (2 * pi)
posPsi = psi % (2 * pi)
if (rho >= 0):
halfAngle = self.pathSeg.seg_length / 2.0
else:
halfAngle = self.pathSeg.seg_length / 2.0 - pi
# find beta in terms of starting angle
if (posPsi > posPhi):
beta = posPsi - posPhi
else:
beta = 2 * pi - posPhi + posPsi
# figure out what region the angle is in
if (beta >= 0 and
beta <= halfAngle + pi): # beta is in the specified arc
alpha = beta
else:
alpha = beta - 2 * pi
self.segDistDone = alpha / self.pathSeg.seg_length
else:
pass # should probably throw an unknown segment type error
# update the current velocity
self.v = vel_cmd.linear.x
self.w = vel_cmd.angular.z
# update the current point and heading
self.point = point
self.psi = psi
def updateState(self, vel_cmd, point, psi):
"""
Integrates along the desired path vector and projects the actual path
vector onto the desired vector
Inputs
------
vel_cmd is expected to be of type Twist
point is expected to be of type Point
Returns
-------
True if everything went okay
False if an error occurred
"""
dt = 1.0/20.0
if(self.pathSeg.seg_type == PathSegmentMsg.LINE):
approx_equal = lambda a,b,t:abs(a-b)<t
# grab the angle of the path
angle = State.getYaw(self.pathSeg.init_tan_angle)
# grab the starting point
p0 = self.pathSeg.ref_point
# calculate another point along the line
p1 = PointMsg()
p1.x = p0.x + self.pathSeg.seg_length*cos(angle)
p1.y = p0.y + self.pathSeg.seg_length*sin(angle)
# line segment
tanVecMag = pow(p0.x-p1.x,2) + pow(p0.y-p1.y,2)
# intersection point
u = ((point.x - p0.x)*(p1.x-p0.x) + (point.y-p0.y)*(p1.y-p0.y))/tanVecMag
intersect = PointMsg()
intersect.x = point.x + u*cos(angle)
intersect.y = point.y + u*sin(angle)
d = sqrt(pow(intersect.x-p0.x,2)+pow(intersect.y-p0.y,2))
# see where this point will lead if followed along the line for a distance d
testX = intersect.x + d*cos(angle)
testY = intersect.y + d*sin(angle)
# if the distance gets smaller then the d should be negative
# if the distance gets bigger then the d should be positive
if(sqrt(pow(testX-p0.x,2)+pow(testY-p0.y,2)) < d):
d = -d
self.segDistDone = d/self.pathSeg.seg_length
elif(self.pathSeg.seg_type == PathSegmentMsg.ARC):
self.segDistDone += abs((vel_cmd.angular.z*dt)/(self.pathSeg.seg_length/abs(self.pathSeg.curvature)))
"""
tanAngStart = State.getYaw(self.pathSeg.init_tan_angle)
rho = self.pathSeg.curvature
r = 1/abs(rho)
if(rho >= 0):
startAngle = tanAngStart - pi/2
else:
startAngle = tanAngStart + pi/2
ref_point = self.pathSeg.ref_point
rVec = State.createVector(ref_point, point)
theta = atan2(rVec.y,rVec.x)
phi = startAngle % (2*pi)
if(abs(phi-2*pi) < phi):
phi = phi - 2*pi
posPhi = phi % (2*pi)
posTheta = theta % (2*pi)
# figure out angle halfway between end and start angle
if(rho >= 0):
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = self.pathSeg.seg_length/r
else:
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = -self.pathSeg.seg_length/r
# find theta in terms of starting angle
if(rho >= 0):
if(posTheta > posPhi):
beta = posTheta - posPhi
else:
beta = 2*pi-posPhi+posTheta
else:
if(posTheta < posPhi):
beta = posTheta - posPhi
else:
beta = posTheta-posPhi-(2*pi)
# figure out what region the angle is in
if(rho >= 0):
if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc
alpha = beta
else:
alpha = -(2*pi-beta)
else:
if(beta >= halfAngle-pi and beta <= 0):
alpha = beta;
else:
alpha = beta + (2*pi)
if(rho >= 0):
self.segDistDone = r*alpha/self.pathSeg.seg_length
else:
self.segDistDone = -r*alpha/self.pathSeg.seg_length
"""
elif(self.pathSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE):
rho = self.pathSeg.curvature
phi = State.getYaw(self.pathSeg.init_tan_angle)
posPhi = phi % (2*pi)
posPsi = psi % (2*pi)
if(rho >=0):
halfAngle = self.pathSeg.seg_length/2.0
else:
halfAngle = self.pathSeg.seg_length/2.0-pi
# find beta in terms of starting angle
if(posPsi > posPhi):
beta = posPsi - posPhi
else:
beta = 2*pi-posPhi+posPsi
# figure out what region the angle is in
if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc
alpha = beta
else:
alpha = beta - 2*pi
self.segDistDone = alpha/self.pathSeg.seg_length
else:
pass # should probably throw an unknown segment type error
# update the current velocity
self.v = vel_cmd.linear.x
self.w = vel_cmd.angular.z
# update the current point and heading
self.point= point
self.psi = psi
goalPoint.none = True;
goalPoint.point = goalList[i]
def main():
global pose
global goalList
rospy.init_node('goal_planner_alpha')
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
goalPointPub = rospy.Publisher('goal_point', Goal)
naptime = rospy.Rate(RATE)
endOfObstacles = PointMsg()
endOfObstacles.x = -3.48
endOfObstacles.y = 20.69
goalList.append(endOfObstacles)
needsToTurn = PointMsg()
needsToTurn.x = -2.37
needsToTurn.y = 21.34
goalList.append(needsToTurn)
finalGoalPoint = PointMsg()
finalGoalPoint.x = 5.47
finalGoalPoint.y = 12.04
goalList.append(finalGoalPoint)
while not rospy.is_shutdown():
def main():
global RATE
global lastVCmd
global lastOCmd
global obs
global obsDist
global obsExists
global stopped
global seg_number
global currSeg
global nextSeg
global pose
global ping_angle
rospy.init_node('velocity_profiler_alpha')
desVelPub = rospy.Publisher('des_vel',TwistMsg) # Steering reads this and adds steering corrections on top of the desired velocities
segStatusPub = rospy.Publisher('seg_status', SegStatusMsg) # Lets the other nodes know what path segment the robot is currently executing
rospy.Subscriber("motors_enabled", BoolMsg, eStopCallback) # Lets velocity profiler know the E-stop is enabled
rospy.Subscriber("obstacles", ObstaclesMsg, obstaclesCallback) # Lets velocity profiler know where along the path there is an obstacle
rospy.Subscriber("cmd_vel", TwistMsg, velCmdCallback) #
rospy.Subscriber("path_seg", PathSegmentMsg, pathSegmentCallback)
rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback)
naptime = rospy.Rate(RATE)
vel_cmd = TwistMsg()
point = PointMsg()
# while(not ros.Time.isValid()):
#pass
print "Entering main loop"
while not rospy.is_shutdown():
if(currSeg is not None):
# print "seg type %i " % currSeg.seg_type
# print "ref_point %s " % currSeg.ref_point
# print "curv %f" % currSeg.curvature
print "dist done %f" % currState.segDistDone
if stopped:
stopForEstop(desVelPub,segStatusPub)
continue
if(currSeg is not None):
# Eventually this should work with arcs, spins and lines, but right
# now its only working with lines
if(currSeg.seg_type == PathSegmentMsg.LINE):
# if there is an obstacle and the obstacle is within the segment length
print ping_angle
if(obsExists and obsDist/currSeg.seg_length < 1.0-currState.segDistDone and ping_angle > 60 and ping_angle < 140):
stopForObs(desVelPub,segStatusPub)
continue
vel_cmd.linear.x = lastVCmd
vel_cmd.angular.z = lastOCmd
point.x = pose.pose.position.x
point.y = pose.pose.position.y
point.z = pose.pose.position.z
currState.updateState(vel_cmd, point, State.getYaw(pose.pose.orientation)) # update where the robot is at
(sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(currSeg,nextSeg) # figure out the switching points in the trajectory
#print "sVAccel: %f sVDecel: %f" % (sVAccel,sVDecel)
#print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel)
des_vel = getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel) # figure out the robot commands given the current state and the desired trajectory
desVelPub.publish(des_vel) # publish the commands
publishSegStatus(segStatusPub) # let everyone else know the status of the segment
# see if its time to switch segments yet
if(currState.segDistDone > 1.0):
print "Finished segment type %i" % (currState.pathSeg.seg_type)
print "currState.segDistDone %f" % (currState.segDistDone)
currSeg = None
else:
# try and get new segments
if(nextSeg is not None):
currSeg = nextSeg # move the nextSegment up in line
nextSeg = None # assume no segment until code below is run
else: # didn't have a next segment before
try: # so try and get a new one from the queue
currSeg = segments.get(False)
except QueueEmpty: # if the queue is still empty then
currSeg = None # just set it to None
try: # see if a next segment is specified
nextSeg = segments.get(False) # try and get it
except QueueEmpty: # if nothing specified
nextSeg = None # set to None
point = PointMsg()
point.x = pose.pose.position.x
point.y = pose.pose.position.y
point.z = pose.pose.position.z
currState.newPathSegment(currSeg, point, pose.pose.orientation)
des_vel = TwistMsg()
desVelPub.publish(des_vel) # publish all zeros for the des_vel
publishSegStatus(segStatusPub) # publish that there is no segment
if(currSeg is not None):
rospy.logwarn("Starting a new segment, type %i" %currSeg.seg_type)
rospy.logwarn("\tcurvature: %f" % currSeg.curvature)
rospy.logwarn("\tref_point: %s" % currSeg.ref_point)
naptime.sleep()
continue
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x,corner1y)
corner2 = (corner2x,corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1,corner2,numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic,GoalMsg,goalCallback)
rospy.Subscriber(inflatedTopic,GridCellsMsg,inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()
def test_stop(self):
'''
Test the stop method
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2) * 2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi / 4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 1.5
point = PointMsg()
maxIter = 300
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = pathSeg.seg_length * (count /
(maxIter / 2.0)) * math.cos(
math.pi / 4.0)
point.y = pathSeg.seg_length * (count /
(maxIter / 2.0)) * math.sin(
math.pi / 4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
self.assertEquals(state.v, 0.5)
self.assertEquals(state.o, 1.5)
state.stop()
self.assertEquals(state.v, 0.0)
self.assertEquals(state.o, 0.0)
