完成导航启动，暂停，导航状态发布。

CMakeLists.txt

@@ -9,19 +9,19 @@ cmake_minimum_required(VERSION 2.8.3)
 project(navigation)
 
 #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+#SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
 #---------------------------------------------------------------------------------------
 # find package and the dependecy
 #---------------------------------------------------------------------------------------
-find_package(Boost 1.54 REQUIRED COMPONENTS
-	system
-	thread
-	chrono
-	)
+#find_package(Boost 1.54 REQUIRED COMPONENTS
+#	system
+#	thread
+#	chrono
+#	)
 
 find_package (Eigen3 REQUIRED CONFIG QUIET)
-find_package(PCL REQUIRED)
+#find_package(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
 
@@ -44,12 +44,11 @@ include_directories(
 		define
 		/usr/local/include
 		${EIGEN3_INCLUDE_DIR}
-		${PCL_INCLUDE_DIRS}
 		)
 link_directories("/usr/local/lib")
 ## PCL library
-link_directories(${PCL_LIBRARY_DIRS})
-add_definitions(${PCL_DEFINITIONS})
+#link_directories(${PCL_LIBRARY_DIRS})
+#add_definitions(${PCL_DEFINITIONS})
 
 #---------------------------------------------------------------------------------------
 # generate excutable and add libraries
@@ -75,15 +74,12 @@ add_executable(${PROJECT_NAME}
 		)
 
 #---------------------------------------------------------------------------------------
-# link libraries
+# link libraries ${Boost_LIBRARY} -lpthread
 #---------------------------------------------------------------------------------------
 target_link_libraries(${PROJECT_NAME}
-		${PCL_LIBRARIES}
-		${Boost_LIBRARY}
 		${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES}
 		libglog.a libgflags.a
 		paho-mqtt3a
-		-lpthread
 		ipopt
 		)
 #---------------------------------------------------------------------------------------

MPC/monitor/emqx/message.pb.cc

@@ -168,6 +168,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::NavCmd, action_),
   PROTOBUF_FIELD_OFFSET(::NavCmd, trajs_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavStatu, _internal_metadata_),
@@ -179,6 +180,8 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavStatu, action_type_),
   PROTOBUF_FIELD_OFFSET(::NavStatu, angular_),
   PROTOBUF_FIELD_OFFSET(::NavStatu, velocity_),
+  PROTOBUF_FIELD_OFFSET(::NavStatu, select_traj_),
+  PROTOBUF_FIELD_OFFSET(::NavStatu, predict_traj_),
 };
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::AGVStatu)},
@@ -186,7 +189,7 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 17, -1, sizeof(::Pose2dMsg)},
   { 25, -1, sizeof(::TrajectoryMsg)},
   { 31, -1, sizeof(::NavCmd)},
-  { 37, -1, sizeof(::NavStatu)},
+  { 38, -1, sizeof(::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -204,11 +207,14 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "th\030\005 \001(\002\"(\n\016ChangeSpeedCmd\022\t\n\001v\030\001 \001(\002\022\013\n"
   "\003vth\030\002 \001(\002\"0\n\tPose2dMsg\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030"
   "\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"+\n\rTrajectoryMsg\022\032\n"
-  "\006points\030\001 \003(\0132\n.Pose2dMsg\"\'\n\006NavCmd\022\035\n\005t"
-  "rajs\030\001 \003(\0132\016.TrajectoryMsg\"t\n\010NavStatu\022\021"
-  "\n\tisrunning\030\001 \001(\010\022\035\n\005trajs\030\002 \003(\0132\016.Traje"
-  "ctoryMsg\022\023\n\013action_type\030\003 \001(\005\022\017\n\007angular"
-  "\030\004 \001(\002\022\020\n\010velocity\030\005 \001(\002b\006proto3"
+  "\006points\030\001 \003(\0132\n.Pose2dMsg\"7\n\006NavCmd\022\016\n\006a"
+  "ction\030\001 \001(\005\022\035\n\005trajs\030\002 \003(\0132\016.TrajectoryM"
+  "sg\"\277\001\n\010NavStatu\022\021\n\tisrunning\030\001 \001(\010\022\035\n\005tr"
+  "ajs\030\002 \003(\0132\016.TrajectoryMsg\022\023\n\013action_type"
+  "\030\003 \001(\005\022\017\n\007angular\030\004 \001(\002\022\020\n\010velocity\030\005 \001("
+  "\002\022#\n\013select_traj\030\006 \001(\0132\016.TrajectoryMsg\022$"
+  "\n\014predict_traj\030\007 \001(\0132\016.TrajectoryMsgb\006pr"
+  "oto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -222,7 +228,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 392,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 484,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 6, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 6, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -1218,11 +1224,13 @@ NavCmd::NavCmd(const NavCmd& from)
   : ::PROTOBUF_NAMESPACE_ID::Message(),
       trajs_(from.trajs_) {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  action_ = from.action_;
   // @@protoc_insertion_point(copy_constructor:NavCmd)
 }
 
 void NavCmd::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavCmd_message_2eproto.base);
+  action_ = 0;
 }
 
 NavCmd::~NavCmd() {
@@ -1257,6 +1265,7 @@ void NavCmd::Clear() {
   (void) cached_has_bits;
 
   trajs_.Clear();
+  action_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1268,16 +1277,23 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // repeated .TrajectoryMsg trajs = 1;
+      // int32 action = 1;
       case 1:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 10)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
+          action_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // repeated .TrajectoryMsg trajs = 2;
+      case 2:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 18)) {
           ptr -= 1;
           do {
             ptr += 1;
             ptr = ctx->ParseMessage(_internal_add_trajs(), ptr);
             CHK_(ptr);
             if (!ctx->DataAvailable(ptr)) break;
-          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<10>(ptr));
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<18>(ptr));
         } else goto handle_unusual;
         continue;
       default: {
@@ -1308,12 +1324,18 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // repeated .TrajectoryMsg trajs = 1;
+  // int32 action = 1;
+  if (this->action() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_action(), target);
+  }
+
+  // repeated .TrajectoryMsg trajs = 2;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_trajs_size()); i < n; i++) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(1, this->_internal_trajs(i), target, stream);
+      InternalWriteMessage(2, this->_internal_trajs(i), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1332,13 +1354,20 @@ size_t NavCmd::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // repeated .TrajectoryMsg trajs = 1;
+  // repeated .TrajectoryMsg trajs = 2;
   total_size += 1UL * this->_internal_trajs_size();
   for (const auto& msg : this->trajs_) {
     total_size +=
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg);
   }
 
+  // int32 action = 1;
+  if (this->action() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_action());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -1371,6 +1400,9 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   (void) cached_has_bits;
 
   trajs_.MergeFrom(from.trajs_);
+  if (from.action() != 0) {
+    _internal_set_action(from._internal_action());
+  }
 }
 
 void NavCmd::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -1395,6 +1427,7 @@ void NavCmd::InternalSwap(NavCmd* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   trajs_.InternalSwap(&other->trajs_);
+  swap(action_, other->action_);
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {
@@ -1405,11 +1438,25 @@ void NavCmd::InternalSwap(NavCmd* other) {
 // ===================================================================
 
 void NavStatu::InitAsDefaultInstance() {
+  ::_NavStatu_default_instance_._instance.get_mutable()->select_traj_ = const_cast< ::TrajectoryMsg*>(
+      ::TrajectoryMsg::internal_default_instance());
+  ::_NavStatu_default_instance_._instance.get_mutable()->predict_traj_ = const_cast< ::TrajectoryMsg*>(
+      ::TrajectoryMsg::internal_default_instance());
 }
 class NavStatu::_Internal {
  public:
+  static const ::TrajectoryMsg& select_traj(const NavStatu* msg);
+  static const ::TrajectoryMsg& predict_traj(const NavStatu* msg);
 };
 
+const ::TrajectoryMsg&
+NavStatu::_Internal::select_traj(const NavStatu* msg) {
+  return *msg->select_traj_;
+}
+const ::TrajectoryMsg&
+NavStatu::_Internal::predict_traj(const NavStatu* msg) {
+  return *msg->predict_traj_;
+}
 NavStatu::NavStatu(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena),
   trajs_(arena) {
@@ -1421,6 +1468,16 @@ NavStatu::NavStatu(const NavStatu& from)
   : ::PROTOBUF_NAMESPACE_ID::Message(),
       trajs_(from.trajs_) {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  if (from._internal_has_select_traj()) {
+    select_traj_ = new ::TrajectoryMsg(*from.select_traj_);
+  } else {
+    select_traj_ = nullptr;
+  }
+  if (from._internal_has_predict_traj()) {
+    predict_traj_ = new ::TrajectoryMsg(*from.predict_traj_);
+  } else {
+    predict_traj_ = nullptr;
+  }
   ::memcpy(&isrunning_, &from.isrunning_,
     static_cast<size_t>(reinterpret_cast<char*>(&velocity_) -
     reinterpret_cast<char*>(&isrunning_)) + sizeof(velocity_));
@@ -1429,9 +1486,9 @@ NavStatu::NavStatu(const NavStatu& from)
 
 void NavStatu::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavStatu_message_2eproto.base);
-  ::memset(&isrunning_, 0, static_cast<size_t>(
+  ::memset(&select_traj_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&velocity_) -
-      reinterpret_cast<char*>(&isrunning_)) + sizeof(velocity_));
+      reinterpret_cast<char*>(&select_traj_)) + sizeof(velocity_));
 }
 
 NavStatu::~NavStatu() {
@@ -1442,6 +1499,8 @@ NavStatu::~NavStatu() {
 
 void NavStatu::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
+  if (this != internal_default_instance()) delete select_traj_;
+  if (this != internal_default_instance()) delete predict_traj_;
 }
 
 void NavStatu::ArenaDtor(void* object) {
@@ -1466,6 +1525,14 @@ void NavStatu::Clear() {
   (void) cached_has_bits;
 
   trajs_.Clear();
+  if (GetArena() == nullptr && select_traj_ != nullptr) {
+    delete select_traj_;
+  }
+  select_traj_ = nullptr;
+  if (GetArena() == nullptr && predict_traj_ != nullptr) {
+    delete predict_traj_;
+  }
+  predict_traj_ = nullptr;
   ::memset(&isrunning_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&velocity_) -
       reinterpret_cast<char*>(&isrunning_)) + sizeof(velocity_));
@@ -1520,6 +1587,20 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // .TrajectoryMsg select_traj = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 50)) {
+          ptr = ctx->ParseMessage(_internal_mutable_select_traj(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // .TrajectoryMsg predict_traj = 7;
+      case 7:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 58)) {
+          ptr = ctx->ParseMessage(_internal_mutable_predict_traj(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -1580,6 +1661,22 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(5, this->_internal_velocity(), target);
   }
 
+  // .TrajectoryMsg select_traj = 6;
+  if (this->has_select_traj()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        6, _Internal::select_traj(this), target, stream);
+  }
+
+  // .TrajectoryMsg predict_traj = 7;
+  if (this->has_predict_traj()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        7, _Internal::predict_traj(this), target, stream);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -1603,6 +1700,20 @@ size_t NavStatu::ByteSizeLong() const {
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg);
   }
 
+  // .TrajectoryMsg select_traj = 6;
+  if (this->has_select_traj()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *select_traj_);
+  }
+
+  // .TrajectoryMsg predict_traj = 7;
+  if (this->has_predict_traj()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *predict_traj_);
+  }
+
   // bool isrunning = 1;
   if (this->isrunning() != 0) {
     total_size += 1 + 1;
@@ -1657,6 +1768,12 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   (void) cached_has_bits;
 
   trajs_.MergeFrom(from.trajs_);
+  if (from.has_select_traj()) {
+    _internal_mutable_select_traj()->::TrajectoryMsg::MergeFrom(from._internal_select_traj());
+  }
+  if (from.has_predict_traj()) {
+    _internal_mutable_predict_traj()->::TrajectoryMsg::MergeFrom(from._internal_predict_traj());
+  }
   if (from.isrunning() != 0) {
     _internal_set_isrunning(from._internal_isrunning());
   }
@@ -1696,9 +1813,9 @@ void NavStatu::InternalSwap(NavStatu* other) {
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
       PROTOBUF_FIELD_OFFSET(NavStatu, velocity_)
       + sizeof(NavStatu::velocity_)
-      - PROTOBUF_FIELD_OFFSET(NavStatu, isrunning_)>(
-          reinterpret_cast<char*>(&isrunning_),
-          reinterpret_cast<char*>(&other->isrunning_));
+      - PROTOBUF_FIELD_OFFSET(NavStatu, select_traj_)>(
+          reinterpret_cast<char*>(&select_traj_),
+          reinterpret_cast<char*>(&other->select_traj_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavStatu::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -830,9 +830,10 @@ class NavCmd PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kTrajsFieldNumber = 1,
+    kTrajsFieldNumber = 2,
+    kActionFieldNumber = 1,
   };
-  // repeated .TrajectoryMsg trajs = 1;
+  // repeated .TrajectoryMsg trajs = 2;
   int trajs_size() const;
   private:
   int _internal_trajs_size() const;
@@ -850,6 +851,15 @@ class NavCmd PROTOBUF_FINAL :
   const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::TrajectoryMsg >&
       trajs() const;
 
+  // int32 action = 1;
+  void clear_action();
+  ::PROTOBUF_NAMESPACE_ID::int32 action() const;
+  void set_action(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_action() const;
+  void _internal_set_action(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
   // @@protoc_insertion_point(class_scope:NavCmd)
  private:
   class _Internal;
@@ -858,6 +868,7 @@ class NavCmd PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::TrajectoryMsg > trajs_;
+  ::PROTOBUF_NAMESPACE_ID::int32 action_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -977,6 +988,8 @@ class NavStatu PROTOBUF_FINAL :
 
   enum : int {
     kTrajsFieldNumber = 2,
+    kSelectTrajFieldNumber = 6,
+    kPredictTrajFieldNumber = 7,
     kIsrunningFieldNumber = 1,
     kActionTypeFieldNumber = 3,
     kAngularFieldNumber = 4,
@@ -1000,6 +1013,42 @@ class NavStatu PROTOBUF_FINAL :
   const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::TrajectoryMsg >&
       trajs() const;
 
+  // .TrajectoryMsg select_traj = 6;
+  bool has_select_traj() const;
+  private:
+  bool _internal_has_select_traj() const;
+  public:
+  void clear_select_traj();
+  const ::TrajectoryMsg& select_traj() const;
+  ::TrajectoryMsg* release_select_traj();
+  ::TrajectoryMsg* mutable_select_traj();
+  void set_allocated_select_traj(::TrajectoryMsg* select_traj);
+  private:
+  const ::TrajectoryMsg& _internal_select_traj() const;
+  ::TrajectoryMsg* _internal_mutable_select_traj();
+  public:
+  void unsafe_arena_set_allocated_select_traj(
+      ::TrajectoryMsg* select_traj);
+  ::TrajectoryMsg* unsafe_arena_release_select_traj();
+
+  // .TrajectoryMsg predict_traj = 7;
+  bool has_predict_traj() const;
+  private:
+  bool _internal_has_predict_traj() const;
+  public:
+  void clear_predict_traj();
+  const ::TrajectoryMsg& predict_traj() const;
+  ::TrajectoryMsg* release_predict_traj();
+  ::TrajectoryMsg* mutable_predict_traj();
+  void set_allocated_predict_traj(::TrajectoryMsg* predict_traj);
+  private:
+  const ::TrajectoryMsg& _internal_predict_traj() const;
+  ::TrajectoryMsg* _internal_mutable_predict_traj();
+  public:
+  void unsafe_arena_set_allocated_predict_traj(
+      ::TrajectoryMsg* predict_traj);
+  ::TrajectoryMsg* unsafe_arena_release_predict_traj();
+
   // bool isrunning = 1;
   void clear_isrunning();
   bool isrunning() const;
@@ -1044,6 +1093,8 @@ class NavStatu PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::TrajectoryMsg > trajs_;
+  ::TrajectoryMsg* select_traj_;
+  ::TrajectoryMsg* predict_traj_;
   bool isrunning_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_type_;
   float angular_;
@@ -1317,7 +1368,27 @@ TrajectoryMsg::points() const {
 
 // NavCmd
 
-// repeated .TrajectoryMsg trajs = 1;
+// int32 action = 1;
+inline void NavCmd::clear_action() {
+  action_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavCmd::_internal_action() const {
+  return action_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavCmd::action() const {
+  // @@protoc_insertion_point(field_get:NavCmd.action)
+  return _internal_action();
+}
+inline void NavCmd::_internal_set_action(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  action_ = value;
+}
+inline void NavCmd::set_action(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_action(value);
+  // @@protoc_insertion_point(field_set:NavCmd.action)
+}
+
+// repeated .TrajectoryMsg trajs = 2;
 inline int NavCmd::_internal_trajs_size() const {
   return trajs_.size();
 }
@@ -1479,6 +1550,172 @@ inline void NavStatu::set_velocity(float value) {
   // @@protoc_insertion_point(field_set:NavStatu.velocity)
 }
 
+// .TrajectoryMsg select_traj = 6;
+inline bool NavStatu::_internal_has_select_traj() const {
+  return this != internal_default_instance() && select_traj_ != nullptr;
+}
+inline bool NavStatu::has_select_traj() const {
+  return _internal_has_select_traj();
+}
+inline void NavStatu::clear_select_traj() {
+  if (GetArena() == nullptr && select_traj_ != nullptr) {
+    delete select_traj_;
+  }
+  select_traj_ = nullptr;
+}
+inline const ::TrajectoryMsg& NavStatu::_internal_select_traj() const {
+  const ::TrajectoryMsg* p = select_traj_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::TrajectoryMsg*>(
+      &::_TrajectoryMsg_default_instance_);
+}
+inline const ::TrajectoryMsg& NavStatu::select_traj() const {
+  // @@protoc_insertion_point(field_get:NavStatu.select_traj)
+  return _internal_select_traj();
+}
+inline void NavStatu::unsafe_arena_set_allocated_select_traj(
+    ::TrajectoryMsg* select_traj) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(select_traj_);
+  }
+  select_traj_ = select_traj;
+  if (select_traj) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavStatu.select_traj)
+}
+inline ::TrajectoryMsg* NavStatu::release_select_traj() {
+  
+  ::TrajectoryMsg* temp = select_traj_;
+  select_traj_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::TrajectoryMsg* NavStatu::unsafe_arena_release_select_traj() {
+  // @@protoc_insertion_point(field_release:NavStatu.select_traj)
+  
+  ::TrajectoryMsg* temp = select_traj_;
+  select_traj_ = nullptr;
+  return temp;
+}
+inline ::TrajectoryMsg* NavStatu::_internal_mutable_select_traj() {
+  
+  if (select_traj_ == nullptr) {
+    auto* p = CreateMaybeMessage<::TrajectoryMsg>(GetArena());
+    select_traj_ = p;
+  }
+  return select_traj_;
+}
+inline ::TrajectoryMsg* NavStatu::mutable_select_traj() {
+  // @@protoc_insertion_point(field_mutable:NavStatu.select_traj)
+  return _internal_mutable_select_traj();
+}
+inline void NavStatu::set_allocated_select_traj(::TrajectoryMsg* select_traj) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete select_traj_;
+  }
+  if (select_traj) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(select_traj);
+    if (message_arena != submessage_arena) {
+      select_traj = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, select_traj, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  select_traj_ = select_traj;
+  // @@protoc_insertion_point(field_set_allocated:NavStatu.select_traj)
+}
+
+// .TrajectoryMsg predict_traj = 7;
+inline bool NavStatu::_internal_has_predict_traj() const {
+  return this != internal_default_instance() && predict_traj_ != nullptr;
+}
+inline bool NavStatu::has_predict_traj() const {
+  return _internal_has_predict_traj();
+}
+inline void NavStatu::clear_predict_traj() {
+  if (GetArena() == nullptr && predict_traj_ != nullptr) {
+    delete predict_traj_;
+  }
+  predict_traj_ = nullptr;
+}
+inline const ::TrajectoryMsg& NavStatu::_internal_predict_traj() const {
+  const ::TrajectoryMsg* p = predict_traj_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::TrajectoryMsg*>(
+      &::_TrajectoryMsg_default_instance_);
+}
+inline const ::TrajectoryMsg& NavStatu::predict_traj() const {
+  // @@protoc_insertion_point(field_get:NavStatu.predict_traj)
+  return _internal_predict_traj();
+}
+inline void NavStatu::unsafe_arena_set_allocated_predict_traj(
+    ::TrajectoryMsg* predict_traj) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(predict_traj_);
+  }
+  predict_traj_ = predict_traj;
+  if (predict_traj) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavStatu.predict_traj)
+}
+inline ::TrajectoryMsg* NavStatu::release_predict_traj() {
+  
+  ::TrajectoryMsg* temp = predict_traj_;
+  predict_traj_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::TrajectoryMsg* NavStatu::unsafe_arena_release_predict_traj() {
+  // @@protoc_insertion_point(field_release:NavStatu.predict_traj)
+  
+  ::TrajectoryMsg* temp = predict_traj_;
+  predict_traj_ = nullptr;
+  return temp;
+}
+inline ::TrajectoryMsg* NavStatu::_internal_mutable_predict_traj() {
+  
+  if (predict_traj_ == nullptr) {
+    auto* p = CreateMaybeMessage<::TrajectoryMsg>(GetArena());
+    predict_traj_ = p;
+  }
+  return predict_traj_;
+}
+inline ::TrajectoryMsg* NavStatu::mutable_predict_traj() {
+  // @@protoc_insertion_point(field_mutable:NavStatu.predict_traj)
+  return _internal_mutable_predict_traj();
+}
+inline void NavStatu::set_allocated_predict_traj(::TrajectoryMsg* predict_traj) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete predict_traj_;
+  }
+  if (predict_traj) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(predict_traj);
+    if (message_arena != submessage_arena) {
+      predict_traj = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, predict_traj, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  predict_traj_ = predict_traj;
+  // @@protoc_insertion_point(field_set_allocated:NavStatu.predict_traj)
+}
+
 #ifdef __GNUC__
   #pragma GCC diagnostic pop
 #endif  // __GNUC__

MPC/monitor/emqx/mqttmsg.cpp

@@ -214,3 +214,46 @@ bool MqttMsg::toNavCmd(NavCmd& cmd)
 
   return ret.ok();
 }
+
+void MqttMsg::fromNavStatu(const 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(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();
+}

MPC/monitor/emqx/mqttmsg.h

@@ -32,10 +32,12 @@ class MqttMsg
     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);
 
     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);
 
 
  protected:

MPC/navigation.cpp

@@ -12,6 +12,12 @@ Navigation::~Navigation()
     delete terminator_;
   if(monitor_)
     delete monitor_;
+  if(thread_!= nullptr)
+  {
+    if(thread_->joinable())
+      thread_->join();
+    delete thread_;
+  }
 }
 
 bool Navigation::Init(const Navigation_parameter& parameter)
@@ -42,16 +48,65 @@ bool Navigation::Init(const Navigation_parameter& parameter)
 
 }
 
+void Navigation::publish_statu(ActionType type,float velocity,float angular)
+{
+  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());
+      }
+      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());
+        }
+      }
+      //发布 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());
+        }
+      }
+    }
+  }
+  terminator_->publish_nav_statu(statu);
+}
 
 void Navigation::ResetStatu(double v,double a)
 {
-  timedV_.reset(v,0.05);
-  timedA_.reset(a,0.05);
+  timedV_.reset(v,0.5);
+  timedA_.reset(a,0.5);
 }
 
 void Navigation::ResetPose(const Pose2d& pose)
 {
-  timedPose_.reset(pose,0.1);
+  timedPose_.reset(pose,0.5);
 }
 
 bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
@@ -82,22 +137,14 @@ bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
 void Navigation::Cancel()
 {
   cancel_=true;
-  if(thread_!= nullptr)
-  {
-    if(thread_->joinable())
-    {
-      thread_->join();
-      delete thread_;
-      thread_= nullptr;
-    }
-  }
+
 }
 void Navigation::Pause()
 {
+  monitor_->stop();
   pause_=true;
 }
 
-
 Navigation::Navigation()
 {
   thread_= nullptr;
@@ -109,6 +156,21 @@ Navigation::Navigation()
 void Navigation::NavCmdCallback(const NavCmd& cmd,void* context)
 {
   Navigation* navigator=(Navigation*)context;
+
+  if(cmd.action()==3)
+  {
+    navigator->Cancel();
+    return ;
+  }
+  if(cmd.action()==2) {
+    navigator->pause_=false;
+    return ;
+  }
+  if(cmd.action()==1) {
+    navigator->Pause();
+    return ;
+  }
+
   std::queue<Trajectory> trajs;
   for(int i=0;i<cmd.trajs_size();++i)
   {
@@ -192,6 +254,7 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
       {
         theta=(theta>0)?maxtheta:-maxtheta;
       }
+      publish_statu(eRotate,0,theta);
       monitor_->rotate(theta);
       printf("旋转 diff.y:%.5f\n",diff.y());
       continue;
@@ -209,6 +272,7 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
       {
         v=(v>0)?maxv:-maxv;
       }
+      publish_statu(eLine,v,0);
       monitor_->set_speed(v,0);
       printf("x移动 diff.x():%.5f\n",diff.x());
       continue;
@@ -227,6 +291,7 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
       {
         theta=(theta>0)?maxtheta:-maxtheta;
       }
+      publish_statu(eRotate,0,theta);
       monitor_->rotate(theta);
       printf("旋转angular:%.5f diff.theta():%.5f\n",theta,diff.theta());
     }
@@ -237,7 +302,6 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
   return true;
 }
 
-//待完成
 bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
 {
   if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
@@ -272,15 +336,6 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
   return ret;
 }
 
-Trajectory Navigation::selected_trajectory()
-{
-  return selected_traj_.Get();
-}
-Trajectory Navigation::predict_trajectory()
-{
-  return predict_traj_.Get();
-}
-
 bool Navigation::IsArrived(const Pose2d& cur,double speed,const Pose2d& target,const Pose2d& diff)
 {
     if(Pose2d::abs(cur-target)<diff)
@@ -313,7 +368,6 @@ bool Navigation::execute_edge(const Trajectory& child,const Pose2d& head_diff,co
       //发送暂停消息
       continue;
     }
-
     if(timedPose_.timeout()==true)
     {
       printf(" navigation Error:Pose is timeout \n");
@@ -350,7 +404,7 @@ 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]);
     }
 
@@ -381,9 +435,8 @@ void Navigation::navigatting()
   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)
+  while(global_trajectorys_.empty()==false && cancel_==false)
   {
-
     Trajectory traj=global_trajectorys_.front();
     if(execute_edge(traj,head_diff,target_diff))
     {
@@ -397,7 +450,7 @@ void Navigation::navigatting()
 #endif
   monitor_->stop();
   if(cancel_==true)
-    printf(" navigation sia canceled\n");
+    printf(" navigation canceled\n");
   printf(" navigation end...\n");
   if(global_trajectorys_.size()==0)
     printf("navigation completed!!!\n");

MPC/navigation.h

@@ -4,7 +4,6 @@
 
 #ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
 #define LIO_LIVOX_CPP_MPC_NAVIGATION_H_
-#include "../define/typedef.h"
 #include "../define/timedlockdata.hpp"
 #include "pose2d.h"
 #include "trajectory.h"
@@ -15,6 +14,13 @@
 #include <thread>
 class Navigation
 {
+public:
+    enum ActionType
+    {
+        eRotate=0,
+        eLine,
+        eMPC
+    };
  public:
     Navigation();
     ~Navigation();
@@ -37,9 +43,6 @@ class Navigation
     void Cancel();
     void Pause();
 
-    Trajectory selected_trajectory();
-    Trajectory predict_trajectory();
-
  protected:
 
     static void RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context);
@@ -51,7 +54,10 @@ class Navigation
     bool mpc_once(const Trajectory& traj,std::vector<double>& out);
     void navigatting();
 
-    void publish_statu();
+    /*
+     * 发布导航模块状态，当前动作类型、角速度、线速度
+     */
+    void publish_statu(ActionType type,float velocity,float angular);
 
  protected:
     std::mutex  mtx_;

define/typedef.h

@@ -1,37 +0,0 @@
-//
-// Created by zx on 22-11-2.
-//
-
-#ifndef SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
-#define SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-typedef pcl::PointXYZINormal PointType;
-typedef pcl::PointCloud<PointType> PointCloud;
-typedef struct
-{
-    double timebase;
-    pcl::PointCloud<PointType>::Ptr cloud;
-}TimeCloud;
-
-typedef void(*odomCallback)(Eigen::Matrix4d,double);
-typedef void(*CloudMappedCallback)(pcl::PointCloud<PointType>::Ptr,double);
-typedef void(*CostCallback)(double);
-
-
-typedef struct
-{
-    double x;
-    double y;
-    double z;
-}Data3d;
-typedef struct
-{
-    double timebase;
-    Data3d angular_velocity;
-    Data3d linear_acceleration;
-
-} ImuData;
-
-#endif //SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_

dijkstraSample.cpp

@@ -1,292 +0,0 @@
-//
-// Created by zx on 22-11-29.
-//
-
-#include "dijkstra/dijkstra.h"
-#include <pangolin/var/var.h>
-#include <pangolin/var/varextra.h>
-#include <pangolin/gl/gl.h>
-#include <pangolin/gl/gldraw.h>
-#include <pangolin/display/display.h>
-#include <pangolin/display/view.h>
-#include <pangolin/display/widgets.h>
-#include <pangolin/display/default_font.h>
-#include <pangolin/handler/handler.h>
-
-int start=1;
-int end=5;
-
-
-void DrawPath(PathMap& map,std::vector<int> path)
-{
-  float z=0;
-
-}
-
-void DrawMap(PathMap& map,std::vector<int> path,pangolin::GlFont* text_font)
-{
-
-  //绘制顶点
-  std::map<int,PathMap::MapNode> nodes=map.GetNodes();
-  std::map<int,PathMap::MapNode>::iterator it=nodes.begin();
-  float z=0;
-
-  glPointSize(50.0);
-// 开始画点
-  glBegin ( GL_POINTS );
-  glColor3f(0, 1, 0);
-  for(it;it!=nodes.end();it++)
-  {
-    PathMap::MapNode node=it->second;
-    // 设置点颜色，rgb值，float类型
-    glVertex3f(node.x, node.y, z);
-  }
-  glEnd();
-
-  glLineWidth(5);
-  glBegin(GL_LINES);
-  //绘制边
-  glColor3f(1.0, 1.0, 0.0);
-  std::vector<PathMap::MapEdge> edges=map.GetEdges();
-  for(int i=0;i<edges.size();++i)
-  {
-    PathMap::MapEdge edge=edges[i];
-    float x1,y1,x2,y2;
-    map.GetVertexPos(edge.id1,x1,y1);
-    map.GetVertexPos(edge.id2,x2,y2);
-
-    glVertex3d(x1,y1, z);
-    glVertex3d(x2,y2,z);
-  }
-
-  glEnd();
-
-  if(path.size()>0)
-  {
-    glLineWidth(10);
-    glBegin(GL_LINE_STRIP);
-    glColor3f(0, 0, 1);
-    for (int i = 0; i < path.size() ; ++i)
-    {
-      float x1, y1;
-        if (map.GetVertexPos(path[i], x1, y1))
-        {
-            glVertex3d(x1, y1, z);
-        }
-
-    }
-
-    glEnd();
-  }
-
-
-  glColor3f(1,1,1);
-  it=nodes.begin();
-  for(it;it!=nodes.end();it++)
-  {
-    PathMap::MapNode node=it->second;
-
-    char idx[16]={0};
-    sprintf(idx,"%d",it->first);
-    text_font->Text(idx).Draw(node.x,node.y,z);
-
-  }
-
-}
-
-int main()
-{
-
-  pangolin::GlFont *text_font = new pangolin::GlFont("../config/tt0102m_.ttf", 30.0);
-  pangolin::CreateWindowAndBind("Main", 1280, 960);
-
-  // 3D Mouse handler requires depth testing to be enabled
-  glEnable(GL_DEPTH_TEST);
-  glEnable(GL_BLEND);   // 启用混合
-  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-
-  // Define Camera Render Object (for view / scene browsing)
-  pangolin::OpenGlRenderState s_cam(
-          pangolin::ProjectionMatrix(1280, 960, 840, 840, 640, 640, 0.2, 100),
-          pangolin::ModelViewLookAt(5, 3, 8, 5, 3, 0, pangolin::AxisY)
-  );
-
-  // Choose a sensible left UI Panel width based on the width of 20
-  // charectors from the default font.
-  const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();
-
-  // Add named OpenGL viewport to window and provide 3D Handler
-  pangolin::View &d_cam = pangolin::CreateDisplay()
-          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 960.0f)
-          .SetHandler(new pangolin::Handler3D(s_cam));
-
-  // Add named Panel and bind to variables beginning 'ui'
-  // A Panel is just a View with a default layout and input handling
-  pangolin::CreatePanel("ui")
-          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
-
-  // Safe and efficient binding of named variables.
-  // Specialisations mean no conversions take place for exact types
-  // and conversions between scalar types are cheap.
-  pangolin::Var<bool> lvx_checkbox("ui.lvx_Checkbox", false, true);
-  pangolin::Var<std::string> lvx_file("ui.lvx_file_String", "../map/lidarbag/start1.lvx");
-
-
-  pangolin::Var<int> beg_Int("ui.beg", 1, 1, 18);
-  pangolin::Var<int> end_Int("ui.end", 5, 1, 18);
-
-
-  pangolin::Var<std::string> timequeue_string("ui.tq_String", "time string");
-
-  pangolin::Var<std::string> path_string("ui.path", "cost");
-  pangolin::Var<std::string> cost_string("ui.final_cost", "cost");
-
-
-
-  // std::function objects can be used for Var's too. These work great with C++11 closures.
-  pangolin::Var<std::function<void(void)>> save_window("ui.Save_Window", []() {
-    pangolin::SaveWindowOnRender("window");
-  });
-
-  std::mutex mutex;
-  std::vector<int> path;
-  PathMap map;
-  map.AddVertex(1, 0, 0);
-  map.AddVertex(2, 3, 0);
-  map.AddVertex(3, 6, 0);
-  map.AddVertex(4, 9, 0);
-  map.AddVertex(5, 9, 2);
-  map.AddVertex(6, 6, 2);
-  map.AddVertex(7, 3, 2);
-  map.AddVertex(8, 0, 2);
-  map.AddVertex(9, 0, 4);
-  map.AddVertex(10, 3, 4);
-  map.AddVertex(11, 6, 4);
-  map.AddVertex(12, 9, 4);
-  map.AddVertex(13, 9, 6);
-  map.AddVertex(14, 6, 6);
-  map.AddVertex(15, 3, 6);
-  map.AddVertex(16, 0, 6);
-
-  map.AddEdge(1, 2, false);
-  map.AddEdge(1, 8, false);
-
-  map.AddEdge(2, 3, false);
-  map.AddEdge(2, 7, false);
-
-  map.AddEdge(3, 4, false);
-  map.AddEdge(3, 6, false);
-
-  map.AddEdge(4, 5, false);
-
-  map.AddEdge(5, 12, false);
-  map.AddEdge(5, 6, false);
-
-  map.AddEdge(6, 11, false);
-  map.AddEdge(6, 7, false);
-
-  map.AddEdge(7, 10, false);
-  map.AddEdge(7, 8, false);
-
-  map.AddEdge(8, 9, false);
-
-  map.AddEdge(9, 10, false);
-  map.AddEdge(9, 16, false);
-
-  map.AddEdge(10, 11, false);
-  map.AddEdge(10, 15, false);
-
-  map.AddEdge(11, 12, false);
-  map.AddEdge(11, 14, false);
-
-  map.AddEdge(12, 13, false);
-  map.AddEdge(13, 14, false);
-  map.AddEdge(14, 15, false);
-  map.AddEdge(15, 16, false);
-
-  map.AddEdge(15, 17, false);
-  map.AddEdge(14, 17, false);
-  map.AddEdge(17, 18, false);
-
-  pangolin::Var<std::function<void(void)>> solve_view("ui.Solve", [&]() {
-
-    auto start_tm = std::chrono::system_clock::now();
-
-    start = beg_Int.Get();
-    end = end_Int.Get();
-
-
-
-    float distance = 0;
-    mutex.lock();
-    path.clear();
-    map.GetShortestPath(start, end, path, distance);
-    mutex.unlock();
-
-    char path_str[64] = {0};
-    sprintf(path_str, "%d-%d:", start, end);
-
-    for (int i = path.size() - 1; i >= 0; --i)
-    {
-      sprintf(path_str + strlen(path_str), "%d-", path[i]);
-    }
-    sprintf(path_str + strlen(path_str), "%d", end);
-    path_string = path_str;
-
-    char cost[62] = {0};
-    sprintf(cost, "d(%d-%d):%.3f", start, end, distance);
-    cost_string = cost;
-
-    auto end_tm = std::chrono::system_clock::now();
-    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_tm - start_tm);
-    double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
-    char tm[32] = {0};
-    sprintf(tm, "time:%.5fs", time);
-    timequeue_string = tm;
-
-  });
-
-  // Demonstration of how we can register a keyboard hook to alter a Var
-  pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'b', [&]() {
-    //a_double = 3.5;
-
-  });
-
-  // Default hooks for exiting (Esc) and fullscreen (tab).
-
-  double zoom = 1.0;
-  while (!pangolin::ShouldQuit())
-  {
-    // Clear entire screen
-    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-
-    if (d_cam.IsShown())
-    {
-      //s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(odom_pt[0],odom_pt[1],60, odom_pt[0],odom_pt[1],0, pangolin::AxisY));
-
-      d_cam.Activate(s_cam);
-      pangolin::glDrawAxis(1);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴
-
-    }
-
-
-    mutex.lock();
-    std::vector<int> solve_path=path;
-    mutex.unlock();
-    std::reverse(solve_path.begin(),solve_path.end());
-    solve_path.push_back(end);
-    DrawMap(map, solve_path,text_font);
-
-
-    // Swap frames and Process Events
-    pangolin::FinishFrame();
-    std::this_thread::sleep_for(std::chrono::microseconds(10));
-  }
-
-  delete text_font;
-
-
-  return 0;
-}

message.proto

@@ -28,7 +28,8 @@ message TrajectoryMsg
 
 message NavCmd
 {
-    repeated TrajectoryMsg trajs=1;
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
+  repeated TrajectoryMsg trajs=2;
 }
 
 message NavStatu
@@ -38,6 +39,8 @@ message NavStatu
     int32 action_type=3;
     float angular=4;
     float velocity=5;
+    TrajectoryMsg select_traj=6;
+    TrajectoryMsg predict_traj=7;
 }