Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FIx negative index problem for dynamic maps #31

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

FIx negative index problem for dynamic maps #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c87c9bd
Add co-ordinate map-to-world transform for proper index accessing
grimmmyshini Feb 8, 2020
1e2f54a
Generalize map bound checking
grimmmyshini Feb 8, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion include/node2d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define NODE2D_H

#include <cmath>
#include <geometry_msgs/Pose.h>

#include "constants.h"
namespace HybridAStar {
Expand Down Expand Up @@ -48,6 +49,10 @@ class Node2D {
bool isDiscovered() const { return d; }
/// get a pointer to the predecessor
Node2D* getPred() const { return pred; }
/// Get x as on map
int getGridX(){ return (int)((x - mp.position.x) / res); }
/// Get y as on map
int getGridY(){ return (int)((y - mp.position.y) / res); }

// SETTER METHODS
/// set the x position
Expand All @@ -59,7 +64,7 @@ class Node2D {
/// set the cost-to-come (heuristic value)
void setH(const float& h) { this->h = h; }
/// set and get the index of the node in the 2D array
int setIdx(int width) { this->idx = y * width + x; return idx;}
int setIdx(int width) { this->idx = this->getGridY() * width + this->getGridX(); return idx;}
/// open the node
void open() { o = true; c = false; }
/// close the node
Expand All @@ -70,6 +75,8 @@ class Node2D {
void discover() { d = true; }
/// set a pointer to the predecessor of the node
void setPred(Node2D* pred) { this->pred = pred; }
/// set map meta data
static void setMeta(geometry_msgs::Pose p, float r){ mp = p; res = r; }

// UPDATE METHODS
/// Updates the cost-so-far for the node x' coming from its predecessor. It also discovers the node.
Expand Down Expand Up @@ -118,6 +125,10 @@ class Node2D {
bool d;
/// the predecessor pointer
Node2D* pred;
/// map pose
static geometry_msgs::Pose mp;
/// resolution
static float res;
};
}
#endif // NODE2D_H
13 changes: 12 additions & 1 deletion include/node3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define NODE3D_H

#include <cmath>
#include <geometry_msgs/Pose.h>

#include "constants.h"
#include "helper.h"
Expand Down Expand Up @@ -53,6 +54,10 @@ class Node3D {
bool isClosed() const { return c; }
/// determine whether the node is open
const Node3D* getPred() const { return pred; }
/// Get x as on map
int getGridX(){ return (int)((x - mp.position.x) / res); }
/// Get y as on map
int getGridY(){ return (int)((y - mp.position.y) / res); }

// SETTER METHODS
/// set the x position
Expand All @@ -66,13 +71,15 @@ class Node3D {
/// set the cost-to-come (heuristic value)
void setH(const float& h) { this->h = h; }
/// set and get the index of the node in the 3D grid
int setIdx(int width, int height) { this->idx = (int)(t / Constants::deltaHeadingRad) * width * height + (int)(y) * width + (int)(x); return idx;}
int setIdx(int width, int height) { this->idx = (int)(t / Constants::deltaHeadingRad) * width * height + this->getGridY() * width + this->getGridX(); return idx;}
/// open the node
void open() { o = true; c = false;}
/// close the node
void close() { c = true; o = false; }
/// set a pointer to the predecessor of the node
void setPred(const Node3D* pred) { this->pred = pred; }
/// set map meta data
static void setMeta(geometry_msgs::Pose p, float r){ mp = p; res = r; }

// UPDATE METHODS
/// Updates the cost-so-far for the node x' coming from its predecessor. It also discovers the node.
Expand Down Expand Up @@ -125,6 +132,10 @@ class Node3D {
int prim;
/// the predecessor pointer
const Node3D* pred;
/// map pose
static geometry_msgs::Pose mp;
/// resolution
static float res;
};
}
#endif // NODE3D_H
6 changes: 3 additions & 3 deletions src/algorithm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -432,14 +432,14 @@ void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLo

// if twoD heuristic is activated determine shortest path
// unconstrained with obstacles
if (Constants::twoD && !nodes2D[(int)start.getY() * width + (int)start.getX()].isDiscovered()) {
if (Constants::twoD && !nodes2D[start.getGridY() * width + start.getGridX()].isDiscovered()) {
// ros::Time t0 = ros::Time::now();
// create a 2d start node
Node2D start2d(start.getX(), start.getY(), 0, 0, nullptr);
// create a 2d goal node
Node2D goal2d(goal.getX(), goal.getY(), 0, 0, nullptr);
// run 2d astar and return the cost of the cheapest path for that node
nodes2D[(int)start.getY() * width + (int)start.getX()].setG(aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization));
nodes2D[start.getGridY() * width + start.getGridX()].setG(aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization));
// ros::Time t1 = ros::Time::now();
// ros::Duration d(t1 - t0);
// std::cout << "calculated 2D Heuristic in ms: " << d * 1000 << std::endl;
Expand All @@ -449,7 +449,7 @@ void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLo
// offset for same node in cell
twoDoffset = sqrt(((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) * ((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) +
((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())) * ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())));
twoDCost = nodes2D[(int)start.getY() * width + (int)start.getX()].getG() - twoDoffset;
twoDCost = nodes2D[start.getGridY() * width + start.getGridX()].getG() - twoDoffset;

}

Expand Down
6 changes: 5 additions & 1 deletion src/node2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,10 @@

using namespace HybridAStar;

// initialize static data
geometry_msgs::Pose Node2D::mp;
float Node2D::res = 1.0;

// possible directions
const int Node2D::dir = 8;
// possible movements
Expand All @@ -12,7 +16,7 @@ const int Node2D::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };
// IS ON GRID
//###################################################
bool Node2D::isOnGrid(const int width, const int height) const {
return x >= 0 && x < width && y >= 0 && y < height;
return (y > mp.position.y && y <= (res * height + mp.position.y)) && (x > mp.position.x && x <= (res * width + mp.position.x));
}

//###################################################
Expand Down
7 changes: 6 additions & 1 deletion src/node3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,10 @@

using namespace HybridAStar;

//Initialize static map data
geometry_msgs::Pose Node3D::mp;
float Node3D::res = 1.0;

// CONSTANT VALUES
// possible directions
const int Node3D::dir = 3;
Expand All @@ -28,7 +32,7 @@ const float Node3D::dt[] = { 0, 0.1178097, -0.1178097};
// IS ON GRID
//###################################################
bool Node3D::isOnGrid(const int width, const int height) const {
return x >= 0 && x < width && y >= 0 && y < height && (int)(t / Constants::deltaHeadingRad) >= 0 && (int)(t / Constants::deltaHeadingRad) < Constants::headings;
return (y > mp.position.y && y <= (res * height + mp.position.y)) && (x > mp.position.x && x <= (res * width + mp.position.x)) && (int)(t / Constants::deltaHeadingRad) >= 0 && (int)(t / Constants::deltaHeadingRad) < Constants::headings;
}


Expand Down Expand Up @@ -300,3 +304,4 @@ bool Node3D::operator == (const Node3D& rhs) const {
(std::abs(t - rhs.t) <= Constants::deltaHeadingRad ||
std::abs(t - rhs.t) >= Constants::deltaHeadingNegRad);
}

15 changes: 13 additions & 2 deletions src/planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,15 @@ void Planner::setMap(const nav_msgs::OccupancyGrid::Ptr map) {
configurationSpace.updateGrid(map);
//create array for Voronoi diagram
// ros::Time t0 = ros::Time::now();

geometry_msgs::Pose map_pose;
//convert map origin to world
map_pose.position.x = map->info.origin.position.x + (map->info.origin.position.x + 0.5) * map->info.resolution;
map_pose.position.y = map->info.origin.position.y + (map->info.origin.position.y + 0.5) * map->info.resolution;
//set Meta data for Node 2D and 3D
Node3D::setMeta(map_pose, map->info.resolution);
Node2D::setMeta(map_pose, map->info.resolution);

int height = map->info.height;
int width = map->info.width;
bool** binMap;
Expand Down Expand Up @@ -110,7 +119,8 @@ void Planner::setStart(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&

std::cout << "I am seeing a new start x:" << x << " y:" << y << " t:" << Helper::toDeg(t) << std::endl;

if (grid->info.height >= y && y >= 0 && grid->info.width >= x && x >= 0) {
if ((y > grid->info.origin.position.y && y <= ( grid->info.resolution * grid->info.height + grid->info.origin.position.y) ) &&
(x > grid->info.origin.position.x && x <= ( grid->info.resolution * grid->info.width + grid->info.origin.position.x) )){
validStart = true;
start = *initial;

Expand All @@ -134,7 +144,8 @@ void Planner::setGoal(const geometry_msgs::PoseStamped::ConstPtr& end) {

std::cout << "I am seeing a new goal x:" << x << " y:" << y << " t:" << Helper::toDeg(t) << std::endl;

if (grid->info.height >= y && y >= 0 && grid->info.width >= x && x >= 0) {
if ((y > grid->info.origin.position.y && y <= ( grid->info.resolution * grid->info.height + grid->info.origin.position.y) ) &&
(x > grid->info.origin.position.x && x <= ( grid->info.resolution * grid->info.width + grid->info.origin.position.x) )){
validGoal = true;
goal = *end;

Expand Down