Пример #1
0
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;
	}
}
Пример #3
0
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; 
}
Пример #4
0
 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;
}
Пример #7
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;
    }
Пример #8
0
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_);
 }
Пример #10
0
  /**
   * 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;
	}
}
Пример #12
0
/*----------------------------------------------------------------------------*/
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;
    }
}
Пример #13
0
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);
}
Пример #14
0
//-------- Begin of function Lightning::progress ----------//
double Lightning::progress()
{
	if(goal())
		return 1;
	else
		return (double) steps / expect_steps;
}
Пример #15
0
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;
}
Пример #16
0
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;
    }
}
Пример #17
0
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;
    }
}
Пример #18
0
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;
}
Пример #20
0
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);
	}
Пример #21
0
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;
    }
}
Пример #22
0
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;
        }
    }
}
Пример #23
0
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;
}
Пример #24
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" );
	}
}
Пример #25
0
/// 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);
}
Пример #26
0
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;
}
Пример #27
0
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;
}
Пример #28
0
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(&regionSamplingWithGS,
        ss.getSpaceInformation(), ss.getProblemDefinition(), &region,
        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();
}
Пример #29
0
//-------- 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);
			}
		}

	}
}
Пример #30
0
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;
}