修改末端精度，

CMakeLists.txt

@@ -72,7 +72,7 @@ add_executable(${PROJECT_NAME}
 		tool/pathcreator.cpp
 		tool/proto_tool.cpp
 		emqx/terminator_emqx.cpp
-		)
+        emqx/agv.cpp)
 target_compile_options(${PROJECT_NAME}
 		PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
 		)

main.cpp

@@ -31,7 +31,13 @@ TimedLockData<NavMessage::NavStatu> timed_navStatu_;
 TimedLockData<NavMessage::AGVStatu> timed_AgvStatu_;
 TimedLockData<NavMessage::AGVStatu> timed_AgvBrotherStatu_;
 
-bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
+//创建单车由指定点进入车位导航指令
+bool CreateIntoSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
+//创建单车出车位去指定点导航指令
+bool CreateOutSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
+//创建单车出车位去指定点导航指令
+bool CreateTestNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
+
 bool NavIsCompleted()
 {
   if(timed_navStatu_.timeout()==false)
@@ -93,7 +99,7 @@ void navigation_start() {
   auto_test_count++;
   //发布指令
   NavMessage::NavCmd cmd;
-  if (false == CreateNavCmd(g_shortest_path, cmd))
+  if (false == CreateIntoSpaceNavCmd(g_shortest_path, cmd))
     return;
   std::cout << " ------------------------------------new NavCmd ---------------------------" << std::endl;
   printf("----第 %d 次测试---- from %d ---- %d -------begin navigatting...-----\n\n",
@@ -119,6 +125,7 @@ void navigation_pause()
 }
 void navigation_stop()
 {
+  g_auto_test=false;
   if(NavIsCompleted()==false)
   {
     printf("Last nav has not completed!! set last node = -1\n");
@@ -147,7 +154,7 @@ void pangolinSpinOnce(PangolinViewer* viewer) {
         //随机终点
         int randid=0; //[1-8]
         while(randid==0 || randid==next_begin_node) {
-          randid = rand() % 7 + 1;
+          randid = rand() % 8 + 1;
           usleep(1000*1000);
         }
         printf(" nav completed ...... reset beg:%d  rand end:%d\n",next_begin_node,randid);
@@ -243,7 +250,6 @@ void NavStatuArrived(const MqttMsg& msg,void* context)
 
 }
 
-
 int main(int argc, char** argv)
 {
   srand((unsigned int)time(nullptr));
@@ -322,7 +328,6 @@ bool InitMap(std::string config_file)
   return true;
 }
 
-
 bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
 {
   if(nodes_id.size()<2)
@@ -346,7 +351,111 @@ bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajec
   return true;
 }
 
-bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
+bool CreateIntoSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
+{
+
+  if(nodes.size()==0)
+    return false;
+
+  cmd.set_action(0);  //开始新导航
+  char key[255]={0};
+  static int N=1111;
+  sprintf(key,"abcd-efg-%d",N++);
+  cmd.set_key(key);
+
+  NavMessage::Pose2d adjustdiff,node_mpcdiff,enddiff;
+  adjustdiff.set_x(0.1);
+  adjustdiff.set_y(0.05);
+  adjustdiff.set_theta(0.5*M_PI/180.0);
+
+  node_mpcdiff.set_x(0.1);
+  node_mpcdiff.set_y(0.1);
+  node_mpcdiff.set_theta(3*M_PI/180.0);
+
+  enddiff.set_x(0.1);
+  enddiff.set_y(0.1);
+  enddiff.set_theta(5*M_PI/180.0);
+
+
+  //调整到起点，朝向与当前朝向一致
+  if(timed_AgvStatu_.timeout())
+  {
+    printf(" agv statu is timeout can not get current direction\n");
+    return false;
+  }
+  float current_theta=timed_AgvStatu_.Get().theta();
+  float x,y;
+  DijkstraMap.GetVertexPos(nodes[0],x,y);
+
+  NavMessage::SpeedLimit v_limit,angular_limit,horize_limit;
+  v_limit.set_min(0.1);
+  v_limit.set_max(0.2);
+  horize_limit.set_min(0.05);
+  horize_limit.set_max(0.2);
+  angular_limit.set_min(1);
+  angular_limit.set_max(25.0);
+
+  NavMessage::SpeedLimit mpc_x_limit,mpc_angular_limit;
+  mpc_x_limit.set_min(0.05);
+  mpc_x_limit.set_max(1.5);
+
+  mpc_angular_limit.set_min(0*M_PI/180.0);
+  mpc_angular_limit.set_max(3*M_PI/180.0);
+
+  NavMessage::Action act_head;
+  act_head.set_type(1);
+  act_head.mutable_target()->set_x(x);
+  act_head.mutable_target()->set_y(y);
+  act_head.mutable_target()->set_theta(current_theta);
+  act_head.mutable_target_diff()->CopyFrom(adjustdiff);
+
+  act_head.mutable_velocity_limit()->CopyFrom(v_limit);
+  act_head.mutable_horize_limit()->CopyFrom(horize_limit);
+  act_head.mutable_angular_limit()->CopyFrom(angular_limit);
+  cmd.add_actions()->CopyFrom(act_head);
+
+  Pose2d begin;
+  for(int i=1;i<nodes.size();++i)
+  {
+    //调整到上一点位姿，朝向当前点
+    float lx,ly;
+    DijkstraMap.GetVertexPos(nodes[i-1],lx,ly);
+    DijkstraMap.GetVertexPos(nodes[i],x,y);
+    float theta=Trajectory::gen_axis_angle(Pose2d(lx,ly,0),Pose2d(x,y,0));
+    begin=Pose2d(lx,ly,theta);
+    NavMessage::Action act_adjust;
+    act_adjust.set_type(1);
+    act_adjust.mutable_target()->set_x(lx);
+    act_adjust.mutable_target()->set_y(ly);
+    act_adjust.mutable_target()->set_theta(theta);
+    act_adjust.mutable_target_diff()->CopyFrom(adjustdiff);
+    act_adjust.mutable_velocity_limit()->CopyFrom(v_limit);
+    act_adjust.mutable_horize_limit()->CopyFrom(horize_limit);
+    act_adjust.mutable_angular_limit()->CopyFrom(angular_limit);
+    cmd.add_actions()->CopyFrom(act_adjust);
+
+    //MPC 控制
+    NavMessage::Action act_along;
+    act_along.set_type(2);
+    act_along.mutable_begin()->set_x(begin.x());
+    act_along.mutable_begin()->set_y(begin.y());
+    act_along.mutable_begin()->set_theta(begin.theta());
+    act_along.mutable_target()->set_x(x);
+    act_along.mutable_target()->set_y(y);
+    act_along.mutable_target()->set_theta(theta);
+    if(i==nodes.size()-1) //最后一个节点
+      act_along.mutable_target_diff()->CopyFrom(enddiff);
+    else
+      act_along.mutable_target_diff()->CopyFrom(node_mpcdiff);
+    act_along.mutable_velocity_limit()->CopyFrom(mpc_x_limit);
+    act_along.mutable_angular_limit()->CopyFrom(mpc_angular_limit);
+    cmd.add_actions()->CopyFrom(act_along);
+  }
+  return true;
+
+}
+
+bool CreateOutSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
 {
 
   if(nodes.size()==0)
@@ -368,7 +477,6 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
   enddiff.set_theta(1.0*M_PI/180.0);
 
 
-  //调整到起点，朝向与当前朝向一致
   if(timed_AgvStatu_.timeout())
   {
     printf(" agv statu is timeout can not get current direction\n");
@@ -408,11 +516,16 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
   Pose2d begin;
   for(int i=1;i<nodes.size();++i)
   {
+
     //调整到上一点位姿，朝向当前点
     float lx,ly;
     DijkstraMap.GetVertexPos(nodes[i-1],lx,ly);
     DijkstraMap.GetVertexPos(nodes[i],x,y);
     float theta=Trajectory::gen_axis_angle(Pose2d(lx,ly,0),Pose2d(x,y,0));
+    //第一点倒退出车位
+    if(i==1)
+      theta=Trajectory::gen_axis_angle(Pose2d(x,y,0),Pose2d(lx,ly,0));
+
     begin=Pose2d(lx,ly,theta);
     NavMessage::Action act_adjust;
     act_adjust.set_type(1);
@@ -445,3 +558,24 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
   return true;
 
 }
+
+bool CreateTestNavCmd(int begin,int space,int outnode,NavMessage::NavCmd& cmd)
+{
+  //去车位生成路径
+  std::vector<int> t_shortestPath;
+  float distance = -1;
+  bool ret = DijkstraMap.GetShortestPath(begin, space, t_shortestPath, distance);
+  if (ret) {
+    pathMutex.lock();
+    g_shortest_path = t_shortestPath;
+    std::reverse(g_shortest_path.begin(), g_shortest_path.end());
+    g_shortest_path.push_back(end_node);
+    pathMutex.unlock();
+  }
+  //生成去车位指令
+  NavMessage::NavCmd toCmd;
+
+
+  return false;
+
+}

proto.sh

@@ -2,3 +2,4 @@
 protoc -I=./ message.proto --cpp_out=./emqx
 
 protoc -I=./ map.proto --cpp_out=./
+protoc -I=./ parameter.proto --cpp_out=./emqx