navigation ...

MPC/navigation.cpp

@@ -36,17 +36,18 @@ bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
     printf(" navigation is running pls cancel before\n");
     return false;
   }
-  if(thread_==nullptr)
+  if(thread_!= nullptr)
   {
+    if(thread_->joinable())
+      thread_->join();
+    delete thread_;
+  }
+
     //add 先检查当前点与起点的距离
     global_trajectorys_=global_trajectorys;
     thread_=new std::thread(&Navigation::navigatting,this);
     return true;
-  }
-  else
-  {
-    return false;
-  }
+
 }
 void Navigation::Cancel()
 {
@@ -90,22 +91,23 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
       break;
     }
 
-    Pose2d diff = timedPose_.Get() - pose;
+    //计算目标pose在当前pose下的pose
+    Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
     if (Pose2d::abs(diff) < thresh)
     {
       printf(" Move to pt complete\n");
       break;
     }
-    //先旋转角度
-    if(Pose2d::abs(diff).theta()>thresh.theta())
+    //先旋转使得Y=0摆直，再移动x，再X最后旋转
+    if(Pose2d::abs(diff).y()>thresh.y())
     {
-      printf("旋转 diff.theta:%.5f\n",diff.theta());
+      printf("旋转 diff.y:%.5f\n",diff.y());
 
-    }else if(Pose2d::abs(diff).y()>thresh.y()){
-      printf("y移动 diff.y():%.5f\n",diff.y());
+    }else if(Pose2d::abs(diff).x()>thresh.x()){
+      printf("y移动 diff.x():%.5f\n",diff.x());
     }
-    else if(Pose2d::abs(diff).x()>thresh.x()){
-      printf("x移动 diff.x():%.5f\n",diff.x());
+    else if(Pose2d::abs(diff).theta()>thresh.theta()){
+      printf("旋转 diff.theta():%.5f\n",diff.theta());
     }
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
@@ -192,24 +194,28 @@ bool Navigation::execute_child_trajectory(const Trajectory& child)
   return false;
 }
 
-void Navigation::navigatting(Navigation* p)
+void Navigation::navigatting()
 {
-  p->pause_=false;
-  p->cancel_=false;
-  p->running_=true;
-  while(p->global_trajectorys_.empty()==false)
+  pause_=false;
+  cancel_=false;
+  running_=true;
+  printf(" navigation beg...\n");
+  while(global_trajectorys_.empty()==false)
   {
-    Trajectory traj=p->global_trajectorys_.front();
-    if(p->execute_child_trajectory(traj))
+    Trajectory traj=global_trajectorys_.front();
+    if(execute_child_trajectory(traj))
     {
-      p->global_trajectorys_.pop();
+      global_trajectorys_.pop();
     }
     else
     {
       break;
     }
   }
-  if(p->cancel_==true)
+  if(cancel_==true)
     printf(" navigation sia canceled\n");
-  p->running_=false;
+  printf(" navigation end...\n");
+  if(global_trajectorys_.size()==0)
+    printf("navigation completed!!!\n");
+  running_=false;
 }

MPC/navigation.h

@@ -26,7 +26,7 @@ class Navigation
     bool execute_child_trajectory(const Trajectory& child);
     bool moveToPoint(const Pose2d& pose,const Pose2d& distance);
     bool mpc_once(const Trajectory& traj);
-    static void navigatting(Navigation* p);
+    void navigatting();
 
  protected:
     static Navigation* navigation_;

MPC/pose2d.cpp

@@ -32,6 +32,13 @@ const float Pose2d::gridient() const
   return gradient;
 }
 
+Pose2d Pose2d::PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose)
+{
+  Pose2d diff=world_pose-axis_pose;
+  Pose2d nPose=diff.rotate(-axis_pose.theta());
+  return Pose2d(nPose.x(),nPose.y(),diff.theta());
+}
+
 float Pose2d::distance(const Pose2d& pose1,const Pose2d& pose2)
 {
   Pose2d offset=pose1-pose2;

MPC/pose2d.h

@@ -54,7 +54,6 @@ class Pose2d
       return x()<pose.x()&&y()<pose.y()&&theta()<pose.theta();
     }
 
-
     /*
      * 将点顺时针旋转theta
      */
@@ -69,6 +68,7 @@ class Pose2d
     const float gridient()const;
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
+    static Pose2d PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose);
 
  protected:
     std::atomic<float>                      m_x;

MPC/trajectory.cpp

@@ -206,30 +206,20 @@ Trajectory Trajectory::create_trajectory(const Pose2d& start,const Pose2d& end,i
 
 Trajectory Trajectory::create_line_trajectory(const Pose2d& start,const Pose2d& end,int point_count)
 {
-  int extend_num=point_count/2;
 
   Trajectory trajectory;
-
   double angle =gen_axis_angle(start,end);
+  double dt=1.0/float(point_count);
 
-  /*
   //生成离散点,间距
-  for(double x=x0-delta_x*extend_num;x<x1+delta_x*extend_num;x+=delta_x)
-  {
-    double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
-    double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
-    trajectory.push_point(Pose2d(x,y,gradient2theta(gradient)));
-  }
-
-  //将轨迹点反变换到原坐标系
-  Trajectory trajectory_map;
-  for(int i=0;i<trajectory.size();++i)
+  Pose2d diff=end-start;
+  for(int i=0;i<point_count;++i)
   {
-    Pose2d point=trajectory[i];
-    Pose2d new_point=point.rotate(angle);
-    trajectory_map.push_point(new_point);
+    double x=start.x()+i*dt*diff.x();
+    double y=start.y()+i*dt*diff.y();
+    trajectory.push_point(Pose2d(x,y,angle));
   }
-
-  return trajectory_map;*/
+  trajectory.push_point(Pose2d(end.x(),end.y(),angle));
+  return trajectory;
 }
 

dijkstra/dijkstra.h

@@ -67,8 +67,8 @@ class PathMap
     bool GetShortestPath(int id1,int id2,std::vector<int>& path,float& shortest_distance);
 
     bool GetVertexPos(int id,float& x,float& y);
-    std::map<int,MapNode> GetNodes(){return node_;};
-    std::vector<MapEdge> GetEdges(){return edge_;}
+    std::map<int,MapNode> GetNodes()const{return node_;};
+    std::vector<MapEdge> GetEdges()const{return edge_;}
 
  protected:
     std::map<int,MapNode> node_;

lio/src/lio/mapper.cpp

@@ -414,7 +414,7 @@ void Mapper::process()
 
     if (newfullCloud == false)
     {
-      std::this_thread::yield();
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
       continue;
     }
 

main.cpp

@@ -1,5 +1,4 @@
 
-
 #include <csignal>
 #include "lio/Estimator.h"
 #include "lio/mapper.h"
@@ -43,6 +42,8 @@ TimedLockData<PointCloud::Ptr> localmap_cloud;
 TimedLockData<Eigen::Matrix4d> cur_pose;
 double final_cost=0;
 
+bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs);
+
 void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 {
     scan_cloud.reset(*cloud,0.1);
@@ -179,6 +180,23 @@ bool OnDijkstraBtn(int beg,int end,float& distance,
   return ret;
 }
 
+void navigation_start()
+{
+  pathMutex.lock();
+  std::vector<int> solve_path=shortest_path;
+  pathMutex.unlock();
+  std::reverse(solve_path.begin(),solve_path.end());
+  solve_path.push_back(node_end);
+
+  std::queue<Trajectory> global_trajectorys;
+  Create_trajectoris(DijkstraMap,solve_path,global_trajectorys);
+  Navigation::GetInstance()->Start(global_trajectorys);
+}
+void navigation_stop()
+{
+  Navigation::GetInstance()->Cancel();
+}
+
 int main(int argc, char** argv)
 {
   //reg参数
@@ -192,8 +210,6 @@ int main(int argc, char** argv)
     return -2;
   }
 
-  //Navigation::GetInstance();
-
   //初始化雷达
   double publish_freq  = 10.0;
   if(!InitLidar(publish_freq))
@@ -201,11 +217,13 @@ int main(int argc, char** argv)
 
   PangolinViewer* viewer=PangolinViewer::CreateViewer();
   viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
+  viewer->SetNavigationCallbacks(navigation_start,navigation_stop);
   viewer->Join();
 
   TimerRecord::PrintAll();
   lddc->Stop();
   pMaper->completed();
+  delete lidarFeatureExtractor;
   delete pMaper;
   delete lddc;
   delete viewer;
@@ -348,3 +366,25 @@ bool InitLidar(double publish_freq)
   lddc->SetImuCallback(ImuDataCallback);
   return true;
 }
+
+bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
+{
+  if(nodes_id.size()<2)
+    return false;
+  for(int i=0;i<nodes_id.size()-1;++i)
+  {
+    float x1,y1,x2,y2;
+    map.GetVertexPos(nodes_id[i],x1,y1);
+    map.GetVertexPos(nodes_id[i+1],x2,y2);
+
+    Pose2d start(x1,y1,0);
+    Pose2d end(x2,y2,0);
+    Trajectory traj=Trajectory::create_line_trajectory(start,end);
+    if(traj.size()>1)
+      trajs.push(traj);
+    else
+      return false;
+
+  }
+  return true;
+}

pangolinViewer.cpp

@@ -6,20 +6,6 @@
 #include "MPC/navigation.h"
 PangolinViewer* PangolinViewer::viewer_= nullptr;
 
-
-
-bool Create_trajectoris(PathMap map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
-{
-  if(nodes_id.size()<2)
-    return false;
-  for(int i=0;i<nodes_id.size()-1;++i)
-  {
-    Trajectory traj;
-
-  }
-}
-
-
 PangolinViewer* PangolinViewer::CreateViewer()
 {
   if (viewer_== nullptr)
@@ -54,7 +40,14 @@ void PangolinViewer::Join()
   }
 }
 
-void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,StartBtnCallback startBtn,
+void PangolinViewer::SetNavigationCallbacks(StartBtnCallback startBtn,
+                            StopBtnCallback stopBtn)
+{
+  naviagtion_startBtn_callback_=startBtn;
+  navigation_stopBtn_callback_=stopBtn;
+}
+
+void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,StartLocateBtnCallback startBtn,
                   StopBtnCallback stopBtn,FreqChangedCallback freqChanged,DijkstraBtnCallback dijkstra)
 {
   spinOnce_callback_=spinOnce;
@@ -66,7 +59,7 @@ void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,StartBtnCallba
 
 void PangolinViewer::Init()
 {
-  pangolin::CreateWindowAndBind("Main", 1280, 640);
+  pangolin::CreateWindowAndBind("Main", 1280, 760);
   // 3D Mouse handler requires depth testing to be enabled
   glEnable(GL_DEPTH_TEST);
   glEnable(GL_BLEND);   // 启用混合
@@ -74,12 +67,12 @@ void PangolinViewer::Init()
 
   text_font_ = new pangolin::GlFont("../config/tt0102m_.ttf", 30.0);
    s_cam_=new pangolin::OpenGlRenderState(
-          pangolin::ProjectionMatrix(1480, 640, 840, 840, 480, 320, 0.1, 1000),
+          pangolin::ProjectionMatrix(1480, 760, 840, 840, 480, 380, 0.1, 1000),
           pangolin::ModelViewLookAt(25, 0, 90, 25, 0, 20, pangolin::AxisY)
   );
   const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();
   d_cam_ = &pangolin::CreateDisplay()
-          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 640.0f)
+          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 760.0f)
           .SetHandler(new pangolin::Handler3D(*s_cam_));
   pangolin::CreatePanel("ui")
           .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
@@ -139,20 +132,22 @@ void PangolinViewer::Init()
 
   });
 
-  /*pangolin::Var<std::function<void(void)>> navigation_start("ui.navigation_start", [&]()
+  pangolin::Var<std::function<void(void)>> navigation_start("ui.navigation_start", [&]()
   {
-    std::queue<Trajectory> global_trajectorys;
-    Navigation::GetInstance()->Start(global_trajectorys);
+    if(naviagtion_startBtn_callback_!=nullptr)
+      naviagtion_startBtn_callback_();
   });
 
 
-  pangolin::Var<std::function<void(void)>> navigation_end("ui.navigation_start", [&]()
+  pangolin::Var<std::function<void(void)>> navigation_end("ui.navigation_cancel", [&]()
   {
-    Navigation::GetInstance()->Cancel();
+    if(navigation_stopBtn_callback_!= nullptr)
+      navigation_stopBtn_callback_();
+
   });
-*/
 
 }
+
 void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
                double alpha,double psize)
 {
@@ -167,6 +162,7 @@ void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
   }
   glEnd();
 }
+
 void PangolinViewer::DrawDijkastraMap(PathMap& map,std::vector<int> path)
 {
   //绘制顶点
@@ -234,7 +230,6 @@ void PangolinViewer::DrawDijkastraMap(PathMap& map,std::vector<int> path)
 
 }
 
-
 void PangolinViewer::ShowRealtimePose(Eigen::Matrix4d pose)
 {
   double l=2;

pangolinViewer.h

@@ -24,15 +24,19 @@ class PangolinViewer
 {
  public:
     typedef void(*ViewerSpinOnceCallback)(PangolinViewer*);
-    typedef void(*StartBtnCallback)(std::string);
+    typedef void(*StartLocateBtnCallback)(std::string);
+    typedef void(*StartBtnCallback)();
     typedef void(*StopBtnCallback)();
     typedef void(*FreqChangedCallback)(int);
     typedef bool(*DijkstraBtnCallback)(int ,int ,float& ,std::vector<int>&);
  public:
     static PangolinViewer* CreateViewer();
 
-    void SetCallbacks(ViewerSpinOnceCallback spinOnce,StartBtnCallback startBtn,
+    void SetCallbacks(ViewerSpinOnceCallback spinOnce,StartLocateBtnCallback startBtn,
      StopBtnCallback stopBtn,FreqChangedCallback freqChanged,DijkstraBtnCallback dijkstra);
+
+    void SetNavigationCallbacks(StartBtnCallback startBtn,
+                      StopBtnCallback stopBtn);
     void ShowRealtimePose(Eigen::Matrix4d pose);
     void ShowSlamCost(double cost);
     void ShowSlamQueueTime(int queue,double tm);
@@ -77,11 +81,13 @@ class PangolinViewer
     std::vector<int> shortest_path_;
 
     ViewerSpinOnceCallback spinOnce_callback_=nullptr;
-    StartBtnCallback startBtn_callback_=nullptr;
+    StartLocateBtnCallback startBtn_callback_=nullptr;
     StopBtnCallback stopBtn_callback_= nullptr;
     FreqChangedCallback freqChanged_callback_= nullptr;
     DijkstraBtnCallback dijkstraBtn_callback_= nullptr;
 
+    StartBtnCallback naviagtion_startBtn_callback_=nullptr;
+    StopBtnCallback navigation_stopBtn_callback_= nullptr;
 
 };