void Map::generate(const unsigned int city_count,
const unsigned int width,
const unsigned int height,
const unsigned int seed)
{
cities.resize(city_count);
std::default_random_engine generator(seed);
std::uniform_int_distribution<unsigned int> width_distribution(0, width);
std::uniform_int_distribution<unsigned int> height_distribution(0, height);
std::uniform_int_distribution<unsigned int> city_distribution(0, city_count - 1);
for (size_t c = 0; c < city_count; c++)
{
cities[c].x = width_distribution(generator);
cities[c].y = height_distribution(generator);
}
start = city_distribution(generator);
end = city_distribution(generator);
m_adaptor = MapAdaptor(cities);
city_tree_t city_tree(2, m_adaptor,
nanoflann::KDTreeSingleIndexAdaptorParams(10));
city_tree.buildIndex();
for (size_t c = 0; c < cities.size(); c++)
{
// Set index of city and generate connections
cities[c].index = c; // Do we need this?
// Consider 5 closest
std::vector<size_t> ret_index(CITY_SEARCH + 1);
std::vector<unsigned int>dist_sqr(CITY_SEARCH + 1);
city_tree.knnSearch(
&cities[c].x,
CITY_SEARCH + 1,
&ret_index[0],
&dist_sqr[0]);
for (size_t i = 1; i < ret_index.size(); i++)
{
// Do we include you or not?
// Bernoulli, do your job buddy!
std::bernoulli_distribution city_test_distribution(CITY_RATIO);
if (city_test_distribution(generator))
{
// Update our information
cities[c].neighbors.insert(
std::make_pair(ret_index[i],
std::sqrt(dist_sqr[i])));
cities[ret_index[i]].neighbors.insert(
std::make_pair(c,
std::sqrt(dist_sqr[i])));
}
}
}
}
void CFruchtermanReingold::GenerateRandomCoordinates() {
std::default_random_engine generator;
std::uniform_int_distribution<int> height_distribution(vgc_nodeRadius, vgc_areaHeight - vgc_nodeRadius);
std::uniform_int_distribution<int> width_distribution(vgc_nodeRadius, vgc_areaWidth - vgc_nodeRadius);
for(int i =0; i < vgc_nodes_num; ++i) {
vgc_vertices[i].v_coordinates.setX(width_distribution(generator));
vgc_vertices[i].v_coordinates.setY(height_distribution(generator));
}
}
void ObstaclePointCloud::broadcast() {
if (q_obstacles_.size() < 1) {
return;
}
const auto sim_obstacles = q_obstacles_.front();
using Cell = std::pair<float, float>;
std::vector<Cell> all_cells;
for (const auto& line : sim_obstacles->obstacles) {
const auto cells = pedsim::LineObstacleToCells(line.start.x, line.start.y,
line.end.x, line.end.y);
std::copy(cells.begin(), cells.end(), std::back_inserter(all_cells));
}
constexpr int point_density = 100;
const int num_points = all_cells.size() * point_density;
std::default_random_engine generator;
// \todo - Read params from config file.
std::uniform_int_distribution<int> color_distribution(1, 255);
std::uniform_real_distribution<float> height_distribution(0, 1);
std::uniform_real_distribution<float> width_distribution(-0.5, 0.5);
sensor_msgs::PointCloud pcd_global;
pcd_global.header.stamp = ros::Time::now();
pcd_global.header.frame_id = sim_obstacles->header.frame_id;
pcd_global.points.resize(num_points);
pcd_global.channels.resize(1);
pcd_global.channels[0].name = "intensities";
pcd_global.channels[0].values.resize(num_points);
sensor_msgs::PointCloud pcd_local;
pcd_local.header.stamp = ros::Time::now();
pcd_local.header.frame_id = robot_odom_.header.frame_id;
pcd_local.points.resize(num_points);
pcd_local.channels.resize(1);
pcd_local.channels[0].name = "intensities";
pcd_local.channels[0].values.resize(num_points);
// prepare the transform to robot odom frame.
tf::StampedTransform robot_transform;
try {
transform_listener_->lookupTransform(robot_odom_.header.frame_id,
sim_obstacles->header.frame_id,
ros::Time(0), robot_transform);
} catch (tf::TransformException& e) {
ROS_WARN_STREAM_THROTTLE(5.0, "TFP lookup from ["
<< sim_obstacles->header.frame_id
<< "] to [" << robot_odom_.header.frame_id
<< "] failed. Reason: " << e.what());
return;
}
size_t index = 0;
for (const auto& cell : all_cells) {
const int cell_color = color_distribution(generator);
for (size_t j = 0; j < point_density; ++j) {
if (fov_->inside(cell.first, cell.second)) {
const tf::Vector3 point(cell.first + width_distribution(generator),
cell.second + width_distribution(generator),
0.);
const auto transformed_point = transformPoint(robot_transform, point);
pcd_local.points[index].x = transformed_point.getOrigin().x();
pcd_local.points[index].y = transformed_point.getOrigin().y();
pcd_local.points[index].z = height_distribution(generator);
pcd_local.channels[0].values[index] = cell_color;
// Global observations.
pcd_global.points[index].x = cell.first + width_distribution(generator);
pcd_global.points[index].y =
cell.second + width_distribution(generator);
pcd_global.points[index].z = height_distribution(generator);
pcd_global.channels[0].values[index] = cell_color;
}
index++;
}
}
if (pcd_local.channels[0].values.size() > 1) {
pub_signals_local_.publish(pcd_local);
}
if (pcd_global.channels[0].values.size() > 1) {
pub_signals_global_.publish(pcd_global);
}
q_obstacles_.pop();
};
