void work()
{
if (allocator.empty() || get_random_bool(malloc_probability)) {
requested_memory_sum += allocator.malloc_random_memory();
} else {
requested_memory_sum -= allocator.free_random_memory();
}
}
visualization_msgs::MarkerArray test_dsh_collision_map(std::default_random_engine& generator)
{
sdf_tools::COLLISION_CELL default_cell(0.0);
sdf_tools::COLLISION_CELL filled_cell(1.0);
sdf_tools::DynamicSpatialHashedCollisionMapGrid test_col_map("test_voxel_grid", 1.0, 5, 5, 5, default_cell);
// Add a bunch of random data
for (int idx = 0; idx < 500; idx++)
{
// Get a random location in +-10m
Eigen::Vector3d random_location = get_random_location(generator, -20.0, -20.0, -20.0, 20.0, 20.0, 20.0);
// Get a random bool to choose between cell/chunk
bool use_cell = get_random_bool(generator);
// Update the col map
if (use_cell)
{
test_col_map.SetCell(random_location, filled_cell);
}
else
{
test_col_map.SetChunk(random_location, filled_cell);
}
}
// Get the Rviz markers
std_msgs::ColorRGBA filled_color;
filled_color.a = 1.0;
filled_color.b = 0.0;
filled_color.g = 0.0;
filled_color.r = 1.0;
std_msgs::ColorRGBA free_color;
free_color.a = 0.1;
free_color.b = 0.0;
free_color.g = 1.0;
free_color.r = 0.0;
std_msgs::ColorRGBA unknown_color;
unknown_color.a = 0.5;
unknown_color.b = 1.0;
unknown_color.g = 0.0;
unknown_color.r = 0.0;
std::vector<visualization_msgs::Marker> display_markers = test_col_map.ExportForDisplay(filled_color, free_color, unknown_color);
visualization_msgs::MarkerArray display_rep;
display_rep.markers = display_markers;
return display_rep;
}
