aruco_pose, clover: Move subscriptions to the end of init

CopterExpress · Jun 8, 2021 · 1a2e87b · 1a2e87b
1 parent 7dbd983
commit 1a2e87b
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 12 deletions.
diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp
@@ -111,18 +111,18 @@ class ArucoDetect : public nodelet::Nodelet {
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
 
-		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
-		debug_pub_ = it_priv.advertise("debug", 1);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
-		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
-		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
-
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
 		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
 
 		cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
 		dyn_srv_->setCallback(cb);
 
+		debug_pub_ = it_priv.advertise("debug", 1);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
+		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
+
 		NODELET_INFO("ready");
 	}
 

diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp
@@ -124,18 +124,18 @@ class ArucoMap : public nodelet::Nodelet {
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
 		markers_sub_.subscribe(nh_, "markers", 1);
 
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
 
-		publishMarkersFrames();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
-
 		NODELET_INFO("ready");
 	}
 

diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp
@@ -70,7 +70,6 @@ class OpticalFlow : public nodelet::Nodelet
 		roi_rad_ = nh_priv.param("roi_rad", 0.0);
 		calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
 
-		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 		img_pub_ = it_priv.advertise("debug", 1);
 		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
 		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
@@ -83,6 +82,8 @@ class OpticalFlow : public nodelet::Nodelet
 		flow_.distance = -1; // no distance sensor available
 		flow_.temperature = 0;
 
+		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
+
 		NODELET_INFO("Optical Flow initialized");
 	}