uas mavlink#71: Use single TF2 objects for broadcasting and subscript…

…ion.
TSC21 · Jul 12, 2015 · 0a20ace · 0a20ace
1 parent 182fb73
commit 0a20ace
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 7 deletions.
diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h
@@ -22,6 +22,8 @@
 #include <Eigen/Eigen>
 #include <Eigen/Geometry>
 #include <tf/transform_datatypes.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <mavconn/interface.h>
 
@@ -202,6 +204,12 @@ class UAS {
 	sensor_msgs::NavSatFix::Ptr get_gps_fix();
 
 
+	/* -*- transform -*- */
+
+	tf2_ros::Buffer tf2_buffer;
+	tf2_ros::TransformListener tf2_listener;
+	tf2_ros::TransformBroadcaster tf2_broadcaster;
+
 	/* -*- time sync -*- */
 
 	inline void set_time_offset(uint64_t offset_ns) {

diff --git a/mavros/src/lib/uas_data.cpp b/mavros/src/lib/uas_data.cpp
@@ -21,6 +21,7 @@
 using namespace mavros;
 
 UAS::UAS() :
+	tf2_listener(tf2_buffer, true),
 	type(MAV_TYPE_GENERIC),
 	autopilot(MAV_AUTOPILOT_GENERIC),
 	target_system(1),

diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp
@@ -21,7 +21,6 @@
 #include <mavros/gps_conversions.h>
 #include <pluginlib/class_list_macros.h>
 #include <eigen_conversions/eigen_msg.h>
-#include <tf2_ros/transform_broadcaster.h>
 
 #include <std_msgs/Float64.h>
 #include <sensor_msgs/NavSatFix.h>
@@ -95,8 +94,6 @@ class GlobalPositionPlugin : public MavRosPlugin {
 	ros::Publisher gp_hdg_pub;
 	ros::Publisher gp_rel_alt_pub;
 
-	tf2_ros::TransformBroadcaster tf2_broadcaster;
-
 	std::string frame_id;		//!< frame for topic headers
 	std::string tf_frame_id;	//!< origin for TF
 	std::string tf_child_frame_id;	//!< frame for TF and Pose
@@ -282,7 +279,7 @@ class GlobalPositionPlugin : public MavRosPlugin {
 			transform.transform.translation.y = pose_cov->pose.pose.position.y;
 			transform.transform.translation.z = pose_cov->pose.pose.position.z;
 
-			tf2_broadcaster.sendTransform(transform);
+			uas->tf2_broadcaster.sendTransform(transform);
 		}
 	}
 

diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp
@@ -17,7 +17,6 @@
 
 #include <mavros/mavros_plugin.h>
 #include <pluginlib/class_list_macros.h>
-#include <tf2_ros/transform_broadcaster.h>
 #include <eigen_conversions/eigen_msg.h>
 
 #include <geometry_msgs/PoseStamped.h>
@@ -61,7 +60,6 @@ class LocalPositionPlugin : public MavRosPlugin {
 	UAS *uas;
 
 	ros::Publisher local_position;
-	tf2_ros::TransformBroadcaster tf2_broadcaster;
 
 	std::string frame_id;		//!< frame for Pose
 	std::string tf_frame_id;	//!< origin for TF
@@ -96,7 +94,7 @@ class LocalPositionPlugin : public MavRosPlugin {
 			transform.transform.rotation = pose->pose.orientation;
 			tf::vectorEigenToMsg(position, transform.transform.translation);
 
-			tf2_broadcaster.sendTransform(transform);
+			uas->tf2_broadcaster.sendTransform(transform);
 		}
 	}
 };