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)