/// <summary>
/// sets the centroid position of the robot's head
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public void SetHeadPosition(
float x,
float y,
float z)
{
robot_geometry.SetHeadPosition(x, y, z);
}
public steersman(
int body_width_mm,
int body_length_mm,
int body_height_mm,
int centre_of_rotation_x,
int centre_of_rotation_y,
int centre_of_rotation_z,
int head_centroid_x,
int head_centroid_y,
int head_centroid_z,
string sensormodels_filename,
int no_of_stereo_cameras,
float baseline_mm,
float dist_from_centre_of_tilt_mm,
int image_width,
int image_height,
float FOV_degrees,
float head_diameter_mm,
float default_head_orientation_degrees,
int no_of_grid_levels,
int dimension_mm,
int dimension_vertical_mm,
int cellSize_mm)
{
rnd = new Random(0);
int grid_type = metagrid.TYPE_SIMPLE;
int localisationRadius_mm = dimension_mm * 50 / 100;
int maxMappingRange_mm = dimension_mm * 50 / 100;
float vacancyWeighting = 0.5f;
buffer = new metagridBuffer(
no_of_grid_levels,
grid_type,
dimension_mm,
dimension_vertical_mm,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
robot_geometry = new robotGeometry();
robot_geometry.SetBodyDimensions(body_width_mm, body_length_mm, body_height_mm);
robot_geometry.SetCentreOfRotation(centre_of_rotation_x, centre_of_rotation_y, centre_of_rotation_z);
robot_geometry.SetHeadPosition(head_centroid_x, head_centroid_y, head_centroid_z);
robot_geometry.CreateStereoCameras(
no_of_stereo_cameras,
baseline_mm,
dist_from_centre_of_tilt_mm,
image_width,
image_height,
FOV_degrees,
head_diameter_mm,
default_head_orientation_degrees);
robot_geometry.CreateSensorModels(buffer);
}
public void SaveAndLoad()
{
string filename = "tests_robotGeometry_SaveAndLoad.xml";
int no_of_stereo_cameras = 2;
robotGeometry geom1 = new robotGeometry();
geom1.CreateStereoCameras(no_of_stereo_cameras, 120, 0, 320, 240, 78, 100, 0);
geom1.SetBodyDimensions(800,700,600);
geom1.SetCentreOfRotation(400, 350, 10);
geom1.SetHeadPosition(400,350, 1000);
geom1.Save(filename);
robotGeometry geom2 = new robotGeometry();
geom2.Load(filename);
Assert.AreEqual(geom1.body_width_mm, geom2.body_width_mm);
Assert.AreEqual(geom1.body_length_mm, geom2.body_length_mm);
Assert.AreEqual(geom1.body_height_mm, geom2.body_height_mm);
Assert.AreEqual(geom1.body_centre_of_rotation_x, geom2.body_centre_of_rotation_x);
Assert.AreEqual(geom1.body_centre_of_rotation_y, geom2.body_centre_of_rotation_y);
Assert.AreEqual(geom1.body_centre_of_rotation_z, geom2.body_centre_of_rotation_z);
Assert.AreEqual(geom1.head_centroid_x, geom2.head_centroid_x);
Assert.AreEqual(geom1.head_centroid_y, geom2.head_centroid_y);
Assert.AreEqual(geom1.head_centroid_z, geom2.head_centroid_z);
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
Assert.AreEqual(geom1.baseline_mm[cam], geom2.baseline_mm[cam]);
Assert.AreEqual(geom1.image_width[cam], geom2.image_width[cam]);
Assert.AreEqual(geom1.image_height[cam], geom2.image_height[cam]);
Assert.AreEqual(geom1.FOV_degrees[cam], geom2.FOV_degrees[cam]);
Assert.AreEqual(geom1.stereo_camera_position_x[cam], geom2.stereo_camera_position_x[cam]);
Assert.AreEqual(geom1.stereo_camera_position_y[cam], geom2.stereo_camera_position_y[cam]);
Assert.AreEqual(geom1.stereo_camera_position_z[cam], geom2.stereo_camera_position_z[cam]);
Assert.AreEqual(geom1.stereo_camera_pan[cam], geom2.stereo_camera_pan[cam]);
Assert.AreEqual(geom1.stereo_camera_tilt[cam], geom2.stereo_camera_tilt[cam]);
Assert.AreEqual(geom1.stereo_camera_roll[cam], geom2.stereo_camera_roll[cam]);
}
}
public steersman(
int body_width_mm,
int body_length_mm,
int body_height_mm,
int centre_of_rotation_x,
int centre_of_rotation_y,
int centre_of_rotation_z,
int head_centroid_x,
int head_centroid_y,
int head_centroid_z,
string sensormodels_filename,
int no_of_stereo_cameras,
float baseline_mm,
float dist_from_centre_of_tilt_mm,
int image_width,
int image_height,
float FOV_degrees,
float head_diameter_mm,
float default_head_orientation_degrees,
int no_of_grid_levels,
int dimension_mm,
int dimension_vertical_mm,
int cellSize_mm)
{
rnd = new Random(0);
int grid_type = metagrid.TYPE_SIMPLE;
int localisationRadius_mm = dimension_mm * 50/100;
int maxMappingRange_mm = dimension_mm * 50/100;
float vacancyWeighting = 0.5f;
buffer = new metagridBuffer(
no_of_grid_levels,
grid_type,
dimension_mm,
dimension_vertical_mm,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
robot_geometry = new robotGeometry();
robot_geometry.SetBodyDimensions(body_width_mm, body_length_mm, body_height_mm);
robot_geometry.SetCentreOfRotation(centre_of_rotation_x, centre_of_rotation_y, centre_of_rotation_z);
robot_geometry.SetHeadPosition(head_centroid_x, head_centroid_y, head_centroid_z);
robot_geometry.CreateStereoCameras(
no_of_stereo_cameras,
baseline_mm,
dist_from_centre_of_tilt_mm,
image_width,
image_height,
FOV_degrees,
head_diameter_mm,
default_head_orientation_degrees);
robot_geometry.CreateSensorModels(buffer);
}