optical_flow: Pass nodelet callback queue to TransformListener

CopterExpress · May 29, 2020 · 2814fea · 2814fea
1 parent b85326c
commit 2814fea
Showing 1 changed file with 10 additions and 8 deletions.
diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp
@@ -34,8 +34,7 @@ class OpticalFlow : public nodelet::Nodelet
 {
 public:
 	OpticalFlow():
-		camera_matrix_(3, 3, CV_64F),
-		tf_listener_(tf_buffer_)
+		camera_matrix_(3, 3, CV_64F)
 	{}
 
 private:
@@ -51,8 +50,8 @@ class OpticalFlow : public nodelet::Nodelet
 	Mat hann_;
 	Mat prev_, curr_;
 	Mat camera_matrix_, dist_coeffs_;
-	tf2_ros::Buffer tf_buffer_;
-	tf2_ros::TransformListener tf_listener_;
+	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 
 	void onInit()
@@ -62,6 +61,9 @@ class OpticalFlow : public nodelet::Nodelet
 		image_transport::ImageTransport it(nh);
 		image_transport::ImageTransport it_priv(nh_priv);
 
+		tf_buffer_.reset(new tf2_ros::Buffer());
+		tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh));
+
 		nh.param<std::string>("mavros/local_position/tf/frame_id", local_frame_id_, "map");
 		nh.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "base_link");
 		nh_priv.param("roi", roi_px_, 128);
@@ -183,7 +185,7 @@ class OpticalFlow : public nodelet::Nodelet
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
 			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
 			try {
-				tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+				tf_buffer_->transform(flow_camera, flow_fcu, fcu_frame_id_);
 			} catch (const tf2::TransformException& e) {
 				// transform is not available yet
 				return;
@@ -197,7 +199,7 @@ class OpticalFlow : public nodelet::Nodelet
 				try {
 					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
 					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
-					tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
+					tf_buffer_->transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
 					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
 					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
@@ -244,8 +246,8 @@ class OpticalFlow : public nodelet::Nodelet
 	geometry_msgs::Vector3Stamped calcFlowGyro(const std::string& frame_id, const ros::Time& prev, const ros::Time& curr)
 	{
 		tf2::Quaternion prev_rot, curr_rot;
-		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
-		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
+		tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
+		tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
 
 		geometry_msgs::Vector3Stamped flow;
 		flow.header.frame_id = frame_id;