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;
}
