|
|
@@ -67,10 +67,10 @@ double next_speed(double speed,double target_speed,double acc,double dt=0.1)
|
|
|
|
|
|
|
|
|
|
|
|
-bool Navigation::Init(const Navigation_parameter& parameter)
|
|
|
+bool Navigation::Init(const NavParameter::Navigation_parameter& parameter)
|
|
|
{
|
|
|
parameter_=parameter;
|
|
|
- AgvEmqx_parameter agv_p=parameter.agv_emqx();
|
|
|
+ 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)
|
|
|
@@ -83,7 +83,7 @@ bool Navigation::Init(const Navigation_parameter& parameter)
|
|
|
monitor_->AddCallback(agv_p.subspeedtopic(),Navigation::RobotSpeedCallback,this);
|
|
|
}
|
|
|
|
|
|
- Emqx_parameter terminal_p=parameter.terminal_emqx();
|
|
|
+ NavParameter::Emqx_parameter terminal_p=parameter.terminal_emqx();
|
|
|
if(terminator_== nullptr) {
|
|
|
terminator_ = new Terminator_emqx(terminal_p.nodeid());
|
|
|
|
|
|
@@ -98,7 +98,7 @@ bool Navigation::Init(const Navigation_parameter& parameter)
|
|
|
}
|
|
|
|
|
|
if(parameter.has_brother_emqx()) {
|
|
|
- BrotherEmqx brotherEmqx = parameter.brother_emqx();
|
|
|
+ NavParameter::BrotherEmqx brotherEmqx = parameter.brother_emqx();
|
|
|
if (brotherEmqx_ == nullptr) {
|
|
|
brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());
|
|
|
|
|
|
@@ -557,71 +557,103 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
-bool Navigation::execute_InOut_space(NavMessage::NewAction action)
|
|
|
-{
|
|
|
- if(action.type()!=1 && action.type()!=2){
|
|
|
+bool Navigation::execute_vehicle_mode(NavMessage::NewAction action){
|
|
|
+ if(action.type()==5 ) //最优动作导航 or 导航中保证朝向严格一致
|
|
|
+ {
|
|
|
+ if (!action.has_nodevelocitylimit() || !action.has_nodeangularlimit() ||
|
|
|
+ !action.has_adjustvelocitylimit() || !action.has_adjusthorizonlimit()
|
|
|
+ || action.pathnodes_size() == 0) {
|
|
|
+ std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
|
|
|
+ return false;
|
|
|
+
|
|
|
+ //原地调整起点
|
|
|
+ NavMessage::PathNode first=action.pathnodes(0);
|
|
|
+ //速度限制
|
|
|
+ stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
|
|
|
+ stLimit adjust_h={ action.adjusthorizonlimit().min(),action.adjusthorizonlimit().max() };
|
|
|
+ stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
|
|
|
+ stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
|
|
|
+ //目标，精度
|
|
|
+ double first_yaw=timedPose_.Get().theta();
|
|
|
+ if(1<action.pathnodes_size()){
|
|
|
+ NavMessage::PathNode next=action.pathnodes(1);
|
|
|
+ first_yaw=Pose2d::vector2yaw(next.pose().x()-first.pose().x(),next.pose().y()-first.pose().y());
|
|
|
+ }
|
|
|
+ Pose2d first_target(first.pose().x(),first.pose().y(),first_yaw);
|
|
|
+ Pose2d first_accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
|
|
|
+ //execute_adjust_action(first_target,first_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+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(!action.has_inoutvlimit() || !action.has_spacenode()||
|
|
|
- !action.has_streetnode()) {
|
|
|
+ if (!action.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(timedPose_.timeout()==true){
|
|
|
- printf(" inout_space failed type:%d : current pose is timeout\n",action.type());
|
|
|
+ if (timedPose_.timeout() == true) {
|
|
|
+ printf(" inout_space failed type:%d : current pose is timeout\n", action.type());
|
|
|
return false;
|
|
|
}
|
|
|
- Pose2d begin,target,begin_accuracy,target_accuracy;
|
|
|
- if(action.type()==1) //入库,起点为streetNode
|
|
|
- {
|
|
|
- begin = Pose2d(action.streetnode().pose().x(), action.streetnode().pose().y(), action.streetnode().pose().theta());
|
|
|
- begin_accuracy=Pose2d(action.streetnode().accuracy().x(),
|
|
|
- action.streetnode().accuracy().y(),
|
|
|
- action.streetnode().accuracy().theta());
|
|
|
- target = Pose2d(action.spacenode().pose().x(), action.spacenode().pose().y(), action.spacenode().pose().theta());
|
|
|
- //目标点精度
|
|
|
- target_accuracy=Pose2d(action.spacenode().accuracy().x(),
|
|
|
- action.spacenode().accuracy().y(),
|
|
|
- action.spacenode().accuracy().theta());
|
|
|
- }else{ //出库起点终点对调
|
|
|
- Pose2d mid=target;
|
|
|
- Pose2d mid_accuracy=target_accuracy;
|
|
|
- target=begin;
|
|
|
- target_accuracy=begin_accuracy;
|
|
|
- begin=mid;
|
|
|
- begin_accuracy=mid_accuracy;
|
|
|
+ Pose2d begin, target, begin_accuracy, target_accuracy;
|
|
|
+ //入库,起点为streetNode
|
|
|
+
|
|
|
+ begin = Pose2d(action.streetnode().pose().x(), action.streetnode().pose().y(), action.streetnode().pose().theta());
|
|
|
+ begin_accuracy = Pose2d(action.streetnode().accuracy().x(),
|
|
|
+ action.streetnode().accuracy().y(),
|
|
|
+ action.streetnode().accuracy().theta());
|
|
|
+ target = Pose2d(action.spacenode().pose().x(), action.spacenode().pose().y(), action.spacenode().pose().theta());
|
|
|
+ //目标点精度
|
|
|
+ target_accuracy = Pose2d(action.spacenode().accuracy().x(),
|
|
|
+ action.spacenode().accuracy().y(),
|
|
|
+ action.spacenode().accuracy().theta());
|
|
|
+ double yaw = Pose2d::vector2yaw(target.x() - begin.x(), target.y() - begin.y());
|
|
|
+ if (action.type() == 2) { //出库起点终点对调
|
|
|
+ Pose2d mid = target;
|
|
|
+ Pose2d mid_accuracy = target_accuracy;
|
|
|
+ target = begin;
|
|
|
+ target_accuracy = begin_accuracy;
|
|
|
+ begin = mid;
|
|
|
+ begin_accuracy = mid_accuracy;
|
|
|
+ yaw = Pose2d::vector2yaw(begin.x() - target.x(), begin.y() - target.y());
|
|
|
}
|
|
|
//给起点赋予朝向,保证agv以正对车位的方向行进
|
|
|
- double yaw=Pose2d::vector2yaw(target.x()-begin.x(),target.y()-begin.y());
|
|
|
- if(action.type()==2) //出库
|
|
|
- yaw=Pose2d::vector2yaw(begin.x()-target.x(),begin.y()-target.y());
|
|
|
- begin.mutable_theta()=yaw;
|
|
|
+
|
|
|
+ begin.mutable_theta() = yaw;
|
|
|
|
|
|
//严格调整到起点
|
|
|
- stLimit adjust_x={ 0.05,0.2};
|
|
|
- stLimit adjust_h={ 0.05,0.2};
|
|
|
- stLimit adjust_angular={ 2*M_PI/180.0, 20*M_PI/180.0 };
|
|
|
- stLimit mpc_velocity={0.05,0.3 };
|
|
|
+ stLimit adjust_x = {0.05, 0.2};
|
|
|
+ stLimit adjust_h = {0.05, 0.2};
|
|
|
+ stLimit adjust_angular = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
|
|
|
+ stLimit mpc_velocity = {0.05, 0.3};
|
|
|
|
|
|
//判断当前点与起点的距离
|
|
|
- Pose2d distance(0.5,0.5,M_PI*2);
|
|
|
- if(action.type()==2) { //出库要求起点距离当前点非常近
|
|
|
- distance = Pose2d(0.05, 0.05, 1 * M_PI / 180.0);
|
|
|
- adjust_x={ 0.03,0.1};
|
|
|
- adjust_h={ 0.03,0.1};
|
|
|
- adjust_angular={ 2*M_PI/180.0, 5*M_PI/180.0 };
|
|
|
- }
|
|
|
- if( !(Pose2d::abs(begin-timedPose_.Get())<distance) ){
|
|
|
- printf(" Inout space failed type:%d: current pose too far from begin node\n",action.type());
|
|
|
+ Pose2d distance(0.5, 0.5, M_PI * 2);
|
|
|
+ if (action.type() == 2) { //出库要求起点距离当前点非常近
|
|
|
+ distance = Pose2d(0.05, 0.05, 2 * M_PI);
|
|
|
+ adjust_x = {0.03, 0.1};
|
|
|
+ adjust_h = {0.03, 0.1};
|
|
|
+ adjust_angular = {2 * M_PI / 180.0, 5 * M_PI / 180.0};
|
|
|
+ }
|
|
|
+ if (!(Pose2d::abs(begin - timedPose_.Get()) < distance)) {
|
|
|
+ printf(" Inout space failed type:%d: current pose too far from begin node\n", action.type());
|
|
|
+ std::cout << " begin：" << begin << " current:" << timedPose_.Get() << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- if(execute_adjust_action(begin,begin_accuracy,adjust_x,adjust_h,adjust_angular,false))
|
|
|
+ if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
|
|
|
return false;
|
|
|
|
|
|
if (execute_along_action(begin, target, target_accuracy, mpc_velocity, false) == false)
|
|
|
- return false;
|
|
|
+ return false;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
@@ -655,18 +687,22 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
|
|
|
|
|
|
Trajectory optimize_trajectory;
|
|
|
Trajectory selected_trajectory;
|
|
|
-
|
|
|
+ NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
|
|
|
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();
|
|
|
}
|
|
|
bool ret = false;
|
|
|
LoadedMPC MPC(relative, limit_v.min, limit_v.max);
|
|
|
- float r=20.0*1.3; //最小转弯半径
|
|
|
- if(directY) r=3*2.45;
|
|
|
- ret = MPC.solve(traj, traj[traj.size() - 1], statu,r, out, selected_trajectory, optimize_trajectory);
|
|
|
+ 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);
|
|
|
|
|
|
//std::cout<<" traj size %d %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
|
|
|
predict_traj_.reset(optimize_trajectory, 1);
|
|
|
@@ -681,14 +717,14 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
|
|
|
Pose2d distance=Pose2d::relativePose(target,cur);
|
|
|
Pose2d absDiff=Pose2d::abs(distance);
|
|
|
|
|
|
- std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
|
|
|
+ //std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
|
|
|
if(direct==1){ //纵向MPC
|
|
|
if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
|
|
|
{
|
|
|
float diffYaw=absDiff.theta(); //[0-pi)
|
|
|
float diffYaw2=absDiff.theta()-M_PI;
|
|
|
bool yawArrived=( fabs(diffYaw)<diff.theta() || fabs(diffYaw2)<diff.theta() );
|
|
|
- if(yawArrived && fabs(velocity) < 0.03 && fabs(angular) < 5 * M_PI / 180.0)
|
|
|
+ if(yawArrived && fabs(velocity) < 0.05 && fabs(angular) < 5 * M_PI / 180.0)
|
|
|
return true;
|
|
|
}
|
|
|
}else if(direct==2){ //横向MPC
|
