From 56a98d1092d509646bf2921db59042654745b2bf Mon Sep 17 00:00:00 2001 From: LaurentWang Date: Thu, 30 Sep 2021 23:15:57 +0800 Subject: [PATCH 1/2] [feature] added flight_stage_manager framework. --- .../gaas_map}/registration_map.h | 4 +- .../gaas_map}/registration_map_manager.h | 4 +- .../{ => CommonLibs}/gaas_msgs/CMakeLists.txt | 1 + .../gaas_msgs/msg/GAASNavigationBlockMap.msg | 0 .../msg/GAASNavigationDynamicBlockGrid.msg | 0 .../msg/GAASNavigationDynamicBlockMap.msg | 0 .../msg/GAASNavigationMapBlockGrid.msg | 0 .../gaas_msgs/msg/GAASNavigationPath.msg | 0 .../msg/GAASPerceptionObstacleCluster.msg | 0 .../GAASPerceptionObstacleClustersList.msg | 0 ...SSystemManagementFlightControllerState.msg | 0 .../{ => CommonLibs}/gaas_msgs/package.xml | 0 .../gaas_msgs/srv/GAASGetAStarPath.srv | 0 .../gaas_msgs/srv/GAASSwitchFlightStage.srv | 6 ++ .../src/localization_algorithm_abstract.h | 2 +- .../src/map_manager_test.cpp | 2 +- .../src/map_publisher.cpp | 2 +- .../src/registration_localization_node.cpp | 2 +- .../basic_state_libs/src/FlightStage.h | 13 ++-- .../flight_stage_manager/CMakeLists.txt | 46 +++++++++++++ .../flight_stage_manager/README.md | 31 +++++++-- .../src/FlightStageManager.h | 65 +++++++++++++++++-- .../src/flight_stage_manager_node.cpp | 6 ++ simulation/Airsim/scripts/run_airsim.sh | 2 +- 24 files changed, 162 insertions(+), 24 deletions(-) rename algorithms/src/{LocalizationAndMapping/registration_localization/src => CommonLibs/gaas_map}/registration_map.h (96%) rename algorithms/src/{LocalizationAndMapping/registration_localization/src => CommonLibs/gaas_map}/registration_map_manager.h (98%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/CMakeLists.txt (99%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASNavigationBlockMap.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASNavigationDynamicBlockGrid.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASNavigationDynamicBlockMap.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASNavigationMapBlockGrid.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASNavigationPath.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASPerceptionObstacleCluster.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASPerceptionObstacleClustersList.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/msg/GAASSystemManagementFlightControllerState.msg (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/package.xml (100%) rename algorithms/src/{ => CommonLibs}/gaas_msgs/srv/GAASGetAStarPath.srv (100%) create mode 100644 algorithms/src/CommonLibs/gaas_msgs/srv/GAASSwitchFlightStage.srv create mode 100644 algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt create mode 100644 algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map.h b/algorithms/src/CommonLibs/gaas_map/registration_map.h similarity index 96% rename from algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map.h rename to algorithms/src/CommonLibs/gaas_map/registration_map.h index 3c4fe2d7..e1fae439 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map.h +++ b/algorithms/src/CommonLibs/gaas_map/registration_map.h @@ -1,8 +1,8 @@ #ifndef REGISTRATION_MAP_H_FILE #define REGISTRATION_MAP_H_FILE -#include "../../../CommonLibs/gaas_types/typedefs.h" -#include "../../../CommonLibs/gaas_location/gps_info.h" +#include "../gaas_types/typedefs.h" +#include "../gaas_location/gps_info.h" #include "opencv2/core/persistence.hpp" #include #include diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map_manager.h b/algorithms/src/CommonLibs/gaas_map/registration_map_manager.h similarity index 98% rename from algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map_manager.h rename to algorithms/src/CommonLibs/gaas_map/registration_map_manager.h index 44d30f43..b7e78a8b 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_map_manager.h +++ b/algorithms/src/CommonLibs/gaas_map/registration_map_manager.h @@ -10,8 +10,8 @@ #include #include #include -#include "../../../CommonLibs/gaas_timer/Timer.h" -#include "../../../CommonLibs/gaas_location/geometry.h" +#include "../gaas_timer/Timer.h" +#include "../gaas_location/geometry.h" //#include "submap_manager.h" #include "pcl/filters/crop_box.h" diff --git a/algorithms/src/gaas_msgs/CMakeLists.txt b/algorithms/src/CommonLibs/gaas_msgs/CMakeLists.txt similarity index 99% rename from algorithms/src/gaas_msgs/CMakeLists.txt rename to algorithms/src/CommonLibs/gaas_msgs/CMakeLists.txt index f4794f93..51a43fae 100644 --- a/algorithms/src/gaas_msgs/CMakeLists.txt +++ b/algorithms/src/CommonLibs/gaas_msgs/CMakeLists.txt @@ -87,6 +87,7 @@ add_message_files( add_service_files( FILES GAASGetAStarPath.srv + GAASSwitchFlightStage.srv # Service1.srv # Service2.srv ) diff --git a/algorithms/src/gaas_msgs/msg/GAASNavigationBlockMap.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationBlockMap.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASNavigationBlockMap.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationBlockMap.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASNavigationDynamicBlockGrid.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationDynamicBlockGrid.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASNavigationDynamicBlockGrid.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationDynamicBlockGrid.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASNavigationDynamicBlockMap.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationDynamicBlockMap.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASNavigationDynamicBlockMap.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationDynamicBlockMap.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASNavigationMapBlockGrid.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationMapBlockGrid.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASNavigationMapBlockGrid.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationMapBlockGrid.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASNavigationPath.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationPath.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASNavigationPath.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASNavigationPath.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASPerceptionObstacleCluster.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASPerceptionObstacleCluster.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASPerceptionObstacleCluster.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASPerceptionObstacleCluster.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASPerceptionObstacleClustersList.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASPerceptionObstacleClustersList.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASPerceptionObstacleClustersList.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASPerceptionObstacleClustersList.msg diff --git a/algorithms/src/gaas_msgs/msg/GAASSystemManagementFlightControllerState.msg b/algorithms/src/CommonLibs/gaas_msgs/msg/GAASSystemManagementFlightControllerState.msg similarity index 100% rename from algorithms/src/gaas_msgs/msg/GAASSystemManagementFlightControllerState.msg rename to algorithms/src/CommonLibs/gaas_msgs/msg/GAASSystemManagementFlightControllerState.msg diff --git a/algorithms/src/gaas_msgs/package.xml b/algorithms/src/CommonLibs/gaas_msgs/package.xml similarity index 100% rename from algorithms/src/gaas_msgs/package.xml rename to algorithms/src/CommonLibs/gaas_msgs/package.xml diff --git a/algorithms/src/gaas_msgs/srv/GAASGetAStarPath.srv b/algorithms/src/CommonLibs/gaas_msgs/srv/GAASGetAStarPath.srv similarity index 100% rename from algorithms/src/gaas_msgs/srv/GAASGetAStarPath.srv rename to algorithms/src/CommonLibs/gaas_msgs/srv/GAASGetAStarPath.srv diff --git a/algorithms/src/CommonLibs/gaas_msgs/srv/GAASSwitchFlightStage.srv b/algorithms/src/CommonLibs/gaas_msgs/srv/GAASSwitchFlightStage.srv new file mode 100644 index 00000000..24a04211 --- /dev/null +++ b/algorithms/src/CommonLibs/gaas_msgs/srv/GAASSwitchFlightStage.srv @@ -0,0 +1,6 @@ +std_msgs/Header header +std_msgs/Int32 new_mode +std_msgs/String description +--- +bool success + diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/localization_algorithm_abstract.h b/algorithms/src/LocalizationAndMapping/registration_localization/src/localization_algorithm_abstract.h index 50a1133b..4e5755f0 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/localization_algorithm_abstract.h +++ b/algorithms/src/LocalizationAndMapping/registration_localization/src/localization_algorithm_abstract.h @@ -2,7 +2,7 @@ #define LOCALIZATION_ALGORITHM_ABSTRACT #include -#include "registration_map_manager.h" +#include "../../../CommonLibs/gaas_map/registration_map_manager.h" #include "GPS_AHRS_sync.h" #include #include diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/map_manager_test.cpp b/algorithms/src/LocalizationAndMapping/registration_localization/src/map_manager_test.cpp index 2a8a2533..311c9ae8 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/map_manager_test.cpp +++ b/algorithms/src/LocalizationAndMapping/registration_localization/src/map_manager_test.cpp @@ -1,4 +1,4 @@ -#include "registration_map_manager.h" +#include "../../../CommonLibs/gaas_map/registration_map_manager.h" diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/map_publisher.cpp b/algorithms/src/LocalizationAndMapping/registration_localization/src/map_publisher.cpp index 8cca4e25..f451f80b 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/map_publisher.cpp +++ b/algorithms/src/LocalizationAndMapping/registration_localization/src/map_publisher.cpp @@ -15,7 +15,7 @@ #include "../../../CommonLibs/gaas_types/typedefs.h" #include "../../../CommonLibs/gaas_timer/Timer.h" -#include "registration_map_manager.h" +#include "../../../CommonLibs/gaas_map/registration_map_manager.h" bool loadPCDmap(MapCloudT::Ptr& pmap_cloud) { diff --git a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_localization_node.cpp b/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_localization_node.cpp index f3224b48..7a1d8a16 100644 --- a/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_localization_node.cpp +++ b/algorithms/src/LocalizationAndMapping/registration_localization/src/registration_localization_node.cpp @@ -1,5 +1,5 @@ -#include "registration_map_manager.h" +#include "../../../CommonLibs/gaas_map/registration_map_manager.h" #include "ndt_localization_algorithm.h" #include "icp_localization_algorithm.h" #include "GPS_AHRS_sync.h" diff --git a/algorithms/src/SystemManagement/basic_state_libs/src/FlightStage.h b/algorithms/src/SystemManagement/basic_state_libs/src/FlightStage.h index a8a8ee30..044ffb8f 100644 --- a/algorithms/src/SystemManagement/basic_state_libs/src/FlightStage.h +++ b/algorithms/src/SystemManagement/basic_state_libs/src/FlightStage.h @@ -8,11 +8,14 @@ namespace GAASManagement { typedef unsigned int FlightStageT; namespace FlightStage{//飞行阶段 - - static const FlightStageT IDLE_STAGE = 1; - static const FlightStageT TAKEOFF_STAGE = 2; - static const FlightStageT CRUISE_STAGE = 4; - static const FlightStageT LANDING_STAGE = 8; + static const FlightStageT NOT_INITIALIZED_STAGE = 1; //not initialized, unknown state. + static const FlightStageT GPS_AHRS_STAGE = 2; + static const FlightStageT LIDAR_LOCALIZATION_STAGE = 4; + + static const FlightStageT STOPPED = 8; + static const FlightStageT TAKE_OFF_STAGE = 16; + static const FlightStageT CRUISE_STAGE = 32; + static const FlightStageT LANDING_STAGE = 64; } typedef unsigned int FlightStageT; diff --git a/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt b/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt new file mode 100644 index 00000000..408e60c9 --- /dev/null +++ b/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 2.8.3) +project(flight_stage_manager) + +## 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(Boost 1.65 COMPONENTS filesystem iostreams timer 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} + ${PCL_INCLUDE_DIRS} +) +catkin_package() +set(REQUIRED_LIBS + ${OpenCV_LIBS} + ${GLOG_LIBRARY} + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${Boost_LIBRARIES} + glog +) +add_executable(flight_stage_manager_node src/flight_stage_manager_node.cpp) +target_link_libraries(flight_stage_manager_node ${REQUIRED_LIBS}) diff --git a/algorithms/src/SystemManagement/flight_stage_manager/README.md b/algorithms/src/SystemManagement/flight_stage_manager/README.md index 6e828d0b..54964f18 100644 --- a/algorithms/src/SystemManagement/flight_stage_manager/README.md +++ b/algorithms/src/SystemManagement/flight_stage_manager/README.md @@ -1,8 +1,27 @@ -飞行阶段管理. +# Flight State Management + +# 飞行阶段管理. + +The main functionality of this module is to control multiple map switching and localization method( GPS-AHRS, lidar_localization) switching. + +主要功能是控制多地图加载调用和导航模式切换. + + +Example pipeline: + +系统运行完整流程: + + +Load all maps -> switch to map1 -> start lidar localization -> take off -> switch to gps-ahrs localization -> cruising -> switch to map2 -> switch to lidar localization -> land + +读取所有地图 切换起降点1地图 Lidar定位 起飞 切换GPS定位 巡航 切换起降点地图2 GPS定位切换到Lidar定位 降落 + + +So this module shall support the transformation of these states and control other modules with proper gaas_srvs. + +因此这个模块要通过调用适当的gaas_srvs 支持对其他模块的状态转换控制. + + + -飞行阶段有待命,起飞,巡航,降落四个阶段. -其中,待命阶段,飞行器与地面站联系,双方协商航线和着陆方案. -起飞阶段飞行器提升高度,躲避低空障碍物并缓慢移动. -巡航阶段飞行器稳定飞行,速度较快. -降落阶段飞行器缓慢移动,躲避低空障碍物并稳定降落在目标着陆位置. diff --git a/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h b/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h index dbab4106..5c2fbc85 100644 --- a/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h +++ b/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h @@ -2,18 +2,75 @@ #define GAAS_FLIGHT_MODE_MANAGER_HEADER #include "../../basic_state_libs/src/FlightStage.h" #include "../../basic_state_libs/src/FlightStatus.h" -#include "../../basic_state_libs/src/MapManager.h" #include "../../basic_state_libs/src/VehicleStaticInfo.h" +#include "../../../CommonLibs/gaas_map/registration_map_manager.h" namespace GAASManagement { +//We need a standard for flight missions of GAAS. So a domain specific language is needed. + +typedef int FlightMissionType; +static const FlightMissionType TAKEOFF = 0; + +class FlightMission +{ +public: + FlightMissionType type; + +}; +class MissionTakeoff: public FlightMission +{ + double height_m; +}; +class MissionAccuratePath: public FlightMission +{ + typedef std::array PathPoint3dT; + std::vector points; +}; +class MissionCruise: public FlightMission +{ + typedef std::array LonLatAltT; + std::vector points; +}; + + +class FlightMissionParser +{ +public: + void parse(const string& full_text) + //implementation of a parser to parse a file of FlightMission. + { + ; + } +}; + + class GAASFlightStageManager { +private: + FlightStageT flight_stage = FlightStage::STOPPED|FlightStage::GPS_AHRS_STAGE|FlightStage::NOT_INITIALIZED_STAGE; public: - GAASFlightStatusT current_flight_status; - MapManager map_manager; - VehicleStaticInfo vehicle_static_info; + RegistrationMapManager::Ptr map_manager; + bool initFlightStageManager(std::shared_ptr pNH); + +//localization switch + bool switchToGPS_AHRSLocalization(); + bool switchToLidarLocalization(); + + + +//state tranfer + bool switchToTakeoffMode(); + bool switchToCruiseMode(); + bool switchToLandMode(); + + bool launchFlightStageManagerWithMissionFile(const string& file_path)//execute mission file. + { + ; + } + + FlightStageT getCurrentFlightStage();//获取当前阶段 bool reportStatusToGroundControlStationAsync(int report_request_id);//异步向地面站报告状态. diff --git a/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp b/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp new file mode 100644 index 00000000..f089ac43 --- /dev/null +++ b/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp @@ -0,0 +1,6 @@ +#include "FlightStageManager.h" + +int main(int argc, char** argv) +{ + return 0; +} diff --git a/simulation/Airsim/scripts/run_airsim.sh b/simulation/Airsim/scripts/run_airsim.sh index d550836f..78f9b015 100755 --- a/simulation/Airsim/scripts/run_airsim.sh +++ b/simulation/Airsim/scripts/run_airsim.sh @@ -23,7 +23,7 @@ # session name: init #sudo nvidia-smi --power-limit=90 tmux new -s init -d -tmux neww -n processes "(roscore)" +tmux neww -n processes "(roscore);sleep 10000" tmux neww -n processes "(cd ~/Downloads/Airsim/MSBuild2018/LinuxNoEditor && ./MSBuild2018.sh );sleep 10000" tmux neww -n processes "(cd ~/Downloads/Airsim/AirSim/PX4/PX4-Autopilot && make px4_sitl_default none_iris)" tmux neww -n processes "(sleep 5 && roslaunch mavros px4_airsim.launch;sleep 10000)" From 4c6ce1a0ba9970b770e326700b8996ce88233fe0 Mon Sep 17 00:00:00 2001 From: LaurentWang Date: Sat, 9 Oct 2021 18:33:19 +0800 Subject: [PATCH 2/2] [feature] added flight mission processing func. --- .../basic_state_libs/src/FlightStatus.h | 4 +- .../flight_stage_manager/CMakeLists.txt | 1 + .../src/FlightStageManager.h | 181 ++++++++++++++++-- .../src/flight_stage_manager_node.cpp | 2 + 4 files changed, 173 insertions(+), 15 deletions(-) diff --git a/algorithms/src/SystemManagement/basic_state_libs/src/FlightStatus.h b/algorithms/src/SystemManagement/basic_state_libs/src/FlightStatus.h index e47182d7..fa4100d5 100644 --- a/algorithms/src/SystemManagement/basic_state_libs/src/FlightStatus.h +++ b/algorithms/src/SystemManagement/basic_state_libs/src/FlightStatus.h @@ -11,8 +11,8 @@ class GAASFlightStatusT:public JSONSerializableAbstract//用于和地面同步 { public: string current_map_name; - FlightStageT current_mode = FlightStage::IDLE_STAGE; - LocationT currentLocation,TargetLocation; + FlightStageT current_mode = FlightStage::NOT_INITIALIZED_STAGE; + LocationT currentLocation,targetLocation; void initFlightStatus(const string& curr_map_name,const FlightStageT& curr_stage, diff --git a/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt b/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt index 408e60c9..2afe21c2 100644 --- a/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt +++ b/algorithms/src/SystemManagement/flight_stage_manager/CMakeLists.txt @@ -32,6 +32,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} + ${OPENCV_INCLUDE_DIRECTORIES} ) catkin_package() set(REQUIRED_LIBS diff --git a/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h b/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h index 5c2fbc85..c7e16543 100644 --- a/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h +++ b/algorithms/src/SystemManagement/flight_stage_manager/src/FlightStageManager.h @@ -4,70 +4,225 @@ #include "../../basic_state_libs/src/FlightStatus.h" #include "../../basic_state_libs/src/VehicleStaticInfo.h" #include "../../../CommonLibs/gaas_map/registration_map_manager.h" - +#include namespace GAASManagement { //We need a standard for flight missions of GAAS. So a domain specific language is needed. -typedef int FlightMissionType; -static const FlightMissionType TAKEOFF = 0; +class GAASFlightStageManager; class FlightMission { public: - FlightMissionType type; + using Ptr = std::shared_ptr; + std::string type=std::string("unknown"); + virtual ~FlightMission() + { + ; + } }; class MissionTakeoff: public FlightMission { +public: + using Ptr = std::shared_ptr; double height_m; }; class MissionAccuratePath: public FlightMission { +public: + using Ptr = std::shared_ptr; typedef std::array PathPoint3dT; std::vector points; + std::string map_name; + }; class MissionCruise: public FlightMission { +public: + using Ptr = std::shared_ptr; typedef std::array LonLatAltT; std::vector points; + }; class FlightMissionParser { +protected: + //parse params. + void parseTakeoff(const cv::FileNodeIterator& it, MissionTakeoff::Ptr& pMission); + void parseMapAccuratePath(const cv::FileNodeIterator& it, MissionAccuratePath::Ptr& pMission); + void parseCruise(const cv::FileNodeIterator& it, MissionCruise::Ptr& pMission); public: - void parse(const string& full_text) + //parse types. + std::vector parseMissions(const std::string& mission_file_path) //implementation of a parser to parse a file of FlightMission. { - ; + cv::FileStorage fs; + fs.open(mission_file_path,cv::FileStorage::READ); + if(!fs.isOpened()) + { + LOG(ERROR)<<"ERROR: mission_file_path invalid!"< missions_ret; + auto missions = fs["Missions"]; + cv::FileNodeIterator it = missions.begin(), it_end = missions.end(); + for (; it != it_end; ++it) + { + std::string mission_type; + (*it)["type"]>>mission_type; + if(mission_type == "takeoff") + { + MissionTakeoff::Ptr pmission_takeoff(new MissionTakeoff); + parseTakeoff(it,pmission_takeoff); + FlightMission::Ptr pFlightMission = std::dynamic_pointer_cast(pmission_takeoff); + missions_ret.push_back(pFlightMission); + } + else if(mission_type == "map_accurate_path") + { + MissionAccuratePath::Ptr pmission_map_path; + parseMapAccuratePath(it,pmission_map_path); + FlightMission::Ptr pFlightMission = std::dynamic_pointer_cast(pmission_map_path); + missions_ret.push_back(pFlightMission); + } + else if(mission_type == "mission_cruise") + { + MissionCruise::Ptr pmission_cruise; + parseCruise(it,pmission_cruise); + FlightMission::Ptr pFlightMission = std::dynamic_pointer_cast(pmission_cruise); + missions_ret.push_back(pFlightMission); + } + else + { + LOG(ERROR)<<"Unknown mission type, check your mission file!"<>pMission->height_m; +} +void FlightMissionParser::parseCruise(const cv::FileNodeIterator &it, MissionCruise::Ptr &pMission) +{ + auto targets_node = (*it)["targets"]; + cv::FileNodeIterator targets_it = targets_node.begin(), targets_it_end = targets_node.end(); + for(;targets_it!=targets_it_end;++targets_it) + { + MissionCruise::LonLatAltT gps_target; + double lon,lat,alt; + (*targets_it)["lon"] >> lon; + (*targets_it)["lat"] >> lat; + (*targets_it)["alt"] >> alt; + gps_target.at(0) = lon; + gps_target.at(1) = lat; + gps_target.at(2) = alt; + pMission->points.push_back(gps_target); + } +} +void FlightMissionParser::parseMapAccuratePath(const cv::FileNodeIterator &it, MissionAccuratePath::Ptr &pMission) +{ + (*it)["map"]>> pMission->map_name; // set map name. the key of map config file. + auto targets_node = (*it)["targets"]; + cv::FileNodeIterator targets_it = targets_node.begin(), targets_it_end = targets_node.end(); + for(;targets_it!=targets_it_end;++targets_it) + { + MissionAccuratePath::PathPoint3dT pt; + double x,y,z; + (*targets_it)["x"]>>x; + (*targets_it)["y"]>>y; + (*targets_it)["z"]>>z; + pt.at(0) = x; + pt.at(1) = y; + pt.at(2) = z; + pMission->points.push_back(pt); + } +} + + + class GAASFlightStageManager { -private: +protected: FlightStageT flight_stage = FlightStage::STOPPED|FlightStage::GPS_AHRS_STAGE|FlightStage::NOT_INITIALIZED_STAGE; -public: + RegistrationMapManager::Ptr map_manager; bool initFlightStageManager(std::shared_ptr pNH); -//localization switch + //localization switch bool switchToGPS_AHRSLocalization(); bool switchToLidarLocalization(); -//state tranfer + //state tranfer bool switchToTakeoffMode(); bool switchToCruiseMode(); bool switchToLandMode(); + bool doTakeoffMission(MissionTakeoff::Ptr pMission) + { + ; + } + bool doMapAccuratePath(MissionAccuratePath::Ptr pMission) + { + map_manager->selectMapByName(pMission->map_name); + for(const auto& pt:pMission->points) + { + //if(!map_manager->check_coordinate_valid(pt))//TODO:检查地图坐标合法性. + //{ + // LOG(ERROR)<<"Invalid coordinate in map:"<points) + { + //设置目标并检查是否完成 + //auto submission = setMapAccurateTarget(map_name,pt); + //submission.waitTillFinished(); + } + } + bool doCruise(MissionCruise::Ptr pMission) + { + for(const auto pt:pMission->points) + { + //设置并检查目标完成 + //auto submission = setGlobalGPSTarget(pt); + //submission.waitTillFinished(); + } + } +public: bool launchFlightStageManagerWithMissionFile(const string& file_path)//execute mission file. { - ; + FlightMissionParser parser; + vector missions = parser.parseMissions(file_path); + for(const FlightMission::Ptr& mission_ptr:missions) + { + tryFlightStageStateTransfer(mission_ptr->type); //状态转移 + //do operations. + if(mission_ptr->type == "takeoff") + { + auto pMission = std::dynamic_pointer_cast(mission_ptr); + doTakeoffMission(pMission); + } + else if(mission_ptr->type == "map_accurate_path") + { + auto pMission = std::dynamic_pointer_cast(mission_ptr); + doMapAccuratePath(pMission); + } + else if(mission_ptr->type == "mission_cruise") + { + auto pMission = std::dynamic_pointer_cast(mission_ptr); + doCruise(pMission); + } + } } @@ -75,8 +230,8 @@ class GAASFlightStageManager FlightStageT getCurrentFlightStage();//获取当前阶段 bool reportStatusToGroundControlStationAsync(int report_request_id);//异步向地面站报告状态. int queryReportRequestStatus(int report_request_id);//查询是否报告完成. - bool tryFlightStageStateTransfer(const FlightStageT& new_mode);//尝试状态转换 - + bool tryFlightStageStateTransfer(const std::string& state);//尝试状态转换 包含检查之前任务完成状态 处理状态机等操作 + void setMapAccurateTarget(const std::string& map_name,const MissionAccuratePath::PathPoint3dT& target); void visualizeCurrentState() { ; diff --git a/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp b/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp index f089ac43..73863626 100644 --- a/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp +++ b/algorithms/src/SystemManagement/flight_stage_manager/src/flight_stage_manager_node.cpp @@ -2,5 +2,7 @@ int main(int argc, char** argv) { + GAASManagement::FlightMissionParser fmp; + fmp.parseMissions("/home/gi/GAAS/config/gaas_mission_demo.yaml"); return 0; }