/*================================================================== * 函数名 : 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); }