int main(int argc , char ** argv) {
signal(SIGSEGV , util_abort_signal); /* Segmentation violation, i.e. overwriting memory ... */
test_grid();
exit(0);
}
void TestMappingUpdate(std::ostream& output)
{
sensor_msgs::LaserScan test_scan;
test_scan.angle_min = angles::from_degrees(-90.0);
test_scan.angle_max = angles::from_degrees(+90.0);
test_scan.angle_increment = angles::from_degrees(45.0);
test_scan.range_min = 1.7;
test_scan.range_max = 5.5;
for(double angle = test_scan.angle_min; angle <= test_scan.angle_max; angle += test_scan.angle_increment)
{
test_scan.ranges.push_back(4.5);
}
tf::Vector3 loc(-5.0, 0.0, 0.0);
tf::Quaternion rot;
rot.setRPY(0.0, 0.0, angles::from_degrees(-90.0));
tf::Transform robot(rot, loc);
OccupancyGrid test_grid(12, 12, 1.0, 0.0, 0.0);
_mappingUpdate(test_grid, test_scan, robot);
tf::Vector3 beam_start(0.0, 0.0, 0.0);
tf::Vector3 beam_end(0.0, 6.0, 0.0);
_mapUpdateBeamHit(test_grid, beam_start, beam_end);
output << test_grid;
}
void TestOccupancyGrid(std::ostream& output)
{
tf::Vector3 origin(1.0, 1.0, 0.0);
OccupancyGrid test_grid(5, 5, 2.0, 1.0, 1.0);
test_grid.valueAt(0,0) = 0.5;
test_grid.valueAt(2,3) = 0.7;
output << test_grid;
}
int main(int argc , char ** argv) {
int nx = 10;
int ny = 7;
int nz = 8;
test_grid( nx,ny,nz );
exit(0);
}
void test_voxel_grid_serialization()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
test_grid.SetValue(x_index, y_index, z_index, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
std::vector<uint8_t> buffer;
VoxelGrid::VoxelGrid<int>::Serialize(test_grid, buffer, arc_utilities::SerializeFixedSizePOD<int>);
const VoxelGrid::VoxelGrid<int> read_grid = VoxelGrid::VoxelGrid<int>::Deserialize(buffer, 0, arc_utilities::DeserializeFixedSizePOD<int>).first;
// Check the values
int check_index = 0;
bool pass = true;
for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < read_grid.GetNumZCells(); z_index++)
{
int ref_val = read_grid.GetImmutable(x_index, y_index, z_index).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Check pass" << std::endl;
}
else
{
std::cout << "Check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "VG-I de/serialize - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-I de/serialize - Checks failed ***" << std::endl;
}
}
void test_dsh_voxel_grid_locations()
{
VoxelGrid::DynamicSpatialHashedVoxelGrid<int> test_grid(1.0, 4, 4, 4, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
test_grid.SetCellValue(x_pos, y_pos, z_pos, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Value check pass" << std::endl;
}
else
{
std::cout << "Value check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "DSHVG - All checks pass" << std::endl;
}
else
{
std::cout << "*** DSHVG - Checks failed ***" << std::endl;
}
}
void test_voxel_grid_indices()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
test_grid.SetValue(x_index, y_index, z_index, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
int ref_val = test_grid.GetImmutable(x_index, y_index, z_index).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Check pass" << std::endl;
}
else
{
std::cout << "Check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "VG-I - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-I - Checks failed ***" << std::endl;
}
}
/*
Tests Definitions
*/
void TestGridRayTrace(std::ostream& output)
{
tf::Vector3 start(1.0, 1.0, 0.0);
tf::Vector3 end(10.0, 10.0, 0.0);
output << start.getX() << "," << start.getY();
output << " -> ";
output << end.getX() << "," << end.getY();
output << "\n";
OccupancyGrid test_grid(0, 0, 2.0, 0.0, 0.0);
int i, j;
GridRayTrace test_trace(start, end, test_grid);
while(test_trace.getNextPoint(i, j))
output << i << ", " << j << "\n";
}
void test_voxel_grid_locations()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
test_grid.SetValue(x_pos, y_pos, z_pos, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Value check pass" << std::endl;
}
else
{
std::cout << "Value check fail" << std::endl;
pass = false;
}
check_index++;
//std::cout << "Query point - " << PrettyPrint::PrettyPrint(query_point) << std::endl;
const VoxelGrid::GRID_INDEX query_index = test_grid.LocationToGridIndex(x_pos, y_pos, z_pos);
//std::cout << "Query index - " << PrettyPrint::PrettyPrint(query_index) << std::endl;
const Eigen::Vector4d query_location = test_grid.GridIndexToLocation(query_index);
//std::cout << "Query location - " << PrettyPrint::PrettyPrint(query_location) << std::endl;
const VoxelGrid::GRID_INDEX found_query_index = test_grid.LocationToGridIndex4d(query_location);
//std::cout << "Found query index - " << PrettyPrint::PrettyPrint(found_query_index) << std::endl;
if (x_pos == query_location(0) && y_pos == query_location(1) && z_pos == query_location(2))
{
//std::cout << "Position check pass" << std::endl;
}
else
{
std::cout << "Position check fail" << std::endl;
pass = false;
}
if (query_index.x == found_query_index.x && query_index.y == found_query_index.y && query_index.z == found_query_index.z)
{
//std::cout << "Position index check pass" << std::endl;
}
else
{
std::cout << "Position index check fail" << std::endl;
pass = false;
}
}
}
}
if (pass)
{
std::cout << "VG-L - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-L - Checks failed ***" << std::endl;
}
}
