// // 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_->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 (msg.toProtoMessage(speed)) { ResetStatu(speed.v(), speed.w()); ResetClamp((ClampStatu) speed.clamp()); ResetLifter((LifterStatus) speed.lifter()); } } void Navigation::RobotSpeedCallback(const MqttMsg &msg, void *context) { Navigation *navigator = (Navigation *) context; navigator->HandleAgvStatu(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()); //printf(" timed_clamp_:%d\n ",timed_clamp_.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); } 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() { isInSpace_ = false; //是否在车位或者正在进入车位 RWheel_position_ = eUnknow; move_mode_ = eSingle; monitor_ = nullptr; terminator_ = nullptr; timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5); } 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; } 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]}; // if(out[0]>=0){ // w[0]=1.*M_PI/180.0; // w[1]=1.*M_PI/180.0; // w[2]=1.*M_PI/180.0; // }else{ // w[0]=-1.*M_PI/180.0; // w[1]=-1.*M_PI/180.0; // w[2]=-1.*M_PI/180.0; // } SendMoveCmd(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; // //下发速度 // if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) { // SendMoveCmd(move_mode_, eRotation, 0, out[0]); // actionType_ = eRotation; // printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n", // timedA_.Get(), out[0], target_yaw_diff, directions); // } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) { // printf(" MPC_rotate refer target completed,cuv:%f\n", target_yaw_diff); // return eMpcSuccess; // } // continue; } return eMpcFailed; } bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w, bool anyDirect, bool enable_rotate) { // LogWriter("MoveToTarget beg"); 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; } //巡线无法到达目标点 if (enableTotarget == false) { if (already_in_mpc && enable_rotate == false) { printf(" Move to node failed ,impossible to target and rotate is not enabled\n"); return false; } Pose2d target(node.x(), node.y(), 0); // if (RotateReferToTarget(target, limit_w, anyDirect) == 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); 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) { 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()}; 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())); node.set_l(0.05); node.set_w(0.10); } else { node.set_theta(0); node.set_l(0.03); node.set_w(0.10); } 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) == false) return false; else isInSpace_ = false; } return true; } printf("execute PathNode failed ,action type is not 3/4 :%d\n", action.type()); return false; } Navigation::MpcResult Navigation::RotateAfterOutSpace(){ 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 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(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) { // 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 rotated = timedPose_.Get(); double x = space.x(); double y = space.y(); //前车先到,后车进入2点,保持与前车一致的朝向 if (brother.in_space() && brother.space_id() == space.id()) { printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__); // rotated.mutable_theta() = brother.odom().theta(); Pose2d vec(x - rotated.x(), y - rotated.y(), 0); rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y()); if (move_mode_ == eSingle) { x -= wheelbase * cos(rotated.theta()); y -= wheelbase * sin(rotated.theta()); printf("确定车位点：eSingle\n"); } } else { //当后车先到,倒车入库 printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__); rotated.mutable_theta() = space.theta(); rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI); } target.set_x(x); target.set_y(y); target.set_theta(rotated.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 = timedPose_.Get(); double yawDiff = (rotated - 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.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(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::execute_InOut_space(NavMessage::NewAction action) { // LogWriter("execute_InOut_space beg"); 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) { // LogWriter("In space beg-"); Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(), 0); action.mutable_streetnode()->set_l(0.05); action.mutable_streetnode()->set_w(0.05); action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y())); if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) { printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type()); return false; } //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false） printf("inout ----------------------------------------\n"); //计算车位朝向 action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y())); //计算当前车是进入车位1点还是2点 NavMessage::PathNode new_target; if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) { return false; } std::string id = action.spacenode().id().substr(1,action.spacenode().id().length()-1); int space_id = stoi(id); if (space_id>99 && space_id< 1000){ //进载车板库前提升机构上升 if (lifter_rise() == false){ printf(" Enter space | lifter rise failed. line:%d\n",__LINE__); } } else{ //否则下降 if (lifter_down() == false){ printf(" Enter space | lifter down failed. line:%d\n",__LINE__); } } if (space_id == 1101){ //单车进入口搬车，夹持杆打开到全开位 } // // //移动到预入库点位，较高精度 // NavMessage::PathNode pre_target; // bool jump_preenter = false; // if (move_mode_ == eSingle){ // double x = action.spacenode().x(); // double y = action.spacenode().y(); // double theta = action.spacenode().theta(); // // double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离） // Pose2d current = timedPose_.Get(); // double diff = Pose2d::distance(current, Pose2d(x,y,theta)); // if (diff < back_distance) jump_preenter = true; // x -= back_distance* cos(theta); // y -= back_distance* sin(theta); // // pre_target.set_x(x); // pre_target.set_y(y); // pre_target.set_theta(theta); // pre_target.set_l(0.10); // pre_target.set_w(0.05); // pre_target.set_id("R_0"); // } // else if (move_mode_ == eDouble){ // double x = action.spacenode().x(); // double y = action.spacenode().y(); // double theta = action.spacenode().theta(); // // double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离） // Pose2d current = timedPose_.Get(); // double diff = Pose2d::distance(current, Pose2d(x,y,theta)); // if (diff < back_distance) jump_preenter = true; // x -= back_distance* cos(theta); // y -= back_distance* sin(theta); // // pre_target.set_x(x); // pre_target.set_y(y); // pre_target.set_theta(theta); // pre_target.set_l(0.05); // pre_target.set_w(0.03); // pre_target.set_id("R_0"); // } // else{ return printf("PreEnter space failed. move_mode_ error!. line: %d\n", __LINE__);} // if (!jump_preenter) { // printf("PreEnter space beg...\npre_target: x:%f, y:%f, theta:%f\n",pre_target.x(),pre_target.y(),pre_target.theta()); // if (MoveToTarget(pre_target, limit_v, limit_w, true, true) == false) { // printf(" PreEnter pre_space:%s failed. type:%d. line:%d\n", pre_target.id().data(), action.type(), // __LINE__); // return false; // } // } //入库 printf("Enter space beg...\n"); if (MoveToTarget(new_target, limit_v, limit_w, true, false) == 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.04); action.mutable_streetnode()->set_w(0.06); 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; } //出库后提升机构下降 if (lifter_down() == false){ printf(" Out space | lifter down failed. line:%d\n",__LINE__); } // if(RotateAfterOutSpace()!=eMpcSuccess) { // printf(" after out space rotate Failed!!!\n"); // return false; // } // LogWriter("Out space end-"); } else { std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff << std::endl; 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 =1.5;// 0.95;//0.85; double obs_h =3.0;// 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 = 3.0;// 0.95;//0.85; obs_h = 1.5;//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:"<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 mode, ActionType type, double v[], double angular[]) { if (monitor_) { monitor_->set_ToAgvCmd(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) { if (monitor_) { monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance); 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_open() { if (monitor_) { printf("Clamp openning...\n"); monitor_->clamp_open(move_mode_); actionType_ = eClampOpen; while (cancel_ == false) { if (timed_clamp_.timeout()) { printf("timed clamp is timeout\n"); return false; } if (timed_clamp_.Get() == eOpened) return true; std::this_thread::sleep_for(std::chrono::milliseconds(100)); monitor_->clamp_open(move_mode_); actionType_ = eClampOpen; } return false; } return false; } bool Navigation::clamp_fully_open(){ if (monitor_){ 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"); monitor_->lifter_rise(move_mode_); actionType_ = eLifterRise; while (cancel_ == false) { if (timed_lifter_.timeout()) { printf("timed lifter is timeout\n"); 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"); monitor_->lifter_down(move_mode_); actionType_ = eLifterDown; while (cancel_ == false) { if (timed_lifter_.timeout()) { printf("timed lifter is timeout\n"); 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; } Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) { // LogWriter("MpcToTarget beg"); 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; } } 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();//关闭文件 return eMpcSuccess; } } //一次变速 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) { Stop(); 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 diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]}; if (directY == false) SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance); else SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance); 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 += " "; 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(); if (act.type() == 1) { //入库 space_id_ = act.spacenode().id(); isInSpace_ = true; if (!execute_InOut_space(act)) { printf(" In space failed\n"); isInSpace_ = false; break; } } else if (act.type() == 2) { //出库 if (!execute_InOut_space(act)) { printf(" out space failed\n"); break; } else { isInSpace_ = false; } } else if (act.type() == 3 || act.type() == 4) { //马路导航 if (!execute_nodes(act)){ 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_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) { 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; }