Merge branch 'system_management_dev' of https://github.com/cyanine-gi…

…/GAAS_contrib into system_management_dev

algorithms/src/SystemManagement/README.md

@@ -0,0 +1,9 @@
+机上管理系统.
+
+主要负责机上飞行状态切换,与地面站通信,飞行前申请空域,宏观上规划/协商飞行器的最优路径,监控飞行器自身状态并向地面汇报;是连接机上状态和地面站通信的桥梁.
+
+System Management module.
+
+In charge of flight status management(FSM), communication with ground control station, negotiation of best path and surveillance of vehicle state.
+
+This module is the bridge between vehicle and ground control station.

algorithms/src/SystemManagement/basic_state_libs/README.md

@@ -0,0 +1,15 @@
+Basic state libs.
+
+To manage the state of vehicle.
+
+Takeoff -- Cruise -- Landing
+
+During takeoff and landing there are multiple submaps. So we need to load these maps dynamically.
+
+
+基本状态信息类别库.
+
+控制载具的飞行状态:起飞,巡航,降落.
+
+起降阶段会使用不同的地图.我们需要动态加载这些地图.
+

algorithms/src/SystemManagement/basic_state_libs/src/FlightMode.h

@@ -0,0 +1,22 @@
+#ifndef GAAS_FLIGHT_MODE_HEADER
+#define GAAS_FLIGHT_MODE_HEADER
+
+
+#include "typedefs.h"
+
+namespace GAASManagement {
+
+typedef unsigned int FlightModeT;
+namespace FlightMode{
+
+    static const FlightModeT IDLE_STATE = 1;
+    static const FlightModeT TAKEOFF_STATE = 2;
+    static const FlightModeT CRUISE_STATE = 4;
+    static const FlightModeT LANDING_STATE = 8;
+}
+typedef unsigned int FlightModeT;
+
+}
+
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/FlightStatus.h

@@ -0,0 +1,27 @@
+#ifndef GAAS_FLIGHT_STATUS_HEADER
+#define GAAS_FLIGHT_STATUS_HEADER
+
+#include "FlightMode.h"
+#include "location_and_coordinates/LocationT.h"
+
+namespace GAASManagement {
+
+struct GAASFlightStatusT//用于和地面同步飞行状态.
+{
+    string current_map_name;
+    FlightModeT current_mode = FlightMode::IDLE_STATE;
+    LocationT currentLocation,TargetLocation;
+
+
+    void initFlightStatus(const string& curr_map_name,const FlightModeT& curr_mode,
+                          const LocationT& curr_location,const LocationT& target_location);
+    LocationT getCurrentLocation();
+    LocationT getTargetLocation();
+    void serialize();
+    void deserialize();
+};
+
+
+}
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/MapManager.h

@@ -0,0 +1,41 @@
+#ifndef GAAS_MAP_MANAGER_HEADER
+#define GAAS_MAP_MANAGER_HEADER
+
+
+#include "typedefs.h"
+
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/transforms.h>
+#include "../../../LocalizationAndMapping/lidar_localization/src/ndt_algo.h"
+
+struct GAASMapT
+{
+    string map_name;
+    MapCloudT::Ptr pCloud;
+    MapGPSInfo map_gps_info; // from localization package.
+
+    vector<LocationT> getTakeoffLocationAlternatives();
+    vector<LocationT> getLandingLocationAlternatives();
+    void serialize();
+    void deserialize();
+};
+
+class MapManager
+{
+public:
+    std::map<string,std::shared_ptr<GAASMapT> > name_to_map;
+    bool activateMapByName(const string& map_name);//切换地图
+    bool dynamicallyLoadMap(const string& map_name,std::shared_ptr<GAASMapT>& pNewMap);
+    bool loadMaps();//加载地图
+    //bool getMapByNameFromInternet();//网络动态加载
+    string getCurrentMapName();//获取当前地图名称
+
+    vector<LocationT> getCurrentMapTakeoffLocationAlternatives();//获取当前地图起飞用的地点.
+    vector<LocationT> getCurrentMapLandingLocationAlternatives();//获取当前地图可用降落地点.
+
+
+};
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/VehicleStaticInfo.h

@@ -0,0 +1,19 @@
+#ifndef GAAS_VEHICLE_STATIC_INFO_HEADER
+#define GAAS_VEHICLE_STATIC_INFO_HEADER
+
+
+
+
+struct VehicleStaticInfo;
+struct VehicleStaticInfo
+{
+    string vehicle_name;//飞行器名称
+    double vehicle_size_x,vehicle_size_y,vehicle_size_z;// 尺寸
+    bool isVTOL = true;//支持垂直起降.
+    double left_span_hour = 8.0;//剩余续航时间(hour) 默认8h.
+
+    void serialize();
+    void deserialize();
+    void loadFromConfigFile();//从配置文件加载.
+};
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/location_and_coordinates/Airspace.h

@@ -0,0 +1,44 @@
+#ifndef GAAS_AIRSPACE_HEADER
+#define GAAS_AIRSPACE_HEADER
+
+#include "LocationT.h"
+
+struct AirspaceRange; // 空域范围
+struct AirspaceStatus;//空域当前状态.
+
+
+struct AirspaceRange
+{
+    bool isInRange(const LocationT& current_location);//判断是否在空域内部.
+};
+
+
+
+struct AirspaceStatus
+{
+    const static int STATUS_CLEAR_AND_SAFE = 0;
+    const static int STATUS_DANGER = 1;
+
+    int space_status = STATUS_CLEAR_AND_SAFE;
+
+    bool support_landing = true;//可以降落
+    bool HDmap_avail = true;//有高精地图
+    string map_name="unknown_map_name";//地图名称.
+
+    LocationT airspace_center;
+    AirspaceRange airspace_range;
+
+    double temperature_celsius;//当地温度
+
+    double windspeed_N_mps;//当地风速测量 m/s
+    double windspeed_W_mps;
+    double windspeed_U_mps;
+    string weather = "Sunny";//默认是晴.
+
+    void serialize();
+    void deserialize();
+};
+
+
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/location_and_coordinates/LocationT.h

@@ -0,0 +1,13 @@
+#ifndef GAAS_LOCATION_T_HEADER
+#define GAAS_LOCATION_T_HEADER
+
+struct LocationWGS84;
+struct LocationWGS84
+{
+    double lon,lat,alt;
+    void serialize();
+    void deserialize();
+};
+typedef LocationWGS84 LocationT;
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/multi_stage_manager.h

@@ -0,0 +1,23 @@
+#ifndef GAAS_MULTI_STAGE_MANAGER_HEADER
+#define GAAS_MULTI_STAGE_MANAGER_HEADER
+
+//定义一些飞行状态和查询/切换状态的API
+
+//主要分为这几个模块:
+//1.飞行器当前坐标表达和转换
+//2.地图管理
+//3.飞行器当前航行状态
+//4.与地面站通信
+
+namespace GAASManagement{
+
+
+
+
+
+
+
+}
+
+
+#endif

algorithms/src/SystemManagement/basic_state_libs/src/multi_state_manager_node.cpp

@@ -0,0 +1 @@
+#include "FlightModeManager.h"

algorithms/src/SystemManagement/basic_state_libs/src/typedefs.h

@@ -0,0 +1,24 @@
+#ifndef MULTISTAGE_TYPEDEFS_H
+#define MULTISTAGE_TYPEDEFS_H
+
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <deque>
+
+using std::string;
+using std::vector;
+using std::deque;
+using std::cout;
+using std::endl;
+typedef pcl::PointXYZI LidarPointT;
+typedef pcl::PointCloud<LidarPointT> LidarCloudT;
+typedef pcl::PointXYZI MapPointT;
+typedef pcl::PointCloud<MapPointT> MapCloudT;
+
+
+
+
+#endif

algorithms/src/SystemManagement/json_request_response_lib/README.md

@@ -0,0 +1,2 @@
+加密json request库.
+通过双向签名,保证传输全程加密,认证双方身份,并确保内容不可抵赖.

algorithms/src/SystemManagement/json_request_response_lib/src/EncryptedJSON.h

@@ -0,0 +1,47 @@
+#ifndef GAAS_ENCRYPTED_JSON_HEADER
+#define GAAS_ENCRYPTED_JSON_HEADER
+
+#include <iostream>
+#include <mutex>
+
+
+
+using std::string;
+using std::endl;
+
+
+
+struct EncryptedJSONRequest
+{
+
+};
+struct EncryptedJSONResponse
+{
+
+};
+struct RequestAndResponse
+{
+    EncryptedJSONRequest req;
+    EncryptedJSONResponse resp;
+    bool finished = false;
+};
+class EncryptedJSONClient
+{
+private:
+    string vehicle_id; // 飞行器唯一ID.
+    string vehicle_pubkey; //公钥.用于加密通信.
+    string vehicle_private_key;//私钥
+    string https_server_url;
+    std::map<string,RequestAndResponse> curr_requests;
+public:
+    bool initEncryptedJSONClient(string https_server_url_setting);
+
+    bool startAsyncRequest();
+    bool queryAsyncRequest();
+    bool getAsyncResponse();
+
+};
+
+
+
+#endif

algorithms/src/SystemManagement/px4_state_reporter/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(px4_state_reporter)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+#add_compile_options(-std=c++11 -O0)
+add_compile_options(-std=c++17 -O3)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+#set(CMAKE_BUILD_TYPE DEBUG)
+set(CMAKE_BUILD_TYPE RELEASE)
+find_package(OpenCV 3.4.5 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+find_package(Glog REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(GTSAM REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  geometry_msgs
+  image_transport
+  roscpp
+  rosbag
+  sensor_msgs
+  nav_msgs
+  tf
+  tf2
+  tf2_ros
+  visualization_msgs
+)
+include_directories(
+        ${catkin_INCLUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIR}
+        ${GTSAM_INCLUDE_DIR}
+)
+catkin_package()
+set(REQUIRED_LIBS
+        ${OpenCV_LIBS}
+        ${GLOG_LIBRARY}
+        ${catkin_LIBRARIES}
+        ${PCL_LIBRARIES}
+        ${GTSAM_LIBRARIES}
+        glog
+        gtsam
+)
+add_executable(px4_state_reporter_node src/px4_state_reporter_node.cpp)
+target_link_libraries(px4_state_reporter_node ${REQUIRED_LIBS})
+
+
+
+

algorithms/src/SystemManagement/px4_state_reporter/README.md

@@ -0,0 +1 @@
+这里实现px4飞控消息到通用飞控消息格式的转换与对应msg的转发.