修改导航动作流程，原地调整与mpc巡线

CMakeLists.txt

@@ -71,7 +71,7 @@ add_executable(${PROJECT_NAME}
 		tool/pathcreator.cpp
 		tool/singleton.cpp
 		tool/proto_tool.cpp
-		)
+       )
 
 #---------------------------------------------------------------------------------------
 # link libraries ${Boost_LIBRARY} -lpthread

MPC/monitor/emqx/mqttmsg.cpp

@@ -57,7 +57,7 @@ MqttMsg::~MqttMsg(){
   length_=0;
 }
 
-
+/*
 void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
 {
   std::lock_guard<std::mutex> lock(mutex_);
@@ -117,19 +117,16 @@ bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vt
   return ret.ok();
 }
 
-void MqttMsg::fromSpeed(double v, double vth)
+void MqttMsg::fromNavSpeed(const NavMessage::Speed& speed)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_)
     free(data_);
-  ChangeSpeedCmd cmd;
-  cmd.set_v(v);
-  cmd.set_vth(vth);
 
   google::protobuf::util::JsonPrintOptions option;
   option.always_print_primitive_fields=true;
   std::string json_str;
-  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);
+  google::protobuf::util::MessageToJsonString(speed,&json_str,option);
 
   length_=json_str.size();
   data_=(char*)malloc(length_);
@@ -137,13 +134,13 @@ void MqttMsg::fromSpeed(double v, double vth)
 
 }
 
-bool MqttMsg::toSpeed(double &v, double &vth)
+bool MqttMsg::toNavSpeed(NavMessage::Speed& speed)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_== nullptr)
     return false;
 
-  ChangeSpeedCmd cmd;
+
   util::Status ret;
   char* buf=(char*)malloc(length_+1);
   memset(buf,0,length_+1);
@@ -152,8 +149,8 @@ bool MqttMsg::toSpeed(double &v, double &vth)
   {
 
     //printf("%s\n",buf);
-    ret = util::JsonStringToMessage(buf, &cmd);
-    printf("change speed v:%f,  vth:%f\n",v,vth);
+    ret = util::JsonStringToMessage(buf, &speed);
+    printf("change speed v:%f,  vth:%f\n",speed.v(),speed.vth());
 
   }
   catch (std::exception &e)
@@ -162,17 +159,11 @@ bool MqttMsg::toSpeed(double &v, double &vth)
   }
 
   free(buf);
-  if(ret.ok())
-  {
-    v=cmd.v();
-    vth=cmd.vth();
-
-  }
 
   return ret.ok();
 }
 
-void MqttMsg::fromNavCmd(const NavCmd& cmd)
+void MqttMsg::fromNavCmd(const NavMessage::NavCmd& cmd)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_)
@@ -189,7 +180,7 @@ void MqttMsg::fromNavCmd(const NavCmd& cmd)
   memcpy(data_,json_str.c_str(),length_);
 }
 
-bool MqttMsg::toNavCmd(NavCmd& cmd)
+bool MqttMsg::toNavCmd(NavMessage::NavCmd& cmd)
 {
   if(strlen(data())<length_)
     return false;
@@ -215,7 +206,7 @@ bool MqttMsg::toNavCmd(NavCmd& cmd)
   return ret.ok();
 }
 
-void MqttMsg::fromNavStatu(const NavStatu &statu)
+void MqttMsg::fromNavStatu(const NavMessage::NavStatu &statu)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_)
@@ -232,7 +223,7 @@ void MqttMsg::fromNavStatu(const NavStatu &statu)
   memcpy(data_,json_str.c_str(),length_);
 }
 
-bool MqttMsg::toNavStatu(NavStatu &statu)
+bool MqttMsg::toNavStatu(NavMessage::NavStatu &statu)
 {
   if(strlen(data())<length_)
     return false;
@@ -257,3 +248,42 @@ bool MqttMsg::toNavStatu(NavStatu &statu)
 
   return ret.ok();
 }
+*/
+
+void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(message,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+}
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &message);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+  return ret.ok();
+}

MPC/monitor/emqx/mqttmsg.h

@@ -29,15 +29,19 @@ class MqttMsg
     char* data()const;
     int length()const;
 
-    void fromStatu(double x,double y,double theta,double v,double vth);
-    void fromSpeed(double v,double vth);
-    void fromNavCmd(const NavCmd& cmd);
-    void fromNavStatu(const NavStatu& statu);
+
+    void fromProtoMessage(const google::protobuf::Message& messge);
+    bool toProtoMessage(google::protobuf::Message& message);
+
+    /*void fromStatu(double x,double y,double theta,double v,double vth);
+    void fromNavSpeed(const NavMessage::Speed& speed);
+    void fromNavCmd(const NavMessage::NavCmd& cmd);
+    void fromNavStatu(const NavMessage::NavStatu& statu);
 
     bool toStatu(double& x,double& y,double& theta,double& v,double& vth);
-    bool toSpeed(double& v,double& vth);
-    bool toNavCmd(NavCmd& cmd);
-    bool toNavStatu(NavStatu& statu);
+    bool toNavSpeed(NavMessage::Speed& speed);
+    bool toNavCmd(NavMessage::NavCmd& cmd);
+    bool toNavStatu(NavMessage::NavStatu& statu);*/
 
 
  protected:

MPC/monitor/monitor_emqx.cpp

@@ -38,28 +38,28 @@ void Monitor_emqx::set_statu_arrived_callback(StatuCallback callback,void* conte
   StatuArrivedCallback_=callback;
   context_=context;
 }
-void Monitor_emqx::set_speed(double v,double a)
+void Monitor_emqx::set_speed(SpeedType type,double v,double a)
 {
   MqttMsg msg;
-  msg.fromSpeed(v,a);
+  NavMessage::Speed speed;
+  speed.set_type(type);
+  speed.set_v(v);
+  speed.set_vth(a);
+  msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);
   else
     printf("set speed failed : emqx client disconnected...\n");
 }
-void Monitor_emqx::rotate(double v)
-{
-  MqttMsg msg;
-  msg.fromSpeed(0,v);
-  if(client_)
-    client_->publish(pubTopic_,1,msg);
-  else
-    printf("rotate failed : emqx client disconnected...\n");
-}
+
 void Monitor_emqx::stop()
 {
   MqttMsg msg;
-  msg.fromSpeed(0,0);
+  NavMessage::Speed speed;
+  speed.set_type(eStop);
+  speed.set_v(0);
+  speed.set_vth(0);
+  msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);
   else
@@ -76,11 +76,11 @@ Monitor_emqx::Monitor_emqx(std::string nodeId,std::string pubTopic,std::string s
 void Monitor_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
   Monitor_emqx* monitor=(Monitor_emqx*)context;
-  double x=0,y=0,theta=0,v=0,vth=0;
+  NavMessage::AGVStatu statu;
 
-  if(msg.toStatu(x,y,theta,v,vth))
+  if(msg.toProtoMessage(statu))
   {
     if (monitor->StatuArrivedCallback_)
-      monitor->StatuArrivedCallback_(x, y, theta, v, vth,monitor->context_);
+      monitor->StatuArrivedCallback_(statu.x(), statu.y(), statu.theta(), statu.v(), statu.vth(),monitor->context_);
   }
 }

MPC/monitor/monitor_emqx.h

@@ -12,13 +12,22 @@ typedef void (*StatuCallback)(double x,double y,double theta,double v,double vth
 
 class Monitor_emqx
 {
+public:
+    enum SpeedType{
+        eRotate=0,
+        eHorizontal,
+        evVrtical,
+        eMPC,
+
+
+        eStop
+    };
  public:
     Monitor_emqx(std::string nodeId,std::string pubTopic,std::string subTopic);
     ~Monitor_emqx();
     bool Connect(std::string ip,int port);
     void set_statu_arrived_callback(StatuCallback callback,void* context);
-    void set_speed(double v,double a);
-    void rotate(double v);
+    void set_speed(SpeedType type,double v,double a);
     void stop();
 
  protected:

MPC/navigation.cpp

@@ -8,6 +8,7 @@
 
 Navigation::~Navigation()
 {
+  exit_=true;
   if(terminator_)
     delete terminator_;
   if(monitor_)
@@ -18,6 +19,22 @@ Navigation::~Navigation()
       thread_->join();
     delete thread_;
   }
+  if(pubthread_!= nullptr)
+  {
+    if(pubthread_->joinable())
+      pubthread_->join();
+    delete pubthread_;
+  }
+}
+
+/*
+ * 高斯函数压缩，x=3
+ */
+double limit(double x,double min,double max){
+  double r=x>=0?1.0:-1.0;
+  double delta=max/3.0;
+  return (max-(max-min)*exp(-x*x/(delta*delta)))*r;
+
 }
 
 bool Navigation::Init(const Navigation_parameter& parameter)
@@ -44,58 +61,85 @@ bool Navigation::Init(const Navigation_parameter& parameter)
     }
   }
   inited_=true;
+
+  if(pubthread_!= nullptr)
+  {
+    exit_=true;
+    if(pubthread_->joinable())
+      pubthread_->join();
+    delete pubthread_;
+  }
+  exit_=false;
+  pubthread_=new std::thread(&Navigation::publish_statu,this);
+
   return true;
 
 }
 
-void Navigation::publish_statu(ActionType type,float velocity,float angular)
+void Navigation::publish_statu()
 {
-  NavStatu statu;
-  if(running_)
-  {
-    statu.set_action_type(type);
-    statu.set_isrunning(running_);
-    std::queue<Trajectory> trajectorys_=global_trajectorys_;
-    while(trajectorys_.empty()==false)
-    {
-      Trajectory traj=trajectorys_.front();
-      TrajectoryMsg* trajMsg=statu.add_trajs();
-      for(int i=0;i<traj.size();++i)
-      {
-        Pose2dMsg* pose=trajMsg->add_points();
-        pose->set_x(traj[i].x());
-        pose->set_y(traj[i].y());
-        pose->set_theta(traj[i].theta());
+  while(exit_==false) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    NavMessage::NavStatu statu;
+    statu.set_statu(eReady); //默认ready
+    NavMessage::Action *current_action = nullptr;
+    if (running_) {
+      statu.set_statu(eRunning);
+      statu.set_key(global_navCmd_.key());
+      std::queue<NavMessage::Action> actios = unfinished_cations_;
+      while (actios.empty() == false) {
+        //保存当前动作
+        NavMessage::Action *act = statu.add_unfinished_actions();
+        current_action = new NavMessage::Action(*act);
+
+        act->CopyFrom(actios.front());
+        actios.pop();
+
       }
-      trajectorys_.pop();
-    }
-    statu.set_velocity(velocity);
-    statu.set_angular(angular);
-    if(type==eMPC)
-    {
-      //发布mpc 预选点
-      if(selected_traj_.timeout()==false) {
-        for (int i = 0; i < selected_traj_.Get().size(); ++i){
-          Pose2d pt=selected_traj_.Get()[i];
-          Pose2dMsg* pose=statu.mutable_select_traj()->add_points();
-          pose->set_x(pt.x());
-          pose->set_y(pt.y());
-          pose->set_theta(pt.theta());
-        }
+
+      statu.set_pose_timeout(true);    //默认设置位姿超时
+      statu.set_twist_timeout(true);
+      NavMessage::AGVStatu agvStatu;
+      if (timedPose_.timeout() == false) {
+        statu.set_pose_timeout(false);
+        Pose2d pose = timedPose_.Get();
+        agvStatu.set_x(pose.x());
+        agvStatu.set_y(pose.y());
+        agvStatu.set_theta(pose.theta());
+      }
+      if (timedV_.timeout() == false && timedA_.timeout() == false) {
+        statu.set_twist_timeout(false);
+        agvStatu.set_v(timedV_.Get());
+        agvStatu.set_vth(timedA_.Get());
       }
-      //发布 mpc 预测点
-      if(predict_traj_.timeout()==false) {
-        for (int i = 0; i < predict_traj_.Get().size(); ++i){
-          Pose2d pt=predict_traj_.Get()[i];
-          Pose2dMsg* pose=statu.mutable_predict_traj()->add_points();
-          pose->set_x(pt.x());
-          pose->set_y(pt.y());
-          pose->set_theta(pt.theta());
+      statu.mutable_current_pose()->CopyFrom(agvStatu);
+
+      if (current_action != nullptr) {
+        delete current_action;
+        //发布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());
+          }
         }
       }
     }
+    terminator_->publish_nav_statu(statu);
   }
-  terminator_->publish_nav_statu(statu);
 }
 
 void Navigation::ResetStatu(double v,double a)
@@ -109,9 +153,9 @@ void Navigation::ResetPose(const Pose2d& pose)
   timedPose_.reset(pose,0.5);
 }
 
-bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
+bool Navigation::Start(const NavMessage::NavCmd& cmd)
 {
-  if(running_==true)
+  if(unfinished_cations_.empty()==false)  //正在运行中，上一次指令未完成
   {
     if(pause_)
     {
@@ -129,7 +173,9 @@ bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
   }
 
     //add 先检查当前点与起点的距离
-    global_trajectorys_=global_trajectorys;
+    global_navCmd_=cmd;
+    for(int i=0;i<cmd.actions_size();++i)
+      unfinished_cations_.push(cmd.actions(i));
     thread_=new std::thread(&Navigation::navigatting,this);
     return true;
 
@@ -153,7 +199,7 @@ Navigation::Navigation()
 }
 
 
-void Navigation::NavCmdCallback(const NavCmd& cmd,void* context)
+void Navigation::NavCmdCallback(const NavMessage::NavCmd& cmd,void* context)
 {
   Navigation* navigator=(Navigation*)context;
 
@@ -171,19 +217,7 @@ void Navigation::NavCmdCallback(const NavCmd& cmd,void* context)
     return ;
   }
 
-  std::queue<Trajectory> trajs;
-  for(int i=0;i<cmd.trajs_size();++i)
-  {
-    Trajectory traj;
-    for(int j=0;j<cmd.trajs(i).points_size();++j)
-    {
-      Pose2dMsg pt_msg=cmd.trajs(i).points(j);
-      Pose2d pt2d(pt_msg.x(),pt_msg.y(),pt_msg.theta());
-      traj.push_point(pt2d);
-    }
-    trajs.push(traj);
-  }
-  navigator->Start(trajs);
+  navigator->Start(cmd);
 }
 void Navigation::RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context)
 {
@@ -192,114 +226,76 @@ void Navigation::RobotStatuCallback(double x,double y,double theta,double v,doub
   navigator->ResetStatu(v,vth);
 }
 
-bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
+bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_diff)
 {
   if(inited_==false) {
     printf(" navigation has not inited\n");
     return false;
   }
-  Pose2d thresh=Pose2d::abs(distance);
-  bool yCompleted=false;
-  bool xCompleted=false;
-  bool aCompleted=false;
-  while(cancel_==false)
-  {
+
+  Pose2d thresh=Pose2d::abs(target_diff);
+
+  int action=0; //1 原地旋转，2横移，3前进
+  while(cancel_==false) {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    if (pause_ == true)
-    {
+    if (pause_ == true) {
       //发送暂停消息
       continue;
     }
-    if (timedPose_.timeout() == true)
-    {
-      printf(" navigation Error:Pose is timeout \n");
+    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
+      printf(" navigation Error:Pose/v/a is timeout \n");
       break;
     }
-
-    //计算目标pose在当前pose下的pose
-    Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
-    if (Pose2d::abs(diff).y() < thresh.y())
-      yCompleted=true;
-    else if(Pose2d::abs(diff).y()>2.0*thresh.y())
-      yCompleted=false;
-
-    if (Pose2d::abs(diff).x() < thresh.x())
-      xCompleted=true;
-    else if(Pose2d::abs(diff).x() >2.0* thresh.x())
-      xCompleted=false;
-
-    if (Pose2d::abs(diff).theta() < thresh.theta())
-      aCompleted=true;
-    else
-      aCompleted=false;
-
-    if(yCompleted&&xCompleted&&aCompleted)
-    {
+    //判断是否到达目标点
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
       monitor_->stop();
       return true;
     }
 
-    //先旋转使得Y=0摆直，再移动x，再X最后旋转
-    if(yCompleted==false)
-    {
-      double theta=atan(diff.y()/diff.x());
-
-      double maxtheta=20.*M_PI/180.0;
-      double mintheta=5.*M_PI/180.0;
-      if(abs(theta)<mintheta)
-      {
-        theta=(theta>0)?mintheta:-mintheta;
-      }
-      if(abs(theta)>maxtheta)
-      {
-        theta=(theta>0)?maxtheta:-maxtheta;
+    //计算目标点在当前pose下的pose
+    Pose2d diff = Pose2d::PoseByPose(target, timedPose_.Get());
+
+    //停止状态
+    if (action == 0) {
+      //优先进入旋转
+      if (Pose2d::abs(diff).theta() > thresh.theta()) {
+        action = 1; //原地旋转
+      } else if (Pose2d::abs(diff).y() > thresh.y()) {
+        //横移
+        action = 2;
+      } else if (Pose2d::abs(diff).x() > thresh.x()) {
+        //前进
+        action = 3;
       }
-      publish_statu(eRotate,0,theta);
-      monitor_->rotate(theta);
-      printf("旋转 diff.y:%.5f\n",diff.y());
-      continue;
-
     }
-    if(xCompleted==false){
-      double v=diff.x()/10.0;
-      double maxv=1.9;
-      double minv=0.1;
-      if(abs(v)<minv)
-      {
-        v=(v>0)?minv:-minv;
-      }
-      if(abs(v)>maxv)
-      {
-        v=(v>0)?maxv:-maxv;
+    else
+    {
+      if (action == 1) {
+        if (Pose2d::abs(diff).theta() > thresh.theta()) {
+          double theta = limit(diff.theta(),5*M_PI/180.0,40*M_PI/180.0);
+          monitor_->set_speed(Monitor_emqx::eRotate, 0, theta);
+          continue;
+        }
       }
-      publish_statu(eLine,v,0);
-      monitor_->set_speed(v,0);
-      printf("x移动 diff.x():%.5f\n",diff.x());
-      continue;
-    }
-    if(aCompleted==false){
-      double theta=diff.theta()/5.0;
-      if(std::abs(diff.theta())>M_PI)
-        theta=-theta;
-      double maxtheta=20.*M_PI/180.0;
-      double mintheta=5.*M_PI/180.0;
-      if(abs(theta)<mintheta)
-      {
-        theta=(theta>0)?mintheta:-mintheta;
+      if (action == 2) {
+        if (Pose2d::abs(diff).y() > thresh.y()) {
+          monitor_->set_speed(Monitor_emqx::eHorizontal, limit(diff.y(),0.05,0.2), 0);
+          continue;
+        }
       }
-      if(abs(theta)>maxtheta)
-      {
-        theta=(theta>0)?maxtheta:-maxtheta;
+      if (action == 3) {
+        if (Pose2d::abs(diff).x() > thresh.x()) {
+          monitor_->set_speed(Monitor_emqx::eHorizontal, limit(diff.x(),0.05,0.5), 0);
+          continue;
+        }
       }
-      publish_statu(eRotate,0,theta);
-      monitor_->rotate(theta);
-      printf("旋转angular:%.5f diff.theta():%.5f\n",theta,diff.theta());
+      monitor_->stop();
+      printf(" action :%d completed!!! reset action type!!!\n", action);
+      action = 0;
     }
-
   }
-  if(cancel_==true)
-    return false;
-  return true;
+  return false;
+
 }
 
 bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
@@ -336,30 +332,31 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
   return ret;
 }
 
-bool Navigation::IsArrived(const Pose2d& cur,double speed,const Pose2d& target,const Pose2d& diff)
+bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
+                           const Pose2d& target,const Pose2d& diff)
 {
     if(Pose2d::abs(cur-target)<diff)
     {
-        if(fabs(speed)<0.1)
+        if(fabs(velocity)<0.1 && angular< 10*M_PI/180.0)
             return true;
     }
     return false;
 }
-
-bool Navigation::execute_edge(const Trajectory& child,const Pose2d& head_diff,const Pose2d& target_diff)
+bool Navigation::execute_along_action(const Pose2d& target,const Pose2d& target_diff)
 {
   if(inited_==false) {
     printf(" navigation has not inited\n");
     return false;
   }
-
-  if(child.size()==0)
+  if (timedPose_.timeout() ) {
+    printf(" navigation Error:Pose is timeout \n");
+    return false;
+  }
+  //生成轨迹
+  Trajectory traj=Trajectory::create_line_trajectory(timedPose_.Get(),target);
+  if(traj.size()==0)
     return true;
-  Pose2d head=child[0];
-  Pose2d tail=child[child.size()-1];
 
-  Trajectory traj=child;
-  bool init_start_pose=false;
   while(cancel_==false)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -368,31 +365,17 @@ bool Navigation::execute_edge(const Trajectory& child,const Pose2d& head_diff,co
       //发送暂停消息
       continue;
     }
-    if(timedPose_.timeout()==true)
-    {
-      printf(" navigation Error:Pose is timeout \n");
-      break;
+    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
+      printf(" navigation Error:Pose/v/a is timeout \n");
+      return false;
     }
     //判断是否到达终点
-    Pose2d diff=Pose2d::abs(timedPose_.Get()-tail);
-    if(IsArrived(timedPose_.Get(),timedV_.Get(),tail,target_diff))
+    if(IsArrived(timedPose_.Get(),timedV_.Get(),timedA_.Get(),target,target_diff))
     {
         monitor_->stop();
         printf(" edge navigation completed !!!\n");
         return true;
-    }else if(diff.x()<target_diff.x() && diff.y()<target_diff.y()){
-      monitor_->stop();
-        moveToPoint(tail,target_diff);
-    }
-    //std::cout<<"diff:"<<Pose2d::abs(timedPose_.Get()-tail)<<std::endl;
-
-    //如果与起点差距过大先运动到起点
-    if(false==init_start_pose)
-    {
-      init_start_pose= moveToPoint(head, head_diff);
-      continue;
     }
-    else{
       //一次变速
       std::vector<double> out;
       bool ret;
@@ -404,11 +387,10 @@ bool Navigation::execute_edge(const Trajectory& child,const Pose2d& head_diff,co
         monitor_->stop();
         return false;
       }
-      publish_statu(eMPC,out[0],out[1]);
-      monitor_->set_speed(out[0],out[1]);
+      monitor_->set_speed(Monitor_emqx::eMPC,out[0],out[1]);
     }
 
-  }
+
   return false;
 }
 
@@ -423,36 +405,37 @@ void Navigation::navigatting()
   running_=true;
   printf(" navigation beg...\n");
 
-#if 0
-  Trajectory gtraj;
-  while(global_trajectorys_.empty()==false)
-  {
-    gtraj=gtraj+global_trajectorys_.front();
-    global_trajectorys_.pop();
-  }
-  execute_edge(gtraj);
-#else
-  double v=0,a=0;
-  Pose2d head_diff(0.1,0.1,3/180.0*M_PI);
-  Pose2d target_diff(0.03,0.05,0.5/180.0*M_PI);
-  while(global_trajectorys_.empty()==false && cancel_==false)
+  while(unfinished_cations_.empty()==false && cancel_==false)
   {
-    Trajectory traj=global_trajectorys_.front();
-    if(execute_edge(traj,head_diff,target_diff))
+    NavMessage::Action act=unfinished_cations_.front();
+    Pose2d target(act.target().x(),act.target().y(),act.target().theta());
+    Pose2d target_diff(act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
+    if(act.type()==1)
+    {
+      if(exec_adjust_action(target,target_diff)==false)
+        break;
+    }
+    if(act.type()==2)
     {
-      global_trajectorys_.pop();
+      if(execute_along_action(target,target_diff)==false)
+        break;
     }
     else
     {
+      printf(" unknow action type:%d\n",act.type());
       break;
     }
+    unfinished_cations_.pop();
   }
-#endif
+
   monitor_->stop();
-  if(cancel_==true)
+  if(cancel_==true) {
     printf(" navigation canceled\n");
-  printf(" navigation end...\n");
-  if(global_trajectorys_.size()==0)
-    printf("navigation completed!!!\n");
+    while(unfinished_cations_.empty()==false)
+      unfinished_cations_.pop();
+  }else {
+    if (unfinished_cations_.empty())
+      printf("navigation completed!!!\n");
+  }
   running_=false;
 }

MPC/navigation.h

@@ -17,9 +17,8 @@ class Navigation
 public:
     enum ActionType
     {
-        eRotate=0,
-        eLine,
-        eMPC
+        eReady=0,
+        eRunning
     };
  public:
     Navigation();
@@ -39,28 +38,30 @@ public:
     TimedLockData<double>& RealTimeA(){
       return timedA_;
     }
-    bool Start(std::queue<Trajectory> global_trajectorys);
+    bool Start(const NavMessage::NavCmd& cmd);
     void Cancel();
     void Pause();
 
  protected:
 
     static void RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context);
-    static void NavCmdCallback(const NavCmd& cmd,void* context);
+    static void NavCmdCallback(const NavMessage::NavCmd& cmd,void* context);
 
-    static bool IsArrived(const Pose2d& cur,double speed,const Pose2d& target,const Pose2d& diff);
-    bool execute_edge(const Trajectory& child,const Pose2d& head_diff,const Pose2d& target_diff);
-    bool moveToPoint(const Pose2d& pose,const Pose2d& distance);
+    static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,const Pose2d& diff);
+    bool execute_along_action(const Pose2d& target,const Pose2d& target_diff);
+    bool exec_adjust_action(const Pose2d& target,const Pose2d& target_diff);
     bool mpc_once(const Trajectory& traj,std::vector<double>& out);
     void navigatting();
 
     /*
-     * 发布导航模块状态，当前动作类型、角速度、线速度
+     * 发布导航模块状态
      */
-    void publish_statu(ActionType type,float velocity,float angular);
+    void publish_statu();
 
  protected:
     std::mutex  mtx_;
+    bool exit_=false;
+    std::thread* pubthread_= nullptr;
     bool inited_=false;
     Monitor_emqx* monitor_= nullptr;
     Terminator_emqx* terminator_= nullptr;
@@ -68,7 +69,8 @@ public:
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
-    std::queue<Trajectory> global_trajectorys_; //多段轨迹
+    NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
+    std::queue<NavMessage::Action> unfinished_cations_; //未完成的动作
     std::thread* thread_= nullptr;
     bool running_=false;
     bool pause_=false;

message.proto

@@ -1,5 +1,5 @@
 syntax = "proto3";
-
+package NavMessage;
 message AGVStatu {
   float x=1;
   float y=2;
@@ -8,39 +8,51 @@ message AGVStatu {
   float vth=5;
 }
 
-message ChangeSpeedCmd {
-  float v=1;
-  float vth=2;
+message Speed {
+  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他：停止
+  float v=2;
+  float vth=3;
 }
 
 
-message Pose2dMsg
+message Pose2d
 {
     float x=1;
     float y=2;
     float theta=3;
 }
 
-message TrajectoryMsg
+message Trajectory
+{
+  repeated Pose2d poses=1;
+}
+
+message Action
 {
-    repeated Pose2dMsg points=1;
+  int32 type=1; // 1:原地调整，2：巡线
+    Pose2d target=2;
+    Pose2d target_diff=3;
 }
 
+
 message NavCmd
 {
   int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
-  repeated TrajectoryMsg trajs=2;
+  string key=2;
+  repeated Action actions=3;
 }
 
 message NavStatu
 {
-    bool isrunning=1;
-    repeated TrajectoryMsg trajs=2;
-    int32 action_type=3;
-    float angular=4;
-    float velocity=5;
-    TrajectoryMsg select_traj=6;
-    TrajectoryMsg predict_traj=7;
+    bool statu=1; //0:ready  1:running
+  string key=2; // 任务唯一码
+  repeated Action unfinished_actions=3;  //未完成的动作，第一个即为当前动作
+  AGVStatu current_pose=4; //当前位姿
+  bool pose_timeout=5;
+  bool twist_timeout=6;   //速度是否超时
+    Trajectory selected_traj=7;
+    Trajectory predict_traj=8;
+
 }
 
 

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -14,12 +14,12 @@
 using namespace webots;
 
 Paho_client* client_= nullptr;
-double g_v=0,g_vth=0;
+NavMessage::Speed g_speed;
 
 
 void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
-  msg.toSpeed(g_v,g_vth);
+  msg.toProtoMessage(g_speed);
 }
 
 double generae_random(std::default_random_engine& generator,double mean,double sigma)
@@ -65,7 +65,7 @@ void Rotating(double wmg,double w,double l,
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV","AgvCmd/cmd1"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV","monitor/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;
@@ -161,56 +161,63 @@ int main(int argc, char **argv) {
     if(client_)
     {
       MqttMsg msg;
-      msg.fromStatu(x,y,theta,
-                    v,vth);
-      client_->publish("AgvStatu/robot1",1,msg);
+      NavMessage::AGVStatu statu;
+      statu.set_x(x);
+      statu.set_y(y);
+      statu.set_theta(theta);
+      statu.set_v(v);
+      statu.set_vth(vth);
+      msg.fromProtoMessage(statu);
+      client_->publish("monitor/statu",1,msg);
     }
 
     //变速，角速度封顶
-    if(fabs(g_vth)>0.4)
+    double cmd_v=g_speed.v();
+    double cmd_vth=g_speed.vth();
+    if(fabs(cmd_vth)>0.4)
     {
-      g_vth=g_vth>0?0.4:-0.4;
+      cmd_vth=cmd_vth>0?0.4:-0.4;
     }
     //初始值
     double R1=0,R2=0,R3=0,R4=0;
-    double L1=g_v;
-    double L2=g_v;
-    double L3=g_v;
-    double L4=g_v;
+    double L1=cmd_v;
+    double L2=cmd_v;
+    double L3=cmd_v;
+    double L4=cmd_v;
     double theta1=0;
     double theta2=0;
 
     //无角速度，直线
-    if(fabs(g_vth)<0.0001)
+    if(fabs(cmd_vth)<0.0001)
     {
       R1=R2=R3=R4=0;
-      L1=L2=L3=L4=g_v;
+      L1=L2=L3=L4=cmd_v;
     }
     else  //有角速度
     {
-      if (fabs(g_v) < 0.00001)
+      if (fabs(cmd_vth) < 0.00001)
       {
         //原地旋转
-        if (fabs(g_vth) > 0.0001)
-          Rotating(g_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+        if (fabs(cmd_vth) > 0.0001)
+          Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
       }
       else
       {
         //默认原地旋转
-        Rotating(g_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
-        double base = pow(g_v / g_vth,2)-(l*l/4);
+        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+        double base = pow(cmd_v / cmd_vth,2)-(l*l/4);
         //满足运动方程
         if(base>=0.1)
         {
           double BaseR=sqrt(base)-w/2;
           if(BaseR>0)
           {
-            if(g_vth*g_v>=0)
+            if(cmd_vth*cmd_v>=0)
             {
               R1=atan(l/BaseR);
               R2=atan(l/(BaseR+w));
-              L3=BaseR*g_vth;
-              L4=(BaseR+w)*g_vth;
+              L3=BaseR*cmd_vth;
+              L4=(BaseR+w)*cmd_vth;
               R3=0;
               R4=0;
               L1=L3/cos(R1);
@@ -221,8 +228,8 @@ int main(int argc, char **argv) {
             {
               R1=-atan(l/(BaseR+w));
               R2=-atan(l/BaseR);
-              L3=-(BaseR+w)*g_vth;
-              L4=-BaseR*g_vth;
+              L3=-(BaseR+w)*cmd_vth;
+              L4=-BaseR*cmd_vth;
               R3=0;
               R4=0;
               L1=L3/cos(R1);

projects/controllers/AGV_controller/emqx-client/mqttmsg.cpp

@@ -4,7 +4,7 @@
 
 #include "mqttmsg.h"
 #include <string.h>
-#include "message.pb.h"
+
 #include <google/protobuf/util/json_util.h>
 using namespace google::protobuf;
 MqttMsg::MqttMsg()
@@ -57,7 +57,7 @@ MqttMsg::~MqttMsg(){
   length_=0;
 }
 
-
+/*
 void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
 {
   std::lock_guard<std::mutex> lock(mutex_);
@@ -80,16 +80,15 @@ void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
   data_=(char*)malloc(length_);
 
   memcpy(data_,json_str.c_str(),length_);
-
-
 }
 
 bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vth)
 {
+  if(strlen(data())<length_)
+    return false;
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_== nullptr)
     return false;
-
   AGVStatu statu;
   util::Status ret;
   char* buf=(char*)malloc(length_+1);
@@ -118,21 +117,16 @@ bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vt
   return ret.ok();
 }
 
-
-
-void MqttMsg::fromSpeed(double v, double vth)
+void MqttMsg::fromNavSpeed(const NavMessage::Speed& speed)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_)
     free(data_);
-  ChangeSpeedCmd cmd;
-  cmd.set_v(v);
-  cmd.set_vth(vth);
 
   google::protobuf::util::JsonPrintOptions option;
   option.always_print_primitive_fields=true;
   std::string json_str;
-  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);
+  google::protobuf::util::MessageToJsonString(speed,&json_str,option);
 
   length_=json_str.size();
   data_=(char*)malloc(length_);
@@ -140,13 +134,13 @@ void MqttMsg::fromSpeed(double v, double vth)
 
 }
 
-bool MqttMsg::toSpeed(double &v, double &vth)
+bool MqttMsg::toNavSpeed(NavMessage::Speed& speed)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   if(data_== nullptr)
     return false;
 
-  ChangeSpeedCmd cmd;
+
   util::Status ret;
   char* buf=(char*)malloc(length_+1);
   memset(buf,0,length_+1);
@@ -155,8 +149,8 @@ bool MqttMsg::toSpeed(double &v, double &vth)
   {
 
     //printf("%s\n",buf);
-    ret = util::JsonStringToMessage(buf, &cmd);
-    printf("change speed v:%f,  vth:%f\n",v,vth);
+    ret = util::JsonStringToMessage(buf, &speed);
+    printf("change speed v:%f,  vth:%f\n",speed.v(),speed.vth());
 
   }
   catch (std::exception &e)
@@ -165,13 +159,131 @@ bool MqttMsg::toSpeed(double &v, double &vth)
   }
 
   free(buf);
-  if(ret.ok())
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavCmd(const NavMessage::NavCmd& cmd)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavCmd(NavMessage::NavCmd& cmd)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &cmd);
+  }
+  catch (std::exception &e)
   {
-    v=cmd.v();
-    vth=cmd.vth();
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavStatu(const NavMessage::NavStatu &statu)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(statu,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavStatu(NavMessage::NavStatu &statu)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
 
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &statu);
   }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
 
   return ret.ok();
 }
+*/
 
+void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(message,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+}
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &message);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+  return ret.ok();
+}

projects/controllers/AGV_controller/emqx-client/mqttmsg.h

@@ -4,7 +4,7 @@
 
 #ifndef PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
 #define PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
-
+#include "message.pb.h"
 #include <mutex>
 
 class MqttMsg
@@ -29,11 +29,20 @@ class MqttMsg
     char* data()const;
     int length()const;
 
-    void fromStatu(double x,double y,double theta,double v,double vth);
-    void fromSpeed(double v,double vth);
+
+    void fromProtoMessage(const google::protobuf::Message& messge);
+    bool toProtoMessage(google::protobuf::Message& message);
+
+    /*void fromStatu(double x,double y,double theta,double v,double vth);
+    void fromNavSpeed(const NavMessage::Speed& speed);
+    void fromNavCmd(const NavMessage::NavCmd& cmd);
+    void fromNavStatu(const NavMessage::NavStatu& statu);
 
     bool toStatu(double& x,double& y,double& theta,double& v,double& vth);
-    bool toSpeed(double& v,double& vth);
+    bool toNavSpeed(NavMessage::Speed& speed);
+    bool toNavCmd(NavMessage::NavCmd& cmd);
+    bool toNavStatu(NavMessage::NavStatu& statu);*/
+
 
  protected:
     char* data_=nullptr;

projects/controllers/AGV_controller/emqx-client/paho_client.cpp

@@ -8,6 +8,7 @@
 Paho_client::Paho_client(std::string clientid){
   clientid_=clientid;
   context_= nullptr;
+  connected_=false;
 }
 Paho_client::~Paho_client(){}
 void Paho_client::set_maxbuf(int size)
@@ -98,6 +99,7 @@ int Paho_client::messageArrived(void* context, char* topicName, int topicLen, MQ
     cl->arrivedCallback_(topicName,message->qos,msg,cl->context_);
   MQTTAsync_freeMessage(&message);
   MQTTAsync_free(topicName);
+  return 1;
 }
 
 

tool/threadSafeQueue.h

@@ -1,101 +0,0 @@
-//
-// Created by zx on 2019/12/17.
-//
-
-#ifndef SRC_THREADSAFEQUEUE_H
-#define SRC_THREADSAFEQUEUE_H
-
-
-#include <queue>
-#include <memory>
-#include <mutex>
-#include <condition_variable>
-template<typename T>
-class threadsafe_queue
-{
-private:
-    mutable std::mutex mut;
-    std::queue<T> data_queue;
-    std::condition_variable data_cond;
-public:
-    threadsafe_queue() {}
-    threadsafe_queue(threadsafe_queue const& other)
-    {
-        std::lock_guard<std::mutex> lk(other.mut);
-        data_queue = other.data_queue;
-    }
-    ~threadsafe_queue()
-    {
-        while (!empty())
-        {
-            try_pop();
-        }
-    }
-    size_t size()
-    {
-        return data_queue.size();
-    }
-    void push(T new_value)//��Ӳ���
-    {
-        std::lock_guard<std::mutex> lk(mut);
-        data_queue.push(new_value);
-        data_cond.notify_one();
-    }
-    void wait_and_pop(T& value)//ֱ����Ԫ�ؿ���ɾ��Ϊֹ
-    {
-        std::unique_lock<std::mutex> lk(mut);
-        data_cond.wait(lk, [this] {return !data_queue.empty(); });
-        value = data_queue.front();
-        data_queue.pop();
-    }
-    std::shared_ptr<T> wait_and_pop()
-    {
-        std::unique_lock<std::mutex> lk(mut);
-        data_cond.wait(lk, [this] {return !data_queue.empty(); });
-        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
-        data_queue.pop();
-        return res;
-    }
-    //ֻ���� �� pop
-    bool front(T& value)
-    {
-        std::lock_guard<std::mutex> lk(mut);
-        if (data_queue.empty())
-            return false;
-        value = data_queue.front();
-        return true;
-    }
-
-    bool try_pop(T& value)//������û�ж���Ԫ��ֱ�ӷ���
-    {
-        if (data_queue.empty())
-            return false;
-        std::lock_guard<std::mutex> lk(mut);
-        value = data_queue.front();
-        data_queue.pop();
-        return true;
-    }
-    std::shared_ptr<T> try_pop()
-    {
-        std::lock_guard<std::mutex> lk(mut);
-        if (data_queue.empty())
-            return std::shared_ptr<T>();
-        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
-        data_queue.pop();
-        return res;
-    }
-    bool empty() const
-    {
-        std::lock_guard<std::mutex> lk(mut);
-        return data_queue.empty();
-    }
-    void clear()
-    {
-        while (!empty()) {
-            try_pop();
-        }
-    }
-};
-
-
-#endif //SRC_THREADSAFEQUEUE_H