diff --git a/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp b/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
index 9529f92..d6d0122 100644
--- a/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
+++ b/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
@@ -36,9 +36,9 @@ class FlightPilot {
 	void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
 
 	// unity
-	bool setUnity(const bool render);
+	bool setUnity(void);
 	bool connectUnity(void);
-	bool disconnectUnity();
+	void disconnectUnity(void);
 	bool loadParams(const YAML::Node& cfg);
 	bool configCamera(const YAML::Node& cfg);
 	void computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth);
diff --git a/flightlib/src/ros_nodes/flight_pilot.cpp b/flightlib/src/ros_nodes/flight_pilot.cpp
index 59190b4..6e73035 100644
--- a/flightlib/src/ros_nodes/flight_pilot.cpp
+++ b/flightlib/src/ros_nodes/flight_pilot.cpp
@@ -32,10 +32,12 @@ FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
 	timestamp       = ros::Time::now();
 
 	// connect unity and setup unity
-	setUnity(unity_render_);
+	setUnity();
 	connectUnity();
-	if (!unity_ready_)
+	if (!unity_ready_){
 		ROS_ERROR("[FlightRos] Connection Faild! Start the Flightmare Unity First!");
+		exit(1);
+	}
 	spawnTreesAndSavePointCloud();
 
 	timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), &FlightPilot::mainLoopCallback, this);
@@ -137,8 +139,7 @@ void FlightPilot::mainLoopCallback(const ros::TimerEvent& event) {
 	}
 }
 
-bool FlightPilot::setUnity(const bool render) {
-	unity_render_ = render;
+bool FlightPilot::setUnity() {
 	if (unity_render_ && unity_bridge_ptr_ == nullptr) {
 		// create unity bridge
 		unity_bridge_ptr_ = UnityBridge::getInstance();
@@ -171,9 +172,9 @@ bool FlightPilot::connectUnity() {
 	return unity_ready_;
 }
 
-bool FlightPilot::disconnectUnity() {
+void FlightPilot::disconnectUnity() {
 	if (unity_render_ && unity_bridge_ptr_ != nullptr)
-		;
+		return;
 	unity_bridge_ptr_->disconnectUnity();
 	unity_ready_ = false;
 }
@@ -243,8 +244,7 @@ bool FlightPilot::configCamera(const YAML::Node& cfg) {
 	std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
 	Vector<3> t_BC(t_BC_vec.data());
 	Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
-	                     AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
-	                        .toRotationMatrix();  // the rotation order has been verified
+	                     AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ())).toRotationMatrix();  // the rotation order has been verified
 	// Convert the horizontal FOV (usually used) to vertical FOV (flightmare).
 	Scalar rgb_fov_deg_    = cfg["rgb_camera_left"]["fov"].as<Scalar>();
 	double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
@@ -272,8 +272,7 @@ bool FlightPilot::configCamera(const YAML::Node& cfg) {
 
 		Vector<3> t_BC_r(t_BC_vec_r.data());
 		Matrix<3, 3> r_BC_r = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
-		                       AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
-		                          .toRotationMatrix();
+		                       AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ())).toRotationMatrix();
 
 		rgb_camera_right->setFOV(flightmare_fov);
 		rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
diff --git a/flightlib/src/ros_nodes/yopo_planner_node.cpp b/flightlib/src/ros_nodes/yopo_planner_node.cpp
index 2ebd9da..48e9229 100644
--- a/flightlib/src/ros_nodes/yopo_planner_node.cpp
+++ b/flightlib/src/ros_nodes/yopo_planner_node.cpp
@@ -95,18 +95,21 @@ void yopo_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
 	// std::cout<<"yopo开始预测到当前时间: "<<delta_t<<std::endl;
 	sample_t = 0.0;  // = delta_t
 
-	pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
-	traj_to_pcl(traj_opt_bridge, best_traj_cld);
-	pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp);  // for test
-	best_traj_cld->header.frame_id = "world";
-	best_traj_visual_pub.publish(best_traj_cld);
+	if (best_traj_visual_pub.getNumSubscribers() > 0){
+		pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
+		traj_to_pcl(traj_opt_bridge, best_traj_cld);
+		pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp);
+		best_traj_cld->header.frame_id = "world";
+		best_traj_visual_pub.publish(best_traj_cld);
+	}
 	yopo_init = true;
 }
 
 void trajs_vis_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
 	if (!odom_init)
 		return;
-
+	if ((trajs_visual_pub.getNumSubscribers() == 0) && (lattice_trajs_visual_pub.getNumSubscribers() == 0))
+		return;
 	// ---------------- visualization of all trajs --------------------
 	std::vector<std::vector<double>> endstates_b;
 	endstates_b.resize(msg->layout.dim[0].size);