1.进库下发指令时，添加车位号与距目标距离信息

MPC/monitor/monitor_emqx.cpp

@@ -135,5 +135,30 @@ void Monitor_emqx::stop()
   Publish(speedcmd_topic_,msg);
 }
 
+void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v, double w, double L, int P, double D) {
+//    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
+//    {
+//        printf(" Main agv mpc must set wheelBase\n ");
+//        return;
+//    }
+    double w_correct=fabs(w)>0.001?w:0.0;
+    double velocity=fabs(v)>0.001?v:0;
+    MqttMsg msg;
+    NavMessage::ToAgvCmd agvCmd;
+    heat_=(heat_+1)%255;
+    agvCmd.set_h(heat_);
+    agvCmd.set_m(mode);
+    agvCmd.set_t(type);
+    agvCmd.set_v(velocity);
+    agvCmd.set_w(w_correct);
+    agvCmd.set_l(L);
+    agvCmd.set_p(P);
+    agvCmd.set_d(D);
+
+    agvCmd.set_end(1);
+    msg.fromProtoMessage(agvCmd);
+    Publish(speedcmd_topic_,msg);
+}
+
 
 

MPC/monitor/monitor_emqx.h

@@ -17,6 +17,7 @@ class Monitor_emqx : public Terminator_emqx
     void set_speedcmd_topic(std::string speedcmd);
     void set_speed(int mode,ActionType type,double v,double a);
     void set_speed(int mode,ActionType type,double v,double a,double L);
+    void set_ToAgvCmd(int mode,ActionType type,double v,double w,double L=0,int P=0,double D=0);
     void clamp_close(int mode);
     void clamp_open(int mode);
     void lifter_rise(int mode);

MPC/navigation.cpp

@@ -919,6 +919,18 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
+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;
+    }
+}
+
 bool Navigation::clamp_close() {
     if (monitor_) {
         printf("Clamp closing...\n");
@@ -1085,10 +1097,17 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }
         //下发速度
         //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')){
+            std::string id = node.id().substr(1,node.id().length()-1);
+            space_id = stoi(id);
+        }
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, out[0], out[1]);
+            SendMoveCmd(move_mode_, eVertical, out[0], out[1], space_id, distance);
         else
-            SendMoveCmd(move_mode_, eHorizontal, out[0], out[1]);
+            SendMoveCmd(move_mode_, eHorizontal, out[0], out[1], space_id, distance);
         actionType_ = directY ? eHorizontal : eVertical;
         /*
          * 判断车辆是否在终点附近徘徊，无法到达终点

MPC/navigation.h

@@ -89,6 +89,8 @@ protected:
 
     virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);
 
+    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance);
+
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
 
     static void RobotSpeedCallback(const MqttMsg &msg, void *context);

message.proto

@@ -26,7 +26,9 @@ message ToAgvCmd {
     float V = 4;  //角速度
     float W = 5;  //线速度
     float L = 6;  //轴距
-    int32 end = 7;
+    int32 P = 7; //车位编号
+    float D = 8; //距目标点距离
+    int32 end = 9;
 }
 
 message Pose2d

parameter.proto

@@ -59,3 +59,7 @@ message Navigation_parameter
   string rpc_ipport=10;
 }
 
+message Version
+{
+  string version=1;
+}