void OccupancyMap::buildPath(int i, int j, Path *path) { while (!(i == starti_ && j == startj_)) { int index = MAP_INDEX(map_, i, j); float x = MAP_WXGX(map_, i); float y = MAP_WYGY(map_, j); path->push_back(Eigen::Vector2f(x, y)); i = prev_i_[index]; j = prev_j_[index]; } float x = MAP_WXGX(map_, i); float y = MAP_WYGY(map_, j); path->push_back(Eigen::Vector2f(x, y)); }
pf_vector_t AMCLocalizer::uniformPoseGenerator(void* arg) { map_t* map = (map_t*)arg; #if NEW_UNIFORM_SAMPLING unsigned int rand_index = drand48() * free_space_indices.size(); std::pair<int,int> free_point = free_space_indices[rand_index]; pf_vector_t p; p.v[0] = MAP_WXGX(map, free_point.first); p.v[1] = MAP_WYGY(map, free_point.second); p.v[2] = drand48() * 2 * M_PI - M_PI; #else double min_x, max_x, min_y, max_y; min_x = (map->size_x * map->scale)/2.0 - map->origin_x; max_x = (map->size_x * map->scale)/2.0 + map->origin_x; min_y = (map->size_y * map->scale)/2.0 - map->origin_y; max_y = (map->size_y * map->scale)/2.0 + map->origin_y; pf_vector_t p; ROS_DEBUG("Generating new uniform sample"); for(;;) { p.v[0] = min_x + drand48() * (max_x - min_x); p.v[1] = min_y + drand48() * (max_y - min_y); p.v[2] = drand48() * 2 * M_PI - M_PI; // Check that it's a free cell int i,j; i = MAP_GXWX(map, p.v[0]); j = MAP_GYWY(map, p.v[1]); if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1)) break; } #endif return p; }
const Path& OccupancyMap::prepareShortestPaths(double x, double y, double min_distance, double max_distance, double max_occ_dist, bool allow_unknown) { endpoints_.clear(); if (map_ == NULL) { ROS_WARN("OccupancyMap::prepareShortestPaths() Map not set"); return endpoints_; } if (map_->max_occ_dist < max_occ_dist) { ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated " "up to %f, but max_occ_dist=%.2f", map_->max_occ_dist, max_occ_dist); ROS_BREAK(); } initializeSearch(x, y); Node curr_node; while (nextNode(max_occ_dist, &curr_node, allow_unknown)) { double node_dist = curr_node.true_cost * map_->scale; if (min_distance <= node_dist && node_dist < max_distance) { float x = MAP_WXGX(map_, curr_node.coord.first); float y = MAP_WYGY(map_, curr_node.coord.second); endpoints_.push_back(Eigen::Vector2f(x, y)); } else if (node_dist > max_distance) { break; } } return endpoints_; }
inline double OccupancyMap::maxX() { return MAP_WXGX(map_, map_->size_x); }
inline double OccupancyMap::minX() { return MAP_WXGX(map_, 0); }