mpc 动作消息增加起点， mpc速度设置1m/s，通过现场测试

CMakeLists.txt

@@ -58,6 +58,7 @@ add_definitions(${PCL_DEFINITIONS})
 #---------------------------------------------------------------------------------------
 add_executable(${PROJECT_NAME}
 		main.cpp
+		map.pb.cc
 		define/TimerRecord.cpp
 		pangolinViewer.cpp
 		dijkstra/dijkstra.cpp
@@ -69,6 +70,7 @@ add_executable(${PROJECT_NAME}
 		emqx/paho_client.cpp
 		emqx/message.pb.cc
 		tool/pathcreator.cpp
+		tool/proto_tool.cpp
 		emqx/terminator_emqx.cpp
 		)
 target_compile_options(${PROJECT_NAME}

config/navigation.prototxt

@@ -1,17 +0,0 @@
-Agv_emqx
-{
-	NodeId:"agv1"
-	ip:"127.0.0.1"
-	port:1883
-	pubTopic:"AgvCmd/cmd1"
-	subTopic:"AgvStatu/robot1"
-}
-
-Terminal_emqx
-{
-	NodeId:"agv1"
-	ip:"192.168.2.100"
-	port:1883
-	pubTopic:"NavStatu/nav1"
-	subTopic:"NavCmd/cmd1"
-}

emqx/mqttmsg.cpp

@@ -265,9 +265,8 @@ void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
   data_=(char*)malloc(length_);
   memcpy(data_,json_str.c_str(),length_);
 }
-bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message)const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
   if(data_== nullptr)
     return false;
 

emqx/mqttmsg.h

@@ -10,15 +10,6 @@
 class MqttMsg
 {
  public:
-    enum Dtype
-    {
-      ePointCloudXYZ,
-      ePointCloudXYZI,
-      eString,
-      eImage,
-      eAGVStatu,
-      eAGVSpeed
-    };
  public:
     MqttMsg();
     MqttMsg(char* data,const int length);
@@ -31,7 +22,7 @@ class MqttMsg
 
 
     void fromProtoMessage(const google::protobuf::Message& messge);
-    bool toProtoMessage(google::protobuf::Message& message);
+    bool toProtoMessage(google::protobuf::Message& message)const;
 
     /*void fromStatu(double x,double y,double theta,double v,double vth);
     void fromNavSpeed(const NavMessage::Speed& speed);

emqx/terminator_emqx.cpp

@@ -15,55 +15,59 @@ Terminator_emqx::~Terminator_emqx()
 
 bool Terminator_emqx::Connect(std::string ip,int port)
 {
-
   if(client_!= nullptr)
   {
     client_->disconnect();
     delete client_;
   }
   client_=new Paho_client(nodeId_);
-  bool ret= client_->connect(ip,port);
-  if(ret)
-  {
-    while(!client_->isconnected()) usleep(1000);
-    client_->subcribe(subTopic_,1,StatuArrivedCallback,this);
-  }
-  return ret;
+  connected_= client_->connect(ip,port);
+
+  return connected_;
 
 }
 
-void Terminator_emqx::SendNavCmd(const NavMessage::NavCmd& cmd)
+void Terminator_emqx::Publish(std::string topic,const MqttMsg& msg)
 {
-  MqttMsg msg;
-  msg.fromProtoMessage(cmd);
   if(client_)
-    client_->publish(pubTopic_,1,msg);
+    client_->publish(topic,1,msg);
   else
-    printf("Send NavCmd failed : emqx client disconnected...\n");
+    printf("publish  failed : emqx client maybe disconnected...\n");
 }
 
-void Terminator_emqx::set_statu_arrived(NavStatuCallback callback, void *context)
+bool Terminator_emqx::AddCallback(std::string topic,Callback callback,void* context)
 {
-  StatuArrivedCallback_=callback;
-  context_=context;
+  CallBackInfo info;
+  info.func=callback;
+  info.contextx=context;
+  sub_topic_callbacks_[topic]=info;
+  if(connected_)
+  {
+    while(!client_->isconnected()) usleep(1000);
+    client_->subcribe(topic,1,StatuArrivedCallback,this);
+    return true;
+  }
+  else
+  {
+    printf(" emqx has not connected ,subcribe topic:%s  failed ...!!\n",topic.c_str());
+    return false;
+  }
 }
 
 
-Terminator_emqx::Terminator_emqx(std::string nodeId,std::string pubTopic,std::string subTopic)
-    :client_(nullptr),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic)
+Terminator_emqx::Terminator_emqx(std::string nodeId)
+    :client_(nullptr),nodeId_(nodeId),connected_(false)
 {
-  StatuArrivedCallback_= nullptr;
-  context_= nullptr;
 }
 
 void Terminator_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
   Terminator_emqx* terminator=(Terminator_emqx*)context;
-  double x=0,y=0,theta=0,v=0,vth=0;
-  NavMessage::NavStatu statu;
-  if(msg.toProtoMessage(statu))
-  {
-    if (terminator->StatuArrivedCallback_)
-      terminator->StatuArrivedCallback_(statu,terminator->context_);
+
+  if(terminator->sub_topic_callbacks_.find(topic)!=terminator->sub_topic_callbacks_.end()) {
+    CallBackInfo info = terminator->sub_topic_callbacks_[topic];
+    if (info.func)
+      info.func(msg, info.contextx);
   }
+
 }

emqx/terminator_emqx.h

@@ -7,30 +7,32 @@
 #include <mutex>
 #include "paho_client.h"
 
-typedef void (*NavStatuCallback)(const NavMessage::NavStatu& statu,void* context);
+typedef void (*Callback)(const MqttMsg& msg,void* context);
+
+typedef struct{
+    Callback func;
+    void* contextx;
+}CallBackInfo;
 
 class Terminator_emqx {
 public:
-    Terminator_emqx(std::string nodeId,std::string pubTopic,std::string subTopic);
+    Terminator_emqx(std::string nodeId);
     ~Terminator_emqx();
     bool Connect(std::string ip,int port);
-    void set_statu_arrived(NavStatuCallback callback,void* context);
-    void SendNavCmd(const NavMessage::NavCmd& cmd);
+    bool AddCallback(std::string topic,Callback callback,void* context);
+    void Publish(std::string topic,const MqttMsg& statu);
 
 protected:
 
-    NavStatuCallback StatuArrivedCallback_;
     static void StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context);
 
     std::mutex mtx_;
 
     Paho_client* client_;
-    void* context_;
+    bool connected_;
     std::string nodeId_;
-    std::string pubTopic_;
-    std::string subTopic_;
 public:
-    MqttMsg msg_;
+    std::map<std::string,CallBackInfo> sub_topic_callbacks_;
 };
 
 #endif //NAVIGATION_TERMINATOR_EMQX_H

main.cpp

@@ -10,8 +10,9 @@
 #include "dijkstra/trajectory.h"
 #include "emqx/terminator_emqx.h"
 #include "tool/pathcreator.h"
+#include "tool/proto_tool.h"
 #include <queue>
-
+#include "map.pb.h"
 
 Terminator_emqx* g_terminator= nullptr;
 //路径相关
@@ -22,30 +23,48 @@ PathMap DijkstraMap;
 
 //控制相关
 TimedLockData<NavMessage::NavStatu> timed_navStatu_;
+TimedLockData<NavMessage::AGVStatu> timed_AgvStatu_;
+TimedLockData<NavMessage::AGVStatu> timed_AgvBrotherStatu_;
 
 
 bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs);
 
 bool InitMap(std::string config_file);
-bool InitDijkstraMap();
+
 
 void pangolinSpinOnce(PangolinViewer* viewer) {
 
+  if (timed_AgvStatu_.timeout() == false) {
+    NavMessage::AGVStatu statu = timed_AgvStatu_.Get();
+
+    Pose2d pose2d(statu.x(), statu.y(), statu.theta());
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
+                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
+                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
+
+    Eigen::Matrix4d pose;
+    pose.topLeftCorner(3, 3) = rotation_matrix3;
+    pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
+    viewer->ShowRealtimeStatu(pose, statu.v(), statu.vth());
+  }
+  //显示另一节agv状态
+  if (timed_AgvBrotherStatu_.timeout() == false) {
+    NavMessage::AGVStatu statu = timed_AgvBrotherStatu_.Get();
+    Pose2d pose2d(statu.x(), statu.y(), statu.theta());
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
+                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
+                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
+
+    Eigen::Matrix4d pose;
+    pose.topLeftCorner(3, 3) = rotation_matrix3;
+    pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
+    viewer->ShowBrotherStatu(pose, statu.v(), statu.vth());
+  }
+  //绘制导航实时状态
   if (timed_navStatu_.timeout() == false) {
     NavMessage::NavStatu statu = timed_navStatu_.Get();
-    if (statu.pose_timeout() == false && statu.twist_timeout() == false) {
-      Pose2d pose2d(statu.current_pose().x(), statu.current_pose().y(), statu.current_pose().theta());
-      Eigen::Matrix3d rotation_matrix3;
-      rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
-                         Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
-                         Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
-
-      Eigen::Matrix4d pose;
-      pose.topLeftCorner(3, 3) = rotation_matrix3;
-      pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
-      viewer->ShowRealtimeStatu(pose, statu.current_pose().v(), statu.current_pose().vth());
-    }
-
     Trajectory select_traj;
     Trajectory predict_traj;
     for (int i = 0; i < statu.selected_traj().poses_size(); ++i) {
@@ -62,7 +81,6 @@ void pangolinSpinOnce(PangolinViewer* viewer) {
     viewer->DrawTrajectory(predict_traj, 0, 1, 1, 15);
   }
 
-
   //绘制dijkstra地图及最短路径
   pathMutex.lock();
   std::vector<int> solve_path = shortest_path;
@@ -107,8 +125,11 @@ void navigation_start()
     return;
   std::cout<<" ------------------------------------new NavCmd ---------------------------"<<std::endl;
   std::cout<<cmd.DebugString()<<std::endl<<std::endl;
-  if(g_terminator)
-    g_terminator->SendNavCmd(cmd);
+  if(g_terminator) {
+    MqttMsg msg;
+    msg.fromProtoMessage(cmd);
+    g_terminator->Publish("agv1_child/nav_cmd", msg);
+  }
 
 }
 
@@ -116,44 +137,69 @@ void navigation_pause()
 {
   NavMessage::NavCmd cmd;
   cmd.set_action(1);  //暂停
-  if(g_terminator)
-    g_terminator->SendNavCmd(cmd);
+  if(g_terminator) {
+    MqttMsg msg;
+    msg.fromProtoMessage(cmd);
+    g_terminator->Publish("agv1_child/nav_cmd", msg);
+  }
 }
 void navigation_stop()
 {
   NavMessage::NavCmd cmd;
   cmd.set_action(3);  //取消导航
-  if(g_terminator)
-    g_terminator->SendNavCmd(cmd);
+  if(g_terminator) {
+    MqttMsg msg;
+    msg.fromProtoMessage(cmd);
+    g_terminator->Publish("agv1_child/nav_cmd", msg);
+  }
 }
 
+void AgvStatuArrived(const MqttMsg& msg,void* context)
+{
+  NavMessage::AGVStatu statu;
+  if(msg.toProtoMessage(statu))
+    timed_AgvStatu_.reset(statu,0.5);
 
-void NavStatuArrived(const NavMessage::NavStatu& statu,void* context)
+}
+void AgvBrotherStatuArrived(const MqttMsg& msg,void* context)
+{
+  NavMessage::AGVStatu statu;
+  if(msg.toProtoMessage(statu))
+    timed_AgvBrotherStatu_.reset(statu,0.5);
+
+}
+void NavStatuArrived(const MqttMsg& msg,void* context)
 {
-  timed_navStatu_.reset(statu,0.5);
+  NavMessage::NavStatu statu;
+  if(msg.toProtoMessage(statu))
+    timed_navStatu_.reset(statu,0.5);
 
 }
 
+
 int main(int argc, char** argv)
 {
-  //reg参数
-  std::string config_file="../config/horizon_config.yaml";
+  if(argc<2)
+  {
+    printf(" argc must >2  exe mapfile\n");
+    return -1;
+  }
+  std::string config_file=argv[1];
   if(InitMap(config_file)==false)
     return -1;
 
-  if(!InitDijkstraMap())
-  {
-    printf("Dijkstra Map load failed\n");
-    return -2;
-  }
 
-  g_terminator=new Terminator_emqx("UI_nav","agv1/nav_cmd","agv1/nav_statu");
-  g_terminator->set_statu_arrived(NavStatuArrived, nullptr);
-  if(g_terminator->Connect("192.168.2.100",1883)==false)
+  //"agv1/nav_statu"
+  g_terminator=new Terminator_emqx("UI_nav-child");
+
+  if(g_terminator->Connect("192.168.0.70",1883)==false)
   {
     printf(" AGV emqx connect failed\n");
     return -1;
   }
+  g_terminator->AddCallback("agv1_child/nav_statu",NavStatuArrived, nullptr);
+  g_terminator->AddCallback("agv1_child/agv_statu",AgvStatuArrived, nullptr);
+  g_terminator->AddCallback("agv1/agv_statu",AgvBrotherStatuArrived, nullptr);
 
 
   PangolinViewer* viewer=PangolinViewer::CreateViewer();
@@ -170,32 +216,43 @@ int main(int argc, char** argv)
 
 bool InitMap(std::string config_file)
 {
+  MapMessage::Map proto_map;
+  if(proto_tool::read_proto_param(config_file,proto_map)==false)
+  {
+    printf(" read map proto failed\n");
+    return false;
+  }
+
+  std::map<int,int> exist_id;
+  for(int i=0;i<proto_map.nodes_size();++i)
+  {
+    MapMessage::Node node=proto_map.nodes(i);
+    if(exist_id.find(node.id())!=exist_id.end())
+    {
+      printf("Can not add vertex, Node Id :%d exist\n",node.id());
+      return false;
+    }
+    DijkstraMap.AddVertex(node.id(),node.x(),node.y());
+    exist_id[node.id()]=0;
+  }
+
+  for(int i=0;i<proto_map.edges_size();++i)
+  {
+    MapMessage::Edge edge=proto_map.edges(i);
+    if(exist_id.find(edge.id1())!=exist_id.end() && exist_id.find(edge.id2())!=exist_id.end())
+    {
+      if(false==DijkstraMap.AddEdge(edge.id1(),edge.id2(),edge.directed()))
+        return false;
+    }
+    else
+    {
+      printf(" Cant add edge id1 or id2 (%d %d) is not exist\n",edge.id1(),edge.id2());
+      return false;
+    }
+  }
   return true;
 }
 
-bool InitDijkstraMap()
-{
-  DijkstraMap.AddVertex(1, 0, 0);
-  DijkstraMap.AddVertex(2, 20, 0);
-  DijkstraMap.AddVertex(3, 60, 0);
-  DijkstraMap.AddVertex(4, 100, 0);
-  DijkstraMap.AddVertex(5, 100, -16);
-  DijkstraMap.AddVertex(6, 60, -16);
-  DijkstraMap.AddVertex(7, 20, 20);
-
-  bool ret=true;
-  ret=ret&&DijkstraMap.AddEdge(1, 2, false);
-  ret=ret&&DijkstraMap.AddEdge(1, 3, false);
-  ret=ret&&DijkstraMap.AddEdge(1, 4, false);
-  ret=ret&&DijkstraMap.AddEdge(2, 3, false);
-  ret=ret&&DijkstraMap.AddEdge(2, 4, false);
-  ret=ret&&DijkstraMap.AddEdge(2, 7, false);
-  ret=ret&&DijkstraMap.AddEdge(3, 4, false);
-  ret=ret&&DijkstraMap.AddEdge(3, 6, false);
-  ret=ret&&DijkstraMap.AddEdge(4, 5, false);
-  ret=ret&&DijkstraMap.AddEdge(5, 6, false);
-  return ret;
-}
 
 bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
 {
@@ -237,27 +294,49 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
   headdiff.set_y(0.1);
   headdiff.set_theta(2*M_PI/180.0);
 
-  enddiff.set_x(0.05);
-  enddiff.set_y(0.03);
+  enddiff.set_x(0.03);
+  enddiff.set_y(0.05);
   enddiff.set_theta(0.5*M_PI/180.0);
 
+
   //调整到起点，朝向与当前朝向一致
-  if(timed_navStatu_.timeout()|| timed_navStatu_.Get().pose_timeout())
+  if(timed_AgvStatu_.timeout())
   {
     printf(" agv statu is timeout can not get current direction\n");
     return false;
   }
-  float current_theta=timed_navStatu_.Get().current_pose().theta();
+  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(10);
+
+  NavMessage::SpeedLimit mpc_x_limit,mpc_angular_limit;
+  mpc_x_limit.set_min(0.05);
+  mpc_x_limit.set_max(1.0);
+
+  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(headdiff);
+
+  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)
   {
     //调整到上一点位姿，朝向当前点
@@ -265,17 +344,24 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
     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(headdiff);
+    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);
@@ -283,8 +369,10 @@ bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
       act_along.mutable_target_diff()->CopyFrom(enddiff);
     else
       act_along.mutable_target_diff()->CopyFrom(headdiff);
+    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;
 
-}
+}

message.proto

@@ -9,11 +9,18 @@ message AGVStatu {
 }
 
 message Speed {
-  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他：停止
-  float v=2;
-  float vth=3;
+  int32 H=1;  //心跳
+  int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
+  float V=3;  //角速度
+  float W=4;  //线速度
+  int32 end=5;
 }
 
+message SpeedLimit
+{
+  float min=1;
+  float max=2;
+}
 
 message Pose2d
 {
@@ -29,9 +36,13 @@ message Trajectory
 
 message Action
 {
-  int32 type=1; // 1:原地调整，2：巡线
-  Pose2d target=2;
-  Pose2d target_diff=3;
+  int32 type = 1; // 1:原地调整，2：巡线
+  Pose2d begin = 2;
+  Pose2d target = 3;
+  Pose2d target_diff = 4;
+  SpeedLimit velocity_limit=5;
+  SpeedLimit angular_limit=6;
+  SpeedLimit horize_limit=7;
 }
 
 
@@ -44,14 +55,11 @@ message NavCmd
 
 message NavStatu
 {
-  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;
+  bool statu = 1; //0:ready  1:running
+  string key = 2; // 任务唯一码
+  repeated Action unfinished_actions = 3;  //未完成的动作，第一个即为当前动作
+  Trajectory selected_traj = 4;
+  Trajectory predict_traj = 5;
 
 }
 

pangolinViewer.cpp

@@ -80,8 +80,8 @@ void PangolinViewer::Init()
   rpy_string_=new pangolin::Var<std::string>("ui.rpy_String", "rpy");
   velocity_string_=new pangolin::Var<std::string>("ui.velocity", "");
 
-  beg_Int_=new pangolin::Var<int>("ui.beg", 1, 1, 7);
-  end_Int_=new pangolin::Var<int>("ui.end", 5, 1, 7);
+  beg_Int_=new pangolin::Var<int>("ui.beg", 1, 1, 8);
+  end_Int_=new pangolin::Var<int>("ui.end", 5, 1, 8);
   v_=new pangolin::Var<double>("ui.v", 1.0, -2.0,2.0 );
   a_=new pangolin::Var<double>("ui.a", 1.0, -10.0, 10.0);
   path_string_=new pangolin::Var<std::string>("ui.path", "cost");
@@ -246,7 +246,7 @@ void PangolinViewer::DrawDijkastraMap(PathMap& map,std::vector<int> path)
 
 }
 
-void PangolinViewer::ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,double angular)
+void PangolinViewer::ShowBrotherStatu(Eigen::Matrix4d pose,double velocity,double angular)
 {
   double l=2;
   Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
@@ -254,6 +254,29 @@ void PangolinViewer::ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,doub
   Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
   Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;
 
+  glLineWidth(5);
+  glBegin(GL_LINES);
+  glColor3f(1.0, 0, 0.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Xw[0], Xw[1], Xw[2]);
+  glColor3f(1.0, 1.0, 1.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Yw[0], Yw[1], Yw[2]);
+  glColor3f(0, 1.0, 1.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Zw[0], Zw[1], Zw[2]);
+  glEnd();
+
+}
+
+void PangolinViewer::ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,double angular)
+{
+  double l=10;
+  Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
+  Eigen::Vector3d Xw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(1,0,0))+Ow;
+  Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
+  Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;
+
   glLineWidth(5);
   glBegin(GL_LINES);
   glColor3f(1.0, 0.0, 0.0);

pangolinViewer.h

@@ -38,6 +38,7 @@ class PangolinViewer
                                 PauseBtnCallback pauseBtn,
                       StopBtnCallback stopBtn);
     void ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,double angular);
+    void ShowBrotherStatu(Eigen::Matrix4d pose,double velocity,double angular);
 
     void DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
      double alpha,double psize);

proto.sh

@@ -1,4 +1,4 @@
 
 protoc -I=./ message.proto --cpp_out=./emqx
 
-
+protoc -I=./ map.proto --cpp_out=./