/*================================================================== * 函数名 ： void CReverse::MaintainPts(POSE_INFO pose_info) * 功能 ： 维护全局倒车点 * 输入参数： POSE_INFO pose_info 车体位姿 * 输出参数： * 返回值 ： * 作者 ： [email protected] * 日期 ： 2014.June.16 * 修改记录： *==================================================================*/ void CReverse::MaintainPts(POSE_INFO pose_info) { COOR2 cur_pt; COOR2 old_pt; cur_pt.x = pose_info.ins_coord.x; cur_pt.y = pose_info.ins_coord.y; if (m_reverse_pts.size() == 0) { m_reverse_last_pt = cur_pt; m_reverse_pts.push_back(cur_pt); return; } else { if (m_breverse_flag == false) {//[正常行驶维护，添加全局点] double dist = dist_point(&cur_pt, &m_reverse_last_pt); m_reverse_forward_sum_dist += (int)dist; //[每隔累积行驶距离大于g_reverse_step时，记录一次] if (m_reverse_forward_sum_dist >= m_reverse_step && m_reverse_forward_sum_dist < 1500) { if ((int)m_reverse_pts.size() < m_reverse_pt_num_limit) { m_reverse_pts.push_back(cur_pt); } else { #ifdef MBUG_OPEN_ MBUG("m_reverse_pts num : %d\n", (int)m_reverse_pts.size()); #endif if (m_reverse_pts.size() != 0) m_reverse_pts.pop_front(); m_reverse_pts.push_back(cur_pt); } m_reverse_forward_sum_dist -= m_reverse_step; } m_reverse_back_sum_dist = 0; } else {//[倒车时维护] m_reverse_forward_sum_dist = 0; coor2_e2v(&pose_info, &m_reverse_pts.back(), &old_pt); while (old_pt.y >= 0) { m_reverse_pts.pop_back(); if (m_reverse_pts.size() == 0) break; coor2_e2v(&pose_info, &m_reverse_pts.back(), &old_pt); } m_reverse_pts.push_back(cur_pt); } } m_reverse_last_pt = cur_pt; }
bool tryRecover()
{
//检查自动恢复路点标记
if(Recover != gpState)
return false;
if(true != triggerRecover)
return false;
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Recover", "N/A");
// gp.intit(static_cast<unsigned int>(gpInput->reco_point.recovery),gps);
if(RNDF_ERROR == gp.stat())
{
MBUG("\t<tryRecover> GP recover faild at %d.\n", gpInput->reco_point.recovery);
gpEvent->event = E_FAILURE;//发送错误事件
return 1;
}
MBUG("\t<tryRecover> GP has recovered from %d.\n", gpInput->reco_point.recovery);
//如果恢复点位不是1号点，则发送直接切换到道路跟踪
if(0 == gpInput->reco_point.recovery)
{
gpEvent->event = E_WAIT_LIGHT;
MBUG("\t<tryRecover> GP Send Event = E_WAIT_LIGHT, No Recovery Information(start from first)!\n");
}
else if(1 == gpInput->reco_point.recovery)
{
gpEvent->event = E_WAIT_LIGHT;
MBUG("\t<tryRecover> GP Send Event = E_WAIT_LIGHT, Specific Recovery Information(first point)!\n");
}
else
{
gpEvent->event = E_RUN_ROAD;
MBUG("\t<tryRecover> GP Send Event = E_RUN_ROAD, Specific Recovery Infomation!\n");
}
//执行完毕后切回Run，并返回
triggerRecover = false;
gpState = Run;
return true;
}
/*==================================================================
* 函数名 ： int CReverse::ReversePlan_DirectBack(POSE_INFO pose_info,
STATE state,
int grid_map[GRID_HEIGHT][GRID_WIDTH],
COOR2 grid_center,
int g_frame_id,
PL_CS_DATA *pl_cs_data)
* 功能 ： 直线倒车规划
* 输入参数： POSE_INFO pose_info 车体位姿数据
STATE state 规划状态，用于填写输出数据用
int grid_map[GRID_HEIGHT][GRID_WIDTH] 倒车规划用到的栅格数据
COOR2 grid_center 栅格中心
int g_frame_id 规划帧号
PL_CS_DATA *pl_cs_data 规划输出数据
* 输出参数：
* 返回值 ： int 0 有可执行路径 -1 无可执行路径
* 作者 ：
* 日期 ：
* 修改记录：
*==================================================================*/
int CReverse::ReversePlan_DirectBack(POSE_INFO pose_info,
STATE state,
int grid_map[GRID_HEIGHT][GRID_WIDTH],
COOR2 grid_center,
int g_frame_id,
PL_CS_DATA *pl_cs_data)
{
int ret = 0;
int i;
POSE_INFO gps;
COOR2 pt;
deque<COOR2> reverse_path;
#ifdef MBUG_OPEN_
MBUG("------------------ In reverse plan -------------------\n");
#endif
//[更新数据]
memcpy(m_grid_map, grid_map, sizeof(int) * GRID_WIDTH * GRID_HEIGHT);
m_grid_center = grid_center;
memset(pl_cs_data, 0, sizeof(PL_CS_DATA));
gps = pose_info;
//[1.第一帧初始化]
if (m_breverse_init == false)
{
m_reverse_start_pt = gps.ins_coord;
m_reverse_back_last_pt = gps.ins_coord;
m_reverse_back_sum_dist = 0;
m_reverse_forward_sum_dist = 0;
m_breverse_init = true;
}
//[2.统计所有路径点的总长度]
// sum_dits = 0;
// for (i = 1; i < (int)m_reverse_pts.size(); i++)
// sum_dits += dist_point(&m_reverse_pts[i - 1], &m_reverse_pts[i]);
//
// if (sum_dits < m_reverse_plan_dist)
// {//[剩余路径长度不足够规划]
// pl_cs_data->speed = 0;
// pl_cs_data->sys_command = 0;
// pl_cs_data->id = g_frame_id;
// pl_cs_data->isok = 1;
// pl_cs_data->state = state;
// memset(pl_cs_data->path, 0, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
// pl_cs_data->number_of_effective_points = 0;
//
// m_reverse_back_sum_dist = 0;
// m_reverse_forward_sum_dist = 0;
// m_breverse_flag = false;
// m_breverse_init = false;;
// ret = -1;
// return ret;
// }
//[3.生成倒车用的局部路径点]
// COOR2 tmp1, tmp2;
// deque<COOR2> tmp_path;
// int num = 0;
// tmp1.x = 0;
// tmp1.y = 0;
// tmp_path.resize(m_reverse_pts.size());
// for (i = 0; i < (int)m_reverse_pts.size(); i++)
// {
// pt = m_reverse_pts[i];
// coor2_e2v(&gps, &pt, &tmp2);
// if (dist_point(&tmp1, &tmp2) >= m_reverse_plan_pts_step)
// {
// tmp_path[num++] = tmp2;
// tmp1 = tmp2;
// }
// }
// reverse_path.resize(num);
// for (i = 0; i < num; i++)
// {
// reverse_path[i] = tmp_path[i];
// }
// reverse(reverse_path.begin(), reverse_path.end());
//[4.倒车规划线]
memset(m_Path, 0, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
//memset(pts, 0, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
//pts_num = 0;
for (i = 0; i < NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE; i++)
{
m_Path[i].x = 0;
m_Path[i].y = i * (-50);
//pts[i] = reverse_path[i];
}
//pts_num = i;
//get_bezier_line(pts, pts_num, m_Path, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE);
m_PathPtsNum = NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE;
//[5.碰撞检测]
ret = collision_check_and_replan(grid_center);
if (ret == 0)
{
pl_cs_data->speed = m_reverse_speed;
pl_cs_data->sys_command |= SYS_COMMAND_REV;
pl_cs_data->id = g_frame_id;
pl_cs_data->isok = 1;
pl_cs_data->state = state;
memcpy(pl_cs_data->path, m_Path, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
pl_cs_data->number_of_effective_points = m_PathPtsNum;
}
else
{
pl_cs_data->speed = (int)(0 * CM);
pl_cs_data->sys_command = 0;
pl_cs_data->id = g_frame_id;
pl_cs_data->isok = 1;
pl_cs_data->state = state;
memset(pl_cs_data->path, 0, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
pl_cs_data->number_of_effective_points = 0;
m_reverse_back_sum_dist = 0;
m_reverse_forward_sum_dist = 0;
m_breverse_flag = false;
m_breverse_init = false;;
ret = -1;
return ret;
}
//[倒车距离到达阈值，结束倒车规划]
pt = gps.ins_coord;
m_reverse_back_sum_dist += (int)dist_point(&m_reverse_back_last_pt, &pt);
if (m_reverse_back_sum_dist > m_reverse_back_stop_dist)
{
pl_cs_data->speed = (int)(0 * CM);
pl_cs_data->sys_command = 0;
pl_cs_data->id = g_frame_id;
pl_cs_data->isok = 1;
pl_cs_data->state = state;
memset(pl_cs_data->path, 0, NUM_OF_MAX_2D_POINTS_IN_EDGE_LINE * sizeof(COOR2));
pl_cs_data->number_of_effective_points = 0;
m_reverse_back_sum_dist = 0;
m_reverse_forward_sum_dist = 0;
m_breverse_flag = false;
m_breverse_init = false;;
ret = -1;
return ret;
}
m_reverse_back_last_pt = gps.ins_coord;
#ifdef MBUG_OPEN_
MBUG("\n");
MBUG("---------------------reverse plan END-----------------------\n");
#endif
return 0;
}
int Global_Plan(GP_FUNC_INPUT * pInput, GP_INFO * pData, GP_LOCAL_DATA * pLocal)
{
/* 0 全局规划预处理 */
/* 0.1 调用计数器自增 */
if(myFrameid==1)
MBUG("WARNING:this shell is for simulate only, no use any more\n");
++myFrameid;
pData->header.id = myFrameid;
MBUG("\n*************************FRAME_ID %d***********************************\n", myFrameid);
/* 0.2 检查规划器状态 */
if(RNDF_ERROR == gp.stat())
{
MBUG("GP@%d : GP is INVALID(RNDF STATUS ERROR, CHECK FILE task OR RECOVERY POINT)!\n", myFrameid);
MBUG("**********************************************************************\n");
return 1;
}
/* 0.3 预置全局规划信息，更新动态数据 */
gpInput = pInput;
gpOutput = pData;
gpEvent = pLocal;
memcpy(&pData->state, &pInput->state, sizeof(STATE));
gps[0] = pInput->state.pos.com_coord.x;
gps[1] = pInput->state.pos.com_coord.y;
gps[2] = pInput->state.pos.yaw*(180.0*1e-8)/3.1415926535898;
#ifdef USING_ROBIX4
rbxReadStatus(&robixStatus);
#else
if(myFrameid==1)
robixStatus=S_INVALID;
else
robixStatus=S_ROAD_NAV;
#endif
/* 1 执行全局规划 */
switch(robixStatus)
{
case S_INVALID:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_INVALID");
tryRecover();
MBUG("**********************************************************************\n");
break;
case S_WAIT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_WAIT");
tryRecover();
MBUG("**********************************************************************\n");
break;
case S_WAIT_LIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_WAIT_LIGHT");
gp.context(gps,gpOutput);
MBUG("**********************************************************************\n");
break;
case S_ROAD_NAV:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_ROAD_NAV");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_CROSS_UND:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_CROSS_UND");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_STRAIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_STRAIGHT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_LEFT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_LEFT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_RIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_RIGHT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_UTURN:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_UTURN");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_PARKING:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_PARKING");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_FREE_RUN:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_FREE_RUN");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_TASK_OVER:
MBUG("GP@%d: gpState = {%s} (robixStatus = %s)\n", myFrameid, "Off", "S_TASK_OVER");
gpState = Off;
gp.context(gps,gpOutput);
MBUG("**********************************************************************\n");
break;
default:
break;
}
return 0;
}
/* appends if idx == -1, otherwise overwrites record idx. */
static int mlog_vfs_write_rec(struct mlog_handle *loghandle,
struct mlog_rec_hdr *rec,
struct mlog_cookie *reccookie,
int cookiecount,
void *buf, int idx)
{
struct mlog_log_hdr *mlh;
int reclen = rec->mrh_len;
int index = 0;
int ret = 0;
struct mlog_rec_tail *mrt;
struct file *file;
size_t left;
struct mtfs_lowerfs *lowerfs = NULL;
MENTRY();
mlh = loghandle->mgh_hdr;
file = loghandle->mgh_file;
lowerfs = loghandle->mgh_ctxt->moc_lowerfs;
/* record length should not bigger than MLOG_CHUNK_SIZE */
if (buf){
ret = (reclen > MLOG_CHUNK_SIZE - sizeof(struct mlog_rec_hdr) -
sizeof(struct mlog_rec_tail)) ? -E2BIG : 0;
} else {
ret = (reclen > MLOG_CHUNK_SIZE) ? -E2BIG : 0;
}
if (ret) {
goto out;
}
if (buf) {
/* write_blob adds header and tail to mrh_len. */
reclen = sizeof(*rec) + rec->mrh_len +
sizeof(struct mlog_rec_tail);
}
if (idx != -1) {
loff_t saved_offset;
/* no header: only allowed to insert record 1 */
if (idx != 1 && !i_size_read(file->f_dentry->d_inode)) {
MERROR("idx != -1 in empty log\n");
MBUG();
}
if (idx && mlh->mlh_size && mlh->mlh_size != rec->mrh_len) {
ret = -EINVAL;
goto out;
}
if (!ext2_test_bit(idx, mlh->mlh_bitmap)) {
MERROR("Modify unset record %u\n", idx);
}
if (idx != rec->mrh_index) {
MERROR("Index mismatch %d %u\n", idx, rec->mrh_index);
}
ret = mlog_vfs_write_blob(lowerfs, file, &mlh->mlh_hdr, NULL, 0);
/* we are done if we only write the header or on error */
if (ret || idx == 0) {
goto out;
}
/* Assumes constant mrh_len */
saved_offset = sizeof(*mlh) + (idx - 1) * reclen;
if (buf) {
struct mlog_rec_hdr check;
/* We assume that caller has set mgh_cur_* */
saved_offset = loghandle->mgh_cur_offset;
MDEBUG("modify record %I64x: idx:%d/%u/%d, len:%u "
"offset %llu\n",
loghandle->mgh_id.mgl_oid, idx, rec->mrh_index,
loghandle->mgh_cur_idx, rec->mrh_len,
(long long)(saved_offset - sizeof(*mlh)));
if (rec->mrh_index != loghandle->mgh_cur_idx) {
MERROR("modify idx mismatch %u/%d\n",
idx, loghandle->mgh_cur_idx);
ret = -EFAULT;
goto out;
}
#if 1 /* FIXME remove this safety check at some point */
/* Verify that the record we're modifying is the
right one. */
ret = mlog_vfs_read_blob(lowerfs, file, &check,
sizeof(check), saved_offset);
if (check.mrh_index != idx || check.mrh_len != reclen) {
MERROR("bad modify idx %u/%u size %u/%u (%d)\n",
idx, check.mrh_index, reclen,
check.mrh_len, ret);
ret = -EFAULT;
goto out;
}
#endif
}
ret = mlog_vfs_write_blob(lowerfs, file, rec, buf, saved_offset);
if (ret == 0 && reccookie) {
reccookie->mgc_mgl = loghandle->mgh_id;
reccookie->mgc_index = idx;
ret = 1;
}
goto out;
}
/* Make sure that records don't cross a chunk boundary, so we can
* process them page-at-a-time if needed. If it will cross a chunk
* boundary, write in a fake (but referenced) entry to pad the chunk.
*
* We know that mlog_current_log() will return a loghandle that is
* big enough to hold reclen, so all we care about is padding here.
*/
left = MLOG_CHUNK_SIZE - (file->f_pos & (MLOG_CHUNK_SIZE - 1));
/* NOTE: padding is a record, but no bit is set */
if (left != 0 && left != reclen &&
left < (reclen + MLOG_MIN_REC_SIZE)) {
index = loghandle->mgh_last_idx + 1;
ret = mlog_vfs_pad(lowerfs, file, left, index);
if (ret) {
goto out;
}
loghandle->mgh_last_idx++; /*for pad rec*/
}
/* if it's the last idx in log file, then return -ENOSPC */
if (loghandle->mgh_last_idx >= MLOG_BITMAP_SIZE(mlh) - 1) {
ret = -ENOSPC;
goto out;
}
index = ++loghandle->mgh_last_idx;
rec->mrh_index = index;
if (buf == NULL) {
mrt = (struct mlog_rec_tail *)
((char *)rec + rec->mrh_len - sizeof(*mrt));
mrt->mrt_len = rec->mrh_len;
mrt->mrt_index = rec->mrh_index;
}
/*The caller should make sure only 1 process access the mgh_last_idx,
*Otherwise it might hit the assert.*/
MASSERT(index < MLOG_BITMAP_SIZE(mlh));
if (ext2_set_bit(index, mlh->mlh_bitmap)) {
MERROR("argh, index %u already set in log bitmap?\n", index);
MBUG(); /* should never happen */
}
mlh->mlh_count++;
mlh->mlh_tail.mrt_index = index;
ret = mlog_vfs_write_blob(lowerfs, file, &mlh->mlh_hdr, NULL, 0);
if (ret) {
goto out;
}
ret = mlog_vfs_write_blob(lowerfs, file, rec, buf, file->f_pos);
if (ret) {
goto out;
}
MDEBUG("added record %I64x: idx: %u, %u bytes\n",
loghandle->mgh_id.mgl_oid, index, rec->mrh_len);
if (ret == 0 && reccookie) {
reccookie->mgc_mgl = loghandle->mgh_id;
reccookie->mgc_index = index;
ret = 1;
}
if (ret == 0 && rec->mrh_type == MLOG_GEN_REC) {
ret = 1;
}
out:
MRETURN(ret);
}
int mlog_init_handle(struct mlog_handle *handle, int flags,
struct mlog_uuid *uuid)
{
int ret = 0;
struct mlog_log_hdr *mlh = NULL;
MENTRY();
MASSERT(handle->mgh_hdr == NULL);
MTFS_ALLOC(mlh, sizeof(*mlh));
if (mlh == NULL) {
MERROR("not enough memory\n");
ret = -ENOMEM;
goto out;
}
handle->mgh_hdr = mlh;
/* first assign flags to use mlog_client_ops */
mlh->mlh_flags = flags;
ret = mlog_read_header(handle);
if (ret == 0) {
flags = mlh->mlh_flags;
if (uuid && !mlog_uuid_equals(uuid, &mlh->mlh_tgtuuid)) {
MERROR("uuid mismatch: %s/%s\n", (char *)uuid->uuid,
(char *)mlh->mlh_tgtuuid.uuid);
ret = -EEXIST;
}
goto out;
} else if (ret != MLOG_EEMPTY || !flags) {
/* set a pesudo flag for initialization */
flags = MLOG_F_IS_CAT;
goto out;
}
ret = 0;
handle->mgh_last_idx = 0; /* header is record with index 0 */
mlh->mlh_count = 1; /* for the header record */
mlh->mlh_hdr.mrh_type = MLOG_HDR_MAGIC;
mlh->mlh_hdr.mrh_len = mlh->mlh_tail.mrt_len = MLOG_CHUNK_SIZE;
mlh->mlh_hdr.mrh_index = mlh->mlh_tail.mrt_index = 0;
mlh->mlh_timestamp = get_seconds();
if (uuid)
memcpy(&mlh->mlh_tgtuuid, uuid, sizeof(mlh->mlh_tgtuuid));
mlh->mlh_bitmap_offset = offsetof(typeof(*mlh), mlh_bitmap);
ext2_set_bit(0, mlh->mlh_bitmap);
out:
if (flags & MLOG_F_IS_CAT) {
MTFS_INIT_LIST_HEAD(&handle->u.chd.chd_head);
mlh->mlh_size = sizeof(struct mlog_logid_rec);
} else if (flags & MLOG_F_IS_PLAIN) {
MTFS_INIT_LIST_HEAD(&handle->u.phd.phd_entry);
} else {
MERROR("Unknown flags: %#x (Expected %#x or %#x\n",
flags, MLOG_F_IS_CAT, MLOG_F_IS_PLAIN);
MBUG();
}
if (ret) {
MTFS_FREE(mlh, sizeof(*mlh));
handle->mgh_hdr = NULL;
}
MRETURN(ret);
}
