1.下发大小yaw

2.缩小出入库防撞距离
3.整车模式下，主车位姿视为整车位姿

MPC/monitor/monitor_emqx.cpp

@@ -178,6 +178,10 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
         w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
         velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
     }
+
+    double yaw1 =fabs(Y1)>0.0001?Y1:0.0;
+    double yaw2 =fabs(Y2)>0.0001?Y2:0.0;
+
     MqttMsg msg;
     NavMessage::ToAgvCmd agvCmd;
     heat_=(heat_+1)%255;
@@ -190,8 +194,8 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     agvCmd.set_w2(w_correct[1]);
     agvCmd.set_v3(velocity[2]);
     agvCmd.set_w3(w_correct[2]);
-    agvCmd.set_y1(Y1);
-    agvCmd.set_y2(Y2);
+    agvCmd.set_y1(yaw1);
+    agvCmd.set_y2(yaw2);
     agvCmd.set_l1(L);
     agvCmd.set_p1(P);
     agvCmd.set_d1(D);

MPC/navigation.cpp

@@ -428,6 +428,7 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
 
 
 bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate) {
+    std::cout<<"Adjust to : "<<target<<std::endl;
     if (inited_ == false) {
         printf(" MPC_rotate has not inited\n");
         return false;
@@ -442,8 +443,8 @@ bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate)
     yawTarget.set_x(current.x() + 100.);
     yawTarget.set_y(current.y());
     int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+    std::cout<<"Adjust RotateRefer : "<<Pose2d(yawTarget.x(),yawTarget.y(),0)<<std::endl;
     RotateReferToTarget(yawTarget, limit_rotate, directions);
-    printf(" exec MPC_rotate autoDirect:%d\n", directions);
 
     current = timedPose_.Get();
     Pose2d diff=target-current;
@@ -463,6 +464,7 @@ bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate)
 
     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);
 
 }
@@ -1507,7 +1509,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }
         double Y1=0.0,Y2=0.0;
         if(move_mode_==eDouble){
-            Y1=timeYawDiff1_.Get();
+            double dy=-timeYawDiff1_.Get();
+            Y1=0.2*(1.0/(1+exp(-16.*dy))-0.5)*1.0;
             Y2=timeYawDiff2_.Get();
         }
         if (directY == false)
@@ -1540,6 +1543,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         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<std::chrono::microseconds>(cur_time - beg_time);
@@ -1670,7 +1675,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
         NavMessage::NewAction last_act = newUnfinished_cations_.back();
         if (act.type() == 1) {  //入库
             space_id_ = act.spacenode().id();
-            obs_w_ = 1.25;
+            obs_w_ = 1.1;
             obs_h_ = 1.87;
             if (!execute_InOut_space(act)) {
                 printf(" In space failed\n");
@@ -1687,7 +1692,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 isInSpace_ = false;
             }
         } else if (act.type() == 3 || act.type() == 4) { //马路导航
-            obs_w_ = 1.25;
+            obs_w_ = 1.1;
             obs_h_ = 1.87;
             int space_id = -1;
             if (last_act.type() == 1) {

MPC/navigation_main.cpp

@@ -32,13 +32,17 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         }
         Pose2d abs_diff = pose - brother;
         //计算两车朝向的方向
-        float theta = Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
+        float theta = pose.theta();//Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
         Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
         Pose2d abs_diff1 = pose - agv;
         Pose2d abs_diff2 = brother - agv;
-        timeYawDiff1_.reset(abs_diff1.theta());
-        timeYawDiff2_.reset(abs_diff2.theta());
-        Navigation::ResetPose(agv);
+        timeYawDiff1_.reset(pose.theta());
+
+        timeYawDiff2_.reset((brother-pose).theta());
+        Pose2d brotherInPose=Pose2d::relativePose(brother,pose);
+        timeYawDiff1_.reset(brotherInPose.y());
+        //timeYawDiff2_.reset(abs_diff2.theta());
+        Navigation::ResetPose(pose);
 //        timedPose_().m_diffYaw1=pose.theta()-theta;
 //        timedPose_().m_diffYaw2=brother.theta()-theta;
 //        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);