Fix segmentation fault during destruction

flightlib/include/flightlib/envs/quadrotor_env.hpp

@@ -101,7 +101,7 @@ class QuadrotorEnv final : public EnvBase {
 	// trajectory optimization
 	int map_idx_{0};
 	Vector<3> goal_;
-	TrajOptimizationBridge *traj_opt_bridge;
+	std::shared_ptr<TrajOptimizationBridge> traj_opt_bridge;
 
 	// data collection
 	Scalar roll_var_, pitch_var_;

flightlib/src/envs/quadrotor_env.cpp

@@ -24,10 +24,10 @@ QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path) : EnvBase() {
 	rew_dim_ = 1;
 
 	// 3、define planner
-	traj_opt_bridge = new TrajOptimizationBridge();
+	traj_opt_bridge = std::make_shared<TrajOptimizationBridge>();
 
 	// 5、add camera
-	sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
+	sgm_ = std::make_shared<sgm_gpu::SgmGpu>(width_, height_);
 
 	if (!configCamera(cfg_)) {
 		logger_.error("Cannot config RGB Camera. Something wrong with the config file");
@@ -39,7 +39,7 @@ QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path) : EnvBase() {
 	steps_        = 0;
 }
 
-QuadrotorEnv::~QuadrotorEnv() { delete traj_opt_bridge; }
+QuadrotorEnv::~QuadrotorEnv() {}
 
 bool QuadrotorEnv::reset(Ref<Vector<>> obs, const bool random) {
 	quad_state_.setZero();

flightlib/src/envs/vec_env.cpp

@@ -295,18 +295,18 @@ bool VecEnv<EnvBase>::spawnTreesAndSavePointcloud(int ply_id_in, float spacing)
 	usleep(1 * 1e6);  // waitting 1s for generating completely
 
 	// KDtree, for collision detection
-	pcl::search::KdTree<pcl::PointXYZ> kdtree;
+	std::shared_ptr<pcl::search::KdTree<pcl::PointXYZ>> kdtree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	std::cout << "Map Path: " << ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply" << std::endl;
 	pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply", *cloud);
-	kdtree.setInputCloud(cloud);  // 0.3s
+	kdtree->setInputCloud(cloud);  // 0.3s
 
 	// ESDF, for gradient calculation (map_id is required)
 	Eigen::Vector3d map_boundary_min, map_boundary_max;
 	std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map = traj_opt::SdfConstruction(cloud, map_boundary_min, map_boundary_max);
 
 	for (int i = 0; i < num_envs_; i++) {
-		envs_[i]->addKdtree(std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(kdtree));
+		envs_[i]->addKdtree(kdtree);
 		envs_[i]->addESDFMap(esdf_map);
 		envs_[i]->addMapSize(map_boundary_min, map_boundary_max);
 	}

flightlib/src/ros_nodes/flight_pilot.cpp

@@ -15,14 +15,9 @@ FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
 
 	// initialization
 	quad_state_.setZero();
-	if (scene_id_ == UnityScene::NATUREFOREST) {
-		quad_state_.x(QS::POSX) = 51;   // 41-61
-		quad_state_.x(QS::POSY) = 108;  // 98-118
-		quad_state_.x(QS::POSZ) = 34;
-	}
 	quad_ptr_->reset(quad_state_);
 
-	sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
+	sgm_ = std::make_shared<sgm_gpu::SgmGpu>(width_, height_);
 
 	// initialize subscriber and publisher
 	left_img_pub = nh_.advertise<sensor_msgs::Image>("RGB_image", 1);
@@ -44,7 +39,7 @@ FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
 	spawnTreesAndSavePointCloud();
 
 	timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), &FlightPilot::mainLoopCallback, this);
-	std::cout<<"[FlightRos] Ros Node is Ready!"<<std::endl;
+	std::cout << "[FlightRos] Ros Node is Ready!" << std::endl;
 }
 
 FlightPilot::~FlightPilot() { disconnectUnity(); }
@@ -198,7 +193,7 @@ bool FlightPilot::loadParams(const YAML::Node& cfg) {
 	unity_render_      = cfg["unity_render"].as<bool>();
 	Scalar quad_size_i = cfg["quad_size"].as<Scalar>();
 	quad_size_         = Vector<3>(quad_size_i, quad_size_i, quad_size_i);
-	spawn_tree_		   = cfg["unity"]["spawn_trees"].as<bool>();
+	spawn_tree_        = cfg["unity"]["spawn_trees"].as<bool>();
 	save_pointcloud_   = cfg["unity"]["save_pointcloud"].as<bool>();
 	avg_tree_spacing_  = cfg["unity"]["avg_tree_spacing"].as<Scalar>();
 	for (int i = 0; i < 3; ++i) {

flightlib/src/sensors/sgm_gpu/sgm_gpu.cu

@@ -47,9 +47,19 @@ SgmGpu::SgmGpu(const int cols, const int rows)
 SgmGpu::~SgmGpu() {
   freeMemory();
 
-  cudaStreamDestroy(stream1_);
-  cudaStreamDestroy(stream2_);
-  cudaStreamDestroy(stream3_);
+  // 设置为 nullptr，防止重复销毁
+  if (stream1_) {
+	  cudaStreamDestroy(stream1_);
+	  stream1_ = nullptr;
+  }
+  if (stream2_) {
+	  cudaStreamDestroy(stream2_);
+	  stream2_ = nullptr;
+  }
+  if (stream3_) {
+	  cudaStreamDestroy(stream3_);
+	  stream3_ = nullptr;
+  }
 }
 
 void SgmGpu::allocateMemory(uint32_t cols, uint32_t rows) {

run/run_yopo.py

@@ -113,6 +113,7 @@ def main():
         model.policy.load_state_dict(saved_variables["state_dict"], strict=False)
         model.test_policy(num_rollouts=20)
 
+    print("Run YOPO Finish!")
 
 if __name__ == "__main__":
     main()