void plot_shared_points(const Route& r, const char* filename) {
std::ofstream os(filename);
SvgWriter svg(r, os);
if(r.empty())
return;
std::vector<Point> sharedPoints;
find_shared_points(r, sharedPoints);
for(const Path& path : r) {
svg.write(path, "stroke:rgb(0,0,0);stroke-width:10;");
}
for(const Point& p : sharedPoints) {
svg.write(p, "stroke:rgb(255,0,0);stroke-width:10;fill:none");
}
svg.write(r.front().front(), "stroke:rgb(0,255,0);stroke-width:40;");
svg.write(r.back().back(), "stroke:rgb(0,0,255);stroke-width:40;");
}
Pathfinding::Route Pathfinding::getPath(Entity* entity, const Vector3& goal) {
stringstream prettyInfo;
Vector3 start = entity->getPosition();
double a = phantom::Util::getTime();
_layer.cleanPathfinding();
Route route;
if(_showDebug) {
cout << endl<< endl<< endl<< endl;
}
if(_visualize) {
getGraphics().clear();
}
// Goal space uses a "strict" heuristic. EG: we don't want to walk into a tree.
Space* goalSpace = _layer.getSpaceAtUsingHeuristic(goal, entity);
// There is no "space" available at the destination. The entity probably wants
// to walk into a tree. Returns an empty route.
if(goalSpace == nullptr) {
if(_showDebug) {
cout << "Goal vector is not a space." << endl;
}
return route;
}
// Start space, first try using a strict heuristic. EG: If we start near a tree
// we don't want to walk into that tree.
Space* startSpace = _layer.getSpaceAtUsingHeuristic(start, entity);
// Ok, did we find a start space with the strict heuristic? If not, it probably
// means that our entity is stuck in a tree. Proceed with a less strict
// heuristic. In most cases this will let the entity "leave" the solid object
// that it's currently stuck on.
if(startSpace == nullptr) {
startSpace = _layer.getSpaceAtUsingHeuristic(start);
}
// Ok, we really can't walk anywhere. This is a rather odd case. Most likely
// the user tried to walk outside of the BSP tree, or you've just found a bug
// in the BSP tree.
if(startSpace == nullptr) {
if(_showDebug) {
cout << "Start vector is not a space." << endl;
}
route.push_back(Vector3(goal));
return route;
}
if(_showDebug) {
cout << "Starting at: " << startSpace->getArea().toString();
cout << "Ending at : " << goalSpace->getArea().toString();
}
if(_visualize) {
Box3& m = startSpace->getArea();
getGraphics().beginPath().setFillStyle(Color(0, 0, 127, 20))
.rect(m.origin.x+4, m.origin.y+4, m.size.x-8, m.size.y-8)
.fill();
Box3& n = goalSpace->getArea();
getGraphics().beginPath().setFillStyle(Color(0, 0, 127, 20))
.rect(n.origin.x+4, n.origin.y+4, n.size.x-8, n.size.y-8)
.fill();
}
priority_queue<Space*, vector<Space*>, CompareShapesAstar> open;
startSpace->isInOpenList = true;
open.push(startSpace);
if(_showBasicDebug) {
prettyInfo << "Pathfinding overhead: " << std::fixed << (phantom::Util::getTime() - a) << " seconds. ";
}
int spacesScanned = 0;
const double startTime = phantom::Util::getTime();
int timeout = 0;
while(true) {
if(open.empty()) {
if(_showBasicDebug || _showDebug) {
cout << " Open list empty." << endl;
double now = phantom::Util::getTime();
if(_showBasicDebug) {
prettyInfo << "No route found, scanned "<< spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds.";
}
}
break;
}
if(timeout++ > 10000) {
cout << " I give up after " << timeout << " tries. " << endl;
double now = phantom::Util::getTime();
cout << "A* scanned " << spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds. " << endl;
break;
}
Space* current = open.top();
open.pop();
if(_visualize) {
//cout << " - Testing: " << current->getArea().toString();
drawRect(current, Color(0, 127, 127, 10));
}
if(current == goalSpace) {
if(_showDebug) {
cout << " **** found! This is a good sign. " << endl;
}
unfoldRoute(route, current, startSpace, entity);
if(!route.empty()) {
route.pop_front();
Vector3 lastpos = goal - entity->getBoundingBox().size * 0.5;
// Replace the last way-point with our mouse click coordinates:
if(route.empty()) {
route.push_back(lastpos);
} else {
route.back() = lastpos;
}
}
double now = phantom::Util::getTime();
if(_showBasicDebug) {
prettyInfo << "Found route, A* scanned " << spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds. Waypoint(s): " << route.size() << ".";
}
break;
}
vector<Space*>& neighbours = _layer.getNeighbours(current, entity);
if(_showDebug && neighbours.empty()) {
cout << " No neighbours found." << endl;
}
for(size_t i = 0; i < neighbours.size(); ++i) {
Space* testing = neighbours[i];
if(!testing->isInOpenList) {
spacesScanned++;
testing->astarParent = current;
testing->isInOpenList = true;
testing->g = current->g + Services::settings()->pathfinding_g_cost;
testing->h = calculateHeuristic(goalSpace, testing);
testing->h = testing->h * testing->h;
open.push(testing);
}
}
}
if(_showBasicDebug) {
//cout << prettyInfo.str() << endl;
Console::log(prettyInfo.str());
}
return route;
}
int main(int argc, char **argv)
{
Route path;
//path.brickDelivery();
path.brickOrder(CELL_1);
path.infoRoute();
ros::init(argc, argv, "mission_node");
ros::NodeHandle nodeHandler;
nodeHandler.param<int>("loopRate", loopRate, 10);
//init publishers
action_publisher = nodeHandler.advertise<msgs::IntStamped>("mission/next_mission",1);
ros::Rate rate(loopRate);
/*
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_navigation("fibonacci", true);
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_to_cell("fibonacci", true);
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_from_cell("fibonacci", true);
action_navigation.waitForServer();
action_to_cell.waitForServer();
action_from_cell.waitForServer();
learning_actionlib::FibonacciGoal goal;
goal.order = 20;
ac.sendGoal(goal);
*/
while(ros::ok())
{
//check mes order
/*if(mes=getBricks)
{
path.brickOrder(CELL);
}
if(mes=Delivery)
{
path.brickDelivery();
}*/
if(path.empty() && path.getCurrentState() != CHARGE)
{
path.goCharge();
}
if(!path.empty() && path.next() != CTR_IDLE)
{
if(navigation_area)
{
if(path.next() == TRANSITION)
{
navigation_area = false;
}
//learning_actionlib::FibonacciGoal goal;
//goal.order = path.next();
//action_navigation.sendGoal(goal);
//action_navigation.waitForResult(); //default 0, which should mean blocking
path.pop();
}
else
{
if(path.next() == TRANSITION)
{
navigation_area = true;
//learning_actionlib::FibonacciGoal goal;
//action_from_cell.sendGoal();
//action_from_cell.waitForResult(); //default 0, which should mean blocking
path.pop();
}
else
{
//learning_actionlib::FibonacciGoal goal;
//goal.order = path.next();
//action_to_cell.sendGoal(goal);
//action_to_cell.waitForResult(); //default 0, which should mean blocking
path.pop();
}
}
}
msgs::IntStamped gui_message;
gui_message.header.stamp = ros::Time::now();
gui_message.data = path.next();
action_publisher.publish(gui_message);
ros::spinOnce();
rate.sleep();
}
return 0;
}
