void PathPlanner::set_destination(FrameObject * obj, int x, int y) { PathAgent & agent = *obj->agent; PathPlanner * planner = (PathPlanner*)agent.planner; obj->agent->dest_x = planner->to_grid(x); obj->agent->dest_y = planner->to_grid(y); }
int main() { // Points points, result; PathPlanner planner; planner.initialize(); planner.mainMenu(); return 0; }
void initializePathPlanner(){ //initializeSegments(filename); planner = PathPlanner(segments); planner.populateTrajectory(); StateTrajectory* traj = planner.getPathTrajectory(); for (int i = 0; i < traj->size(); i++){ cout << traj->at(i).toString() << endl; } }
// TODO this shouldn't be used for checks, should be called from one loop for all bots // Executes the robot's movement towards the desiredLocation // Returns // 1 - success // 0 - working // -1 - failure int Robot::execute() { currentTime += (1.0/60.0); // fprintf(stderr, "currentTime %f\n", currentTime); //fprintf(stderr, "CurrentState: %f, %f, %f\ndesiredLocation: %f, %f, %f\n", CurrentState()[0], CurrentState()[1], CurrentState()[2], desiredLocation[0], desiredLocation[1], desiredLocation[2]); //if(currentFrame == 0) { currentWaypointIndex = 0; //Pass parameters in millimeters PathPlanner planner = PathPlanner(playerID, isYellowTeam); coeffecients = planner.FindPath(CurrentState(), desiredLocation); //fprintf(stderr, "Size of coeffecients: %d\n", coeffecients.size()); } currentFrame += 1; //currentFrame = (currentFrame + 1) % 10; Eigen::Vector3d desiredState; desiredState[0] = coeffecients[currentWaypointIndex][0]; desiredState[1] = coeffecients[currentWaypointIndex][1]; desiredState[2] = coeffecients[currentWaypointIndex][2]; fprintf(stderr, "desiredState: %f, %f, %f\n", desiredState[0], desiredState[1], desiredState[2]); // Computing velocities _currentVelocity = controller.ComputeCommandVelo(CurrentState(), desiredState, _currentVelocity, Eigen::Vector3d(0,0,0)); vAngular = _currentVelocity[2]; vX = _currentVelocity[0]; vY = _currentVelocity[1]; vTangent = vX * cos(_currentPosition[2]) + vY * sin(_currentPosition[2]); vTangent = (abs(vTangent)/vTangent) * min(abs(vTangent), _maxVelocity); vTangent /= 1000.; vNormal = min(- vX * sin(_currentPosition[2]) + vY * cos(_currentPosition[2]), _maxVelocity); vNormal = (abs(vNormal)/vNormal) * min(abs(vNormal), _maxVelocity); vNormal /= 1000.; //fprintf(stderr, "transformed command: %f, %f, %f\n", vTangent, vNormal, vAngular); //vAngular = min(abs(vAngular), _maxVelocity); sendVelocityCommands(); double dist = Eigen::Vector2d((CurrentState() - desiredState)[0], (CurrentState() - desiredState)[1]).norm(); fprintf(stderr, "Distance to desired location: %f \n", dist); //TODO units? if(dist < 3) { ++currentWaypointIndex; if(currentWaypointIndex > coeffecients.size() - 1) { fprintf(stderr, "Executed move to location!\n"); return 1; } } return 0; }
int main() { Serial::open(); Camera* camera = new Camera(HD720, 15.0); ERRCODE code = camera->init(MODE::QUALITY, 0); if (code != SUCCESS) { cout << errcode2str(code) << endl; delete camera; return 1; } int width = camera->getImageSize().width; int height = camera->getImageSize().height; PointCloud *cloud = new PointCloud(width, height); HeightMap *heightMap = new HeightMap(128, 128); CloudViewer *viewer = new CloudViewer(); PathPlanner *planner = new PathPlanner(heightMap); int key = ' '; Mat depth, imLeft; printf("LET'S GO!\n"); Serial::gas(0.25); // application quits when user stikes 'q' while (key != 'q') { camera->setConfidenceThreshold(98); // parameter is reliability index ~[1,100] with 100 as no filtering camera->grab(SENSING_MODE::RAW); depth = camera->retrieveMeasure(MEASURE::DEPTH); imLeft = camera->retrieveImage(SIDE::LEFT); cloud->fill(imLeft.data, (float*) depth.data, camera->getParameters()); cloud->fillHeightMap(heightMap); heightMap->calcSobel(0.6); planner->calcEdges(); steerToward(planner->getTarget()); viewer->AddData(heightMap); viewer->AddPlanner(planner); viewer->AddData(cloud); // Update the value of key so that we can quit when the user strikes 'q' key = viewer->getKey(); } printf("quitting\n"); delete camera; delete cloud; delete viewer; return 0; }
void getInputSegments(){ segments.push_back(Line(20, 55, 60, 55)); segments.push_back(Line(20, 5, 20, 55)); //TODO: real input segments.push_back(Line(60, 5, 20, 5)); segments.push_back(Line(60, 55, 60, 5)); //segments.push_back(Line(20, 5, 40, 63)); planner = PathPlanner(segments); planner.populateTrajectory(); StateTrajectory* traj = planner.getPathTrajectory(); for (int i = 0; i < traj->size(); i++){ cout << traj->at(i).toString() << endl; } }
int main() { // Read the configuration file ConfigManager::ReadParameters(); // Read and inflate map const char* mapurl = ConfigManager::GetMapUrl().erase(ConfigManager::GetMapUrl().size() - 1).c_str(); Map* currMap = LoadMap(mapurl); int robotSize = MAX(ConfigManager::GetRobotHeight(),ConfigManager::GetRobotWidth()); double mapResulotion = ConfigManager::GetMapResolution(); int InflateAddition = ceil((robotSize / 2) / mapResulotion); Map* inflateMap = currMap->Inflate(InflateAddition); Map* Maparticle = currMap->Inflate(5); // Calculate the ratio between the grid resolution and map resolution int res = ConfigManager::GetGridResolution() / ConfigManager::GetMapResolution(); Grid* grid = new Grid(inflateMap->GetMatrix(),inflateMap->GetWidth(),inflateMap->GetHeight(), res); // A* PathPlanner* p = new PathPlanner(); Point* start = ConfigManager::GetStartLocationMapResolution(); Point* end = ConfigManager::GetGoalMapResolution(); int startGridPointX,startGridPointY, endGridPointX, endGridPointY; startGridPointX = start->GetRow() / res; startGridPointY = start->GetCol() / res; endGridPointX = end->GetRow() / res; endGridPointY = end->GetCol() / res; std::string route = p->pathFind(startGridPointX ,startGridPointY,endGridPointX,endGridPointY,grid); // WayPoint calculation Point* startWithRes = new Point(start->GetRow() / 4, start->GetCol() / 4); vector<Point *> waypointsArr = WayPoints::CalculateByDirectionalPath(route, startWithRes); Robot robot("localhost",6665); PlnObstacleAvoid plnOA(&robot); Manager manager(&robot, &plnOA, waypointsArr, Maparticle); manager.run(); }
/** * @function RRTPlan */ void PlannerTab::RRTPlan() { double stepSize = 0.02; PathPlanner *planner = new PathPlanner( *mWorld, mCollision, false, stepSize ); int maxNodes = 5000; bool result = planner->planPath( gRobotId, gLinks, gStartConf, gTargetConf, true, // bidirectional true, // connect true, // greedy false, // smooth maxNodes ); RRTExecute( planner->path ); }
string WaypointManager::Path() { ConfigManager* cm = new ConfigManager(); int const xStart=cm->getStartLocationX(); int const yStart=cm->getStartLocationY(); int const xEnd=cm->getGoalLocationX(); int const yEnd=cm->getGoalLocationY(); PathPlanner* Pathp = new PathPlanner(); vector < pair<int,int> > Points; string FinalDir = ""; int NumOfSteps = 0; int NumOfDir = 0; string Directions = Pathp->pathFind(xStart,yStart,xEnd,yEnd,&Points); pair<int,int> pnt; pnt.first = xStart; pnt.second = yStart; FinalPoints.push_back(pnt); for (int NumOfPnt=(Points.size()-1);NumOfPnt>1;NumOfPnt--) { if (NumOfSteps==3 || Directions[NumOfDir]!=Directions[NumOfDir+1]) { pnt.first = Points[NumOfPnt].first; pnt.second = Points[NumOfPnt].second; FinalPoints.push_back(pnt); FinalDir+=Directions[NumOfDir]; NumOfSteps=0; } NumOfSteps++; NumOfDir++; } FinalDir+=Directions[NumOfDir]; pnt.first = xEnd; pnt.second = yEnd; FinalPoints.push_back(pnt); return FinalDir; }
/// Visualizes a 2D RRT void drawTwoLink () { // Create a robot and a world const double l1 = 1.5, l2 = 1.0; Skeleton* r = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL); World w; w.addSkeleton(r); // Create a path planner and feed it an unfeasible goal to grow the RRT freely PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0); vector <int> links; links.push_back(0); links.push_back(1); list <VectorXd> path; planner.planPath(r, links, Vector2d(-DART_PI+0.1, -DART_PI+0.1), Vector2d(DART_PI-0.1, DART_PI-0.1), path); // Print the nodes RRT* rrt = planner.start_rrt; draw(rrt, NULL); }
void PathPlanner::plan_path(FrameObject * obj) { // XXX this is stupid and slow, but should work well enough FrameObject::PathAgent & agent = *obj->agent; agent.nodes.clear(); PathPlanner * planner = (PathPlanner*)agent.planner; if (agent.x == agent.dest_x && agent.y == agent.dest_y) return; int grid_x = clamp(agent.dest_x, 0, planner->map_width - 1); int grid_y = clamp(agent.dest_y, 0, planner->map_height - 1); int test = planner->to_index(grid_x, grid_y); if (planner->map.get(test)) { #ifndef NDEBUG dump_map(grid_x, grid_y, planner); #endif std::cout << "Destination not possible" << std::endl; return; } Grid grid(*planner, agent); unsigned step = 1; JPS::PathVector path; bool found = JPS::findPath(path, grid, agent.x, agent.y, agent.dest_x, agent.dest_y, step); if (!found) { std::cout << "No path found" << std::endl; return; } JPS::PathVector::iterator it; for (int i = 0; i < int(path.size()); ++i) { JPS::Position & node = path[i]; PathNode n = {node.x, node.y}; agent.nodes.push_back(n); } }
biped* searchTrajectory(){ stateTraj = planner.getPathTrajectory(); RobotSegment curstate = stateTraj->back(); for(int i = stateTraj->size() - 2; i >= 0; i --){ RobotSegment goalstate = stateTraj->at(i); checker = new BipedChecker(&grid, goalstate.robot_pos[0], goalstate.robot_pos[1], inflate_h, inflate_z); //hardcoded r? TODO: fix biped* output=helper.search(curstate.robot_pos[0], curstate.robot_pos[1], curstate.theta, goalstate.robot_pos[0], goalstate.robot_pos[1], 3, goalstate.theta, checker, maxDepth, viewDepth); bipedTrajectory.push_back(output); curstate = goalstate; //stateTraj->pop_back(); } }
//Function to search a trajectory of states void searchTrajectory(){ stateTraj = planner.getPathTrajectory(); //RobotSegment curstate = RobotSegment(init.x(), init.y(), initTheta, Line(0, 0, 0, 0)); RobotSegment curstate = stateTraj->back(); //stateTraj->pop_back(); for(int i = stateTraj->size() - 2; i >= 0; i --){ RobotSegment goalstate = stateTraj->at(i); checker = new BipedChecker(&grid, goalstate.robot_pos[0], goalstate.robot_pos[1], inflate_h, inflate_z); cout << "Moving from " << curstate.toString() << " to " << goalstate.toString() <<endl ; biped* output = helper.search(curstate.robot_pos[0], curstate.robot_pos[1], curstate.theta, goalstate.robot_pos[0], goalstate.robot_pos[1], 3, goalstate.theta, checker, maxDepth, viewDepth); //hardcoded r? TODO: fix bipedTrajectory.push_back(output); curstate = goalstate; //stateTraj->pop_back(); } }
kuri_msgs::NavTasks PathGenerator::generatePaths(const kuri_msgs::Tasks newtasks) { tasks = newtasks; ros::Subscriber uav1_currentPoseSub = nh.subscribe("/uav_1/mavros/local_position/pose", 100, &PathGenerator::uav1_startPositionCallback,this); ros::Subscriber uav2_currentPoseSub = nh.subscribe("/uav_2/mavros/local_position/pose", 100, &PathGenerator::uav2_startPositionCallback,this); ros::Subscriber uav3_currentPoseSub = nh.subscribe("/uav_3/mavros/local_position/pose", 100, &PathGenerator::uav3_startPositionCallback,this); //ros::Subscriber taskSub = nh.subscribe("kuri_msgs/Tasks", 1, &PathGenerator::navTasksCallback,this); //ros::Publisher posePub = nh.advertise<geometry_msgs::PoseStamped>("/uav_1/mavros/setpoint_position/local", 10); rviz_visual_tools::RvizVisualToolsPtr visualTools; visualTools.reset(new rviz_visual_tools::RvizVisualTools("map", "/sspp_visualisation")); visualTools->deleteAllMarkers(); visualTools->setLifetime(0.2); ros::Time timer_start = ros::Time::now(); geometry_msgs::Pose gridStartPose; geometry_msgs::Vector3 gridSize; gridStartPose.position.x = -45; gridStartPose.position.y = -30; gridStartPose.position.z = 0; //THE ARENA SIZE IS is 90 x 60 gridSize.x = 90; gridSize.y = 60; gridSize.z = 20; PathPlanner * pathPlanner; double robotH = 0.9, robotW = 0.5, narrowestPath = 0.987; double distanceToGoal = 2, regGridConRad = 3;//distanceToGoal = 1.5 double gridRes = 2; geometry_msgs::Point robotCenter; robotCenter.x = -0.3f; robotCenter.y = 0.0f; Robot *robot = new Robot("Robot", robotH, robotW, narrowestPath, robotCenter); // Every how many iterations to display the tree int progressDisplayFrequency = 1; pathPlanner = new PathPlanner(nh, robot, regGridConRad, progressDisplayFrequency); // This causes the planner to pause for the desired amount of time and display the search tree, useful for debugging pathPlanner->setDebugDelay(0.0); ROS_INFO("planner object created"); DistanceHeuristic distanceHeuristic(nh, false); distanceHeuristic.setTolerance2Goal(distanceToGoal); pathPlanner->setHeuristicFucntion(&distanceHeuristic); // Generate Grid Samples and visualise it pathPlanner->generateRegularGrid(gridStartPose, gridSize, gridRes, false); std::vector<geometry_msgs::Point> searchSpaceNodes = pathPlanner->getSearchSpace(); std::cout<<"\n\n---->>> Total Nodes in search Space ="<<searchSpaceNodes.size(); visualization_msgs::Marker searchSpaceMarker = drawPoints(searchSpaceNodes,2,1000000); // Connect nodes and visualise it pathPlanner->setMultiAgentSupport(true); pathPlanner->connectNodes(); std::cout<<"\nSpace Generation took:"<<double(ros::Time::now().toSec() - timer_start.toSec())<<" secs"; std::vector<geometry_msgs::Point> searchSpaceConnections = pathPlanner->getConnections(); visualTools->publishPath(searchSpaceConnections, rviz_visual_tools::BLUE, rviz_visual_tools::LARGE, "search_space"); visualization_msgs::Marker connectionsMarker = drawLines(searchSpaceConnections,10000,3,100000000,0.03); searchSpacePub.publish(searchSpaceMarker); connectionsPub.publish(connectionsMarker); //pathPlanner->disconnectNodes(); std::cout<<"\n Nodes are now disconnected"; ROS_INFO("\nstarting the loop"); std::vector<geometry_msgs::Point> pathSegments; geometry_msgs::PoseArray robotPose, sensorPose; geometry_msgs::Point linePoint; double dist = 0, threshold2Dist = 0.5; double yaw; //TODO: Ultimately this should be a vector of paths: one per UAV //Node * path = NULL; //std::vector<Node*> paths; Node * wayPoint = NULL; ros::Rate loopRate(10); while (ros::ok()) { // TODO: allow the handling of multiple paths if (tasks.tasks.size()<1) { tasks = newtasks; std::vector<Node*> paths = pathPlanner->paths; kuri_msgs::NavTasks nav_tasks; int i = 0; for(std::vector<Node*>::iterator it = paths.begin(); it!=paths.end();it++) { Node * path2Add = *it; nav_msgs::Path result; result.header.frame_id="/map"; while (path2Add != NULL) { geometry_msgs::PoseStamped uavWayPoint; uavWayPoint.pose = path2Add->pose.p; result.poses.push_back(uavWayPoint); path2Add = path2Add->next; } geometry_msgs::PoseStamped uavWayPoint; uavWayPoint.pose = endPoses.at(i); i++; //endPoses.pop_back(); result.poses.push_back(uavWayPoint); kuri_msgs::NavTask nav_task; nav_task.path = result; kuri_msgs::Task task = tasks.tasks.back(); tasks.tasks.pop_back(); nav_task.task = task; nav_tasks.nav_tasks.push_back(nav_task); } // Node * path2Send = path; // nav_msgs::Path result; // result.header.frame_id="/map"; // while (path2Send != NULL) { // geometry_msgs::PoseStamped uavWayPoint; // uavWayPoint.pose = path2Send->pose.p; // result.poses.push_back(uavWayPoint); // path2Send = path2Send->next; // } // geometry_msgs::PoseStamped uavWayPoint; // uavWayPoint.pose = endPose; // result.poses.push_back(uavWayPoint); return nav_tasks; }else{ ROS_INFO("Path Planning for a new task"); // Remove one task and serve it kuri_msgs::Task task = tasks.tasks.back(); tasks.tasks.pop_back(); kuri_msgs::Object currentObj = task.object; endPoses.push_back(currentObj.pose.pose); // Find path and visualise it timer_start = ros::Time::now(); //To avoid crawling on the ground, always start at 10m altitude geometry_msgs::Pose currentPose; if(task.uav_id==1){ currentPose = uav1_currentPose; }else if(task.uav_id==2){ currentPose = uav2_currentPose; }else if(task.uav_id==3){ currentPose = uav3_currentPose; } ROS_INFO("New Destination(x,y,z): (%g,%g,%g) Current Location(x,y,z): (%g,%g,%g)", currentObj.pose.pose.position.x, currentObj.pose.pose.position.y, currentObj.pose.pose.position.z, currentPose.position.x, currentPose.position.y, currentPose.position.z); // ToFix: make sure currentPose was acctualy updated before generating the path Pose start(currentPose.position.x, currentPose.position.y, 10, DTOR(0.0));//it was currentPose.position.z = 10 //To allow visual servoying assisted landing, always hover at predefined z: 10m Pose end(currentObj.pose.pose.position.x, currentObj.pose.pose.position.y, 10, DTOR(0.0));//it was z=10 distanceHeuristic.setEndPose(end.p); ROS_INFO("Starting search"); Node * retval = pathPlanner->startSearch(start); std::cout<<"\nPath Finding took:"<<double(ros::Time::now().toSec() - timer_start.toSec())<<" secs"; //path print and visualization if (retval) { pathPlanner->printNodeList(); } else { std::cout << "\nNo Path Found"; continue; } // path = pathPlanner->paths[0]; } ros::spinOnce(); loopRate.sleep(); } delete robot; delete pathPlanner; }