[fix] fixed perception cmake file and state surveillance module.

algorithms/launch/GAAS_contrib_algorithms.launch

@@ -12,4 +12,5 @@
   <include file="$(find global_map_planner)/launch/global_astar_planner_node.launch"/>
   <include file="$(find target_point_navigator)/launch/target_point_navigator.launch"/>
   <include file="$(find obstacle_and_drivable_block_publisher)/launch/dynamic_map_block_generator.launch"/>
+  <include file="$(find px4_state_reporter)/launch/px4_state_reporter.launch"/>
 </launch>

algorithms/src/Perception/vision_lidar_fusion/CMakeLists.txt

@@ -2,17 +2,17 @@ cmake_minimum_required(VERSION 2.8.3)
 project(vision_lidar_fusion)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
-#add_compile_options(-std=c++11 -O0)
-add_compile_options(-std=c++11 -O3)
+#add_compile_options(-std=c++17 -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)
+set(CMAKE_BUILD_TYPE DEBUG)
+#set(CMAKE_BUILD_TYPE RELEASE)
 if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
     find_package(OpenCV 3.4.5 REQUIRED)
-    set (CMAKE_CXX_FLAGS "-march=native -msse4.2 -mfpmath=sse")  # To fix pcl segmentation faults.
+    #set (CMAKE_CXX_FLAGS "-march=native -msse4.2 -mfpmath=sse")  # To fix pcl segmentation faults.
 else()
     set (OpenCV_DIR /home/gi/nvme/opencv-3.4.5/build)
 endif()

algorithms/src/Perception/vision_lidar_fusion/launch/vision_lidar_fusion.launch

@@ -5,9 +5,9 @@
     <param name="min_cluster_size" type="int" value="25"/>
     <param name="max_cluster_size" type="int" value="200000"/>
     <param name="cluster_tolerance" type="double" value="0.25"/>
-    <node name="vision_lidar_fusion_node" pkg="vision_lidar_fusion" type="vision_lidar_fusion_node" output="screen">
+    <node name="vision_lidar_fusion_node" pkg="vision_lidar_fusion" type="vision_lidar_fusion_node" output="screen" launch-prefix="xterm -g 200x60 -e gdb -ex run --args">
         <param name="visualize" type="bool" value="true"/>
-        <param name="config_file_path" type="string" value="/home/gi/GAAS_contrib/config/camera1_lidar1.yaml"/>
+        <param name="config_file_path" type="string" value="/home/gi/GAAS/config/camera1_lidar1.yaml"/>
     </node>
     <node name="euclidean_cluster_extration_node" pkg="vision_lidar_fusion" type="euclidean_cluster_extraction" output="screen"/> <!--launch-prefix="stterm -g 200x60 -e gdb -ex run \-\-args"-->
 </launch>

algorithms/src/Perception/vision_lidar_fusion/src/prev_fusion.h

@@ -25,6 +25,11 @@ class PrevFusion
     {
         cv::FileStorage fs;
         fs.open(file_path,cv::FileStorage::READ);
+        if(!fs.isOpened())
+        {
+            LOG(ERROR)<<"File "<< file_path<<" not found!"<<endl;
+            exit(-1);
+        }
         fs["CameraK"]>>camera_K;
         fs["T_cam_lidar"]>>T_cam_lidar;
         assert(camera_K.cols == 3 && camera_K.rows==3 && camera_K.type()==CV_32F);

algorithms/src/SystemManagement/state_surveillance/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(PCL 1.8 REQUIRED)
 find_package(Glog REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(GTSAM REQUIRED)
+find_package(Boost REQUIRED timer)
 find_package(catkin REQUIRED COMPONENTS
   gaas_msgs
   cv_bridge
@@ -43,6 +44,7 @@ set(REQUIRED_LIBS
         ${catkin_LIBRARIES}
         ${PCL_LIBRARIES}
         ${GTSAM_LIBRARIES}
+        ${Boost_LIBRARIES}
         glog
         gtsam
 )

algorithms/src/SystemManagement/state_surveillance/src/SurveillanceModuleAbstract.h

@@ -30,15 +30,15 @@ class SurveillanceModuleAbstract
     {
         if(current_status == STATUS_NOT_SURE)
         {
-            LOG(INFO)<<module_name<<"state: STATUS_NOT_SURE"<<" "<<state_description;
+            LOG(INFO)<<module_name<<" state: STATUS_NOT_SURE"<<" "<<state_description;
         }
         else if(current_status == STATUS_WARNING)
         {
-            LOG(WARNING)<<module_name<<"state: STATUS_WARNING"<<" "<<state_description;
+            LOG(WARNING)<<module_name<<" state: STATUS_WARNING"<<" "<<state_description;
         }
         else if(current_status == STATUS_ERROR)
         {
-            LOG(ERROR)<<module_name<<"state: STATUS_ERROR"<<" "<<state_description;
+            LOG(ERROR)<<module_name<<" state: STATUS_ERROR"<<" "<<state_description;
         }
     }
 };

algorithms/src/SystemManagement/state_surveillance/src/state_surveillance_node.cpp

@@ -35,7 +35,7 @@ class StateSurveillanceManager
         flightControllerStateSubscriber = pNH->subscribe<gaas_msgs::GAASSystemManagementFlightControllerState>
                 ("/gaas/system_management/flight_controller_state",1,&StateSurveillanceManager::flightControllerStatusCallback,this);
         localizationNDTStatusSubscriber = pNH->subscribe<geometry_msgs::PoseStamped>
-                ("/gaas/localization/ndt_pose",1,&StateSurveillanceManager::localizationNDTStatusCallback,this);
+                ("/gaas/localization/registration_pose",1,&StateSurveillanceManager::localizationNDTStatusCallback,this);
         perceptionStatusSubscriber = pNH->subscribe<gaas_msgs::GAASPerceptionObstacleClustersList>
                 ("/gaas/perception/euclidean_original_clusters_list",1,&StateSurveillanceManager::perceptionStatusCallback,this);
         mavrosOffboardCommandSubscriber = pNH->subscribe<mavros_msgs::PositionTarget>
@@ -75,18 +75,34 @@ class StateSurveillanceManager
     {
         ros::Time time_now = ros::Time::now();
         const double TIMEOUT_THRES = 0.5;
-        if((time_now - this->t_last_fc).toSec()> TIMEOUT_THRES)
+        double time_cost_fc = (time_now - this->t_last_fc).toSec();
+        if(time_cost_fc > TIMEOUT_THRES&&!t_last_fc.isZero())
         {
-            LOG(ERROR)<<"Flight Controller State msg timed out!"<<endl;
+            LOG(ERROR)<<"Flight Controller State msg timed out! "<<time_cost_fc<<"sec."<<endl;
         }
-        if((time_now  - this->t_last_ndt_localization).toSec()> TIMEOUT_THRES)
+        else if(t_last_fc.isZero())
         {
-            LOG(ERROR)<<"Registration localization msg timed out!"<<endl;
+            LOG(WARNING)<<"FCState surveillance still initializing!"<<endl;
         }
-        if((time_now - this->t_last_perception).toSec()> TIMEOUT_THRES)
+        double time_cost_localization = (time_now  - this->t_last_ndt_localization).toSec();
+        if(time_cost_localization > TIMEOUT_THRES&&!t_last_ndt_localization.isZero())
         {
-            LOG(ERROR)<<"Perception Obstacle Clusters timed out!"<<endl;
+            LOG(ERROR)<<"Registration localization msg timed out! "<<time_cost_localization<<"sec."<<endl;
         }
+        else if(t_last_ndt_localization.isZero())
+        {
+            LOG(WARNING)<<"Registration pose still initializing!"<<endl;
+        }
+        double time_cost_perception = (time_now - this->t_last_perception).toSec();
+        if(time_cost_perception > TIMEOUT_THRES&&!t_last_perception.isZero())
+        {
+            LOG(ERROR)<<"Perception Obstacle Clusters timed out! "<<time_cost_perception<<"sec."<<endl;
+        }
+        else if(t_last_perception.isZero())
+        {
+            LOG(WARNING)<<"Perception clusters still initializing!"<<endl;
+        }
+
     }
 
 
@@ -131,6 +147,7 @@ int main(int argc,char** argv)
 {
     FLAGS_alsologtostderr = 1;
     google::InitGoogleLogging("state_surveillance_node");
+    //usleep(1000000);//wait 1s.
     StateSurveillanceManager ssm;
     ssm.initStateSurveillanceManagerNode(argc,argv);