Skip to content
Snippets Groups Projects
Commit d29ba4b8 authored by daviddudas's avatar daviddudas
Browse files

Add a switch to disable publish TF

parent d72db8b6
No related branches found
No related tags found
No related merge requests found
......@@ -52,6 +52,7 @@ class OdometryNode:
self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
self.encoderMin = int(rospy.get_param('~encoder_min', -32768))
self.encoderMax = int(rospy.get_param('~encoder_max', 32767))
self.publishTF = rospy.get_param('~publish_tf', 'true')
self.odometry.setWheelSeparation(self.wheelSeparation)
self.odometry.setWheelSeparationLength(self.wheelSeparationLength)
......@@ -70,6 +71,8 @@ class OdometryNode:
pose = self.odometry.getPose()
q = quaternion_from_euler(0, 0, pose.theta)
if self.publishTF == 'true':
self.tfPub.sendTransform(
(pose.x, pose.y, 0),
(q[0], q[1], q[2], q[3]),
......@@ -78,6 +81,7 @@ class OdometryNode:
self.odomFrameID
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odomFrameID
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment