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