From 6858de7bd85ac282efa319945912481d36aa2974 Mon Sep 17 00:00:00 2001 From: Pedro Arias Date: Thu, 27 Jan 2022 09:47:26 +0100 Subject: [PATCH 1/2] TF braodcaster changed to freq 100hz --- drone_wrapper/src/drone_wrapper/drone_wrapper_class.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index b90229b..ac62d9f 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -614,7 +614,7 @@ def __init__(self, name='drone', ns='', verbose=False): self.frames_tf = FRAMES() self.br = tf.TransformBroadcaster() - self.br_timer = rospy.Timer(rospy.Duration(nsecs=1000000), self.br_base2map) + self.br_timer = rospy.Timer(rospy.Duration(nsecs=10000000), self.br_base2map) self.setpoint_raw_timer = rospy.Timer(rospy.Duration(nsecs=50000000), self.repeat_setpoint_raw) self.setpoint_raw_timer.shutdown() From d373914ecce36195521f7a777b7d6a57efab5ccb Mon Sep 17 00:00:00 2001 From: Pedro Arias Date: Thu, 27 Jan 2022 10:56:22 +0100 Subject: [PATCH 2/2] Only send transfrom of newer times to avoid tf repeated data --- .../src/drone_wrapper/drone_wrapper_class.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index ac62d9f..8ccad7c 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -567,12 +567,15 @@ def land(self): self.land_client(req) def br_base2map(self, event): - my_pose = self.get_position(frame="map") - my_orientation = self.get_orientation(frame="map") - my_orientation[2] += radians(-90) - q = tf.transformations.quaternion_from_euler(*my_orientation) - self.br.sendTransform(my_pose, - q, rospy.Time.now(), "base_link", "map") + now = rospy.Time.now() + if self.last_pub_tf < now: + my_pose = self.get_position(frame="map") + my_orientation = self.get_orientation(frame="map") + my_orientation[2] += radians(-90) + q = tf.transformations.quaternion_from_euler(*my_orientation) + self.br.sendTransform(my_pose, + q, now, "base_link", "map") + self.last_pub_tf = now def shutdown(self): self.br_timer.shutdown() @@ -615,6 +618,7 @@ def __init__(self, name='drone', ns='', verbose=False): self.frames_tf = FRAMES() self.br = tf.TransformBroadcaster() self.br_timer = rospy.Timer(rospy.Duration(nsecs=10000000), self.br_base2map) + self.last_pub_tf = rospy.Time(0) self.setpoint_raw_timer = rospy.Timer(rospy.Duration(nsecs=50000000), self.repeat_setpoint_raw) self.setpoint_raw_timer.shutdown()