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 main(): global segNumber global segAbort global pathList global naptime global pathSegPub rospy.init_node('path_planner_alpha_main') pathSegPub = rospy.Publisher('path', PathListMsg) rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback) rospy.Subscriber('point_list', PointListMsg, pointListCallback) naptime = rospy.Rate(RATE) print "Entering main loop" while not rospy.is_shutdown(): # clear everything when something gets in the way # of the planned path if(segAbort): segNumber = 0 pathList.segments = [] print "len(desPoints) %i" % len(desPoints) print "len(pathList.segments) %i" % len(pathList.segments) if(len(desPoints)-1 > len(pathList.segments)): pathSeg = PathSegmentMsg() num = len(pathList.segments) yaw = m.atan2((desPoints[num+1].y-desPoints[num].y),(desPoints[num+1].x-desPoints[num].x)) pathSeg.init_tan_angle = yawToQuat(yaw) pathSeg.ref_point = desPoints[num] #if(theta > m.pi/6): #SPIN TO NEW ANGLES # addSegToList() #Calculate the length of the given segment #Add angle xDist = m.pow((desPoints[num].x - desPoints[num+1].x),2) yDist = m.pow((desPoints[num].y - desPoints[num+1].y),2) pathSeg.seg_length = m.sqrt(xDist + yDist) addSegToList(pathSeg) pathSegPub.publish(pathList) naptime.sleep()
def main(): global segNumber global segAbort global pathList global naptime global pathSegPub rospy.init_node('path_planner_alpha_main') pathSegPub = rospy.Publisher('path', PathListMsg) rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback) rospy.Subscriber('point_list', PointListMsg, pointListCallback) naptime = rospy.Rate(RATE) print "Entering main loop" while not rospy.is_shutdown(): # clear everything when something gets in the way # of the planned path if (segAbort): segNumber = 0 pathList.segments = [] print "len(desPoints) %i" % len(desPoints) print "len(pathList.segments) %i" % len(pathList.segments) if (len(desPoints) - 1 > len(pathList.segments)): pathSeg = PathSegmentMsg() num = len(pathList.segments) yaw = m.atan2((desPoints[num + 1].y - desPoints[num].y), (desPoints[num + 1].x - desPoints[num].x)) pathSeg.init_tan_angle = yawToQuat(yaw) pathSeg.ref_point = desPoints[num] #if(theta > m.pi/6): #SPIN TO NEW ANGLES # addSegToList() #Calculate the length of the given segment #Add angle xDist = m.pow((desPoints[num].x - desPoints[num + 1].x), 2) yDist = m.pow((desPoints[num].y - desPoints[num + 1].y), 2) pathSeg.seg_length = m.sqrt(xDist + yDist) addSegToList(pathSeg) pathSegPub.publish(pathList) naptime.sleep()