From 361e57fc2b7912b0a3d23ef473252954485920d2 Mon Sep 17 00:00:00 2001 From: Lu Junjie Date: Mon, 4 Nov 2024 23:15:18 +0800 Subject: [PATCH] Fix segmentation fault during destruction --- .../include/flightlib/envs/quadrotor_env.hpp | 2 +- flightlib/src/envs/quadrotor_env.cpp | 6 +++--- flightlib/src/envs/vec_env.cpp | 6 +++--- flightlib/src/ros_nodes/flight_pilot.cpp | 11 +++-------- flightlib/src/sensors/sgm_gpu/sgm_gpu.cu | 16 +++++++++++++--- run/run_yopo.py | 1 + 6 files changed, 24 insertions(+), 18 deletions(-) diff --git a/flightlib/include/flightlib/envs/quadrotor_env.hpp b/flightlib/include/flightlib/envs/quadrotor_env.hpp index 6bdb1a0..02f6d88 100755 --- a/flightlib/include/flightlib/envs/quadrotor_env.hpp +++ b/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 traj_opt_bridge; // data collection Scalar roll_var_, pitch_var_; diff --git a/flightlib/src/envs/quadrotor_env.cpp b/flightlib/src/envs/quadrotor_env.cpp index 6031fda..3b0cf14 100755 --- a/flightlib/src/envs/quadrotor_env.cpp +++ b/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(); // 5、add camera - sgm_.reset(new sgm_gpu::SgmGpu(width_, height_)); + sgm_ = std::make_shared(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> obs, const bool random) { quad_state_.setZero(); diff --git a/flightlib/src/envs/vec_env.cpp b/flightlib/src/envs/vec_env.cpp index 2a91e80..d817260 100644 --- a/flightlib/src/envs/vec_env.cpp +++ b/flightlib/src/envs/vec_env.cpp @@ -295,18 +295,18 @@ bool VecEnv::spawnTreesAndSavePointcloud(int ply_id_in, float spacing) usleep(1 * 1e6); // waitting 1s for generating completely // KDtree, for collision detection - pcl::search::KdTree kdtree; + std::shared_ptr> kdtree = std::make_shared>(); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); std::cout << "Map Path: " << ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply" << std::endl; pcl::io::loadPLYFile(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 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>(kdtree)); + envs_[i]->addKdtree(kdtree); envs_[i]->addESDFMap(esdf_map); envs_[i]->addMapSize(map_boundary_min, map_boundary_max); } diff --git a/flightlib/src/ros_nodes/flight_pilot.cpp b/flightlib/src/ros_nodes/flight_pilot.cpp index 1045d50..23263f6 100644 --- a/flightlib/src/ros_nodes/flight_pilot.cpp +++ b/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(width_, height_); // initialize subscriber and publisher left_img_pub = nh_.advertise("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!"<(); Scalar quad_size_i = cfg["quad_size"].as(); quad_size_ = Vector<3>(quad_size_i, quad_size_i, quad_size_i); - spawn_tree_ = cfg["unity"]["spawn_trees"].as(); + spawn_tree_ = cfg["unity"]["spawn_trees"].as(); save_pointcloud_ = cfg["unity"]["save_pointcloud"].as(); avg_tree_spacing_ = cfg["unity"]["avg_tree_spacing"].as(); for (int i = 0; i < 3; ++i) { diff --git a/flightlib/src/sensors/sgm_gpu/sgm_gpu.cu b/flightlib/src/sensors/sgm_gpu/sgm_gpu.cu index be48339..9a360d1 100644 --- a/flightlib/src/sensors/sgm_gpu/sgm_gpu.cu +++ b/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) { diff --git a/run/run_yopo.py b/run/run_yopo.py index cf76888..2bfd280 100644 --- a/run/run_yopo.py +++ b/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()