plugin: vision_position: Add transform timestamp check.

Issue #60.

src/plugins/vision_position.cpp

@@ -99,6 +99,7 @@ class VisionPositionPlugin : public MavRosPlugin,
 	std::string child_frame_id;
 
 	double tf_rate;
+	ros::Time last_transform_stamp;
 
 	/* -*- low-level send -*- */
 
@@ -129,6 +130,15 @@ class VisionPositionPlugin : public MavRosPlugin,
 		tf::Matrix3x3 orientation(transform.getBasis());
 		orientation.getRPY(roll, pitch, yaw);
 
+		/* Issue #60.
+		 * Note: this now affects pose callbacks too, but i think its not big deal.
+		 */
+		if (last_transform_stamp == stamp) {
+			ROS_DEBUG_THROTTLE_NAMED(10, "position", "Vision: Same transform as last one, dropped.");
+			return;
+		}
+		last_transform_stamp = stamp;
+
 		// TODO: check conversion. Issue #49.
 		vision_position_estimate(stamp.toNSec() / 1000,
 				position.y(), position.x(), -position.z(),