Map::Map(size_t width, size_t height) :
width_(width),
height_(height),
map_(MapGen::Generate(width, height)),
tcodMap_(width, height),
cell_states_(width, height)
{
for (size_t y = 0; y < height_; y++) {
for (size_t x = 0; x < width_; x++) {
tcodMap_.setProperties(x, y, map_(x,y) == MapGen::CellType::hallway, map_(x,y) == MapGen::CellType::hallway);
cell_states_(x, y) = CELL_UNVISITED;
}
}
}
void synchronize(){
// Write Cost Data from the map
for(unsigned int x = 0; x < size_x_; x++){
for (unsigned int y = 0; y < size_y_; y++){
unsigned int ind = x + (y * size_x_);
if(map_(x, y).occ_state == 1)
costmap_[ind] = costmap_2d::LETHAL_OBSTACLE;
else if(map_(x, y).occ_dist < outer_radius_)
costmap_[ind] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE/2;
else
costmap_[ind] = 0;
}
}
}
void Map::Render()
{
for (size_t y = 0; y < height_; y++) {
for (size_t x = 0; x < width_; x++) {
TCODConsole::root->setCharBackground(x,y,TCODColor::black);
TCODConsole::root->setCharForeground(x,y,TCODColor::white);
char c = ' ';
// Field of view
if (tcodMap_.isInFov(x, y)) {
cell_states_(x, y) = CELL_VISIBLE;
} else if (cell_states_(x, y) == CELL_VISIBLE) {
cell_states_(x, y) = CELL_VISITED;
}
if (cell_states_(x, y) == CELL_UNVISITED) {
c = ' ';
} else {
if (cell_states_(x, y) == CELL_VISITED) {
TCODConsole::root->setCharForeground(x,y,TCODColor::grey);
}
switch (map_(x,y)) {
case MapGen::CellType::wall:
c = '#';
break;
case MapGen::CellType::hallway:
c = '.';
break;
}
}
TCODConsole::root->setChar(x,y,c);
}
}
}
std::vector<bool> BasisSet::MapGridBasis(cartGP pt){
// Set map_[ishell] to be avaluated (true) or not (false)
// note: radCutSh_ has to be already populated by calling before radcut
//bool * map_ = new bool[this->nShell()+1];
std::vector<bool> map_(this->nShell()+1);
double x ;
double y ;
double z ;
double r ;
bool nodens = true; //becomes truee if at least one shell hs to evaluated and it is stored in map_[0]
for(auto s1=0l; s1 < this->nShell(); s1++){ //loop over shells
auto center = shells(s1).O;
x = bg::get<0>(pt) - center[0];
y = bg::get<1>(pt) - center[1];
z = bg::get<2>(pt) - center[2];
r = std::pow((x*x + y*y + z*z),(0.5));
map_[s1+1] = false;
if (r < this->radCutSh_[s1]) {
map_[s1+1] = true;
nodens = false;
}
} //End loop over shells
map_[0] = nodens;
return map_;
}
void TrajectoryPlannerTest::footprintObstacles(){
//place an obstacle
map_(4, 6).occ_state = 1;
wa->synchronize();
EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
Trajectory traj(0, 0, 0, 30);
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
//place a wall next to the footprint of the robot
map_(7, 1).occ_state = 1;
map_(7, 3).occ_state = 1;
map_(7, 4).occ_state = 1;
map_(7, 5).occ_state = 1;
map_(7, 6).occ_state = 1;
map_(7, 7).occ_state = 1;
wa->synchronize();
//try to rotate into it
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
}
void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex)
{
nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
//only update map if it changed
// if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
// {
int sizeX = gridMap.getSizeX();
int sizeY = gridMap.getSizeY();
int size = sizeX * sizeY;
std::vector<int8_t>& data = map_.map.data;
//Std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
memset(&data[0], -1, sizeof(int8_t) * size);
if (mapMutex)
{
mapMutex->lockMap();
}
for(int i=0; i < size; ++i)
{
if(p_show_gradient_){
data[i] = convertToOccupancyGrid(gridMap.getValue(i));
}
else{
if(gridMap.isFree(i))
{
data[i] = 0;
}
else if (gridMap.isOccupied(i))
{
data[i] = 100;
}
}
}
lastGetMapUpdateIndex = gridMap.getUpdateIndex();
if (mapMutex)
{
mapMutex->unlockMap();
}
map_.map.header.stamp = timestamp;
mapPublisher.mapPublisher_.publish(map_.map);
}
void memory_mapped_file::open(void* file, uint64_t offset, size_t request_size)
{
DWORD size_high;
DWORD size_low = ::GetFileSize(file, &size_high);
DWORD error_code = ::GetLastError();
if (INVALID_FILE_SIZE == size_low && ERROR_SUCCESS != error_code)
{
throw windows_exception("failed to determine file size", error_code);
}
uint64_t file_size = (static_cast<uint64_t>(size_high) << 32) | size_low;
uint64_t map_size = offset + request_size;;
if (map_size < offset)
{
throw windows_exception("requested region exceeds the available address space", ERROR_INVALID_PARAMETER);
}
if (offset > file_size)
{
if (0 == request_size)
{
throw windows_exception("region out of range", ERROR_INVALID_PARAMETER);
}
}
else if (0 == request_size)
{
#ifdef _WIN64
request_size = file_size - offset;
#else
uint64_t request_size2 = file_size - offset;
if (request_size2 > static_cast<uint64_t>(0xFFFFFFFF))
{
throw windows_exception("region size too large", ERROR_NOT_ENOUGH_MEMORY);
}
request_size = static_cast<size_t>(request_size2);
#endif
map_size = offset + request_size;
}
if (0 == request_size)
{
memory_ = nullptr;
cb_ = 0;
return;
}
file_mapping_handle map_(file, NULL, PAGE_READONLY, map_size, NULL);
memory_ = map_.create_view(FILE_MAP_READ, offset, request_size);
cb_ = request_size;
}
bool HectorMappingRos::saveMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, MapLockerInterface* mapMutex){
nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
int sizeX = gridMap.getSizeX();
int sizeY = gridMap.getSizeY();
int totalsize = sizeX * sizeY;
std::vector<int8_t>& data = map_.map.data;
if (mapMutex)
{
mapMutex->lockMap();
}
//First open the file you wish to save
mapSaver.open(p_map_data_output_path_.c_str());
ROS_INFO("Map Data file opened.");
//Float buffer
char tempstr[300];
//first write the grid size and resolution
int mapDetails = sprintf(tempstr,"%f %f %f %f %f ", p_map_resolution_, 0.0, 0.0, float (p_map_size_x_), float (p_map_size_y_));
//int mapDetails = sprintf(tempstr,"%f %i %i %i %i ", p_map_resolution_, 0 , 0, p_map_size_x_, p_map_size_y_);
mapSaver.write(tempstr, mapDetails);
for(int i=0; i < totalsize; i++){
float temp = gridMap.getValue(i);
//write temp into str format names tempstr
int sizeofStr = sprintf(tempstr,"%f ",temp);
mapSaver.write(tempstr, sizeofStr);
}
;
//Now close the file you have opened
mapSaver.close();
ROS_INFO("Map data file closed. A map of size %i was saved.",totalsize);
if (mapMutex)
{
mapMutex->unlockMap();
}
return true;
}
bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
base_local_planner::MapCell cell = map_(cx, cy);
if (cell.within_robot) {
return false;
}
occ_cost = costmap_.getCost(cx, cy);
if (cell.path_dist >= map_.map_.size() || cell.goal_dist >= map_.map_.size() || occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
return false;
}
path_cost = cell.path_dist;
goal_cost = cell.goal_dist;
double resolution = costmap_.getResolution();
total_cost = pdist_scale_ * resolution * path_cost + gdist_scale_ * resolution * goal_cost + occdist_scale_ * occ_cost;
return true;
}
bool getAngleDiff(double px, double py, double pth, double* diff)
{
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return false;
}
unsigned int path_index = map_(cell_x, cell_y).index;
if(path_index>=yaws_.size())
return false;
*diff = angles::shortest_angular_distance(pth, yaws_[path_index]);
return true;
}
void memory_mapped_file::open(const wchar_t* file_name, uint64_t offset, size_t request_size)
{
file_handle file_(file_name, GENERIC_READ, FILE_SHARE_READ, NULL, OPEN_EXISTING, FILE_FLAG_RANDOM_ACCESS, NULL);
uint64_t file_size = file_.get_size();
uint64_t map_size = offset + request_size;
if (map_size < offset)
{
throw windows_exception("requested region exceeds the available address space", ERROR_INVALID_PARAMETER);
}
if (offset > file_size)
{
if(0 == request_size)
{
throw windows_exception("region out of range", ERROR_INVALID_PARAMETER);
}
}
else if(0 == request_size)
{
#ifdef _WIN64
request_size = file_size - offset;
#else
uint64_t request_size2 = file_size - offset;
if (request_size2 > static_cast<uint64_t>(0xFFFFFFFF))
{
throw windows_exception("region size too large", ERROR_NOT_ENOUGH_MEMORY);
}
request_size = static_cast<size_t>(request_size2);
#endif
map_size = offset + request_size;
}
if (0 == request_size)
{
memory_ = nullptr;
cb_ = 0;
return ;
}
file_mapping_handle map_(file_, NULL, PAGE_READONLY, map_size, NULL);
memory_ = map_.create_view(FILE_MAP_READ, offset, request_size);
cb_ = request_size;
}
void TrajectoryPlannerTest::checkGoalDistance(){
//let's box a cell in and make sure that its distance gets set to max
map_(1, 2).occ_state = 1;
map_(1, 1).occ_state = 1;
map_(1, 0).occ_state = 1;
map_(2, 0).occ_state = 1;
map_(3, 0).occ_state = 1;
map_(3, 1).occ_state = 1;
map_(3, 2).occ_state = 1;
map_(2, 2).occ_state = 1;
wa->synchronize();
//set a goal
tc.map_.resetPathDist();
queue<MapCell*> goal_dist_queue;
MapCell& current = tc.map_(4, 9);
current.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
tc.map_.computeGoalDistance(goal_dist_queue, tc.costmap_);
EXPECT_FLOAT_EQ(tc.map_(4, 8).goal_dist, 1.0);
EXPECT_FLOAT_EQ(tc.map_(4, 7).goal_dist, 2.0);
EXPECT_FLOAT_EQ(tc.map_(4, 6).goal_dist, 100.0); //there's an obstacle here placed above
EXPECT_FLOAT_EQ(tc.map_(4, 5).goal_dist, 6.0);
EXPECT_FLOAT_EQ(tc.map_(4, 4).goal_dist, 7.0);
EXPECT_FLOAT_EQ(tc.map_(4, 3).goal_dist, 8.0);
EXPECT_FLOAT_EQ(tc.map_(4, 2).goal_dist, 9.0);
EXPECT_FLOAT_EQ(tc.map_(4, 1).goal_dist, 10.0);
EXPECT_FLOAT_EQ(tc.map_(4, 0).goal_dist, 11.0);
EXPECT_FLOAT_EQ(tc.map_(5, 8).goal_dist, 2.0);
EXPECT_FLOAT_EQ(tc.map_(9, 4).goal_dist, 10.0);
//check the boxed in cell
EXPECT_FLOAT_EQ(tc.map_(2, 2).goal_dist, 100.0);
}
double PathOrientationCostFunction::scoreTrajectory(Trajectory &traj) {
if(traj.getPointsSize()==0)
return 0.0;
double px, py, pth;
traj.getPoint(traj.getPointsSize()-1, px, py, pth);
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return -4.0;
}
unsigned int path_index = map_(cell_x, cell_y).index;
if(path_index>=yaws_.size())
return 0.0;
double diff = fabs(angles::shortest_angular_distance(pth, yaws_[path_index]));
if(diff > max_trans_angle_ && (traj.xv_ > 0.0 || traj.yv_ > 0.0))
return -1.0;
else
return diff;
}
float MapGridCostFunction::getCost(unsigned int px, unsigned int py) {
double grid_dist = map_(px, py).target_dist;
return grid_dist;
}
bool MLBase::map(VectorFloat inputVector){ return map_( inputVector ); }
void DWAPlanner::generateTrajectory(Eigen::Vector3f pos, const Eigen::Vector3f& vel, base_local_planner::Trajectory& traj, bool two_point_scoring, double sq_dist){
//ROS_ERROR("%.2f, %.2f, %.2f - %.2f %.2f", vel[0], vel[1], vel[2], sim_time_, sim_granularity_);
double impossible_cost = map_.map_.size();
double vmag = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
double eps = 1e-4;
//make sure that the robot is at least moving with one of the required velocities
if((vmag + eps < min_vel_trans_ && fabs(vel[2]) + eps < min_rot_vel_) ||
vmag - eps > max_vel_trans_ ||
oscillationCheck(vel)){
traj.cost_ = -1.0;
return;
}
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps = ceil(std::max((vmag * sim_time_) / sim_granularity_, fabs(vel[2]) / sim_granularity_));
//compute a timestep
double dt = sim_time_ / num_steps;
double time = 0.0;
//initialize the costs for the trajectory
double path_dist = 0.0;
double goal_dist = 0.0;
double occ_cost = 0.0;
//we'll also be scoring a point infront of the robot
double front_path_dist = 0.0;
double front_goal_dist = 0.0;
//create a potential trajectory... it might be reused so we'll make sure to reset it
traj.resetPoints();
traj.xv_ = vel[0];
traj.yv_ = vel[1];
traj.thetav_ = vel[2];
traj.cost_ = -1.0;
//if we're not actualy going to simulate... we may as well just return now
if(num_steps == 0){
traj.cost_ = -1.0;
return;
}
//we want to check against the absolute value of the velocities for collisions later
//Eigen::Vector3f abs_vel = vel.array().abs();
//simulate the trajectory and check for collisions, updating costs along the way
for(int i = 0; i < num_steps; ++i){
//get the mapp coordinates of a point
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(pos[0], pos[1], cell_x, cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
double front_x = pos[0] + forward_point_distance_ * cos(pos[2]);
double front_y = pos[1] + forward_point_distance_ * sin(pos[2]);
unsigned int front_cell_x, front_cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(front_x, front_y, front_cell_x, front_cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
//if we're over a certain speed threshold, we'll scale the robot's
//footprint to make it either slow down or stay further from walls
double scale = 1.0;
if(vmag > scaling_speed_){
//scale up to the max scaling factor linearly... this could be changed later
double ratio = (vmag - scaling_speed_) / (max_vel_trans_ - scaling_speed_);
scale = max_scaling_factor_ * ratio + 1.0;
}
//we want to find the cost of the footprint
double footprint_cost = footprintCost(pos, scale);
//if the footprint hits an obstacle... we'll check if we can stop before we hit it... given the time to get there
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
/* TODO: I'm not convinced this code is working properly
//we want to compute the max allowable speeds to be able to stop
//to be safe... we'll make sure we can stop some time before we actually hit
Eigen::Vector3f max_vel = getMaxSpeedToStopInTime(time - stop_time_buffer_);
//check if we can stop in time
if(abs_vel[0] < max_vel[0] && abs_vel[1] < max_vel[1] && abs_vel[2] < max_vel[2]){
//if we can, then we'll just break out of the loop here.. no point in checking future points
break;
}
else{
//if we can't... then this trajectory is invalid
traj.cost_ = -1.0;
return;
}
*/
}
//compute the costs for this point on the trajectory
occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
path_dist = map_(cell_x, cell_y).path_dist;
goal_dist = map_(cell_x, cell_y).goal_dist;
front_path_dist = front_map_(front_cell_x, front_cell_y).path_dist;
front_goal_dist = front_map_(front_cell_x, front_cell_y).goal_dist;
//if a point on this trajectory has no clear path to the goal... it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
traj.cost_ = -2.0; //-2.0 means that we were blocked because propagation failed
return;
}
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, vel, dt);
time += dt;
}
double goal_heading = tf::getYaw(global_plan_.back().pose.orientation);
double heading_diff = fabs(angles::shortest_angular_distance(pos[2],goal_heading));
//ROS_ERROR("%.2f, %.2f", goal_heading, heading_diff);
double resolution = costmap_.getResolution();
//double sq_dist = squareDist(robot_pose, global_plan_.back());
double heading_scale_ = 0;
if (sq_dist < squared_dist_for_rotating_) {
heading_scale_ = heading_bias_;
}
//if we're not at the last point in the plan, then we can just score
if(two_point_scoring)
traj.cost_ = pdist_scale_ * resolution * ((front_path_dist + path_dist) / 2.0) + gdist_scale_ * resolution * ((front_goal_dist + goal_dist) / 2.0) + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
else
traj.cost_ = pdist_scale_ * resolution * path_dist + gdist_scale_ * resolution * goal_dist + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
//ROS_ERROR("%.2f, %.2f, %.2f, %.2f", vel[0], vel[1], vel[2], traj.cost_);
}
bool MLBase::map(VectorDouble inputVector){ return map_( inputVector ); }
void cs_hash_set(cs_hash_tab *tab, const char *key, void *val) {
map_(tab, key, val);
}
// ---------------------------------------------------------------------------------------------
// Map operator()
BOOST_FORCEINLINE result_type operator()(Decorator const&, Pn const&... pn) const
BOOST_NOEXCEPT_IF(is_noexcept)
{
BOOST_SIMD_DIAG("automap for: " << *this << " decorated by " << Decorator{} );
return map_( storage_kind{}, result_storage_kind{}, element_range{}, pn... );
}
bool Map::Walkable(size_t x, size_t y)
{
return map_(x, y) == MapGen::CellType::hallway;
}
