e_int32 dm_free(data_manager_t *dm, int save) {
int i, ret;
e_assert(dm, E_ERROR_INVALID_HANDLER);
DMSG((STDOUT,"dm_free close file save?%d\n",save));
#if START_VIDEO_SERVER
socket_video_server_stop();
#endif
if (display.buf)
gray_to_jpeg_file(dm->gray_file, display.buf, display.w, display.h);
ret = da_close(&dm->adapters_point_cloud, save);
e_check(ret<=0);
ret = da_close(&dm->adapters_gray, save);
e_check(ret<=0);
fi_close(&dm->f_tunable);
if (dm->points_xyz)
free(dm->points_xyz);
if (dm->points_gray)
free(dm->points_gray);
free(dm);
uninit_display();
return E_OK;
}
static e_int32 thread_scan_func(laser_sick_t *ls) {
int ret;
void* result;
ret = inter_ls_scan(ls);
if (ret > 0) {
DMSG((STDOUT,"sick main thread try wait sub thread....\n"));
//等待子线程
pthread_join(ls->thread_write->thread_id, &result);
pthread_join(ls->thread_read->thread_id, &result);
}
DMSG((STDOUT,"sick main thread all sub thread quit,try stop devices....\n"));
//停止转台
ret = lm_turntable_stop();
e_check(ret<=0);
//停止扫描仪
ret = sld_uninitialize(ls->sick);
e_check(ret<=0);
//等待数据刷新完毕
Delay(1000);
DMSG((STDOUT,"sick main thread save files.... state:%d\n",ls->state));
// dm_free(ls->writer, ls->state == STATE_DONE); //写入文件
dm_free(ls->writer, true); //写入文件
ls->state = STATE_IDLE;
DMSG((STDOUT,"sick main thread sounds on_status_change....\n"));
ls->on_status_change(ls->ctx, ls->state);
return ret;
}
e_int32 dm_update(data_manager_t *dm, int c, int file_right) {
int ret;
e_assert(dm&&dm->state, E_ERROR_INVALID_HANDLER);
ret = da_write_column(&dm->adapters_point_cloud, c, dm->points_xyz,
file_right);
e_check(ret<=0);
ret = da_write_column(&dm->adapters_gray, c, dm->points_gray, file_right);
e_check(ret<=0);
return E_OK;
}
data_manager_t*
dm_alloc(char* ptDir, char *grayDir, int width, int height, float h_w, int mode) {
int i, ret;
data_adapter_t* pda;
system_time_t sys_time;
e_assert(ptDir&&grayDir&&width&&height&& (mode==E_DWRITE||mode==E_WRITE),
E_ERROR_INVALID_PARAMETER);
ret = hd_creatdir(ptDir);
// e_assert(ret>0, E_ERROR);
ret = hd_creatdir(grayDir);
// e_assert(ret>0, E_ERROR);
data_manager_t*dm = calloc(1, sizeof(data_manager_t));
e_assert(dm, E_ERROR_BAD_ALLOCATE);
ret = init_display(width, height, h_w, mode);
e_assert(ret>0, ret);
dm->width = width;
dm->height = height;
GetLocalTime(&sys_time);
sprintf(dm->data_file, "%s/%d-%d-%d-%d-%d-%d.pcd", ptDir, sys_time.year,
sys_time.month, sys_time.day, sys_time.hour, sys_time.minute,
sys_time.second);
sprintf(dm->gray_file, "%s/%d-%d-%d-%d-%d-%d.jpg", grayDir, sys_time.year,
sys_time.month, sys_time.day, sys_time.hour, sys_time.minute,
sys_time.second);
sprintf(dm->tunable_file, "%s/%d-%d-%d-%d-%d-%d.tun", ptDir, sys_time.year,
sys_time.month, sys_time.day, sys_time.hour, sys_time.minute,
sys_time.second);
ret = da_open(&dm->adapters_point_cloud, dm->data_file, width, height, h_w,
mode);
e_check(ret<=0);
ret = da_open(&dm->adapters_gray, "test.memgray", width, height, h_w, mode);
e_check(ret<=0);
ret = fi_open(dm->tunable_file, F_WRITE | F_CREATE, &dm->f_tunable);
e_check(!ret);
#if START_VIDEO_SERVER
socket_video_server_start("127.0.0.1", 9090, E_SOCKET_TCP);
#endif
dm->state = 1;
return dm;
}
e_int32
dm_write_column(data_manager_t *dm, e_uint32 column_idx, point_t* point, int file_right)
{
int i, ret;
data_adapter_t* pda;
e_assert(dm&&dm->state, E_ERROR_INVALID_HANDLER);
pda = dm->adapters;
for (i = 0; i < dm->num; i++) {
if (point->type != pda[i].pnt_type)
continue;
ret = da_write_column(&pda[i], column_idx, point, file_right);
e_check(ret<=0);
}
return E_OK;
}
e_int32
dm_append_points(data_manager_t *dm, point_t* point, int pt_num, int file_right)
{
int i, ret;
data_adapter_t* pda;
e_assert(dm&&dm->state, E_ERROR_INVALID_HANDLER);
pda = dm->adapters;
for (i = 0; i < dm->num; i++) {
if (point->type != pda[i].pnt_type)
continue;
ret = da_append_points(&pda[i], point, pt_num, file_right);
e_check(ret<=0);
}
return E_OK;
}
/*E_READ,E_WRITE*/
e_int32
dm_free(data_manager_t *dm)
{
int i, ret;
data_adapter_t* pda;
e_assert(dm, E_ERROR_INVALID_HANDLER);
pda = dm->adapters;
for (i = 0; i < dm->num; i++) {
ret = da_close(&pda[i]);
e_check(ret<=0);
DMSG((STDOUT,"Close %d da\n",i+1));
}
free(dm);
return E_OK;
}
static void read_pool_data_routine(laser_sick_t *ls) {
e_int32 ret;
scan_data_t sdata;
#if DEBUG_TIME
e_uint32 start_time;
#endif
DMSG((STDOUT,"scan job:read_data_routine start.\r\n"));
//创建数据交错缓冲区
if (ls->interlace_v > 1)
ls->scan_data_buf = (scan_data_t*) malloc(
sizeof(scan_data_t) * ls->interlace_v);
//TODO:更详细的异常处理
while (ls->state == STATE_WORK) {
ret = pool_read(&ls->pool, &sdata);
if (ret <= 0) {
DMSG((STDOUT,"pool has been cleared, leave!\r\n"));
break;
}
#if DEBUG_TIME
start_time = GetTickCount();
#endif
// DMSG((STDOUT,"read_data_routine read data at angle: %f.\r\n", sdata.h_angle));
filter_data(ls, &sdata);
#if DEBUG_TIME
DMSG((STDOUT,"[%d] Phrase data use:%u\n",sdata.layer_num,GetTickCount()-start_time));
#endif
}
free(ls->scan_data_buf);
if (e_check(ls->slip_idx != ls->width, "#ERROR# 列数与要求的不一致!\n")) {
DMSG(
(STDOUT, "ls->slip_idx = %u, ls->width = %u \n", (unsigned int)ls->slip_idx,(unsigned int)ls->width));
ls->width = ls->slip_idx; //TODO:修复这里,不允许简单丢掉！!
}
pool_cancle(&ls->pool);
DMSG((STDOUT,"scan job:read_data_routine done.\r\n"));
}
e_int32 Pipe_Select(pipe_t *pip, e_int32 type, e_int32 timeout_usec)
{
int fd, ret;
fd_set inputs, checkfds;
struct timeval timeout;
e_assert(pip&&pip->state, E_ERROR);
fd = pip->priv->pip_handle;
if (fd == -1) {
fd = open(pip->name, O_WRONLY | O_NONBLOCK, 0);
if (fd == -1)
return E_ERROR;
pip->priv->pip_handle = fd;
}
//用select函数之前先把集合清零
FD_ZERO(&inputs);
//把要检测的句柄fd，加入到集合里。
FD_SET(fd, &inputs);
checkfds = inputs;
/*如果参数timeout设为NULL则表示select（）没有timeout
执行成功则返回文件描述词状态已改变的个数，如果返回0代表在描述词状态改变前已超过timeout时间，
当有错误发生时则返回-1，错误原因存于errno，此时参数readfds，writefds，exceptfds和
timeout的值变成不可预测。
EBADF 文件描述词为无效的或该文件已关闭
EINTR 此调用被信号所中断
EINVAL 参数n 为负值。
ENOMEM 核心内存不足
*/
switch (type)
{
case E_READ:
//DMSG(( STDOUT,"Com_Select FD=%d timeout=%dus",fd, timeout_usec));
if (timeout_usec > 0)
{
timeout.tv_sec = (long) (timeout_usec / ((1000 * 1000)));
timeout.tv_usec = (long) (timeout_usec % ((1000 * 1000)));
ret = select(fd + 1, &checkfds, (fd_set *) 0, (fd_set *) 0,
&timeout);
}
else
{
ret = select(fd + 1, &checkfds, (fd_set *) 0, (fd_set *) 0,
NULL);
}
//DMSG((STDOUT,"Com_Select %d socket can read/write",ret));
break;
case E_WRITE:
if (timeout_usec > 0)
{
timeout.tv_sec = (long) (timeout_usec / ((1000 * 1000)));
timeout.tv_usec = (long) (timeout_usec % ((1000 * 1000)));
ret = select(fd + 1, (fd_set *) 0, &checkfds, (fd_set *) 0,
&timeout);
}
else
{
ret = select(fd + 1, (fd_set *) 0, &checkfds, (fd_set *) 0,
NULL);
}
break;
default:
return E_ERROR_INVALID_PARAMETER;
}
//if (e_check(ret==0,"Pipe select time out."))
if (ret == 0)
{
return E_ERROR_TIME_OUT;
}
if (e_check(ret==-1,"Pipe select error."))
{
return E_ERROR_IO;
}
switch (type)
{
case E_READ:
ret = FD_ISSET(fd, &checkfds);
e_assert(ret, E_ERROR_IO);
ioctl(fd, FIONREAD, &ret); //取得数据长度
e_assert((ret > 0), E_ERROR_IO);
//DMSG((STDOUT,"Com_Select %d data can read",ret));
//可读数据长度>0
return ret;
case E_WRITE:
ret = FD_ISSET(fd, &checkfds);
e_assert(ret, E_ERROR_IO);
//DMSG((STDOUT,"Com_Select socket ready to write"));
return E_OK;
}
return E_ERROR;
}
/* timeout <= 0 表示永不超时 */
e_int32 Socket_Select(socket_t *socket, e_int32 type, e_int32 timeout_usec) {
int sockfd, ret, nread;
fd_set inputs, checkfds, *readfds = NULL, *writefds = NULL;
struct timeval timeout;
e_assert((socket && socket->state), E_ERROR);
sockfd = (int) socket->priv;
//用select函数之前先把集合清零
FD_ZERO(&inputs);
//把要检测的句柄sockfd，加入到集合里。
FD_SET(sockfd, &inputs);
switch (type) {
case E_READ:
readfds = &checkfds;
break;
case E_WRITE:
writefds = &checkfds;
break;
default:
return E_ERROR_INVALID_PARAMETER;
}
for (;;) {
checkfds = inputs;
/*如果参数timeout设为NULL则表示select（）没有timeout
执行成功则返回文件描述词状态已改变的个数，如果返回0代表在描述词状态改变前已超过timeout时间，
当有错误发生时则返回-1，错误原因存于errno，此时参数readfds，writefds，exceptfds和
timeout的值变成不可预测。
EBADF 文件描述词为无效的或该文件已关闭
EINTR 此调用被信号所中断
EINVAL 参数n 为负值。
ENOMEM 核心内存不足
*/
if (timeout_usec > 0) {
timeout.tv_sec = (long) (timeout_usec / (1000 * 1000));
timeout.tv_usec = (long) (timeout_usec % (1000 * 1000));
ret = select(sockfd + 1, readfds, writefds, (fd_set *) 0, &timeout);
} else {
ret = select(sockfd + 1, readfds, writefds, (fd_set *) 0, NULL);
}
if (ret == 0) {
return E_ERROR_TIME_OUT;
} else if (e_check(ret==-1,"Socket select error.\r\n")) {
return E_ERROR_IO;
} else if (ret < 0) {
DMSG((STDOUT,"select ret=%d errno=%d\n",ret,errno));
return E_ERROR_IO;
}
switch (type) {
case E_READ:
ret = FD_ISSET(sockfd, &checkfds);
e_assert(ret, E_ERROR_IO);
ret = ioctl(sockfd, FIONREAD, &nread); //取得数据长度
if (nread <= 0)
if (errno == EINTR || errno == EWOULDBLOCK
|| errno == EAGAIN) {
socket->last_error = E_ERROR_RETRY;
return E_ERROR_RETRY;
} else {
socket->last_error = E_ERROR_INVALID_CALL;
return E_ERROR_INVALID_CALL;
}
//DMSG((STDOUT,"Socket_Select %d data can read",ret));
//可读数据长度>0
return nread;
case E_WRITE:
ret = FD_ISSET(sockfd, &checkfds);
e_assert(ret, E_ERROR_IO);
//DMSG((STDOUT,"Socket_Select socket ready to write"));
return E_OK;
}
}
return E_ERROR;
}
//tick_idx 0...3
static e_int32 one_slip(laser_sick_t* ls, e_int32 tick_idx, scan_data_t * pdata,
e_int32 data_offset, e_uint32 data_num, e_int32 type) {
point_gray_t* pgray;
point_polar_t *ppoint;
int k, cheight;
ppoint = ((point_polar_t*) ls->points_polar->mem) + tick_idx;
pgray = ((point_gray_t*) ls->points_gray->mem) + tick_idx;
cheight = ls->height / ls->interlace_v;
if (e_check(data_num != cheight && data_num+1 != cheight, "#ERROR# 点数个数与要求的不一致!\n")) {
DMSG(
(STDOUT,"#ERROR# 点数个数多一个?不可能 data_num[%u] ls->height[%u]\n", (unsigned int)data_num,(unsigned int)ls->height/ls->interlace_v));
return E_ERROR;
}
if (!type) {
for (k = data_num - 1; k >= 0; --k) {
ppoint->distance = pdata->range_measurements[data_offset + k];
ppoint->angle_v = pdata->angle_measurements[data_offset + k];
ppoint->angle_h = pdata->h_angle;
#if HACK_PNT_ANGLE_H
ppoint->angle_h += ppoint->angle_v * ls->angle_dif_per_degree;
#endif
ppoint->intensity = intensity_filter(
pdata->echo_measurements[data_offset + k]);
pgray->gray = gray_filter(
pdata->echo_measurements[data_offset + k]);
if (ppoint->distance <= 0.0000005) {
// DMSG((STDOUT,"I"));
#ifdef HACK_INVALID_DATA
(*ppoint) = *(ppoint - ls->interlace_v);
(*pgray) = *(pgray - ls->interlace_v);
#endif
}
if (ppoint->distance > 22 || ppoint->intensity > 700)
DMSG(
(STDOUT,"[%d]\t%f\t%f\t%f\t%u\n",k, ppoint->distance,ppoint->angle_h,ppoint->angle_v,(unsigned int)ppoint->intensity));
ppoint += ls->interlace_v;
pgray += ls->interlace_v;
} // end for
} else {
#define SHIFT_NUM 0
//补一行
if (ls->active_sectors.left && ls->active_sectors.right
&& cheight == data_num + 1) {
ppoint += SHIFT_NUM + 1;
pgray += SHIFT_NUM + 1;
}
for (k = 0; k < data_num - SHIFT_NUM; ++k) {
ppoint->distance = pdata->range_measurements[data_offset + k];
ppoint->angle_v = pdata->angle_measurements[data_offset + k];
ppoint->angle_h = pdata->h_angle;
#if HACK_PNT_ANGLE_H
ppoint->angle_h += ppoint->angle_v * ls->angle_dif_per_degree;
#endif
ppoint->intensity = intensity_filter(
pdata->echo_measurements[data_offset + k]);
pgray->gray = gray_filter(
pdata->echo_measurements[data_offset + k]);
if (ppoint->distance <= 0.0000005) {
// DMSG((STDOUT,"|"));
#ifdef HACK_INVALID_DATA
(*ppoint) = *(ppoint - ls->interlace_v);
(*pgray) = *(pgray - ls->interlace_v);
#endif
}
if (ppoint->distance > 20 || ppoint->intensity > 700)
DMSG(
(STDOUT,"[%d]\t%f\t%f\t%f\t%u\n",k, ppoint->distance,ppoint->angle_h,ppoint->angle_v,(unsigned int)ppoint->intensity));
ppoint += ls->interlace_v;
pgray += ls->interlace_v;
} // end for
}
return E_OK;
}
