std::vector<Action>* Board::search() {
Node* initial = new Node(fields);
std::vector<Action>* resultingActions;
// If we have a goal state, return the solution
if (goal(initial->state)) {
solution(initial, resultingActions);
return resultingActions;
}
// If we don't have a goal state, go explore
std::queue<Node*> frontier;
frontier.push(initial);
std::queue<int**> explored;
while (true) {
if (frontier.empty()) return NULL;
Node* current = frontier.front();
frontier.pop();
explored.push((int**) current->state);
std::vector<Action>* problems = actions((int**) current->state);
for (int n = 0; n < problems->size(); n++) {
bool contains = false;
Node* child = new Node(current, &problems->at(n));
int size = explored.size();
for (int n = 0; n < size; n++) {
int** someNode = explored.front();
if (equals(someNode, (int**) child->state)) {
explored.pop();
explored.push(someNode);
contains = true;
break;
}
}
for (int n = 0; n < frontier.size(); n++) {
Node* frontierNode = frontier.front();
frontier.pop();
frontier.push(frontierNode);
if (equals((int**) child->state, (int**) frontierNode->state)) {
contains = true;
}
}
if (!contains) {
if (goal(child->state)) {
solution(child, resultingActions);
return resultingActions;
}
frontier.push(child);
}
}
}
return NULL;
}
//------------------------------------------------------------------------------
// Purpose: routine called to start when a task initially starts
// Input : pTask - the task structure
//------------------------------------------------------------------------------
void CAI_ASW_HealOtherBehavior::StartTask( const Task_t *pTask )
{
switch( pTask->iTask )
{
case TASK_HEAL_OTHER_FIND_TARGET:
{
if ( m_hHealTargetEnt )
{
AI_NavGoal_t goal( GOALTYPE_TARGETENT, ACT_RUN, m_flApproachDistance, AIN_YAW_TO_DEST | AIN_UPDATE_TARGET_POS, m_hHealTargetEnt );
SetTarget( m_hHealTargetEnt );
GetNavigator()->SetArrivalDistance( m_flApproachDistance );
GetNavigator()->SetGoal( goal );
}
else
{
TaskFail( FAIL_NO_TARGET );
m_flDeferUntil = gpGlobals->curtime + 3.0f;
}
break;
}
case TASK_HEAL_OTHER_MOVE_TO_TARGET:
{
break;
}
default:
BaseClass::StartTask( pTask );
break;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "controller");
ros::NodeHandle n("controller");
StageBot robot(n, 0);
StageBot goal(n, 1);
Controller controller;
double distance, angVelz;
ros::Rate rate(20);
while (ros::ok()) {
angVelz = controller.getAngularDistance(robot,goal);
distance = controller.getLinearDistance(robot,goal);
if (std::abs(angVelz) > 1e-4){
robot.move(0, 3 * angVelz);
}
else {
robot.move(distance * 0.8 ,0.0);
}
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
void execute(const Param& tParam)
{
printf("KicktoGoal BotID: %d\n",botID);
Vector2D<int> goal(HALF_FIELD_MAXX, 0);
float finalSlope = Vector2D<int>::angle(goal, state->homePos[botID]);
float turnAngleLeft = normalizeAngle(finalSlope - state->homeAngle[botID]); // Angle left to turn
float dist = Vector2D<int>::dist(state->ballPos, state->homePos[botID]);
if(dist > BOT_BALL_THRESH)
{
float finalSlope = Vector2D<int>::angle(Vector2D<int>(ForwardX(HALF_FIELD_MAXX, 0)), state->ballPos);
captureBall();
return;
}
if(fabs(turnAngleLeft) > 2 * SATISFIABLE_THETA)
{
turnToPoint(goal.x, goal.y);
return;
}
//printf("should kick now %f\n", turnAngleLeft);
kickBallForGoal();
if (state->homePos[botID].absSq() < BOT_BALL_THRESH * BOT_BALL_THRESH)
{
tState = COMPLETED;
}
}
bool OmplRosTaskSpacePlanner::setGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
arm_navigation_msgs::GetMotionPlan::Response &response)
{
ompl::base::ScopedState<ompl::base::CompoundStateSpace> goal(state_space_);
ompl::base::GoalPtr goal_states(new ompl::base::GoalStates(planner_->getSpaceInformation()));
ROS_DEBUG("Setting my goal");
if(!constraintsToOmplState(request.motion_plan_request.goal_constraints,goal))
{
response.error_code.val = response.error_code.PLANNING_FAILED;
ROS_WARN("Problem converting constraints to ompl state");
return false;
}
goal_states->as<ompl::base::GoalStates>()->addState(goal.get());
ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get());
if(!my_checker->isStateValid(goal.get()))
{
response.error_code = my_checker->getLastErrorCode();
if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
response.error_code.val = response.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
response.error_code.val = response.error_code.GOAL_IN_COLLISION;
ROS_ERROR("Goal state is invalid. Reason: %s",arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
return false;
}
planner_->setGoal(goal_states);
ROS_DEBUG("Setting goal state successful");
return true;
}
int
main(int argc, char** argv) {
if (argc != 10) {
printf("Usage:\n%s <width> <height> <x_scal> <y_scale> <map_file> <start_x> <start_y> <goal_x> <goal_y>\n", argv[0]);
return 0;
}
Point start(0, 0, 0);
Point goal(0, 0, 0);
short width, height;
unsigned int uval;
unsigned int xscale;
unsigned int yscale;
sscanf(argv[1], "%hd", &width);
sscanf(argv[2], "%hd", &height);
sscanf(argv[3], "%u", &xscale);
sscanf(argv[4], "%u", &yscale);
sscanf(argv[6], "%u", &uval); start.x = uval;
sscanf(argv[7], "%u", &uval); start.y = uval;
sscanf(argv[8], "%u", &uval); goal.x = uval;
sscanf(argv[9], "%u", &uval); goal.y = uval;
Cost_Map map(argv[5], width, height, xscale, yscale);
Corners_NonBlocking corners_non_blocking = Corners_NonBlocking(&map, start, goal, 100, 4, 4);
Path path = corners_non_blocking.search();
path.print(stdout);
return 0;
}
bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col)
{
if (!ss_)
return false;
ob::ScopedState<> start(ss_->getStateSpace());
start[0] = start_row;
start[1] = start_col;
ob::ScopedState<> goal(ss_->getStateSpace());
goal[0] = goal_row;
goal[1] = goal_col;
ss_->setStartAndGoalStates(start, goal);
// generate a few solutions; all will be added to the goal;
for (int i = 0 ; i < 10 ; ++i)
{
if (ss_->getPlanner())
ss_->getPlanner()->clear();
ss_->solve();
}
const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
OMPL_INFORM("Found %d solutions", (int)ns);
if (ss_->haveSolutionPath())
{
ss_->simplifySolution();
og::PathGeometric &p = ss_->getSolutionPath();
ss_->getPathSimplifier()->simplifyMax(p);
ss_->getPathSimplifier()->smoothBSpline(p);
return true;
}
return false;
}
int ag_ppeval( char* txt ){
std::string goal( txt );
int ret = CouchRunner::S_Agenda->push( goal, S_wm );
std::cout << "Agenda: Forwarding hypothesis: " << goal << std::endl;
std::cout << *CouchRunner::S_Agenda;
return ret;
}
void SinyukovTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel
) {
Eigen::Vector3f goal(100000.0, 100000.0, 0.0); //TODO
initialise(pos, vel, goal, limits_, *vsamples_, discretize_by_time_);
}
/**
* This function is used when other strategies are to be applied,
* but the cost functions for obstacles are to be reused.
*/
bool EDWAPlanner::checkTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples){
oscillation_costs_.resetOscillationFlags();
base_local_planner::Trajectory traj;
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
generator_.generateTrajectory(pos, vel, vel_samples, traj);
double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
//if the trajectory is a legal one... the check passes
if(cost >= 0) {
return true;
}
ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
//otherwise the check fails
return false;
}
//-----------------------------------------------------------------------------
// Purpose:
// Input : *pTask -
//-----------------------------------------------------------------------------
void CAI_BehaviorAlyxInjured::StartTask( const Task_t *pTask )
{
switch( pTask->iTask )
{
case TASK_FIND_COVER_FROM_ENEMY:
{
CBaseEntity *pLeader = GetFollowTarget();
if ( !pLeader )
{
BaseClass::StartTask( pTask );
break;
}
// Find a position behind our follow target
Vector coverPos = vec3_invalid;
if ( FindCoverFromEnemyBehindTarget( pLeader, COVER_DISTANCE, &coverPos ) )
{
AI_NavGoal_t goal( GOALTYPE_LOCATION, coverPos, ACT_RUN, AIN_HULL_TOLERANCE, AIN_DEF_FLAGS );
GetOuter()->GetNavigator()->SetGoal( goal );
GetOuter()->m_flMoveWaitFinished = gpGlobals->curtime + pTask->flTaskData;
TaskComplete();
return;
}
// Couldn't find anything
TaskFail( FAIL_NO_COVER );
break;
}
default:
BaseClass::StartTask( pTask );
break;
}
}
/*----------------------------------------------------------------------------*/
bool GVC::solve(std::vector< std::vector<int> > &map, int v, int ncolors)
{
int nobjects = (int)map.size();
if (goal(v, nobjects))
{
return true;
}
else
{
/*try to color vertex v with each color
c in turn*/
for (int c = 0; c < ncolors; c++)
{
if (valid(map, v, c))
{
ColorClass[v] = c;
if (solve(map, (v+1), ncolors)) return true;
}
}
return false;
}
}
bool spherePlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
{
// Create the ambient space state space for the problem.
auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
ob::RealVectorBounds bounds(3);
bounds.setLow(-2);
bounds.setHigh(2);
rvss->setBounds(bounds);
// Create a shared pointer to our constraint.
auto constraint = std::make_shared<SphereConstraint>();
ConstrainedProblem cp(space, rvss, constraint);
cp.setConstrainedOptions(c_opt);
cp.setAtlasOptions(a_opt);
cp.css->registerProjection("sphere", std::make_shared<SphereProjection>(cp.css));
Eigen::VectorXd start(3), goal(3);
start << 0, 0, -1;
goal << 0, 0, 1;
cp.setStartAndGoalStates(start, goal);
cp.ss->setStateValidityChecker(obstacles);
if (!bench)
return spherePlanningOnce(cp, planners[0], output);
else
return spherePlanningBench(cp, planners);
}
//-------- Begin of function Lightning::progress ----------//
double Lightning::progress()
{
if(goal())
return 1;
else
return (double) steps / expect_steps;
}
void
Map::setMap (int x, int y, int bits) {
assert ((map (x, y) & bits) == 0);
if (goal (x, y) && ((bits & OBJECT) == OBJECT)) objectsLeft_--;
assert (objectsLeft_ >= 0);
currentMap_[y+1][x+1] |= bits;
}
void noShip::HandleState_SeaAttackReturn()
{
Result res = DriveToHarbour();
switch(res)
{
default: return;
case GOAL_REACHED:
{
// Entladen
state = STATE_SEAATTACK_UNLOADING;
this->current_ev = em->AddEvent(this, UNLOADING_TIME, 1);
} break;
case HARBOR_DOESNT_EXIST:
{
// Kein Hafen mehr?
// Dann müssen alle Angreifer ihren Heimatgebäuden Bescheid geben, dass sie
// nun nicht mehr kommen
// Das Schiff muss einen Notlandeplatz ansteuern
for(std::list<noFigure*>::iterator it = figures.begin(); it != figures.end(); ++it)
static_cast<nofAttacker*>(*it)->CancelAtHomeMilitaryBuilding();
state = STATE_TRANSPORT_DRIVING;
HandleState_TransportDriving();
} break;
case NO_ROUTE_FOUND:
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
// Nichts machen und idlen
StartIdling();
} break;
}
}
void noShip::HandleState_ExpeditionDriving()
{
Result res;
// Zum Heimathafen fahren?
if(home_harbor == goal_harbor_id)
res = DriveToHarbour();
else
res = DriveToHarbourPlace();
switch(res)
{
default: return;
case GOAL_REACHED:
{
// Haben wir unsere Expedition beendet?
if(home_harbor == goal_harbor_id)
{
// Sachen wieder in den Hafen verladen
state = STATE_EXPEDITION_UNLOADING;
current_ev = em->AddEvent(this, UNLOADING_TIME, 1);
}
else
{
// Warten auf weitere Anweisungen
state = STATE_EXPEDITION_WAITING;
// Spieler benachrichtigen
if(GAMECLIENT.GetPlayerID() == this->player)
GAMECLIENT.SendPostMessage(new ShipPostMsg(_("A ship has reached the destination of its expedition."), PMC_GENERAL, GAMECLIENT.GetPlayer(player).nation, pos));
// KI Event senden
GAMECLIENT.SendAIEvent(new AIEvent::Location(AIEvent::ExpeditionWaiting, pos), player);
}
} break;
case NO_ROUTE_FOUND:
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
// Nichts machen und idlen
StartIdling();
} break;
case HARBOR_DOESNT_EXIST: //should only happen when an expedition is cancelled and the home harbor no longer exists
{
// Kein Heimathafen mehr?
// Das Schiff muss einen Notlandeplatz ansteuern
if(players->getElement(player)->FindHarborForUnloading(this, pos, &goal_harbor_id, &route_, NULL))
{
curRouteIdx = 0;
home_harbor = goal_harbor_id; //set new home=goal so we will actually unload once we reach the goal
HandleState_ExpeditionDriving();
}
else
{
// Ansonsten als verloren markieren, damit uns später Bescheid gesagt wird
// wenn es einen neuen Hafen gibt
lost = true;
}
} break;
}
}
int MazePathSearcher::estimatedRemainingNodeCost(const MazeCellNode &node)const{
if(m_estimateEnabled){
MazeCellNode *cellGoal = dynamic_cast<MazeCellNode *>(goal());
return abs(cellGoal->pos.x-node.pos.x)+abs(cellGoal->pos.y-node.pos.y);
}else{
return 0;
}
}
void plan(ob::StateSpacePtr space, bool easy)
{
ob::ScopedState<> start(space), goal(space);
ob::RealVectorBounds bounds(2);
bounds.setLow(0);
if (easy)
bounds.setHigh(18);
else
{
bounds.high[0] = 6;
bounds.high[1] = .6;
}
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ob::SpaceInformationPtr si(ss.getSpaceInformation());
ss.setStateValidityChecker(boost::bind(
easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1));
// set the start and goal states
if (easy)
{
start[0] = start[1] = 1.; start[2] = 0.;
goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
}
else
{
start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi<double>();;
goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi<double>();
}
ss.setStartAndGoalStates(start, goal);
// this call is optional, but we put it in to get more output information
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
ss.setup();
ss.print();
// attempt to solve the problem within 30 seconds of planning time
ob::PlannerStatus solved = ss.solve(30.0);
if (solved)
{
std::vector<double> reals;
std::cout << "Found solution:" << std::endl;
ss.simplifySolution();
og::PathGeometric path = ss.getSolutionPath();
path.interpolate(1000);
path.printAsMatrix(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
Pummer::Pummer(int pinR, int pinG, int pinB, boolean anode)
{
pinMode(pR = pinR, OUTPUT);
pinMode(pG = pinG, OUTPUT);
pinMode(pB = pinB, OUTPUT);
nR = nG = nB = 0;
reverse = anode;
show();
goal(255, 255, 255);
}
void noShip::HandleState_ExplorationExpeditionDriving()
{
Result res;
// Zum Heimathafen fahren?
if(home_harbor == goal_harbor_id && covered_distance >= MAX_EXPLORATION_EXPEDITION_DISTANCE)
res = DriveToHarbour();
else
res = DriveToHarbourPlace();
switch(res)
{
default: return;
case GOAL_REACHED:
{
// Haben wir unsere Expedition beendet?
if(home_harbor == goal_harbor_id && covered_distance >= MAX_EXPLORATION_EXPEDITION_DISTANCE)
{
// Dann sind wir fertig -> wieder entladen
state = STATE_EXPLORATIONEXPEDITION_UNLOADING;
current_ev = em->AddEvent(this, UNLOADING_TIME, 1);
}
else
{
// Strecke, die wir gefahren sind, draufaddieren
covered_distance += route_.size();
// Erstmal kurz ausruhen an diesem Punkt und das Rohr ausfahren, um ein bisschen
// auf der Insel zu gucken
state = STATE_EXPLORATIONEXPEDITION_WAITING;
current_ev = em->AddEvent(this, EXPLORATION_EXPEDITION_WAITING_TIME, 1);
}
} break;
case NO_ROUTE_FOUND:
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
unsigned old_visual_range = GetVisualRange();
// Nichts machen und idlen
StartIdling();
// Sichtbarkeiten neu berechnen
gwg->RecalcVisibilitiesAroundPoint(pos, old_visual_range, player, NULL);
} break;
case HARBOR_DOESNT_EXIST:
{
// Neuen Hafen suchen
if(players->getElement(player)->FindHarborForUnloading
(this, pos, &goal_harbor_id, &route_, NULL))
HandleState_TransportDriving();
else
// Ansonsten als verloren markieren, damit uns später Bescheid gesagt wird
// wenn es einen neuen Hafen gibt
lost = true;
} break;
}
}
void noShip::HandleState_GoToHarbor()
{
// Hafen schon zerstört?
if(goal_harbor_id == 0)
{
StartIdling();
}
else
{
Result res = DriveToHarbour();
switch(res)
{
default: return;
case GOAL_REACHED:
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
// Erstmal nichts machen und idlen
StartIdling();
// Hafen Bescheid sagen, dass wir da sind (falls er überhaupt noch existiert)
noBase* nb = gwg->GetNO(goal);
if(nb->GetGOT() == GOT_NOB_HARBORBUILDING)
static_cast<nobHarborBuilding*>(nb)->ShipArrived(this);
} break;
case NO_ROUTE_FOUND:
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
// Dem Hafen Bescheid sagen
gwg->GetSpecObj<nobHarborBuilding>(goal)->ShipLost(this);
// Ziel aktualisieren
goal_harbor_id = 0;
// Nichts machen und idlen
StartIdling();
} break;
case HARBOR_DOESNT_EXIST:
{
// Nichts machen und idlen
StartIdling();
} break;
}
}
}
int main()
{
//Motion planning algorithm parameters
glc::Parameters alg_params;
alg_params.res=16;
alg_params.control_dim = 2;
alg_params.state_dim = 2;
alg_params.depth_scale = 100;
alg_params.dt_max = 5.0;
alg_params.max_iter = 50000;
alg_params.time_scale = 20;
alg_params.partition_scale = 40;
alg_params.x0 = glc::vctr({0.0,0.0});
//Create a dynamic model
SingleIntegrator dynamic_model(alg_params.dt_max);
//Create the control inputs
ControlInputs2D controls(alg_params.res);
//Create the cost function
ArcLength performance_objective(4);
//Create instance of goal region
glc::vctr xg({10.0,10.0});
SphericalGoal goal(xg.size(),0.25,4);
goal.setGoal(xg);
//Create the obstacles
PlanarDemoObstacles obstacles(4);
//Create a heuristic for the current goal
EuclideanHeuristic heuristic(xg,goal.getRadius());
glc::GLCPlanner planner(&obstacles,
&goal,
&dynamic_model,
&heuristic,
&performance_objective,
alg_params,
controls.points);
//Run the planner and print solution
glc::PlannerOutput out;
planner.plan(out);
if(out.solution_found){
std::vector<glc::nodePtr> path = planner.pathToRoot(true);
glc::splinePtr solution = planner.recoverTraj( path );
glc::splineTensor coef(solution->coefficient_array);
glc::printSpline( solution , 20, "Solution");
glc::trajectoryToFile("shortest_path.txt","../plots/",solution,500);
glc::nodesToFile("shortest_path_nodes.txt","../plots/",planner.domain_labels);
}
return 0;
}
void CNPC_Crow::StartTargetHandling( CBaseEntity *pTargetEnt )
{
AI_NavGoal_t goal( GOALTYPE_PATHCORNER, pTargetEnt->GetAbsOrigin(),
ACT_FLY,
AIN_DEF_TOLERANCE, AIN_YAW_TO_DEST);
if ( !GetNavigator()->SetGoal( goal ) )
{
DevWarning( 2, "Can't Create Route!\n" );
}
}
/// Fährt weiter zu einem Hafen
noShip::Result noShip::DriveToHarbour()
{
MapPoint goal(gwg->GetHarborPoint(goal_harbor_id));
// Existiert der Hafen überhaupt noch?
if(gwg->GetGOT(goal) != GOT_NOB_HARBORBUILDING)
return HARBOR_DOESNT_EXIST;
return DriveToHarbourPlace();
//nobHarborBuilding * hb = gwg->GetSpecObj<nobHarborBuilding>(goal.x,goal.y);
}
GoalPtr RobotWorld::newGoal( const std::string& aName,
const Point& aPosition /*= Point(-1,-1)*/,
bool aNotifyObservers /*= true*/)
{
GoalPtr goal( new Goal( aName, aPosition));
goals.push_back( goal);
if (aNotifyObservers == true)
{
notifyObservers();
}
return goal;
}
void planWithSimpleSetup()
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE3StateSpace());
// set the bounds for the R^3 part of SE(3)
ob::RealVectorBounds bounds(3);
bounds.setLow(-10);
bounds.setHigh(10);
space->as<ob::SE3StateSpace>()->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ss.setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
start.random();
// create a random goal state
ob::ScopedState<> goal(space);
goal.random();
// set the start and goal states
ss.setStartAndGoalStates(start, goal);
// this call is optional, but we put it in to get more output information
ss.setup();
ss.print();
// attempt to find an exact solution within five seconds
if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION)
{
og::PathGeometric slnPath = ss.getSolutionPath();
std::cout << std::endl;
std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
// print the path to screen
//slnPath.print(std::cout);
std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl;
ob::PlannerData data(ss.getSpaceInformation());
ss.getPlannerData(data);
ob::PlannerDataStorage dataStorage;
dataStorage.store(data, "myPlannerData");
}
else
std::cout << "No solution found" << std::endl;
}
void planWithIK(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE3StateSpace());
// set the bounds for the R^3 part of SE(3)
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE3StateSpace>()->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// create a random start state
ob::ScopedState<ob::SE3StateSpace> start(space);
start->setXYZ(0, 0, 0);
start->rotation().setIdentity();
ss.addStartState(start);
// define our goal region
MyGoalRegion region(ss.getSpaceInformation());
// bind a sampling function that fills its argument with a sampled state and returns true while it can produce new samples
// we don't need to check if new samples are different from ones previously computed as this is pefromed automatically by GoalLazySamples
ob::GoalSamplingFn samplingFunction = std::bind(®ionSamplingWithGS,
ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion,
std::placeholders::_1, std::placeholders::_2);
// create an instance of GoalLazySamples:
ob::GoalPtr goal(new ob::GoalLazySamples(ss.getSpaceInformation(), samplingFunction));
// we set a goal that is sampleable, but it in fact corresponds to a region that is not sampleable by default
ss.setGoal(goal);
// attempt to solve the problem
ob::PlannerStatus solved = ss.solve(3.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.simplifySolution();
ss.getSolutionPath().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
// the region variable will now go out of scope. To make sure it is not used in the sampling function any more
// (i.e., the sampling thread was able to terminate), we make sure sampling has terminated
goal->as<ob::GoalLazySamples>()->stopSampling();
}
//-------- Begin of function Lightning::draw_whole ----------//
void Lightning::draw_whole(VgaBuf *vgabuf)
{
int prex, prey;
while(!goal() )
{
prex = (int) x;
prey = (int) y;
move_particle();
// ignore clipping, currently
if( energy_level > 4)
{
if( prex >= bound_x1+2 && (int)x >= bound_x1+2 &&
prex <= bound_x2-2 && (int)x <= bound_x2-2 &&
prey >= bound_y1+2 && (int)y >= bound_y1+2 &&
prey <= bound_y2-2 && (int)y <= bound_y2-2 )
{
vgabuf->line(prex+2, prey, (int) x+2, (int) y, OUTLIGHTNCOLOR);
vgabuf->line(prex, prey+2, (int) x, (int) y+2, OUTLIGHTNCOLOR);
vgabuf->line(prex-2, prey, (int) x-2, (int) y, OUTLIGHTNCOLOR);
vgabuf->line(prex, prey-2, (int) x, (int) y-2, OUTLIGHTNCOLOR);
vgabuf->line(prex+1, prey, (int) x+1, (int) y, INLIGHTNCOLOR);
vgabuf->line(prex, prey+1, (int) x, (int) y+1, INLIGHTNCOLOR);
vgabuf->line(prex-1, prey, (int) x-1, (int) y, INLIGHTNCOLOR);
vgabuf->line(prex, prey-1, (int) x, (int) y-1, INLIGHTNCOLOR);
vgabuf->line(prex, prey, (int) x, (int) y, CORELIGHTNCOLOR);
}
}
else if( energy_level > 2)
{
if( prex >= bound_x1+1 && (int)x >= bound_x1+1 &&
prex <= bound_x2-1 && (int)x <= bound_x2-1 &&
prey >= bound_y1+1 && (int)y >= bound_y1+1 &&
prey <= bound_y2-1 && (int)y <= bound_y2-1 )
{
vgabuf->line(prex+1, prey, (int) x+1, (int) y, OUTLIGHTNCOLOR);
vgabuf->line(prex, prey+1, (int) x, (int) y+1, OUTLIGHTNCOLOR);
vgabuf->line(prex, prey, (int) x, (int) y, INLIGHTNCOLOR);
}
}
else
{
if( prex >= bound_x1 && (int)x >= bound_x1 &&
prex <= bound_x2 && (int)x <= bound_x2 &&
prey >= bound_y1 && (int)y >= bound_y1 &&
prey <= bound_y2 && (int)y <= bound_y2 )
{
vgabuf->line(prex, prey, (int) x, (int) y, OUTLIGHTNCOLOR);
}
}
}
}
void plan(int samplerIndex)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));
// set the bounds
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ss.setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a start state
ob::ScopedState<> start(space);
start[0] = start[1] = start[2] = 0;
// create a goal state
ob::ScopedState<> goal(space);
goal[0] = goal[1] = 0.;
goal[2] = 1;
// set the start and goal states
ss.setStartAndGoalStates(start, goal);
// set sampler (optional; the default is uniform sampling)
if (samplerIndex==1)
// use obstacle-based sampling
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
else if (samplerIndex==2)
// use my sampler
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
// create a planner for the defined space
ob::PlannerPtr planner(new og::PRM(ss.getSpaceInformation()));
ss.setPlanner(planner);
// attempt to solve the problem within ten seconds of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.getSolutionPath().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}