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_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 stopForObs():
'''
Responsible for stopping the robot before an obstacle collision
'''
# calculate the stopping acceleration
# this is allowed to override the segment constraints, because
# its more important to stop and not crash than it is to
# follow the speed limit
print "Obstacle detected!"
dt = 1.0 / RATE
decel_rate = -lastVCmd / (2 * (obs.distance - .2))
naptime = rospy.Rate(RATE)
des_vel = TwistMsg()
if (lastVCmd > 0):
v_test = lastVCmd + decel_rate * dt
des_vel.linear.x = max(
v_test, 0.0) # this is assuming that velocity is always positive
# ensure the robot will stop before crashing
if (obs.distance < .25):
des_vel.linear.x = 0
return des_vel
def stopForObs(desVelPub,segStatusPub):
global obsExists
global obsDist
global currState
global currSeg
global nextSeg
global RATE
global segments
# calculate the stopping acceleration
# this is allowed to override the segment constraints, because its more important
# to not crash than to follow the speed limit
print "Obstacle detected!"
dt = 1.0/RATE
decel_rate = pow(currState.v,2)/(2*(obsDist-1.2))
naptime = rospy.Rate(RATE)
des_vel = TwistMsg()
if(decel_rate > 0):
while(currState.v-.0001 > 0 or currState.v+.0001 < 0):
if(not obsExists):
return # if the obstacle went away then resume without fully stopping
# this should take care of negatives
if(currState.v > 0):
v_test = currState.v - decel_rate*dt
des_vel.linear.x = max(v_test,0.0)
desVelPub.publish(des_vel)
currState.updateState(des_vel,pose.pose.position,State.getYaw(pose.pose.orientation))
publishSegStatus(segStatusPub) # let everyone else know the status of the segment
naptime.sleep()
des_vel.linear.x = 0.0
desVelPub.publish(des_vel)
currState.stop()
else: # should already be stopped
desVelPub.publish(des_vel)
currState.stop()
publishSegStatus(segStatusPub)
naptime.sleep()
print "Waiting for obstacle to move..."
startTime = rospy.Time.now()
waitPeriod = rospy.Duration(3.0)
while(obsExists):
if(rospy.Time.now() - startTime > waitPeriod):
print "Aborting"
segments = Queue() # flush the queue, the callback thread probably won't appreciate this
publishSegStatus(segStatusPub,True) # send the abort flag
currSeg = None
nextSeg = None
break
else:
publishSegStatus(segStatusPub)
naptime.sleep()
return
def stopForEstop(desVelPub,segStatusPub):
global RATE
global currState
currState.stop() # make sure currentState knows the robot is stopped
rospy.loginfo("E-Stop enabled. Pausing...")
des_vel = TwistMsg() # publish 0's for velocity
desVelPub.publish(des_vel)
publishSegStatus(segStatusPub) # publish the segment status
naptime = rospy.Rate(RATE)
while(stopped): # stay here until the robot is no longer stopped
desVelPub.publish(des_vel)
publishSegStatus(segStatusPub)
naptime.sleep()
rospy.loginfo("E-Stop disabled. Taking a quick nap...")
rospy.sleep(rospy.Duration(1.0)) # sleep for 1 more second to ensure motor controllers are back online
rospy.loginfo("Good Morning!")
def main():
rospy.init_node('kinect_move_alpha')
desVelPub = rospy.Publisher('cmd_vel', TwistMsg)
rospy.Subscriber("blob_dist", BlobDistance, distanceCallback)
vel_cmd = TwistMsg()
naptime = rospy.Rate(20.0)
while (not rospy.is_shutdown()):
#Max speed is .25
if (distance > THRESHHOLD + 20):
vel_cmd.angular.z = -.25
pass
elif (distance < THRESHHOLD - 20):
vel_cmd.angular.z = .25
else:
vel_cmd.angular.z = 0
desVelPub.publish(vel_cmd)
naptime.sleep()
def getDesiredVelocity(vTrajSeg, wTrajSeg):
'''
Given a velocity trajectory segment and an omega trajectory segment this function will
Compute the scheduled velocity and omega for the robot's current position along the path
'''
global vTrajectory
global wTrajectory
global pathSegments
global currSeg
# print "segDistDone: %f" % (currSeg.segDistDone)
if (vTrajSeg.segType == TrajSeg.ACCEL):
#print "Using velocity acceleration segment"
vCmd = getDesiredVelAccel(vTrajSeg, currSeg.segDistDone)
elif (vTrajSeg.segType == TrajSeg.CONST):
#print "Using constant velocity segment"
vCmd = getDesiredVelConst(vTrajSeg, currSeg.segDistDone)
elif (vTrajSeg.segType == TrajSeg.DECEL):
#print "Using velocity deceleration segment"
vCmd = getDesiredVelDecel(vTrajSeg, currSeg.segDistDone)
#print "vCmd: %f" % vCmd
if (wTrajSeg.segType == TrajSeg.ACCEL):
#print "Using omega acceleration segment"
wCmd = getDesiredVelAccel(wTrajSeg, currSeg.segDistDone, 1)
elif (wTrajSeg.segType == TrajSeg.CONST):
#print "Using constant omega segment"
wCmd = getDesiredVelConst(wTrajSeg, currSeg.segDistDone, 1)
elif (wTrajSeg.segType == TrajSeg.DECEL):
#print "Using omega deceleration segment"
wCmd = getDesiredVelDecel(wTrajSeg, currSeg.segDistDone, 1)
vel_cmd = TwistMsg()
vel_cmd.linear.x = vCmd
vel_cmd.angular.z = wCmd
return vel_cmd
def main():
'''
I assume that the velocity profiler will take care of making the velocity 0
when there is an obstacle. If not then uncomment callback subscriber,
import etc. and make a check before publishing. I also assume that we wont
need to keep track of how far we are in the segment. I'm not sure if this
will mess up arcs. If arcs are messed up then set psiPathSeg = current
desired omega. This steering only changes the heading, not the speed.
'''
global RATE, lastMapPose, nextSeg, desVel
rospy.init_node('steering_alpha')
cmdPub = rospy.Publisher('cmd_vel',TwistMsg)
# rospy.Subscriber('obstacles',ObstaclesMsg,obstaclesCallback)
rospy.Subscriber('odom',OdometryMsg,odomCallback)
rospy.Subscriber('des_vel',TwistMsg,velCallback)
rospy.Subscriber('path_seg',PathSegmentMsg,pathSegCallback)
# rospy.Subscriber('seg_status',SegStatusMsg,segStatusCallback)
naptime = rospy.Rate(RATE)
cmdVel = TwistMsg()
xyRobotCoords = np.array([lastMapPose.pose.position.x,
lastMapPose.pose.position.y])
xyStartCoords = np.array([1,0])
#control params
#TODO: tune these
dThreshold = 1.0 # threshold before we correct
kOmega = 30.0
omegaSat = 2.0 # max omega
dt = 0.01 #timestep
while not rospy.is_shutdown():
while not tfl.canTransform("map", "odom", rospy.Time.now()):
naptime.sleep() # spin till we have some map data
vel = desVel.linear.x #m/s
curSeg = nextSeg
psiRobot = tf.getYaw(lastMapPose.pose.orientation)
psiPathSeg = tf.getYaw(curSeg.init_tan_angle)
tHat = np.array([m.cos(psiPathSeg),m.sin(PsiPathSeg)])
#tranform rot around z pi/2 * tHat
nHat = np.dot(np.array([[0,-1],[1,0]]), tHat)
d = np.subtract(xyRobotCoords, xyStartCoords)
#convert to matrix transpose then convert back
d = np.array(np.matrix(d).T) # the code was mat'*n_hat. I think this works
d = np.dot(d,nHat)
# left of path, point to path
if d > dThreshold:
psiDes = psiPathSeg-m.pi/2
# too far right, point to path
else if d < -dThreshold:
psiDes = psiPathSeg+m.pi/2
#offset is small, gradually parallelize
else:
psiDes = psiPathSeg-(m.pi/2)*d/dThreshold
#normalize to [-pi,pi]
psiError = normalizeToPi(psiRobot-psiDes)
omegaCmd = -kOmega*psiError
if omegaCmd > omegaSat
omegaCmd = omegaSat
else if omegaCmd < -omegaSat
omegaCmd = -omegaSat
psiDot = omegaCmd
xdot = vel*m.cos(psiRobot)
xdot = vel*m.sin(psiRobot)
#integration
psiRobot = psiRobot+psiDot*dt
#normalize to [-pi,pi]
psiRobot = normalizeToPi(psiRobot)
xyRobotCoords = xyRobotCoords+np.array([xdot,ydot])*dt
cmdVel = desVel
cmdVel.angular.z = omegaCmd
cmdPub.publish(cmdVel)
naptime.sleep()
def main():
'''
The main function that is executed while the node is running
'''
global RATE, naptime, lastGoodYaw, waypoints, currPoint, completedPoints
rospy.init_node('steering_alpha_main')
naptime = rospy.Rate(RATE)
cmdPub = rospy.Publisher('cmd_vel', TwistMsg)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
rospy.Subscriber('centroid_point', CentroidPointsMsg,
centroidPointCallback)
print "Entering main loop"
Kd = 0.5
Ktheta = 1.0
while (not rospy.is_shutdown()):
if (currPoint is None):
if (len(waypoints) > 0):
print "New Point"
currPoint = waypoints[0]
print "currPoint: (%f,%f)" % (currPoint.x, currPoint.y)
completedPoints.append(waypoints[0])
del waypoints[0] # remove the point from the list
continue
print "No points"
# For now publish stop messages when no point is detected
# Eventually this will be the spin routine
if (lastGoodYaw is None):
lastGoodYaw = getYaw(orientation)
cmd = TwistMsg()
cmd.angular.z = .5
cmdPub.publish(cmd)
naptime.sleep()
continue
if (lastGoodYaw is not None):
if (approx_equal(getYaw(orientation), lastGoodYaw + pi,
epsilon=.5)):
currPoint = None
cmd = TwistMsg()
cmd.angular.z = .5
cmdPub.publish(cmd)
naptime.sleep()
continue
if (approx_equal(position.x, currPoint.x, 0)
and approx_equal(position.y, currPoint.y, 0)):
completedPoints.append(currPoint)
currPoint = None
#cmdPub.publish(TwistMsg())
#naptime.sleep()
continue
#print "Steering to (%f,%f)" % (currPoint.x,currPoint.y)
print waypoints
xVec = currPoint.x - position.x
yVec = currPoint.y - position.y
actPsi = getYaw(orientation)
desPsi = atan2(yVec, xVec)
dTheta = (desPsi - actPsi) % (2 * pi)
if (dTheta > pi):
dTheta = dTheta - 2 * pi
cmd_vel = TwistMsg()
cmd_vel.angular.z = Ktheta * dTheta
if (cmd_vel.angular.z > .25):
cmd_vel.angular.z = .25
elif (cmd_vel.angular.z < -.25):
cmd_vel.angular.z = -.25
cmd_vel.linear.x = .25 # Eventually this will actually accelerate and decelerate
cmdPub.publish(cmd_vel)
naptime.sleep()
def main():
'''
The main function that is executed while the node is running
'''
global RATE, desVel, naptime
rospy.init_node('steering_alpha_main')
naptime = rospy.Rate(RATE)
cmdPub = rospy.Publisher('cmd_vel',TwistMsg)
rospy.Subscriber('des_vel', TwistMsg, desVelCallback)
rospy.Subscriber('path', PathListMsg, pathListCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback)
print "Steering entering main loop"
Kd = 0.5
Ktheta = 1.0
while not rospy.is_shutdown():
if(currSeg is not None and currSeg.seg_type == PathSegmentMsg.LINE):
(p_s,p_f) = getStartAndEndPoints()
desPsi = getYaw(currSeg.init_tan_angle)
currPsi = getYaw(orientation)
tx = cos(desPsi)
ty = sin(desPsi)
nx = -ty
ny = tx
dTheta = (desPsi - currPsi) % (2*pi)
if(dTheta > pi):
dTheta = dTheta-2*pi
# compute offset error
currX = position.x
currY = position.y
# vector from start point to current robot point
xrs = currX-p_s.x
yrs = currY-p_s.y
# dot this vectory with path normal vector to get the offset (works for line segments)
offset = xrs*nx+yrs*ny
cmd_vel = TwistMsg()
cmd_vel.angular.z = -Kd*offset + Ktheta*dTheta
cmd_vel.linear.x = desVel.linear.x
cmdPub.publish(cmd_vel)
naptime.sleep()
continue
elif(currSeg is not None):
cmdPub.publish(desVel)
naptime.sleep()
continue
#print "--------"
#print "cmd_vel:"
#print "--------"
#print cmd_vel
#print ""
#print "-------"
#print "desVel:"
#print "-------"
#print desVel
#print ""
#print "--------"
#print "currSeg:"
#print "--------"
#print currSeg
#print ""
#print "-------------------"
#print "lastSegComplete: %i" % lastSegComplete
#print "-------------------"
#print ""
#print "---------"
#print "position:"
#print "---------"
#print position
#print ""
#print "----"
#print "psi:"
#print "----"
#print "%f" % (getYaw(orientation))
#print ""
cmdPub.publish(TwistMsg())
naptime.sleep()
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 getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel):
global currState
global RATE
global currSeg
global dV
dt = 1.0/RATE
a_max = currState.pathSeg.accel_limit
d_max = currState.pathSeg.decel_limit
v_max = currState.pathSeg.max_speeds.linear.x
w_max = currState.pathSeg.max_speeds.angular.z
segLength = currState.pathSeg.seg_length
segDistDone = currState.segDistDone
des_vel = TwistMsg()
if(currState.segDistDone < 1.0):
if(currState is None):
return des_vel
#print "seg_type: %i" % (currState.pathSeg.seg_type)
if(currState.pathSeg.seg_type == PathSegmentMsg.ARC):
#print "This is an arc"
(v_max,w_max) = max_v_w(v_max,w_max,currState.pathSeg.curvature)
# figure out the v_cmd
if(segDistDone < sVDecel):
#print "V is accelerating or const"
if(currState.v < v_max):
#print "V is less than max"
v_test = currState.v + a_max*dt
des_vel.linear.x = min(v_test,v_max)
elif(currState.v > v_max):
#print "V is greater than max"
v_test = currState.v - d_max*dt # NOTE: This assumes the d_max is opposite sign of velocity
des_vel.linear.x = max(v_test,v_max)
else:
#print "V is same as max"
des_vel.linear.x = currState.v
else:
#print "V is decelerating"
v_i = currSeg.max_speeds.linear.x - dV
v_scheduled = sqrt(2*(1.0-segDistDone)*segLength*d_max + pow(v_i,2))
if(currState.v > v_scheduled):
#print "V is greater than scheduled"
v_test = currState.v - d_max*dt
des_vel.linear.x = max(v_test,v_max)
elif(currState.v < v_scheduled):
#print "v is less than scheduled"
v_test = currState.v + a_max*dt
des_vel.linear.x = min(v_test,v_max)
else:
#print "v is same as scheduled"
des_vel.linear.x = currState.v
#print "max_v: %f, max_w: %f" % (v_max,w_max)
#print "accel_limit: %f, decel_limit: %f" % (currSeg.accel_limit, currSeg.decel_limit)
#print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel)
#print "sVAccel: %f, sVDecel: %f" % (sVAccel,sVDecel)
#print "segDistDone: %f" % (currState.segDistDone)
#print "curvature: %f" % (currSeg.curvature)
# figure out the w_cmd
if(segDistDone < sWDecel):
#print "Accelerating or Const Velocity"
if(currSeg.curvature >=0):
#print "Curvature is positive"
if(currState.w < w_max):
w_test = currState.w + a_max*dt
des_vel.angular.z = min(w_test,w_max)
elif(currState.w > w_max):
w_test = currState.w + d_max*dt
des_vel.angular.z = max(w_test,w_max)
else:
des_vel.angular.z = currState.w
else:
#print "Curvature is negative"
if(currState.w > -w_max):
#print "Going slower than maximum"
w_test = currState.w - a_max*dt
des_vel.angular.z = max(w_test,-w_max)
elif(currState.w < -w_max):
#print "Going faster than maximum"
w_test = currState.w + d_max*dt
des_vel.angular.z = min(w_test,-w_max)
else:
#print "Going same speed as maximum"
des_vel.angular.z = currState.w
else:
#print "Decelerating"
w_scheduled = sqrt(2*(1.0-segDistDone)*(abs(currState.pathSeg.curvature)/segLength)*d_max)
if(currSeg.curvature >= 0):
#print "Curvature is positive"
if(currState.w > w_scheduled):
#print "Going faster than w_scheduled"
w_test = currState.w - d_max*dt
des_vel.angular.z = min(w_test,w_max)
elif(currState.w < w_scheduled):
#print "Going slower than w_scheduled"
w_test = currState.w + a_max*dt
des_vel.angular.z = max(w_test,w_max)
else:
#print "Going same speed as w_scheduled"
des_vel.angular.z = currState.w
else:
#print "Curvature is negative"
w_scheduled = -w_scheduled
if(currState.w < w_scheduled):
#print "Going faster than w_scheduled"
w_test = currState.w - a_max*dt
des_vel.angular.z = max(w_test,w_scheduled)
elif(currState.w > w_scheduled):
#print "Going slower than w_scheduled"
w_test = currState.w + d_max*dt
des_vel.angular.z = min(w_test,w_scheduled)
else:
#print "Going same speed as w_scheduled"
des_vel.angular.z = currState.w
#print des_vel
return des_vel
def update():
'''
This function is responsible for updating all the state variables each iteration of the node's main loop
'''
global currSeg
global pathSegments
global vTrajectory
global wTrajectory
global lastSegNumber
oldVTraj = None
oldWTraj = None
if (len(vTrajectory) == 0 or len(wTrajectory) == 0):
# Clear the deques because either they should both have elements or neither should
vTrajectory.clear()
wTrajectory.clear()
pathSegments.clear(
) # clear out the path segments because they are now useless
currSeg.newPathSegment()
return
# if it made it to here then there is at least one segment in vTrajectory and wTrajectory
if (currSeg.pathSeg is None):
currSeg.newPathSegment(pathSegments.get(vTrajectory[0].segNumber),
position, State.getYaw(orientation))
last_vel = TwistMsg()
last_vel.linear.x = lastVCmd
last_vel.angular.z = lastWCmd
currSeg.updateState(last_vel, position, State.getYaw(orientation))
if (currSeg.segDistDone >= vTrajectory[0].endS): # this segment is done
oldVTraj = vTrajectory.popleft(
) # temporary storage, this will eventually be thrown away
if (len(vTrajectory) == 0):
lastSegNumber = oldVTraj.segNumber
wTrajectory.clear()
pathSegments.clear()
currSeg.newPathSegment()
return
if (oldVTraj.segNumber != vTrajectory[0].segNumber):
lastSegNumber = oldVTraj.segNumber
wTrajectory.popleft(
) # could potentially be out of sync. This method does not account for that
currSeg.newPathSegment(pathSegments.get(vTrajectory[0].segNumber),
position, State.getYaw(orientation))
pathSegments.pop(
oldVTraj.segNumber) # remove no longer needed pathSegments
if (currSeg.segDistDone >= wTrajectory[0].endS): # this segment is done
oldWTraj = wTrajectory.popleft()
if (len(wTrajectory) == 0):
lastSegNumber = oldWTraj.segNumber
vTrajectory.clear()
pathSegments.clear()
currSeg.newPathSegment()
return
if (oldWTraj.segNumber != wTrajectory[0].segNumber):
lastSegNumber = oldWTraj.segNumber
vTrajectory.popleft()
currSeg.newPathSegment(pathSegments.get(wTrajectory[0].segNumber),
position, State.getYaw(orientation))
pathSegments.pop(oldWTraj.segNumber)
def main():
global naptime
global currSeg
rospy.init_node('velocity_profiler_alpha')
naptime = rospy.Rate(
RATE) # this will be used globally by all functions that need to loop
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", PathListMsg, pathListCallback)
rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback)
abortTime = None # will be set to the time an obstacle is detected
if rospy.has_param('waitTime'):
waitTime = rospy.Duration(rospy.get_param('waitTime'))
else:
waitTime = rospy.Duration(3.0)
print "Velocity Profiler entering main loop"
currSeg = State(dt=1 / RATE)
while not rospy.is_shutdown():
# check where the robot is
last_vel = TwistMsg()
last_vel.linear.x = lastVCmd
last_vel.angular.z = lastWCmd
if (currSeg.pathSeg is not None):
currSeg.updateState(last_vel, position, State.getYaw(orientation))
# check if there are segments to execute
if (len(vTrajectory) != 0 or len(wTrajectory) !=
0): # Note: Either both or neither should have 0 elements
# check for obstacles
if (obsWithinPathSeg()):
# set the timer if necessary
if (abortTime is None):
abortTime = rospy.Time.now()
else:
if (rospy.Time.now() - abortTime > waitTime):
# time to abort
abortTime = None # reset the time
# this method will reset anything
# that needs to be reset
# this may need to block here until the
# path list changes
abortPath()
# make sure the robot is stopped
desVelPub.publish(TwistMsg())
# publish the abort flag
publishSegStatus(segStatusPub, abort=False)
naptime.sleep()
continue
des_vel = stopForObs()
else:
abortTime = None # make sure that the abortTime gets reset
des_vel = getDesiredVelocity(vTrajectory[0], wTrajectory[0])
else:
des_vel = TwistMsg() # initialized to 0's by default
desVelPub.publish(
des_vel) # publish either the scheduled commands or 0's
update(
) # remove completed segments and change currSeg's path segment when necessary
publishSegStatus(segStatusPub)
naptime.sleep()
from geometry_msgs.msg._PoseStamped import PoseStamped as PoseStampedMsg
from geometry_msgs.msg._Point import Point as PointMsg
from geometry_msgs.msg._Twist import Twist as TwistMsg
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from math import cos,sin,tan,pi,sqrt
# set the rate the node runs at
RATE = 20.0
# this will store a Rate instance to keep the node running at the specified RATE
naptime = None # this will be initialized first thing in main
# keeps track of the desired velocity
desVel = TwistMsg()
# the current segment definition to steer to
currSeg = None
# the last segment completed
lastSegComplete = 0
# pose data
position = PointMsg()
orientation = QuaternionMsg()
def segStatusCallback(segStat):
'''
Updates what the last segment completed was.
pathListCallback uses this information to determine
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)
