// // Created by zx on 22-12-2. // #include "navigation.h" #include "loaded_mpc.h" #include "rotate_mpc.h" #include "../define/TimerRecord.h" Navigation::~Navigation() { exit_ = true; if (terminator_) delete terminator_; if (brotherEmqx_) delete brotherEmqx_; if (monitor_) delete monitor_; if (pubthread_ != nullptr) { if (pubthread_->joinable()) pubthread_->join(); delete pubthread_; } } /* * 高斯函数压缩，x=3 */ double limit_gause(double x, double min, double max) { double r = x >= 0 ? 1.0 : -1.0; double delta = max / 1.0; return (max - (max - min) * exp(-x * x / (delta * delta))) * r; } double limit(double x, double min, double max) { double ret = x; if (x >= 0) { if (x > max) ret = max; if (x < min) ret = min; } else { if (x < -max) ret = -max; if (x > -min) ret = -min; } return ret; } double next_speed(double speed, double target_speed, double acc, double dt) { if (fabs(target_speed - speed) / dt < acc) { return target_speed; } else { double r = target_speed - speed >= 0. ? 1.0 : -1.0; return speed + r * acc * dt; } } bool Navigation::PoseTimeout() { // if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) { if (timedPose_.timeout() == false) { return false; } else { if (timedPose_.timeout()) { printf(" current pose is timeout \n"); } // if (timedBrotherPose_.timeout()) { // printf(" brother pose is timeout \n"); // } return true; } } bool Navigation::Init(const NavParameter::Navigation_parameter ¶meter) { parameter_ = parameter; NavParameter::AgvEmqx_parameter agv_p = parameter.agv_emqx(); if (monitor_ == nullptr) { monitor_ = new Monitor_emqx(agv_p.nodeid()); if (monitor_->Connect(agv_p.ip(), agv_p.port()) == false) { printf(" agv emqx connected failed\n"); return false; } monitor_->set_speedcmd_topic(agv_p.pubspeedtopic()); monitor_->set_clampLifterCmd_topic(agv_p.pubclampliftertopic()); monitor_->AddCallback(agv_p.subposetopic(), Navigation::RobotPoseCallback, this); monitor_->AddCallback(agv_p.subspeedtopic(), Navigation::RobotSpeedCallback, this); } NavParameter::Emqx_parameter terminal_p = parameter.terminal_emqx(); if (terminator_ == nullptr) { terminator_ = new Terminator_emqx(terminal_p.nodeid()); if (terminator_->Connect(terminal_p.ip(), terminal_p.port()) == false) { printf(" terminator emqx connected failed\n"); return false; } } if (parameter.has_brother_emqx()) { NavParameter::BrotherEmqx brotherEmqx = parameter.brother_emqx(); if (brotherEmqx_ == nullptr) { brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid()); if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) { brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback, this); } else { printf(" brother emqx connected failed\n"); // return false; } // } } inited_ = true; if (pubthread_ != nullptr) { exit_ = true; if (pubthread_->joinable()) pubthread_->join(); delete pubthread_; } exit_ = false; pubthread_ = new std::thread(&Navigation::pubStatuThreadFuc, this); return true; } void Navigation::RobotPoseCallback(const MqttMsg &msg, void *context) { Navigation *navigator = (Navigation *) context; NavMessage::LidarOdomStatu statu; if (msg.toProtoMessage(statu)) { navigator->ResetPose(Pose2d(statu.x(), statu.y(), statu.theta())); } } void Navigation::HandleAgvStatu(const MqttMsg &msg) { NavMessage::AgvStatu speed; if (IsMasterAGV()) { if (msg.toProtoMessage(speed)) { ResetStatu(speed.v(), speed.w()); ResetClamp((ClampStatu) speed.clamp()); // ResetOtherClamp((ClampStatu) speed.clamp_other()); ResetLifter((LifterStatus) speed.lifter()); // ResetOtherLifter((LifterStatus) speed.lifter_other()); //printf(" clamp:%d other:%d\n",speed.clamp(),speed.clamp_other()); } } else { if (msg.toProtoMessage(speed)) { ResetStatu(speed.v(), speed.w()); ResetClamp((ClampStatu) speed.clamp()); ResetLifter((LifterStatus) speed.lifter()); } } ResetDoor(speed.door()); std::vector vecStatus; for(int i=0;iHandleAgvStatu(msg); } void Navigation::pubStatuThreadFuc(void *p) { Navigation *navogator = (Navigation *) p; if (navogator) { while (navogator->exit_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); NavMessage::NavStatu statu; statu.set_main_agv(false); navogator->publish_statu(statu); } } } void Navigation::publish_statu(NavMessage::NavStatu &statu) { if (timedPose_.timeout() == false) { NavMessage::LidarOdomStatu odom; odom.set_x(timedPose_.Get().x()); odom.set_y(timedPose_.Get().y()); odom.set_theta(timedPose_.Get().theta()); if (timedV_.timeout() == false) odom.set_v(timedV_.Get()); if (timedA_.timeout() == false) odom.set_vth(timedA_.Get()); statu.mutable_odom()->CopyFrom(odom); } statu.set_in_space(isInSpace_); if (isInSpace_) statu.set_space_id(space_id_); //发布nav状态 statu.set_statu(actionType_); // statu.set_move_mode(move_mode_); if (running_) { statu.set_key(global_navCmd_.key()); } //发布MPC信息 //发布mpc 预选点 if (selected_traj_.timeout() == false) { for (int i = 0; i < selected_traj_.Get().size(); ++i) { Pose2d pt = selected_traj_.Get()[i]; NavMessage::Pose2d *pose = statu.mutable_selected_traj()->add_poses(); pose->set_x(pt.x()); pose->set_y(pt.y()); pose->set_theta(pt.theta()); } } //发布 mpc 预测点 if (predict_traj_.timeout() == false) { for (int i = 0; i < predict_traj_.Get().size(); ++i) { Pose2d pt = predict_traj_.Get()[i]; NavMessage::Pose2d *pose = statu.mutable_predict_traj()->add_poses(); pose->set_x(pt.x()); pose->set_y(pt.y()); pose->set_theta(pt.theta()); } } // std::cout<<"nav statu:"<Publish(parameter_.terminal_emqx().pubnavstatutopic(), msg); //发布位姿 ------robot状态-------------------------- NavMessage::RobotStatu robot; if (CreateRobotStatuMsg(robot)) { if (terminator_) { MqttMsg msg; msg.fromProtoMessage(robot); terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg); } } } bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) { //printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout()); if (timedPose_.timeout() == false) { Pose2d pose = timedPose_.Get(); robotStatu.set_x(pose.x()); robotStatu.set_y(pose.y()); robotStatu.set_theta(pose.theta()); if ((timedV_.timeout() == false && timedA_.timeout() == false)) { NavMessage::AgvStatu plc; plc.set_v(timedV_.Get()); plc.set_w(timedA_.Get()); plc.set_clamp(timed_clamp_.Get()); plc.set_lifter(timed_lifter_.Get()); robotStatu.mutable_agvstatu()->CopyFrom(plc); } return true; } return false; } void Navigation::ResetStatu(double v, double a) { timedV_.reset(v, 0.5); timedA_.reset(a, 0.5); } void Navigation::ResetClamp(ClampStatu statu) { timed_clamp_.reset(statu, 1); } void Navigation::ResetLifter(LifterStatus status) { timed_lifter_.reset(status, 1); } void Navigation::ResetPose(const Pose2d &pose) { timedPose_.reset(pose, 1.0); if (pose.theta() > 0) { if (parameter_.main_agv()) { isFront_.reset(true, 1.0); } else { isFront_.reset(false, 1.0); } } else { if (parameter_.main_agv()) { isFront_.reset(false, 1.0); } else { isFront_.reset(true, 1.0); } } } void Navigation::Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) { cancel_ = true; response.set_ret(0); } bool Navigation::Stop() { if (monitor_) { monitor_->stop(); while (cancel_ == false) { if (timedV_.timeout() == false && timedA_.timeout() == false) { if (fabs(timedV_.Get()) < 1e-2 && fabs(timedA_.Get()) < 1e-3) return true; else monitor_->stop(); printf("Stoped wait V/A ==0(1e-4,1e-4),V:%f,A:%f\n", fabs(timedV_.Get()), fabs(timedA_.Get())); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } else { printf("Stop failed V/A timeout\n"); return false; } } } printf("stoped ret false.\n"); return false; } bool Navigation::SlowlyStop() { double acc = 3.0; double dt = 0.1; while (cancel_ == false) { if (timedV_.timeout() == false) { double v = timedV_.Get(); if (fabs(v < 1e-2 && fabs(timedA_.Get()) < 1e-3)) return true; if (v > 0) { double new_v = v - acc * dt; new_v = new_v > 0 ? new_v : 0; SendMoveCmd(move_mode_, actionType_, new_v, 0); } else { double new_v = v + acc * dt; new_v = new_v < 0 ? new_v : 0; SendMoveCmd(move_mode_, actionType_, new_v, 0); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); } else { printf("Stop failed V/A timeout\n"); return false; } } printf("stoped ret false.\n"); return false; } Navigation::Navigation() { is_master_AGV=false; wheelBase_ = 0; isInSpace_ = false; //是否在车位或者正在进入车位 RWheel_position_ = eUnknow; move_mode_ = eSingle; monitor_ = nullptr; terminator_ = nullptr; obs_h_ = 4.0; obs_w_ = 2.0; timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5); LoadSpaceNo2CarrierNo(); } void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) { Navigation *navigator = (Navigation *) context; NavMessage::NavStatu brother_statu; if (msg.toProtoMessage(brother_statu) == false) { std::cout << " msg transform to AGVStatu failed,msg:" << msg.data() << std::endl; return; } //std::cout<timedBrotherPose_.reset(pose, 1); navigator->timedBrotherV_.reset(odom.v(), 1); navigator->timedBrotherA_.reset(odom.vth(), 1); navigator->timedBrotherNavStatu_.reset(brother_statu, 0.1); } } //未使用 /* bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect) { while (cancel_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (timedPose_.timeout()) { printf(" RotateBytarget failed pose timeout\n"); return false; } if (timedA_.timeout()) { printf(" RotateBytarget failed wmg timeout\n"); return false; } Pose2d targetInPose = Pose2d::relativePose(target, timedPose_.Get()); float yawDiff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y()); float minYawdiff = yawDiff; if (anyDirect) { Pose2d p0 = timedPose_.Get(); float yawdiff[4] = {0}; yawdiff[0] = yawDiff; Pose2d relativePose; relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI / 2)); yawdiff[1] = Pose2d::vector2yaw(relativePose.x(), relativePose.y()); ////使用减法，保证角度在【-pi pi】 relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI)); yawdiff[2] = Pose2d::vector2yaw(relativePose.x(), relativePose.y()); relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, 3 * M_PI / 2)); yawdiff[3] = Pose2d::vector2yaw(relativePose.x(), relativePose.y()); for (int i = 1; i < 4; ++i) { if (fabs(yawdiff[i]) < fabs(minYawdiff)) { minYawdiff = yawdiff[i]; targetInPose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, i * M_PI / 2.0)); } } } //std::cout<<" Rotate refer to target:"<0.25,则需要调整 y=a x*x float cuv = compute_cuv(targetInPose); float dt = 0.1; float acc_angular = 15 * M_PI / 180.0; if (cuv > 2 * M_PI / 180.0) { double theta = limit_gause(minYawdiff, limit_rotate.min, limit_rotate.max); double angular = next_speed(timedA_.Get(), theta, acc_angular, dt); double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max); SendMoveCmd(move_mode_, eRotation, 0, limit_angular); actionType_ = eRotation; printf(" Rotate | input angular:%f,next angular:%f,down:%f diff:%f anyDirect:%d\n", timedA_.Get(), angular, limit_angular, minYawdiff, anyDirect); continue; } else { if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) { printf(" Rotate refer target completed,cuv:%f\n", cuv); return true; } continue; } } return false; } */ bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate,int directions) { std::cout << "Adjust to : " << target << std::endl; if (inited_ == false) { printf(" MPC_rotate has not inited\n"); return false; } if (PoseTimeout()) { printf(" MPC_rotate Error:Pose is timeout \n"); return false; } Pose2d current = timedPose_.Get(); NavMessage::PathNode yawTarget; yawTarget.set_x(current.x() + 100.); yawTarget.set_y(current.y()); std::cout << "Adjust RotateRefer : " << Pose2d(yawTarget.x(), yawTarget.y(), 0) << std::endl; RotateReferToTarget(yawTarget, limit_rotate, directions); current = timedPose_.Get(); Pose2d diff = target - current; Pose2d newTarget; NavMessage::PathNode node; if (fabs(diff.x()) > fabs(diff.y())) { newTarget = Pose2d(current.x(), target.y(), current.theta()); node.set_theta(M_PI / 2.0); } else { newTarget = Pose2d(target.x(), current.y(), current.theta()); node.set_theta(0); } node.set_x(newTarget.x()); node.set_y(newTarget.y()); node.set_l(0.03); node.set_w(0.2); stLimit limitV = {0.03, 0.5}; stLimit limitW = {0.03, 0.1}; std::cout << " Adjust MoveTarget:" << Pose2d(node.x(), node.y(), node.theta()) << std::endl; return MoveToTarget(node, limitV, limitW, true, true); } Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node, stLimit limit_rotate, int directions) { if (inited_ == false) { printf(" MPC_rotate has not inited\n"); return eMpcFailed; } if (PoseTimeout()) { printf(" MPC_rotate Error:Pose is timeout \n"); return eMpcFailed; } printf(" exec MPC_rotate autoDirect:%d\n", directions); while (cancel_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(30)); if (pause_ == true) { //发送暂停消息 continue; } if (PoseTimeout()) { printf(" MPC_rotate Error:Pose is timeout \n"); return eMpcFailed; } if (timedA_.timeout()) { printf(" MPC_rotate Error:v/a is timeout \n"); return eMpcFailed; } Pose2d current = timedPose_.Get(); Pose2d target(node.x(), node.y(), node.theta()); Pose2d targetInPose = Pose2d::relativePose(target, current); // 需要旋转的角度 float target_yaw_diff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y()); // 自由方向 if (directions) { std::vector yaw_diffs; if (directions & Direction::eForward) { yaw_diffs.push_back(target_yaw_diff); } if (directions & Direction::eBackward) { Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, M_PI)); yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y())); } if (directions & Direction::eLeft) { Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, 1.0 / 2 * M_PI)); yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y())); } if (directions & Direction::eRight) { Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, 3.0 / 2 * M_PI)); yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y())); } for (auto i = 0; i < yaw_diffs.size(); ++i) { if (fabs(yaw_diffs[i]) < fabs(target_yaw_diff)) target_yaw_diff = yaw_diffs[i]; } // std::cout << std::endl; // std::cout << std::endl << " min_diff: " << current_to_target.theta() << std::endl; } //一次变速 std::vector out; bool ret; TimerRecord::Execute([&, this]() { ret = Rotation_mpc_once(Pose2d(0, 0, target_yaw_diff), limit_rotate, out); }, "Rotation_mpc_once"); if (ret == false) { Stop(); return eMpcFailed; } //下发速度 if (fabs(target_yaw_diff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) { printf(" MPC_rotate | refer target completed\\n,cuv:%f\n", target_yaw_diff); 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; double input_w = timedA_.Get(); /* for (int i = 0; i < down_count; ++i) { printf(" MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n", i, input_w, w[i], w[i] / M_PI * 180, target_yaw_diff, directions); }*/ } continue; } return eMpcFailed; } bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w, bool anyDirect, bool enable_rotate, int clampLifterAction) { if (IsArrived(node)) { return true; } bool already_in_mpc = false; while (cancel_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (PoseTimeout()) { printf(" navigation Error:Pose is timeout \n"); break; } Pose2d current = timedPose_.Get(); if (IsArrived(node)) { break; } //两个方向都无法到达目标点,旋转 朝向目标点 bool x_enable = PossibleToTarget(node, false); bool y_enable = PossibleToTarget(node, true); bool enableTotarget = x_enable || y_enable; if (anyDirect == false) { enableTotarget = x_enable; } //巡线无法到达目标点 Pose2d target(node.x(), node.y(), 0); Pose2d targetInCurrent = Pose2d::relativePose(target, current); if (enableTotarget == false) { int directions = Direction::eForward; if(anyDirect) directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward; bool adjustRet = AdjustReferToTarget(target, limit_w,directions); if (adjustRet == false) return false; /*if (already_in_mpc && enable_rotate == false) { printf(" Move to node failed ,impossible to target and rotate is not enabled\n"); return false; } int directions = Direction::eForward; if (anyDirect) directions = Direction::eForward | Direction::eBackward | Direction::eLeft | Direction::eRight; if (RotateReferToTarget(node, limit_w, directions) != eMpcSuccess) { std::cout << " Rotate refer to target failed,target: " << target << " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl; return false; } continue;*/ } already_in_mpc = true; MpcResult ret = MpcToTarget(node, limit_v, anyDirect, clampLifterAction); if (ret == eMpcSuccess) { printf(" MPC to target:%f %f l:%f,w:%f theta:%f completed\n", node.x(), node.y(), node.l(), node.w(), node.theta()); break; } else if (ret == eImpossibleToTarget) { 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 { return false; } } if (cancel_) { return false; } return true; } bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) { if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致 { if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) { std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl; return false; } bool anyDirect = (action.type() == 3); //速度限制 stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()}; stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()}; ///////////////////////////////////////// int clampLifterAction = 0; if (move_mode_ == eSingle) { clampLifterAction |= eByteClampHalfOpen; }else{ clampLifterAction |= eByteLifterDown; } //////////////////////////////////////////////////////////// for (int i = 0; i < action.pathnodes_size(); ++i) { NavMessage::PathNode node = action.pathnodes(i); if (i + 1 < action.pathnodes_size()) { NavMessage::PathNode next = action.pathnodes(i + 1); 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); } 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); } int directions = Direction::eForward; if (anyDirect) directions = Direction::eForward | Direction::eBackward | Direction::eLeft | Direction::eRight; /*if (i > 0) { if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess) { printf(" before move node RotateReferToNone Failed!!\n"); return false; } }*/ if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false) return false; else isInSpace_ = false; } if (cancel_ == true) { return false; } return true; } printf("execute PathNode failed ,action type is not 3/4 :%d\n", action.type()); return false; } Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() { if (move_mode_ != eDouble) return eMpcSuccess; int clampLifterAction=0; if(move_mode_==eDouble){ clampLifterAction |= eLifterDown; } 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(100)); Pose2d current = timedPose_.Get(); double yawDiff = (target - current.theta()); //一次变速 std::vector 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.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) { printf(" DoubleOutSpaceCorrectTheta refer target completed\\n,cuv:%f\n", yawDiff); Stop(); if(WaitClampLifterStatu(clampLifterAction)==false){ return eMpcFailed; } 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(clampLifterAction, move_mode_, eRotation, v, w); actionType_ = eRotation; printf(" DoubleOutSpaceCorrectTheta | 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) 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(100)); Pose2d current = timedPose_.Get(); double yawDiff = (target - current.theta()); //一次变速 std::vector 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.2 * 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::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target, int clampLifterAction) { // if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) { if (timedPose_.timeout()) { printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n"); return eMpcFailed; } NavMessage::NavStatu brother = timedBrotherNavStatu_.Get(); stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0}; double acc_angular = 20 * M_PI / 180.0; double dt = 0.1; Pose2d current = timedPose_.Get(); double x = space.x(); double y = space.y(); Pose2d vec(x - current.x(), y - current.y(), 0); double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y()); bool isFront = isFront_.Get(); printf(" front__:%d theta:%f\n", isFront, vecTheta); if (vecTheta < 0.) isFront = !isFront; //如果是后车，计算目标点（车位点-轴距） if (isFront == false) { printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__); if (move_mode_ == eSingle) { x -= wheelbase * cos(vecTheta); y -= wheelbase * sin(vecTheta); printf("确定车位点：eSingle\n"); } } else { //当后车先到,倒车入库 printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__); //rotated.mutable_theta() = space.theta(); //rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI); } Pose2d spaceInCurrent = Pose2d::relativePose(Pose2d(space.x(), space.y(), 0), current); target.set_x(x); target.set_y(y); target.set_theta(current.theta()); target.set_l(0.02); target.set_w(0.05); target.set_id(space.id()); printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n", x, y, target.theta()); // //整车协调的时候，保持前后与车位一致即可 // if (move_mode_ == eDouble) { // double yawDiff1 = (rotated - timedPose_.Get()).theta(); // double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta(); // if (fabs(yawDiff1) < fabs(yawDiff2)) { // rotated = rotated + Pose2d(0, 0, M_PI); // } // } while (cancel_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(30)); Pose2d current_now = timedPose_.Get(); if (spaceInCurrent.x() < 0.) current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI); double yawDiff = vecTheta - current_now.theta(); //一次变速 std::vector 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.3 * 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(clampLifterAction, 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; } bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) { if (spaceNode.id().find("S") < 0) return false; std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1); int space_id = stoi(id); if (space_id > 99 && space_id < 1000) { return true; } return false; } int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) { if (spaceNode.id().find("S") >= 0) { std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1); int space_id = stoi(id); return space_id; } else { return -1; } } bool Navigation::TargetIsEntrance(NavMessage::PathNode node) { return node.id().find("S1101") != -1; } bool Navigation::TargetIsExport(NavMessage::PathNode node) { return node.id().find("S1101") != -1; } bool Navigation::execute_InOut_space(NavMessage::NewAction action) { if (action.type() != 1 && action.type() != 2) { printf(" Inout_space failed : msg action type must ==1\n "); return false; } if (!parameter_.has_inoutvlimit() || !action.has_spacenode() || !action.has_streetnode()) { std::cout << "execute enter_space failed ,Action miss some infos:" << action.DebugString() << std::endl; return false; } if (PoseTimeout() == true) { printf(" inout_space failed type:%d : current pose is timeout\n", action.type()); return false; } stLimit limit_v = {parameter_.inoutvlimit().min(), parameter_.inoutvlimit().max()}; stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0}; //入库,起点为streetNode if (action.type() == 1) { Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(), 0); action.mutable_streetnode()->set_l(0.02); action.mutable_streetnode()->set_w(0.2); action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0); /* * 是否需要添加判断，距离较远才先运动到马路点============================= */ int clampLifterAction = 0; if (IsUperSpace(action.spacenode())) { //进载车板库前提升机构上升 clampLifterAction |= eByteLifterUp; } else { //否则下降 clampLifterAction |= eByteLifterDown; } if (move_mode_ == eSingle) clampLifterAction |= eByteClampHalfOpen; if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) { printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type()); return false; } //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false） printf("inout ----------------------------------------\n"); //取车流程，送车到出口前旋转180 if (move_mode_ == eDouble && TargetIsExport(action.spacenode())) { 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())); //计算当前车是进入车位1点还是2点, NavMessage::PathNode new_target; bool retRotate = RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target, clampLifterAction) == eMpcSuccess; if (retRotate == false) { return false; } //去出入口，等内门打开 if (TargetIsExport(action.spacenode()) || TargetIsEntrance(action.spacenode())) { printf(" Wait Inside door opened.......\n"); int port_id = GetPortIDBySpace(action.spacenode()); if (WaitInsideDoorCompleted(port_id, eDoorOpened) == false ) { return false; } printf(" Wait Inside door opened completed!!!\n"); } else { //去车位点 if (IsUperSpace(action.spacenode())) { printf(" Wait Carrier down.......\n"); if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierDown) == false) { return false; } printf(" Wait Carrier down completed!!!\n"); } else { if(!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode())) { printf(" Wait Carrier up.......\n"); if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierUp) == false) { return false; } printf(" Wait Carrier up completed!!!\n"); }else{ printf(" Wait inside door open.......\n"); if(WaitInsideDoorCompleted(GetSpaceId(action.spacenode()),eDoorOpened)==false) return false; printf("inside door opened\n"); } } } if (IsUperSpace(action.spacenode())) { if(lifter_rise()== false){ return false; } } // 单车进库必须全打开 if (move_mode_ == eSingle){ if (clamp_fully_open() == false) { return false; } } //入库 Pose2d rotated = timedPose_.Get(); double x = new_target.x(); double y = new_target.y(); Pose2d vecSpace(x - rotated.x(), y - rotated.y(), 0); rotated.mutable_theta() = Pose2d::vector2yaw(vecSpace.x(), vecSpace.y()); bool isFront = isFront_.Get(); if (rotated.theta() < 0.) isFront = !isFront; // 后进车，且单车，且先进车已过后进车的目标点 Pose2d target(new_target.x(), new_target.y(), 0); if (isFront == false && move_mode_ == eSingle) { while (true) { if (cancel_) { return false; }if(move_mode_==eDouble){ clampLifterAction |= eLifterDown; } Pose2d current = timedPose_.Get(); Pose2d brother_pose = timedBrotherPose_.Get(); Pose2d vecCurent2T = current - target; Pose2d vecBrother2T = brother_pose - target; double dot = vecCurent2T.x() * vecBrother2T.x() + vecCurent2T.y() * vecBrother2T.y(); if (timedBrotherNavStatu_.Get().in_space() == true && dot < 0.) { printf("先进车已经过后进车目标点，后进车停止等待 dot=%f\n", dot); break; } printf(" 后车 等待 先车先入库。。。\n"); usleep(1000 * 500); continue; } } isInSpace_ = true; printf("Enter space beg...\n"); clampLifterAction = 0; if (IsUperSpace(action.spacenode())) { //进载车板库前提升机构上升 clampLifterAction |= eByteLifterUp; } else { //否则下降 clampLifterAction |= eByteLifterDown; } printf(" clampLifter_statu1 :%d\n", clampLifterAction); if (move_mode_ == eSingle) clampLifterAction |= eByteClampFullyOpen; //////// 后续再改为全打开位 else clampLifterAction |= eByteCLampClose; printf(" clampLifter_statu2 :%d\n", clampLifterAction); bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction); if (retMove == false) { printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type()); return false; } } else if (action.type() == 2) { // LogWriter("Out space beg-"); // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿 Pose2d vec(action.streetnode().x() - action.spacenode().x(), action.streetnode().y() - action.spacenode().y(), 0); float theta = Pose2d::vector2yaw(vec.x(), vec.y()); Pose2d space(action.spacenode().x(), action.spacenode().y(), theta); Pose2d current = timedPose_.Get(); Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space)); if (diff.x() < 0.05 || diff.y() < 0.10) { //出库，移动到马路点，允许自由方向，不允许中途旋转 action.mutable_streetnode()->set_theta(theta); action.mutable_streetnode()->set_l(0.03); action.mutable_streetnode()->set_w(0.2); std::cout << " Out space target street node:" << space << std::endl; if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, false) == false) { printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type()); return false; } } else { std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff << std::endl; return false; } isInSpace_ = false; //摆正 if (DoubleOutSpaceCorrectTheta() != eMpcSuccess) return false; } return true; } /* current_to_target: current_to_target.theta()为所需旋转的角度 * */ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit limit_wmg, std::vector &out, bool all_direct) { if (PoseTimeout() == true) { printf(" Rotation_mpc_once Error:Pose is timeout \n"); return false; } if (timedV_.timeout() == true || timedA_.timeout() == true) { printf(" Rotation_mpc_once Error:V/A is timeout \n"); return false; } Pose2d pose = timedPose_.Get(); double line_velocity = timedV_.Get(); //从plc获取状态 double angular_velocity = timedA_.Get(); Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态 statu[0] = pose.x(); statu[1] = pose.y(); statu[2] = pose.theta(); statu[3] = line_velocity; statu[4] = angular_velocity; NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter(); double obs_w = 0.95;//0.85; double obs_h = 1.55;//1.45; MpcError ret = failed; Pose2d brother = timedBrotherPose_.Get(); Pose2d brother_in_self = Pose2d::relativePose(brother, pose); // std::cout<<" brother_in_self: "< 0) out[i] = min_angular_velocity; else if (out[i] < 0) out[i] = -min_angular_velocity; else {} } } } return ret == success || ret == no_solution; } bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector &out, bool directY) { if (PoseTimeout() == true) { printf(" MPC once Error:Pose is timeout \n"); return false; } if (timedV_.timeout() == true || timedA_.timeout() == true) { printf(" MPC once Error:V/A is timeout \n"); return false; } if (traj.size() == 0) { printf("traj size ==0\n"); return false; } Pose2d pose = timedPose_.Get(); double velocity = timedV_.Get(); //从plc获取状态 double angular = timedA_.Get(); // float diffYaw1=timedPose_.Get().m_diffYaw1; // float diffYaw2=timedPose_.Get().m_diffYaw2; Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态 statu[0] = pose.x(); statu[1] = pose.y(); statu[2] = pose.theta(); statu[3] = velocity; statu[4] = angular; Trajectory optimize_trajectory; Trajectory selected_trajectory; NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter(); double obs_w = obs_w_;// 0.95;//0.85; double obs_h = obs_h_;// 1.55;//1.45; if (directY == true) { //将车身朝向旋转90°，车身朝向在(-pi,pi] statu[2] += M_PI / 2; if (statu[2] > M_PI) statu[2] -= 2 * M_PI; mpc_parameter = parameter_.y_mpc_parameter(); obs_w = obs_h_;// 0.95;//0.85; obs_h = obs_w_;//1.55;//1.45; } Pose2d brother = timedBrotherPose_.Get(); Pose2d relative = Pose2d::relativePose(brother, pose); if (directY) relative = Pose2d::relativePose(brother.rotate(brother.x(), brother.y(), M_PI / 2), pose.rotate(pose.x(), pose.y(), M_PI / 2));//计算另一节在当前小车坐标系下的坐标 std::cout << "pose:" << pose << ", brother:" << brother << ", relative:" << relative << std::endl; if (move_mode_ == eDouble) relative = Pose2d(-1e8, -1e8, 0); MpcError ret = failed; LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max); LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(), mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()}; //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); //std::cout<<" traj size %d %d -------- "<set_speed(mode, type, v, angular); if (type == eRotation) RWheel_position_ = eR; if (type == eVertical) RWheel_position_ = eX; if (type == eHorizontal) RWheel_position_ = eY; } } void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type, double v[], double angular[]) { if (monitor_) { monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular); if (type == eRotation) RWheel_position_ = eR; if (type == eVertical) RWheel_position_ = eX; if (type == eHorizontal) RWheel_position_ = eY; } } /*void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance,double Y1,double Y2) { if (monitor_) { monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance,Y1,Y2); if (type == eRotation) RWheel_position_ = eR; if (type == eVertical) RWheel_position_ = eX; if (type == eHorizontal) RWheel_position_ = eY; } }*/ void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type, double v[], double angular[], int space_id, double distance, double Y1, double Y2) { if (monitor_) { monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular, 0, space_id, distance, Y1, Y2); if (type == eRotation) RWheel_position_ = eR; if (type == eVertical) RWheel_position_ = eX; if (type == eHorizontal) RWheel_position_ = eY; } } //void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) { // if (monitor_) { // monitor_->set_ToAgvCmd(mode, type, v, angular, yaw_diff ,0,space_id, distance); // if (type == eRotation) // RWheel_position_ = eR; // if (type == eVertical) // RWheel_position_ = eX; // if (type == eHorizontal) // RWheel_position_ = eY; // } //} bool Navigation::clamp_close() { if (monitor_) { printf("Clamp closing...\n"); monitor_->clamp_close(move_mode_); actionType_ = eClampClose; while (cancel_ == false) { if (timed_clamp_.timeout()) { printf("timed clamp is timeout\n"); return false; } if (timed_clamp_.Get() == eClosed) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->clamp_close(move_mode_); actionType_ = eClampClose; } return false; } return false; } bool Navigation::clamp_half_open() { if (monitor_) { printf("Clamp openning...\n"); monitor_->clamp_half_open(move_mode_); actionType_ = eClampHalfOpen; while (cancel_ == false) { if (timed_clamp_.timeout()) { printf("timed clamp is timeout\n"); return false; } if (timed_clamp_.Get() == eHalfOpened) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->clamp_half_open(move_mode_); actionType_ = eClampHalfOpen; } return false; } return false; } bool Navigation::singleClamp_fullyOpen_until_inspaceBegin() { if (move_mode_ == eDouble) { return true; } while (cancel_ == false) { if (timedV_.timeout()) { return false; } if (fabs(timedV_.Get()) < 0.02) { usleep(1000 * 300); continue; } break; } return clamp_fully_open(); }; bool Navigation::clamp_fully_open() { if (monitor_) { if (timed_clamp_.Get() == eFullyOpened) return true; printf("Clamp fully openning...\n"); monitor_->clamp_fully_open(move_mode_); actionType_ = eClampFullyOpen; while (cancel_ == false) { if (timed_clamp_.timeout()) { printf("timed clamp is timeout\n"); return false; } if (timed_clamp_.Get() == eFullyOpened) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->clamp_fully_open(move_mode_); actionType_ = eClampFullyOpen; } return false; } return false; } bool Navigation::lifter_rise() { if (monitor_) { printf("Lifter upping...\n"); actionType_ = eLifterRise; if (timed_lifter_.Get() == eRose) return true; if (timed_lifter_.Get() != eRose) { if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) { printf(" clamp statu !=eHalfOpened or eClosed, clamp_half_open...\n "); if (!clamp_half_open()) { printf(" clamp half open failed\n"); return false; } } } while (cancel_ == false) { if (timed_lifter_.timeout()) { printf("timed lifter is timeout\n"); return false; } if (timed_lifter_.timeout()) { return false; } if (timed_lifter_.Get() == eRose) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->lifter_rise(move_mode_); actionType_ = eLifterRise; } return false; } return false; } bool Navigation::lifter_down() { if (monitor_) { printf("Lifter downing...\n"); if (timed_lifter_.Get() == eDowned) return true; actionType_ = eLifterDown; if (timed_lifter_.Get() != eDowned) { if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) { printf(" clamp statu !=eHalfOpened or eClosed, clamp_half_open...\n "); if (!clamp_half_open()) { printf(" clamp half open failed\n"); return false; } } } while (cancel_ == false) { if (timed_lifter_.timeout()) { printf("timed lifter is timeout\n"); return false; } if (timed_lifter_.timeout()) { return false; } if (timed_lifter_.Get() == eDowned) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->lifter_down(move_mode_); actionType_ = eLifterDown; } return false; } return false; } bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) { while (cancel_ == false) { if (timed_Door_.timeout()) { return false; } if (timed_Door_.Get()[doorID].second == status) { return true; } usleep(1000 * 100); printf("waiting door_id(ins):%d, to:%d...\n",doorID, status); } if (cancel_ == true) { return false; } return false; } bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) { while (cancel_ == false) { int region_id=-1, carrier_no=-1; SpaceNo2CarrierNo(spaceID, region_id, carrier_no); if (timed_Carrier_[region_id].timeout()) { return false; } CarrierStatu cur_ = GetCarrierStatusBySpaceID(spaceID); if ( cur_ == statu){ return true; } usleep(1000 * 100); printf("waiting space_id:%d, carrier_id[%d,%d] to:%d, cur:%d...\n",spaceID, region_id, carrier_no, statu, cur_); } if (cancel_ == true) { return false; } return false; } bool Navigation::WaitClampLifterStatu(int clampLifterAction) { int currentStatu = 0; while (cancel_ == false) { if (timed_clamp_.timeout() || timed_lifter_.timeout()) { return false; } if (timed_clamp_.Get() == eClosed) { currentStatu |= eByteCLampClose; } if (timed_clamp_.Get() == eHalfOpened) { currentStatu |= eByteClampHalfOpen; } if (timed_clamp_.Get() == eFullyOpened) { currentStatu |= eByteClampFullyOpen; } if (timed_lifter_.Get() == eRose) { currentStatu |= eByteLifterUp; } if (timed_lifter_.Get() == eDowned) { currentStatu |= eByteLifterDown; } printf(" current statu:%d action status:%d last:%d\n", currentStatu,clampLifterAction,currentStatu&clampLifterAction); if((currentStatu&clampLifterAction) == clampLifterAction) return true; double down_v[3] = {0, 0, 0};//下发线速度0 double down_w[3] = {0, 0, 0};//下发角速度0 SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w); usleep(1000 * 100); } if (cancel_ == true) { return false; } return false; } Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect, int clampLifterAction) { if (IsArrived(node)) { printf(" current already in target completed !!!\n"); return eMpcSuccess; } if (inited_ == false) { printf(" navigation has not inited\n"); return eMpcFailed; } if (PoseTimeout()) { printf(" navigation Error:Pose is timeout \n"); return eMpcFailed; } Pose2d current = timedPose_.Get(); Pose2d target(node.x(), node.y(), 0); //生成轨迹 Trajectory traj = Trajectory::create_line_trajectory(current, target, 0.1); if (traj.size() == 0) return eMpcSuccess; //判断 巡线方向 bool directY = false; if (autoDirect) { Pose2d target_relative = Pose2d::relativePose(target, timedPose_.Get()); if (fabs(target_relative.y()) > fabs(target_relative.x())) { //目标轨迹点在当前点Y轴上 std::cout << " MPC Y axis target_relative:" << target_relative << std::endl; directY = true; } else { std::cout << " MPC X axis target_relative:" << target_relative << std::endl; } if (!PossibleToTarget(node, directY)) { directY = !directY; } } printf(" exec along autoDirect:%d\n", autoDirect); //文件log std::ofstream ofs; //创建流对象 std::string log_file_dir = "../data/"; time_t t = time(0); char tmp[32] = {NULL}; strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S.txt", localtime(&t)); std::string log_file_name = tmp; ofs.open(log_file_dir + log_file_name, std::ios::app); //打开的地址和方式 auto beg_time = std::chrono::steady_clock::now(); while (cancel_ == false) { std::this_thread::sleep_for(std::chrono::milliseconds(20)); if (pause_ == true) { //发送暂停消息 continue; } if (PoseTimeout()) { printf(" navigation Error:Pose is timeout \n"); ofs.close();//关闭文件 return eMpcFailed; } if (timedV_.timeout() || timedA_.timeout()) { printf(" navigation Error:v/a is timeout | __LINE__:%d \n", __LINE__); ofs.close();//关闭文件 return eMpcFailed; } //判断是否到达终点 if (IsArrived(node)) { if (Stop()) { printf(" exec along completed !!!\n"); ofs.close();//关闭文件 bool waited = WaitClampLifterStatu(clampLifterAction); if (waited) return eMpcSuccess; else return eMpcFailed; } } //一次变速 std::vector out; // std::vector diff_yaw; //两车分别与整车的相对角度 bool ret; TimerRecord::Execute([&, this]() { ret = mpc_once(traj, limit_v, out, directY); }, "mpc_once"); if (ret == false) { SlowlyStop(); ofs.close();//关闭文件 return eMpcFailed; } const int down_count = 3;//下发前多少步 double down_v[down_count] = {out[0], out[2], out[4]};//下发线速度 double down_w[down_count] = {out[1], out[3], out[5]};//下发角速度 /* * AGV 在终点附近低速巡航时，设置最小速度0.05 */ Pose2d target_in_agv = Pose2d::relativePose(target, timedPose_.Get()); if (directY) { target_in_agv = target_in_agv.rotate(M_PI / 2.0); } if (Pose2d::abs(target_in_agv) < Pose2d(0.2, 2, M_PI * 2)) { for (int i = 0; i < down_count; ++i) { if (down_v[i] >= 0 && down_v[i] < limit_v.min) down_v[i] = limit_v.min; if (down_v[i] < 0 && down_v[i] > -limit_v.min) down_v[i] = -limit_v.min; } } //放大角速度 // out[1] *= 10; //入库，行进方向上最后一米不纠偏 if (newUnfinished_cations_.front().type() == 1 && node.id().find('S') != -1) {//入库 if (fabs(target_in_agv.x()) < 0.5) { printf("target_in_agv: %f", target_in_agv.x()); for (double &i: down_w) { i = 0.0; } } } else if (newUnfinished_cations_.front().type() == 2) {//出库,行进方向上开始一米不纠偏 Pose2d agv_in_space = Pose2d::relativePose( Pose2d(newUnfinished_cations_.front().spacenode().x(), newUnfinished_cations_.front().spacenode().y(), 0), timedPose_.Get()); if (fabs(agv_in_space.x()) < 0.3) { printf("agv_in_space: %f", agv_in_space.x()); for (double &i: down_w) { i = 0.0; } } } //下发速度 //printf(" nav input :%f out:%f\n",timedV_.Get(),out[0]); //添加车位号,距目标点距离信息 double distance = Pose2d::distance(target_in_agv, Pose2d(0, 0, 0)); int space_id = 0; if (node.id().find('S') != -1) { std::string id = node.id().substr(1, node.id().length() - 1); space_id = stoi(id); } double Y1 = 0.0, Y2 = 0.0; if (move_mode_ == eDouble) { double dy = -timeYawDiff1_.Get(); Y1 = 0.2 * (1.0 / (1 + exp(-16. * dy)) - 0.5) * 1.0; Y2 = timeYawDiff2_.Get(); if (directY == false) { Y1 = 0; } } if (directY == false) SendMoveCmd(clampLifterAction, move_mode_, eVertical, down_v, down_w, space_id, distance, Y1, Y2); else SendMoveCmd(clampLifterAction, move_mode_, eHorizontal, down_v, down_w, space_id, distance, Y1, Y2); actionType_ = directY ? eHorizontal : eVertical; //日志打印 std::string log_inf_line = std::to_string(move_mode_); log_inf_line += " "; log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius()) + " " + std::to_string(parameter_.x_mpc_parameter().acc_angular()); log_inf_line += " "; Pose2d cur_pose = timedPose_.Get();//自身x,y,theta log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " + std::to_string(cur_pose.theta()); log_inf_line += " "; Pose2d cur_Bro_pose = timedBrotherPose_.Get();//另一节x,y,theta log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " + std::to_string(cur_Bro_pose.theta()); 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]);//下发速度 log_inf_line += " "; log_inf_line += std::to_string(out[4]) + " " + std::to_string(out[5]);//下发速度 log_inf_line += " "; log_inf_line += std::to_string(Y1) + " " + std::to_string(Y2);//下发速度 log_inf_line += " "; auto cur_time = std::chrono::steady_clock::now(); auto duration = std::chrono::duration_cast(cur_time - beg_time); double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den; log_inf_line += std::to_string(time); ofs << log_inf_line << std::endl; //一行日志的内容 /* * 判断车辆是否在终点附近徘徊，无法到达终点 */ if (PossibleToTarget(node, directY) == false) { printf(" --------------MPC imposible to target -------------------------------------------\n"); if (fabs(timedV_.Get()) > 0.1 || fabs(timedA_.Get()) > 10 * M_PI / 180.0) SlowlyStop(); else Stop(); ofs.close();//关闭文件 return eImpossibleToTarget; } } ofs.close();//关闭文件 return eMpcFailed; } bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY) { if (timedPose_.timeout()) { return false; } Pose2d current = timedPose_.Get(); if (targetNode.id().find('S') != -1) { //车位点强制可到达 return true; } double tx = targetNode.x(); double ty = targetNode.y(); double l = fabs(targetNode.l()); double w = fabs(targetNode.w()); double theta = -targetNode.theta(); // double W = 2.45; // double L = 1.3; // double minYaw = 3 * M_PI / 180.0; // if (move_mode_ == eDouble) { // L = 1.3 + 2.7; // // } double W = 2.5; double L = 1.565; double minYaw = 5 * M_PI / 180.0; if (move_mode_ == eDouble) { L = 1.565 + 2.78; } if (directY) { current = current.rotate(current.x(), current.y(), M_PI / 2); double t = W; W = L; L = t; } double minR = W / 2 + L / tan(minYaw); //printf("l:%f,w:%f\n",l,w); std::vector poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l * 2, w * 2); bool nagY = false; bool posiY = false; for (int i = 0; i < poses.size(); ++i) { Pose2d rpos = poses[i].rotate(tx, ty, theta); Pose2d relative = Pose2d::relativePose(rpos, current); double ax = fabs(relative.x()); double ay = fabs(relative.y()); if (relative.y() > 0) posiY = true; if (relative.y() < 0) nagY = true; if (ax * ax + pow(ay - minR, 2) > minR * minR) { /*printf(" possible to target:%f %f l:%f,w:%f theta:%f minR:%f ax:%f,ay:%f\n", targetNode.x(),targetNode.y(),targetNode.l(),targetNode.w(),targetNode.theta(), minR,ax,ay);*/ return true; } else if (nagY && posiY) { // std::cout << "nagY && posiY" << std::endl; return true; } // printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f minR:%f ax:%f,ay:%f\n", directY, // relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(), // minR, ax, ay); } printf(" impossible to target:%f %f l:%f,w:%f theta:%f, ", targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.theta()); printf(" current:%f, %f, %f\n", current.x(), current.y(), current.theta()); return false; } void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) { if (inited_ == false) { response.set_ret(-1); response.set_info("navigation has not inited"); printf(" navigation has not inited\n"); return; } if (newUnfinished_cations_.empty() == false) //正在运行中，上一次指令未完成 { response.set_ret(-2); response.set_info("navigation is running pls cancel before"); printf(" navigation is running pls cancel before\n"); return; } //add 先检查当前点与起点的距离 global_navCmd_ = cmd; for (int i = 0; i < cmd.newactions_size(); ++i) newUnfinished_cations_.push(cmd.newactions(i)); pause_ = false; cancel_ = false; running_ = true; actionType_ = eReady; printf("Navigation beg...\n"); while (newUnfinished_cations_.empty() == false && cancel_ == false) { std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl; NavMessage::NewAction act = newUnfinished_cations_.front(); NavMessage::NewAction last_act = newUnfinished_cations_.back(); if (act.type() == 1) { //入库 space_id_ = act.spacenode().id(); obs_w_ = 1.1; obs_h_ = 1.87; if (!execute_InOut_space(act)) { printf(" In space failed\n"); isInSpace_ = false; break; } } else if (act.type() == 2) { //出库 obs_w_ = 1.25; obs_h_ = 1.87; if (!execute_InOut_space(act)) { printf(" out space failed\n"); break; } else { isInSpace_ = false; } } else if (act.type() == 3 || act.type() == 4) { //马路导航 obs_w_ = 1.1; obs_h_ = 1.87; int space_id = -1; if (last_act.type() == 1) { std::string id = last_act.spacenode().id().substr(1, last_act.spacenode().id().length() - 1); space_id = stoi(id); } if (!execute_nodes(act, space_id)) { printf(" street nav failed\n"); break; } } else if (act.type() == 5) { printf(" 汽车模型导航....\n"); } else if (act.type() == 6) {//夹持 if (this->clamp_close() == false) { printf("夹持failed ...\n"); break; } } else if (act.type() == 7) { if (this->clamp_fully_open() == false) { printf("打开夹持 failed...\n"); break; } } else if (act.type() == 8) { //切换模式 SwitchMode(act.changedmode(), act.wheelbase()); } else if (act.type() == 9) { //提升机构提升 if (this->lifter_rise() == false) { printf("提升failed ...\n"); break; } } else if (act.type() == 10) { //提升机构下降 if (this->lifter_down() == false) { printf("下降failed ...\n"); break; } } else { printf(" action type invalid not handled !!\n"); break; } newUnfinished_cations_.pop(); } actionType_ = eReady; Stop(); if (cancel_ == true) { response.set_ret(-3); response.set_info("navigation canceled"); printf(" navigation canceled\n"); while (newUnfinished_cations_.empty() == false) newUnfinished_cations_.pop(); } else { if (newUnfinished_cations_.empty()) { response.set_ret(0); response.set_info("navigation completed!!!"); printf("navigation completed!!!\n"); } else { response.set_ret(-4); response.set_info("navigation Failed!!!!"); printf(" navigation Failed\n"); while (newUnfinished_cations_.empty() == false) newUnfinished_cations_.pop(); } } running_ = false; } void Navigation::SwitchMode(int mode, float wheelBase) { wheelBase_ = wheelBase; printf("Child AGV can not Switch Mode\n"); } float Navigation::compute_cuv(const Pose2d &target) { float x = fabs(target.x()); float y = fabs(target.y()); float cuv = atan(y / (x + 1e-8)); //printf(" ---------------------- cuv：%f\n",cuv); return cuv; } void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response) { return; // pause_ = false; // cancel_ = false; // running_ = true; // actionType_ = eReady; // printf("ManualOperation: %d | start...\n", cmd.operation_type()); // // float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向 // double velocity; // std::this_thread::sleep_for(std::chrono::milliseconds(100)); // switch (cmd.operation_type()) { // case 1: // 旋转 // velocity = 2.0 * M_PI / 180.0 * symbol; // SendMoveCmd(move_mode_, eRotation, 0, velocity); // actionType_ = eRotation; // printf(" ManualOperation: %d | input angular_v: %f, down: %f\n", // cmd.operation_type(), timedA_.Get(), velocity); // break; // case 2: // X平移 // velocity = 1.0 * symbol; // SendMoveCmd(move_mode_, eVertical, velocity, 0); // actionType_ = eVertical; // printf(" ManualOperation: %d | input line_v : %f, down: %f\n", // cmd.operation_type(), timedV_.Get(), velocity); // break; // case 3: // Y平移 // velocity = 1.0 * symbol; // SendMoveCmd(move_mode_, eHorizontal, velocity, 0); // actionType_ = eHorizontal; // printf(" ManualOperation: %d | input line_v: %f, down: %f\n", // cmd.operation_type(), timedV_.Get(), velocity); // break; // default: // break; // } // // actionType_ = eReady; // response.set_ret(-3); // response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type()); // printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type()); // running_ = false; } bool Navigation::IsMasterAGV() { return is_master_AGV; } void Navigation::ResetDoor(int32_t status) { // printf("---------------receive status:%d\n",status); DoorStatusMap doorStatusMap; anasisDoorStatu(status, doorStatusMap); // for (auto it = doorStatusMap.begin() ; it != doorStatusMap.end() ; ++it) { // printf("doorID:%d, out:%d, ins:%d\n",it->first, it->second.first, it->second.second); // } timed_Door_.reset(doorStatusMap, 3); } void Navigation::anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status) { // int bytesize = sizeof(int32_t)*8; m_door_status.clear(); for (int i = 1; i >= 0; --i) { int statu = i_door_status>>(i*16); int door_id = (statu&0xFF00)>>8; int outside_door_status = statu&0x00FF; int inside_door_status = outside_door_status>>2; if ((outside_door_status & eDoorOpened) == eDoorOpened){ outside_door_status = eDoorOpened; } else if ((outside_door_status & eDoorClosed) == eDoorClosed){ outside_door_status = eDoorClosed; } else{ outside_door_status = eDoorInvalid; } if ((inside_door_status & eDoorOpened) == eDoorOpened){ inside_door_status = eDoorOpened; } else if ((inside_door_status & eDoorClosed) == eDoorClosed){ inside_door_status = eDoorClosed; } else{ inside_door_status = eDoorInvalid; } m_door_status[door_id] = DoorStatusInOnePort((DoorStatu)outside_door_status, (DoorStatu)inside_door_status); } } #define SWAP_BYTES 0 void Navigation::anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes){ int32_t reverseBytes=bytes; if(SWAP_BYTES){ reverseBytes = (bytes>>24)| ((bytes&0xFF0000)>>8)|((bytes&0xFF00)<<8)|((bytes&0xFF)<<24); } int bytesize=sizeof(int32_t)*8; regionID = (bytes>>((bytesize-1)*8)); for (int i = 0; i < (bytesize-8)/2; ++i) { int statu =(reverseBytes>>(i*2) & 0x03); if ((statu & eCarrierUp) == eCarrierUp) { status[i+1]=eCarrierUp; } else if ((statu & eCarrierDown) == eCarrierDown){ status[i+1]=eCarrierDown; } else { status[i+1]=eCarrierInvalid; } } } void Navigation::ResetCarrier(std::vector status) { for(int32_t statu : status){ int regionID; CarrierStatuRegionMap region; anasisCarierStatu(regionID,region,statu); timed_Carrier_[regionID].reset(region,5); } /*printf("---------------------------------------\n"); for(std::map>::iterator it=timed_Carrier_.begin(); it!=timed_Carrier_.end();it++){ int regionID=it->first; if(it->second.timeout()) continue; printf(">>>>>>> Region ID :%d\n",regionID); std::map region=it->second.Get(); for(std::map::iterator it1=region.begin(); it1!=region.end();++it1){ int id=it1->first; CarrierStatu sta=it1->second; printf("\t \tcarrier id :%d statu:%d\n",id,sta); } }*/ } void Navigation::Stringsplit(const std::string& str, const std::string splits, std::vector& res){ res.clear(); if(str.empty()) return; std::string strs = str+splits; size_t pos = strs.find(splits); int step = splits.size(); while (pos!= std::string::npos){ std::string temp = strs.substr(0, pos); res.push_back(temp); strs = strs.substr(pos+step,strs.size()); pos = strs.find(splits); } } bool Navigation::LoadSpaceNo2CarrierNo(){ std::string file_name = "../config/SpaceNO2CarrarierNO.txt"; std::ifstream in(file_name); std::string line; std::vector data_in_one_line; if (in){ short space_id, region_id, carrier_no; while(std::getline(in,line)){ Stringsplit(line, " ",data_in_one_line); space_id = std::stoi(data_in_one_line[0]); region_id = std::stoi(data_in_one_line[1]); carrier_no = std::stoi(data_in_one_line[2]); spaceNo_2_region_carrier_excel_[space_id] = Region_Carry(region_id,carrier_no); } return true; } else{ printf("no SpaceNO2CarrarierNO.txt in config!\n"); } return false; } bool Navigation::SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no) { Region_Carry region_carry = spaceNo_2_region_carrier_excel_[space_no]; region_id = region_carry.first; carrier_no = region_carry.second; return true; } Navigation::CarrierStatu Navigation::GetCarrierStatusBySpaceID(int space_id) { int region_id; int carrier_no; if (SpaceNo2CarrierNo(space_id,region_id,carrier_no) == false){ printf("no matched carrier!\n"); return eCarrierInvalid; } printf("space_id:%d, region_id:%d, carrier_no:%d\n",space_id,region_id,carrier_no); if (timed_Carrier_[region_id].timeout()){ printf("carrier status is timeout!\n"); return eCarrierInvalid; } else{ return timed_Carrier_[region_id].Get()[carrier_no]; } return eCarrierInvalid; } int Navigation::GetPortIDBySpace(NavMessage::PathNode node) { int port_id = GetSpaceId(node); if (port_id == 1101){ return 1; } else if (port_id == 1102){ return 2; } }