|
|
@@ -275,7 +275,7 @@ void Navigation::ResetLifter(LifterStatus status) {
|
|
|
|
|
|
|
|
|
void Navigation::ResetPose(const Pose2d &pose) {
|
|
|
- timedPose_.reset(pose, 1.0);
|
|
|
+ timedPose_.reset(pose, 1.2);
|
|
|
if (pose.theta() > 0) {
|
|
|
if (parameter_.main_agv()) {
|
|
|
isFront_.reset(true, 1.0);
|
|
|
@@ -592,8 +592,6 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
|
|
|
return true;
|
|
|
}
|
|
|
bool already_in_mpc = false;
|
|
|
- int retry_count = 0;
|
|
|
- int retry_count_limit = 3;
|
|
|
while (cancel_ == false) {
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
if (PoseTimeout()) {
|
|
|
@@ -638,9 +636,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
|
|
|
printf(" MPC to target:%f %f l:%f,w:%f theta:%f impossible ,retry ...\n",
|
|
|
node.x(), node.y(), node.l(), node.w(), node.theta());
|
|
|
} else {
|
|
|
- retry_count++;
|
|
|
- if (retry_count > retry_count_limit)
|
|
|
- return false;
|
|
|
+
|
|
|
}
|
|
|
}
|
|
|
if (cancel_) {
|
|
|
@@ -666,8 +662,6 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
int clampLifterAction = 0;
|
|
|
if (move_mode_ == eSingle) {
|
|
|
clampLifterAction |= eByteClampHalfOpen;
|
|
|
- } else {
|
|
|
- clampLifterAction |= eByteLifterDown;
|
|
|
}
|
|
|
////////////////////////////////////////////////////////////
|
|
|
for (int i = 0; i < action.pathnodes_size(); ++i) {
|
|
|
@@ -677,13 +671,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
|
|
|
node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
|
|
|
node.set_l(0.03);
|
|
|
- node.set_w(0.20);
|
|
|
+ node.set_w(0.25);
|
|
|
} else {
|
|
|
NavMessage::PathNode befor = action.pathnodes(i - 1);
|
|
|
Pose2d vec(node.x() - befor.x(), node.y() - befor.y(), 0);
|
|
|
node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
|
|
|
node.set_l(0.02);
|
|
|
- node.set_w(0.2);
|
|
|
+ node.set_w(0.25);
|
|
|
}
|
|
|
int directions = Direction::eForward;
|
|
|
if (anyDirect)
|
|
|
@@ -716,6 +710,18 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
}
|
|
|
}*/
|
|
|
|
|
|
+ //提升机构提前动作
|
|
|
+ if (GetRemainActionsCount() > 1){
|
|
|
+ NavMessage::NewAction last_action = GetLastAction();
|
|
|
+ if (IsUperSpace(last_action.spacenode())){
|
|
|
+ if ((i+1) == action.pathnodes_size()){
|
|
|
+ clampLifterAction |= eByteLifterUp;
|
|
|
+ }
|
|
|
+ } else{
|
|
|
+ clampLifterAction |= eByteLifterDown;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false)
|
|
|
return false;
|
|
|
else
|
|
|
@@ -782,6 +788,50 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
|
|
|
+Navigation::MpcResult Navigation::RotateAfterOutExport() {
|
|
|
+ printf("RotateAfterOutExport...\n");
|
|
|
+ if (move_mode_ != eDouble)
|
|
|
+ return eMpcSuccess;
|
|
|
+ Pose2d init = timedPose_.Get();
|
|
|
+ double diff1 = fabs(M_PI / 2.0 - init.theta());
|
|
|
+ double diff2 = fabs(-M_PI / 2.0 - init.theta());
|
|
|
+ double target = diff1 > diff2 ? M_PI / 2.0 : -M_PI / 2.0;
|
|
|
+ stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
|
|
|
+ while (cancel_ == false) {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(80));
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ double yawDiff = (target - current.theta());
|
|
|
+
|
|
|
+ //一次变速
|
|
|
+ std::vector<double> out;
|
|
|
+ bool ret;
|
|
|
+ TimerRecord::Execute([&, this]() {
|
|
|
+ ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
|
|
|
+ }, "Rotation_mpc_once");
|
|
|
+ if (ret == false) {
|
|
|
+ Stop();
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
+
|
|
|
+ //下发速度
|
|
|
+ if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
|
|
|
+ Stop();
|
|
|
+ return eMpcSuccess;
|
|
|
+ } else {
|
|
|
+ const int down_count = 3;
|
|
|
+ double v[down_count] = {0, 0, 0};
|
|
|
+ double w[down_count] = {out[0], out[1], out[2]};
|
|
|
+ SendMoveCmd(0, move_mode_, eRotation, v, w);
|
|
|
+ actionType_ = eRotation;
|
|
|
+// printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
|
|
|
+// timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
|
|
|
+ }
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ return eMpcFailed;
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
|
|
|
if (move_mode_ != eDouble)
|
|
|
@@ -808,7 +858,7 @@ Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
|
|
|
Stop();
|
|
|
return eMpcSuccess;
|
|
|
@@ -902,7 +952,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
|
|
|
Stop();
|
|
|
return eMpcSuccess;
|
|
|
@@ -1003,14 +1053,14 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
printf("inout ----------------------------------------\n");
|
|
|
|
|
|
//取车流程，送车到出口前旋转180
|
|
|
- if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
|
|
|
- Pose2d current_pose = timedPose_.Get();
|
|
|
- double current_theta = current_pose.theta();
|
|
|
- if (RotateBeforeIntoExport() != eMpcSuccess) {
|
|
|
- printf("Rotate before in entrance failed\n");
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
+// if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
|
|
|
+// Pose2d current_pose = timedPose_.Get();
|
|
|
+// double current_theta = current_pose.theta();
|
|
|
+// if (RotateBeforeIntoExport() != eMpcSuccess) {
|
|
|
+// printf("Rotate before in entrance failed\n");
|
|
|
+// return false;
|
|
|
+// }
|
|
|
+// }
|
|
|
|
|
|
//计算车位朝向
|
|
|
action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
|
|
|
@@ -1162,6 +1212,20 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
return false;
|
|
|
}
|
|
|
isInSpace_ = false;
|
|
|
+
|
|
|
+ // 存车流程离开入口，掉头
|
|
|
+ printf("SequenceType: %d\n",GetSequenceType());
|
|
|
+ if (GetSequenceType() == eSequencePark) {
|
|
|
+ if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
|
|
|
+ Pose2d current_pose = timedPose_.Get();
|
|
|
+ double current_theta = current_pose.theta();
|
|
|
+ if (RotateAfterOutExport() != eMpcSuccess) {
|
|
|
+ printf("Rotate after out entrance failed\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
//摆正
|
|
|
if (GetRemainActionsCount() > 1) {
|
|
|
if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
|
|
|
@@ -1233,7 +1297,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
|
|
|
return ret == success || ret == no_solution;
|
|
|
}
|
|
|
|
|
|
-bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY) {
|
|
|
+bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int& mpc_result, bool directY) {
|
|
|
if (PoseTimeout() == true) {
|
|
|
printf(" MPC once Error:Pose is timeout \n");
|
|
|
return false;
|
|
|
@@ -1289,13 +1353,14 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
|
|
|
MpcError ret = failed;
|
|
|
LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max);
|
|
|
- // 巡线最大角速度固定0.1745
|
|
|
+ // 巡线最大角加速度固定0.1745
|
|
|
LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
|
|
|
mpc_parameter.acc_velocity(), 0.1745};
|
|
|
//printf(" r:%f dt:%f acc:%f acc_a:%f\n",mpc_parameter.shortest_radius(),
|
|
|
// mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
|
|
|
ret = MPC.solve(traj, traj[traj.size() - 1], statu, parameter,
|
|
|
out, selected_trajectory, optimize_trajectory, directY);
|
|
|
+ mpc_result = ret;
|
|
|
|
|
|
//std::cout<<" traj size %d %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
|
|
|
if (ret == no_solution) {
|
|
|
@@ -1308,6 +1373,18 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
out.push_back(0);
|
|
|
out.push_back(0);
|
|
|
}
|
|
|
+
|
|
|
+ if (ret == unknown) {
|
|
|
+ printf(" mpc solve unknown set v/w = current\n");
|
|
|
+ double current_v = timedV_.Get(), current_w = timedA_.Get();
|
|
|
+ out.clear();
|
|
|
+ out.push_back(current_v);
|
|
|
+ out.push_back(current_w);
|
|
|
+ out.push_back(current_v);
|
|
|
+ out.push_back(current_w);
|
|
|
+ out.push_back(current_v);
|
|
|
+ out.push_back(current_w);
|
|
|
+ }
|
|
|
//
|
|
|
// diff_yaw.push_back(0);
|
|
|
// diff_yaw.push_back(0);
|
|
|
@@ -1732,6 +1809,9 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
ofs.open(log_file_dir + log_file_name, std::ios::app); //打开的地址和方式
|
|
|
auto beg_time = std::chrono::steady_clock::now();
|
|
|
|
|
|
+ int retry_count = 0;
|
|
|
+ int retry_count_limit = 3;
|
|
|
+
|
|
|
while (cancel_ == false) {
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
|
|
if (pause_ == true) {
|
|
|
@@ -1749,37 +1829,38 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
|
|
|
- //出入库实时判断门控或者载车板
|
|
|
- NavMessage::NewAction current_action = GetCurrentAction();
|
|
|
- if (current_action.type() == 1 || current_action.type() == 2) {
|
|
|
- //去出入口，判断内门是否打开
|
|
|
- if (TargetIsExport(current_action.spacenode()) || TargetIsEntrance(current_action.spacenode())) {
|
|
|
- int port_id = GetPortIDBySpace(current_action.spacenode());
|
|
|
- if (timed_Door_.timeout() || timed_Door_.Get()[port_id].second != eDoorOpened) {
|
|
|
- printf(" Inside door is not opened.......| line: %d\n", __LINE__);
|
|
|
- return eMpcFailed;
|
|
|
- }
|
|
|
- } else {
|
|
|
- //去车位点
|
|
|
- if (IsUperSpace(current_action.spacenode())) {
|
|
|
- if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierDown) {
|
|
|
- printf(" Carrier is not downed.......| line: %d\n", __LINE__);
|
|
|
- return eMpcFailed;
|
|
|
- }
|
|
|
- } else {
|
|
|
- int region_id = 0, carrier_no = 0;
|
|
|
- SpaceNo2CarrierNo(GetSpaceId(current_action.spacenode()), region_id, carrier_no);
|
|
|
- if (region_id < 0) {// 子母车位，且不在载车板下方的车位
|
|
|
-
|
|
|
- } else {
|
|
|
- if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierUp) {
|
|
|
- printf(" Carrier is not up.......| line: %d\n", __LINE__);
|
|
|
- return eMpcFailed;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+// //出入库实时判断门控或者载车板
|
|
|
+// float door_no_1_y = -5;
|
|
|
+// NavMessage::NewAction current_action = GetCurrentAction();
|
|
|
+// if (current_action.type() == 1 || current_action.type() == 2) {
|
|
|
+// //去出入口，判断内门是否打开
|
|
|
+// if (TargetIsExport(current_action.spacenode()) || TargetIsEntrance(current_action.spacenode())) {
|
|
|
+// int port_id = GetPortIDBySpace(current_action.spacenode());
|
|
|
+// if (timed_Door_.timeout() || timed_Door_.Get()[port_id].second != eDoorOpened) {
|
|
|
+// printf(" Inside door is not opened.......| line: %d\n", __LINE__);
|
|
|
+// return eMpcFailed;
|
|
|
+// }
|
|
|
+// } else {
|
|
|
+// //去车位点
|
|
|
+// if (IsUperSpace(current_action.spacenode())) {
|
|
|
+// if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierDown) {
|
|
|
+// printf(" Carrier is not downed.......| line: %d\n", __LINE__);
|
|
|
+// return eMpcFailed;
|
|
|
+// }
|
|
|
+// } else {
|
|
|
+// int region_id = 0, carrier_no = 0;
|
|
|
+// SpaceNo2CarrierNo(GetSpaceId(current_action.spacenode()), region_id, carrier_no);
|
|
|
+// if (region_id < 0) {// 子母车位，且不在载车板下方的车位
|
|
|
+//
|
|
|
+// } else {
|
|
|
+// if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierUp) {
|
|
|
+// printf(" Carrier is not up.......| line: %d\n", __LINE__);
|
|
|
+// return eMpcFailed;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
|
|
|
//判断是否到达终点
|
|
|
if (IsArrived(node)) {
|
|
|
@@ -1792,17 +1873,33 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ float v_before_compute = timedV_.Get();
|
|
|
+ float a_before_compute = timedA_.Get();
|
|
|
//一次变速
|
|
|
std::vector<double> out;
|
|
|
// std::vector<double> diff_yaw; //两车分别与整车的相对角度
|
|
|
bool ret;
|
|
|
+ int mpc_result = 0;
|
|
|
TimerRecord::Execute([&, this]() {
|
|
|
- ret = mpc_once(traj, limit_v, out, directY);
|
|
|
+ ret = mpc_once(traj, limit_v, out, mpc_result, directY);
|
|
|
}, "mpc_once");
|
|
|
+
|
|
|
if (ret == false) {
|
|
|
- SlowlyStop();
|
|
|
- ofs.close();//关闭文件
|
|
|
- return eMpcFailed;
|
|
|
+ if (mpc_result == unknown){
|
|
|
+ retry_count++;
|
|
|
+ if (retry_count > retry_count_limit) {
|
|
|
+ printf("MpcToTarget failed,ret:%d\n", ret);
|
|
|
+ SlowlyStop();
|
|
|
+ ofs.close();//关闭文件
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
+ printf("MpcToTarget failed,mpc_result:%d, retry...\n", mpc_result);
|
|
|
+ } else {
|
|
|
+ printf("MpcToTarget failed,ret:%d\n", ret);
|
|
|
+ SlowlyStop();
|
|
|
+ ofs.close();//关闭文件
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
}
|
|
|
const int down_count = 3;//下发前多少步
|
|
|
double down_v[down_count] = {out[0], out[2], out[4]};//下发线速度
|
|
|
@@ -1891,6 +1988,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
std::to_string(cur_Bro_pose.theta());
|
|
|
log_inf_line += " ";
|
|
|
|
|
|
+ log_inf_line += std::to_string(v_before_compute) + " " + std::to_string(a_before_compute);//计算时反馈速度
|
|
|
+ log_inf_line += " ";
|
|
|
+ log_inf_line += std::to_string(timedV_.Get()) + " " + std::to_string(timedA_.Get());//下发时反馈速度
|
|
|
+ log_inf_line += " ";
|
|
|
log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
|
|
|
log_inf_line += " ";
|
|
|
log_inf_line += std::to_string(out[2]) + " " + std::to_string(out[3]);//下发速度
|
|
|
@@ -2360,6 +2461,7 @@ int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
int Navigation::GetRemainActionsCount() {
|
|
|
return newUnfinished_cations_.size();
|
|
|
}
|
|
|
@@ -2368,5 +2470,17 @@ NavMessage::NewAction Navigation::GetCurrentAction() {
|
|
|
return newUnfinished_cations_.front();
|
|
|
}
|
|
|
|
|
|
+NavMessage::NewAction Navigation::GetLastAction() {
|
|
|
+ return newUnfinished_cations_.back();
|
|
|
+}
|
|
|
+
|
|
|
+int Navigation::GetSequenceType() {
|
|
|
+ if (global_navCmd_.has_sequence()){
|
|
|
+ return eSequenceDefault;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ return global_navCmd_.sequence();
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
|
