/*
* Fill the complete poster with information computed by the simulator
* We still have some issues with passing args from python to C, so the
* prototyp of the function is not really correct
*/
int post_sick_poster( POSTER_ID id,
POM_SENSOR_POS* pom_sensor_pos,
char* sick_data)
{
posterTake(id, POSTER_WRITE);
fill_sick_data(pom_sensor_pos, sick_data);
posterGive(id);
return 0;
}
/*
* Yaw, pitch, roll are in degree in input
*/
int post_data( POSTER_ID id, double x, double y, double z,
double yaw, double pitch, double roll)
{
// Variables to use for writing the poster
int offset = 0;
// try to get the pom reference frame
// if we can't get it, just returns
if (ref_id == NULL) {
if (poster_not_found_message == 1)
fprintf(stderr, "ref id is NULL : searching for %s\n", ref_name);
if (posterFind(ref_name, &ref_id) == ERROR) {
if (poster_not_found_message == 1){
fprintf(stderr, "can't find %s : looping\n", ref_name);
poster_not_found_message = 0;
}
ref_id = NULL;
return -1;
}
if (poster_not_found_message == 0){
// poster found! re-enable poster_not_found_message in case we loose it
// (is it actually possible?)
fprintf(stderr, "Found POM poster %s. Good.\n", ref_name);
poster_not_found_message = 1;
}
}
// Declare local versions of the structures used
POM_SENSOR_POS framePos;
posterRead(ref_id, 0, &framePos, sizeof(POM_SENSOR_POS));
POM_ME_POS* pos = posterAddr(id);
posterTake(id, POSTER_WRITE);
// Fill in the POM_POS_EULER
// yaw, pitch, roll are expected in radian
pos->main.euler.yaw = yaw;
pos->main.euler.pitch = pitch;
pos->main.euler.roll = roll;
pos->main.euler.x = x;
pos->main.euler.y = y;
pos->main.euler.z = z;
pos->date1 = framePos.date;
posterGive(id);
return 0;
}
int
main(int argc, char **argv)
{
char *nom;
int i;
POSTER_ID id;
if (argc == 1) {
nom = "test";
} else {
nom = argv[1];
}
if (posterCreate(nom, sizeof(int), &id) != OK) {
printf("Error posterCreate %x\n", errnoGet());
h2printErrno(errnoGet());
exit(-1);
}
while (1) {
scanf("%d", &i);
/*
posterWrite(id, 0, &i, sizeof(int));
*/
if (posterTake(id, POSTER_WRITE) == ERROR) {
fprintf(stderr, "Error posterTake: ");
h2printErrno(errnoGet());
}
*(int *)posterAddr(id) = i;
if (posterGive(id) == ERROR) {
fprintf(stderr, "Error posterGive: ");
h2printErrno(errnoGet());
}
if (i == -1) {
printf("Bye\n");
posterDelete(id);
exit(0);
}
}
return 0;
}
static int
Stereopixel_init(Stereopixel *self, PyObject *args, PyObject *kwds)
{
PyObject *images;
char* poster_name;
if (!PyArg_ParseTuple(args, "snn", &poster_name, &(self->width), &(self->height)))
return -1;
size_t poster_size = sizeof(Spix3DImage) +
self->width * self->height * sizeof(Spix3DPixel);
STATUS s = posterCreate (poster_name, poster_size, &self->id);
if (s == ERROR)
{
char buf[1024];
h2getErrMsg(errnoGet(), buf, sizeof(buf));
printf ("Unable to create the %s poster : %s\n",poster_name, buf);
return -1;
}
printf("Succesfully created poster %s of size %zd\n", poster_name, poster_size);
Spix3DImage* img = posterAddr(self->id);
posterTake(self->id, POSTER_WRITE);
img->width = self->width;
img->height = self->height;
self->size = self->width * self->height;
// dummy matrix
memset(img->intrinseque_rectified, 0, sizeof(img->intrinseque_rectified));
posterGive(self->id);
return 0;
}
static PyObject*
Velodyne_post(Velodyne* self, PyObject* args)
{
VelodyneSimu* info;
PyObject* data;
Py_buffer buf;
size_t i;
if (!PyArg_ParseTuple(args, "OO", &info, &data))
return NULL;
velodyne3DImage* im3d = &self->img;
PyObject_GetBuffer(data, &buf, PyBUF_SIMPLE);
float* p = (float*)buf.buf;
if (self->current_rot == 0) {
// clean the whole point array
memset(im3d->points, 0, sizeof(im3d->points));
// store the position
POM_EULER* local_stm_euler = & (im3d->position.sensorToMain.euler);
local_stm_euler->yaw = info->yaw_cam;
local_stm_euler->pitch = info->pitch_cam;
local_stm_euler->roll = info->roll_cam;
local_stm_euler->x = info->x_cam;
local_stm_euler->y = info->y_cam;
local_stm_euler->z = info->z_cam;
// Fill in the Main to Origin
POM_EULER* local_mto_euler = &(im3d->position.mainToOrigin.euler);
local_mto_euler->yaw = info->yaw_rob;
local_mto_euler->pitch = info->pitch_rob;
local_mto_euler->roll = info->roll_rob;
local_mto_euler->x = info->x_rob;
local_mto_euler->y = info->y_rob;
local_mto_euler->z = info->z_rob;
memcpy( &im3d->position.mainToBase.euler,
&im3d->position.mainToOrigin.euler, sizeof(POM_EULER));
im3d->position.date = info->pom_tag;
// fill the poster
velodyne3DPoint* points = im3d->points;
for (i = 0; i < info->nb_pts; i++)
{
points->coordinates[0] = *p;
points->coordinates[1] = *(p+1);
points->coordinates[2] = *(p+2);
if (points->coordinates[0] == 0.0 &&
points->coordinates[1] == 0.0 &&
points->coordinates[2] == 0.0)
points->status = VELODYNE_BAD_3DPOINT;
else
points->status = VELODYNE_GOOD_3DPOINT;
points++;
p += 3;
}
for (;i < self->size; i++, points++)
points->status = VELODYNE_BAD_3DPOINT;
} else {
// fill the poster, but we need to transform the position of each point
// in the frame of the first scan
T3D sensor2ToMain2;
T3D main2ToOrigin;
T3D sensor2ToOrigin;
T3D sensor1ToMain1;
T3D main1ToOrigin;
T3D sensor1ToOrigin;
T3D originToSensor1;
T3D sensor2ToSensor1;
t3dInit(& sensor2ToMain2, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& main2ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor2ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor1ToMain1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& main1ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor1ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& originToSensor1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor2ToSensor1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
sensor2ToMain2.euler.euler[0] = info->yaw_cam;
sensor2ToMain2.euler.euler[1] = info->pitch_cam;
sensor2ToMain2.euler.euler[2] = info->roll_cam;
sensor2ToMain2.euler.euler[3] = info->x_cam;
sensor2ToMain2.euler.euler[4] = info->y_cam;
sensor2ToMain2.euler.euler[5] = info->z_cam;
main2ToOrigin.euler.euler[0] = info->yaw_rob;
main2ToOrigin.euler.euler[1] = info->pitch_rob;
main2ToOrigin.euler.euler[2] = info->roll_rob;
main2ToOrigin.euler.euler[3] = info->x_rob;
main2ToOrigin.euler.euler[4] = info->y_rob;
main2ToOrigin.euler.euler[5] = info->z_rob;
main1ToOrigin.euler.euler[0] = self->img.position.mainToOrigin.euler.yaw;
main1ToOrigin.euler.euler[1] = self->img.position.mainToOrigin.euler.pitch;
main1ToOrigin.euler.euler[2] = self->img.position.mainToOrigin.euler.roll;
main1ToOrigin.euler.euler[3] = self->img.position.mainToOrigin.euler.x;
main1ToOrigin.euler.euler[4] = self->img.position.mainToOrigin.euler.y;
main1ToOrigin.euler.euler[5] = self->img.position.mainToOrigin.euler.z;
sensor1ToMain1.euler.euler[0] = self->img.position.sensorToMain.euler.yaw;
sensor1ToMain1.euler.euler[1] = self->img.position.sensorToMain.euler.pitch;
sensor1ToMain1.euler.euler[2] = self->img.position.sensorToMain.euler.roll;
sensor1ToMain1.euler.euler[3] = self->img.position.sensorToMain.euler.x;
sensor1ToMain1.euler.euler[4] = self->img.position.sensorToMain.euler.y;
sensor1ToMain1.euler.euler[5] = self->img.position.sensorToMain.euler.z;
t3dCompIn (&sensor2ToOrigin, &sensor2ToMain2, &main2ToOrigin);
t3dCompIn (&sensor1ToOrigin, &sensor1ToMain1, &main1ToOrigin);
t3dInvertIn (&originToSensor1, &sensor1ToOrigin);
t3dCompIn (&sensor2ToSensor1, &sensor2ToOrigin, &originToSensor1);
if (!t3dOk (&sensor2ToSensor1)) {
fprintf (stderr, "Failed to compute transformation\n");
Py_RETURN_NONE;
}
velodyne3DPoint* points = im3d->points + self->current_rot * self->size;
for (i = 0; i < info->nb_pts; i++)
{
double *c;
double x = *p;
double y = *(p+1);
double z = *(p+2);
if (x == 0.0 && y == 0.0 && z == 0.0) {
points->status = VELODYNE_BAD_3DPOINT;
} else {
points->status = VELODYNE_GOOD_3DPOINT;
c = sensor2ToSensor1.matrix.matrix;
// matrix mulplication inlined
points->coordinates[0]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
c += 4;
points->coordinates[1]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
c += 4;
points->coordinates[2]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
}
points++;
p += 3;
}
for (;i < self->size; i++, points++)
points->status = VELODYNE_BAD_3DPOINT;
}
PyBuffer_Release(&buf);
self->current_rot = (self->current_rot + 1) % self->nb_rot;
// we have do one turn, just fill the poster
if (self->current_rot == 0) {
velodyne3DImage* p_im3d = posterAddr(self->id);
posterTake(self->id, POSTER_WRITE);
memcpy(p_im3d, im3d, sizeof(*im3d));
posterGive(self->id);
}
Py_RETURN_NONE;
}
static PyObject*
Stereopixel_post(Stereopixel* self, PyObject* args)
{
StereopixelSimu* info;
PyObject* data;
Py_buffer buf;
size_t i;
if (!PyArg_ParseTuple(args, "OO", &info, &data))
return NULL;
assert(info->nb_pts < self->size);
Spix3DImage* im3d = posterAddr(self->id);
Spix3DPixel* pixels = im3d->pixel;
posterTake(self->id, POSTER_WRITE);
POM_EULER* local_stm_euler = & (im3d->imageLeftPos.sensorToMain.euler);
local_stm_euler->yaw = info->yaw_cam;
local_stm_euler->pitch = info->pitch_cam;
local_stm_euler->roll = info->roll_cam;
local_stm_euler->x = info->x_cam;
local_stm_euler->y = info->y_cam;
local_stm_euler->z = info->z_cam;
// Fill in the Main to Origin
POM_EULER* local_mto_euler = &(im3d->imageLeftPos.mainToOrigin.euler);
local_mto_euler->yaw = info->yaw_rob;
local_mto_euler->pitch = info->pitch_rob;
local_mto_euler->roll = info->roll_rob;
local_mto_euler->x = info->x_rob;
local_mto_euler->y = info->y_rob;
local_mto_euler->z = info->z_rob;
memcpy( &im3d->imageLeftPos.mainToBase.euler,
&im3d->imageLeftPos.mainToOrigin.euler, sizeof(POM_EULER));
im3d->imageLeftPos.date = info->pom_tag;
im3d->imageTicks = info->pom_tag;
PyObject_GetBuffer(data, &buf, PyBUF_SIMPLE);
memcpy(&im3d->imageRightPos, &im3d->imageLeftPos, sizeof(POM_SENSOR_POS));
float* p = (float*)buf.buf;
for (i = 0; i < info->nb_pts; i++)
{
pixels->x = *p;
pixels->y = *(p+1);
pixels->z = *(p+2);
if (pixels->x == 0.0 &&
pixels->y == 0.0 &&
pixels->z == 0.0)
pixels->good = 0;
else
pixels->good = 1;
pixels++;
p += 3;
}
for (;i < self->size; i++, pixels++)
pixels->good = 0;
PyBuffer_Release(&buf);
posterGive(self->id);
Py_RETURN_NONE;
}
