/** * Add a spline to the path * * @param wFrame * @param path * @param pfi */ public void addAHermiteSpline( final SimpleWorldFrame wFrame, final Path path, final PathFinderInput pfi) { TrackedTigerBot bot = wFrame.getBot(pfi.getBotId()); float dstOrient = pfi.getDstOrient(); List<IVector2> mPath = new ArrayList<IVector2>(); mPath.add(DistanceUnit.MILLIMETERS.toMeters(bot.getPos())); for (IVector2 p : path.getPath()) { mPath.add(DistanceUnit.MILLIMETERS.toMeters(p)); } if (pfi.getMoveCon().getSpeed() > 0) { trajGeneratorMove.setPositionTrajParams( pfi.getMoveCon().getSpeed(), Sisyphus.maxLinearAcceleration); } else { trajGeneratorMove.setPositionTrajParams( Sisyphus.maxLinearVelocity, Sisyphus.maxLinearAcceleration); } trajGeneratorMove.setRotationTrajParams(maxRotateVelocity, Sisyphus.maxRotateAcceleration); trajGeneratorMove.setSplineParams(normalAngleToSpeed); trajGeneratorMove.setReducePathScore(pathReductionScore); SplinePair3D trajectories = trajGeneratorMove.create( mPath, bot.getVel(), pfi.getMoveCon().getVelAtDestination(), bot.getAngle(), dstOrient, bot.getaVel(), 0); path.setHermiteSpline(trajectories); trajectories.setStartTime(System.nanoTime()); }
@Override public void onNewPath(final Path path) { currentPaths.put(path.getBotID(), path); Integer counter = newPathCounter.get(path.getBotID()); if (counter == null) { counter = 0; } newPathCounter.put(path.getBotID(), counter + 1); notifyNewPath(path); }
/** * calculates a spline for a bot according a given movement condition * * @param botId * @param worldFrame * @param moveCon * @param pathPlanningParams * @return */ public ISpline calculateSpline( final BotID botId, final SimpleWorldFrame worldFrame, final MovementCon moveCon, final TuneableParameter pathPlanningParams) { Map<BotID, Path> paths = new HashMap<BotID, Path>(); PathFinderInput pfi = new PathFinderInput(botId, paths, 0f, moveCon); pfi.update(worldFrame); IPathFinder pathFinder = new ERRTPlanner_WPC(); if (pathPlanningParams != null) { ((ERRTPlanner_WPC) pathFinder).setAdjustableParams(pathPlanningParams); } Path path = pathFinder.calcPath(pfi); addAHermiteSpline(worldFrame, path, pfi); return path.getSpline(); }
private void notifyPotentialNewPath(final Path path) { synchronized (observers) { IPathConsumer pathConsumer = observers.get(path.getBotID()); if (pathConsumer != null) { pathConsumer.onPotentialNewPath(path); } } }
@Override public void onPotentialNewPath(final Path path) { latestPaths.put(path.getBotID(), path); notifyPotentialNewPath(path); }