Add tf world name control from yaml file

boomer319 · Oct 4, 2022 · b5cbf9c · b5cbf9c
1 parent b35329c
commit b5cbf9c
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py
@@ -87,6 +87,11 @@ def __init__(self):
  "scan": self._log_scan_data_callback,
  "odom": self._log_odom_data_callback}
 
+ self.world_tf_name = "world"
+ try:
+ self.world_tf_name = self._ros_parameters["world_tf_name"]
+ except KeyError:
+ pass
  robot_data = self._ros_parameters["robots"]
 
  # Init a transform broadcaster
@@ -453,7 +458,7 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri):
 
  msg = PoseStamped()
  msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
+ msg.header.frame_id = self.world_tf_name
  msg.pose.position.x = x
  msg.pose.position.y = y
  msg.pose.position.z = z
@@ -465,7 +470,7 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri):
 
  t_base = TransformStamped()
  t_base.header.stamp = self.get_clock().now().to_msg()
- t_base.header.frame_id = 'world'
+ t_base.header.frame_id = self.world_tf_name
  t_base.child_frame_id = cf_name
  t_base.transform.translation.x = x
  t_base.transform.translation.y = y
@@ -493,7 +498,7 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri):
  msg = Odometry()
  msg.child_frame_id = cf_name
  msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
+ msg.header.frame_id = self.world_tf_name
  msg.pose.pose.position.x = x
  msg.pose.pose.position.y = y
  msg.pose.pose.orientation.x = q[0]