/**
* @function planPath
* @brief Main function
*/
bool PathPlanner::planPath( int _robotId,
const Eigen::VectorXi &_links,
const Eigen::VectorXd &_start,
const Eigen::VectorXd &_goal,
bool _bidirectional,
bool _connect,
bool _greedy,
bool _smooth,
unsigned int _maxNodes ) {
//world->mRobots[_robotId]->setQuickDofs( _start ); // Other quick way
world->getRobot(_robotId)->setDofs( _start, _links );
if( world->checkCollision() )
return false;
world->getRobot(_robotId)->setDofs( _goal, _links );
if( world->checkCollision() )
return false;
bool result;
if( _bidirectional ) {
result = planBidirectionalRrt( _robotId, _links, _start, _goal, _connect, _greedy, _maxNodes );
} else {
result = planSingleTreeRrt( _robotId, _links, _start, _goal, _connect, _greedy, _maxNodes );
}
if( result && _smooth ) {
smoothPath( _robotId, _links, path );
}
return result;
}
void PencilTool::release(const TupInputDeviceInformation *input, TupBrushManager *brushManager, TupGraphicsScene *scene)
{
Q_UNUSED(brushManager);
if (!k->item)
return;
double smoothness = k->configurator->exactness();
if (k->firstPoint == input->pos() && k->path.elementCount() == 1) {
smoothness = 0;
qreal radius = ((qreal) brushManager->pen().width()) / ((qreal) 2);
k->path.addEllipse(input->pos().x(), input->pos().y(), radius, radius);
}
smoothPath(k->path, smoothness);
k->item->setBrush(brushManager->brush());
k->item->setPath(k->path);
QDomDocument doc;
doc.appendChild(k->item->toXml(doc));
TupProjectRequest request = TupRequestBuilder::createItemRequest(scene->currentSceneIndex(), scene->currentLayerIndex(), scene->currentFrameIndex(),
0, QPoint(), scene->spaceContext(), TupLibraryObject::Item, TupProjectRequest::Add,
doc.toString());
emit requested(&request);
}
// Performs multiple passes of resampling
p_nav::Path
smoothPathMultiple(const p_nav::Path& path, int passes)
{
p_nav::Path oldpath, newpath;
newpath = path;
for( int i=0; i<passes; i++ ){
oldpath = newpath;
newpath = smoothPath(oldpath);
}
return newpath;
}
void handleMouseEvent(SDL_Event event, GraphicModule * module){
//Welcome to the bottleneck, we got fun and games
//If lagging is your issue, here you must change!
if(withinMenu(module->menu,event.motion.x, event.motion.y) != -1){
int button = checkButtons(module->menu, event.motion.x,event.motion.y);
if(button != -1){
if(mouseDown){
//Click
module->menu->buttons[button]->clicked = 1;
module->menu->buttons[button]->hover = 0;
}else{
//Hover or finished clicking
if(module->menu->buttons[button]->clicked == 1){
//We just finished clicking and are about to change back to a hover state
handleButtonClick(module, button);
}
module->menu->buttons[button]->clicked = 0;
module->menu->buttons[button]->hover = 1;
}
}else{
//Not in any buttons, make sure all of them are in their default state
int i;
for(i=0; i < NUMBER_OF_BUTTONS; i++){
module->menu->buttons[i]->clicked = 0;
module->menu->buttons[i]->hover = 0;
}
}
}else{
//Drawing mode. (Once we are loading documents some more stuff will have to go here)
if(mouseDown){
if(event.motion.x < SCREENWIDTH && event.motion.y < SCREENHEIGHT){
buffered[bufferPointer] = event.motion.x;
buffered[bufferPointer+1] = event.motion.y;
//If smoothing is off this will be optimized out by the compiler
if(SMOOTHING==1){
smoothPath(event.motion.x,event.motion.y,event.motion.xrel,event.motion.yrel);
}
bufferPointer = bufferPointer+2;
}
if(bufferPointer > CLICKBUFFERSIZE){
puts("aw shit ");//what a good error message for now
bufferPointer = 0;
}
}
}
}
void PencilTool::release(const TupInputDeviceInformation *input, TupBrushManager *brushManager, TupGraphicsScene *scene)
{
Q_UNUSED(brushManager);
if (!k->resize) {
if (!k->item)
return;
if (k->firstPoint == input->pos() && k->path.elementCount() == 1) {
QPointF currentPoint = input->pos();
scene->removeItem(k->item);
qreal radius = brushManager->pen().width();
QPointF distance((radius + 2)/2, (radius + 2)/2);
QPen inkPen(brushManager->penColor(), 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
TupEllipseItem *blackEllipse = new TupEllipseItem(QRectF(currentPoint - distance, QSize(radius + 2, radius + 2)));
blackEllipse->setPen(inkPen);
blackEllipse->setBrush(inkPen.brush());
scene->includeObject(blackEllipse);
QDomDocument doc;
doc.appendChild(blackEllipse->toXml(doc));
TupProjectRequest request = TupRequestBuilder::createItemRequest(scene->currentSceneIndex(), scene->currentLayerIndex(), scene->currentFrameIndex(),
0, currentPoint, scene->spaceContext(), TupLibraryObject::Item, TupProjectRequest::Add,
doc.toString());
emit requested(&request);
return;
} else {
double smoothness = k->configurator->smoothness();
if (smoothness > 0)
smoothPath(k->path, smoothness);
}
k->item->setBrush(brushManager->brush());
k->item->setPath(k->path);
QDomDocument doc;
doc.appendChild(k->item->toXml(doc));
TupProjectRequest request = TupRequestBuilder::createItemRequest(scene->currentSceneIndex(), scene->currentLayerIndex(), scene->currentFrameIndex(),
0, QPoint(), scene->spaceContext(), TupLibraryObject::Item, TupProjectRequest::Add,
doc.toString());
emit requested(&request);
}
}
std::vector<Eigen::Vector3f> PathPlanner::getPath(Eigen::Vector3f start, Eigen::Vector3f end, cv::Mat3b * image)
{
std::vector<Eigen::Vector3f> path;
std::vector<Eigen::Vector2i> pathImg;
//Objects are outside the configuration space, so get the closest point
Eigen::Vector2i lastPoint;
if(!validPoint(worldToImg(end)))
{
if(!findClosestPoint(start, end, bot_in_pixels_))
{
return path;
}
}
//Check if we can just go straight there unobstructed
if(traceLine(worldToImg(start), worldToImg(end)))
{
path.push_back(start);
path.push_back(end);
}
else if(aStar(worldToImg(start), worldToImg(end), pathImg))
{
pathImg = smoothPath(pathImg);
for(size_t i = 0; i < pathImg.size(); i++)
{
path.push_back(imgToWorld(pathImg.at(i)));
}
}
if(image)
{
drawPath(path, *image);
}
return path;
}
path *craStar::getPath(graphAbstraction *aMap, node *from, node *to, reservationProvider *rp)
{
// std::cout<<"find path from "<<*from<<"\nto "<<*to<<std::endl;
std::vector<node *> fromChain;
std::vector<node *> toChain;
path *lastPath = 0;
if (aMap->getAbstractGraph(from->getLabelL(kAbstractionLevel))->findEdge(from->getNum(), to->getNum()))
return new path(from, new path(to));
setupSearch(aMap, fromChain, from, toChain, to);
if (fromChain.size() == 0)
return 0;
// do {
// // lastPath = buildNextAbstractPath(aMap, lastPath, fromChain, toChain, rp);
// // lastPath = buildNextAbstractPathQuick(aMap,lastPath,fromChain,toChain,rp);
// } while (lastPath->n->getLabelL(kAbstractionLevel) > 0);
path* abs = buildAbstractPath(aMap, fromChain, toChain, rp);
if (partialLimit > 0)
{
path *trav = abs;
path *thisPart = new path(trav->n);
for (int x = 0; x < partialLimit-1; x++)
{
if (trav->next) {
trav = trav->next;
thisPart->tail()->next = new path(trav->n);
}
else
break;
}
toChain.clear();
findGoalNode(aMap,trav->n,toChain);
lastPath = doRefinement(aMap, thisPart, fromChain,toChain);
//delete thisPart;
}
else
lastPath = doRefinement(aMap, abs, fromChain, toChain);
// if (verbose){
// std::cout<<"before smoothing :";
// lastPath->print();
// std::cout<<std::endl;
//}
if (smoothing)
{
path* p = lastPath;
lastPath = smoothPath(aMap,lastPath);
delete p;
}
return lastPath;
//return trimPath(lastPath, to);
}
bool VoronoiPlanner::computePlan(costmap_2d::Costmap2D* costmap_, DynamicVoronoi * voronoi_,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan) {
//Sure that plan is clear
plan.clear();
ros::NodeHandle n;
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
worldToMap(costmap_, wx, wy, start_x, start_y);
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
worldToMap(costmap_, wx, wy, goal_x, goal_y);
clearRobotCell(costmap_, start_x_i, start_y_i);
bool ** map;
computeVoronoi(voronoi_, costmap_, map);
ros::Time t_b = ros::Time::now();
ros::Time t = ros::Time::now();
std::vector<std::pair<float, float> > path1;
std::vector<std::pair<float, float> > path2;
std::vector<std::pair<float, float> > path3;
ROS_INFO("start_x %f, start_y %f", start_x, start_y);
bool res1 = false, res2 = false, res3 = false;
// If goal not are in cell of the vornoi diagram, we have that best findPath of goal to voronoi diagram without have a cell occupancie
if (!voronoi_->isVoronoi(goal_x, goal_y)) {
res3 = computePath(&path3, goal_x, goal_y, start_x, start_y, voronoi_, 0,
1);
std::cout << "computePath goal to VD " << res3 << std::endl;
goal_x = std::get < 0 > (path3[path3.size() - 1]);
goal_y = std::get < 1 > (path3[path3.size() - 1]);
ROS_INFO("Is voronoi goal compute %d", voronoi_->isVoronoi(goal_x, goal_y));
std::reverse(path3.begin(), path3.end());
}
if (!voronoi_->isVoronoi(start_x, start_y)) {
res1 = computePath(&path1, start_x, start_y, goal_x, goal_y, voronoi_, 0,
1);
std::cout << "computePath start to VD " << res1 << std::endl;
start_x = std::get < 0 > (path1[path1.size() - 1]);
start_y = std::get < 1 > (path1[path1.size() - 1]);
ROS_INFO("Is voronoi start compute %d", voronoi_->isVoronoi(start_x, start_y));
}
res2 = computePath(&path2, start_x, start_y, goal_x, goal_y, voronoi_, 1, 0);
ROS_INFO("computePath %d", res2);
if (!(res1 && res2 && res3)) {
ROS_INFO("Failed to compute full path");
}
path1.insert(path1.end(), path2.begin(), path2.end());
path1.insert(path1.end(), path3.begin(), path3.end());
/*for (int i = 0; i < path1.size(); i++) {
int x = std::get < 0 > (path1[i]);
int y = std::get < 1 > (path1[i]);
if (x > 0 && y > 0)
map[x][y] = 1;
}*/
smoothPath(&path1);
for (int i = 0; i < path1.size(); i++) {
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = std::get < 0 > (path1[i]);
new_goal.pose.position.y = std::get < 1 > (path1[i]);
mapToWorld(costmap_, new_goal.pose.position.x, new_goal.pose.position.y,
new_goal.pose.position.x, new_goal.pose.position.y);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
ROS_INFO("\nTime to get plan: %f sec\n", (ros::Time::now() - t_b).toSec());
return !plan.empty();
}
