diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 92422c0c8..f1658ac7b 100644 --- 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("markers", 1); - vis_markers_pub_ = nh_priv_.advertise("visualization", 1); - img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this); - dyn_srv_ = std::make_shared>(nh_priv_); dynamic_reconfigure::Server::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("markers", 1); + vis_markers_pub_ = nh_priv_.advertise("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 index 1193b9bb3..b9a24e6cb 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -124,6 +124,11 @@ class ArucoMap : public nodelet::Nodelet { vis_markers_pub_ = nh_priv_.advertise("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); @@ -131,11 +136,6 @@ class ArucoMap : public nodelet::Nodelet { sync_.reset(new message_filters::Synchronizer(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 index b1ecf4183..c17b594ed 100644 --- 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/px4flow/raw/send", 1); velo_pub_ = nh_priv.advertise("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"); }