1.开启子车位置获取

2.修正能否到达目标点模型
3.扩大车位点，马路点精度设置

MPC/navigation.cpp

@@ -100,13 +100,13 @@ bool Navigation::Init(const NavParameter::Navigation_parameter &parameter) {
         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;
-//            }
+            if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
+                brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
+                                          this);
+            } else {
+                printf(" brother emqx connected failed\n");
+                // return false;
+            }
 //
         }
     }
@@ -546,7 +546,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
                    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",
+            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;
@@ -629,7 +629,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     target.set_y(y);
     target.set_theta(rotated.theta());
     target.set_l(0.05);
-    target.set_w(0.03);
+    target.set_w(0.10);
     target.set_id(space.id());
 
     //整车协调的时候，保持前后与车位一致即可
@@ -670,25 +670,6 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         }
         continue;
 
-
-//        if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-//            double theta = limit_gause(yawDiff, 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_, Monitor_emqx::eRotation, 0, limit_angular);
-//            actionType_ = eRotation;
-//            printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
-//                   timedA_.Get(), angular, limit_angular, yawDiff);
-//
-//            continue;
-//        } else {
-//            if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
-//                printf(" RotateBeforeEnterSpace refer target completed\n");
-//                printf("---------------- update new target :%f %f %f \n", target.x(), target.y(), target.theta());
-//                return true;
-//            }
-//        }
     }
     return eMpcFailed;
 
@@ -1132,12 +1113,18 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
     double w = fabs(targetNode.w());
     double theta = -targetNode.theta();
 
-    double W = 2.45;
-    double L = 1.3;
+//    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 = 3 * M_PI / 180.0;
     if (move_mode_ == eDouble) {
-        L = 1.3 + 2.7;
-
+        L = 1.565 + 2.78;
     }
     if (directY) {
         current = current.rotate(current.x(), current.y(), M_PI / 2);

MPC/navigation_main.cpp

@@ -205,7 +205,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     double dt = 0.1;
     Pose2d rotated = Pose2d(space.x(), space.y(), space.theta());
     target.set_l(0.05);
-    target.set_w(0.03);
+    target.set_w(0.10);
     target.set_id(space.id());
     //后车先到,当前车进入2点,保持与后车一致的朝向
     if (brother.in_space() && brother.space_id() == space.id()) {

MPC/navigation_main.h

@@ -28,6 +28,7 @@ public:
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
     virtual void SwitchMode(int mode, float wheelBase) {
+        printf("change mode to:%d\n", mode);
         if (move_mode_ == mode)
             return;
         if (mode == eDouble) {
@@ -59,7 +60,7 @@ public:
             else
                 mode = mode | eChildOppositeToMain;
 
-            printf(" Switch MoveMode --> main(%d)\n", mode);
+            printf(" Switch MoveMode --> main(%d), wheelBase: %f\n", mode, wheelBase);
         }
         wheelBase_ = wheelBase;
         move_mode_ = mode;