Add base_link TF

exts/stride.simulator/stride/simulator/backends/ros2_backend.py

@@ -3,6 +3,7 @@
 import numpy as np
 import struct
 
+import rclpy.logging
 from stride.simulator.backends.backend import Backend
 from omni.isaac.core.utils.extensions import disable_extension, enable_extension
 
@@ -26,7 +27,11 @@
     PoseStamped,
     TwistStamped,
     AccelStamped,
+    TransformStamped,
 )
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+from tf2_ros.transform_broadcaster import TransformBroadcaster
+
 
 # set environment variable to use ROS2
 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp"
@@ -68,6 +73,7 @@ def __init__(self, node_name: str):
         # Start the actual ROS2 setup here
         if not rclpy.ok():  # Check if the ROS2 context is already initialized
             rclpy.init()
+        print("ROS2 context initialized")
         self.node = rclpy.create_node(node_name)
 
         # Create publishers for the state of the vehicle in ENU
@@ -90,6 +96,53 @@ def __init__(self, node_name: str):
             PointCloud2, node_name + "/sensors/points", 10
         )
 
+        # Create a static transform broadcaster for the base_link to map transform
+        self.tf_static_broadcaster = StaticTransformBroadcaster(self.node)
+
+        #Initialize the transform from base_link to map
+        self.send_static_transform()
+
+        # Create a transform broadcaster for the base_link to map transform
+        self.tf_broadcaster = TransformBroadcaster(self.node)
+
+    def send_static_transform(self):
+        """
+        send the static transform from base_link to vehicle_link
+        """
+        t = TransformStamped()
+        t.header.stamp = self.node.get_clock().now().to_msg()
+        t.header.frame_id = 'base_link'
+        t.child_frame_id = 'base_link_frd'
+        # TODO(Jeong) : if the state update function is implemented, the position and orientation should be updated
+        # Converts from FLU to FRD
+        t.transform.translation.x = 0.0
+        t.transform.translation.y = 0.0
+        t.transform.translation.z = 0.0
+        t.transform.rotation.x = 1.0
+        t.transform.rotation.y = 0.0
+        t.transform.rotation.z = 0.0
+        t.transform.rotation.w = 0.0
+
+        self.tf_static_broadcaster.sendTransform(t)
+
+        # Create the transform from map, i.e inertial frame (ROS standard) to map_ned (standard in airborn or marine vehicles)
+        t = TransformStamped()
+        t.header.stamp = self.node.get_clock().now().to_msg()
+        t.header.frame_id = "map"
+        t.child_frame_id = "map_ned"
+
+        # Converts ENU to NED
+        t.transform.translation.x = 0.0
+        t.transform.translation.y = 0.0
+        t.transform.translation.z = 0.0
+        t.transform.rotation.x = -0.7071068
+        t.transform.rotation.y = -0.7071068
+        t.transform.rotation.z = 0.0
+        t.transform.rotation.w = 0.0
+
+        self.tf_static_broadcaster.sendTransform(t)
+
+
     def update(self, dt: float):
         """
         Method that when implemented, should be used to update the state of the backend and the information being
@@ -253,3 +306,17 @@ def update_state(self, state):
         self.twist_pub.publish(twist)
         self.twist_inertial_pub.publish(twist_inertial)
         self.accel_pub.publish(accel)
+
+        t = TransformStamped()
+        t.header.stamp = self.node.get_clock().now().to_msg()
+        t.header.frame_id = "map"
+        t.child_frame_id = "base_link"
+        t.transform.translation.x = state.position[0]
+        t.transform.translation.y = state.position[1]
+        t.transform.translation.z = state.position[2]
+        t.transform.rotation.x = state.attitude[0]
+        t.transform.rotation.y = state.attitude[1]
+        t.transform.rotation.z = state.attitude[2]
+        t.transform.rotation.w = state.attitude[3]
+        self.tf_broadcaster.sendTransform(t)
+

exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py

@@ -293,3 +293,5 @@ def _hit_report_callback(self, hit):
         if "/World/layout/GroundPlane" in current_hit_body:
             self._query_info.append(hit.distance)
         return True
+
+    # TODO(Jeong) : Implement the function for the joint positions and velocities related to the quadrupedrobot

exts/stride.simulator/stride/simulator/vehicles/vehicle.py

@@ -187,6 +187,7 @@ def update_state(self, dt: float):
         # Get the body frame interface of the vehicle
         # (this will be the frame used to get the position, orientation, etc.)
         body = self._world.dc_interface.get_rigid_body(self._stage_prefix + "/base")
+        # TODO(Jeong) : Implement the following methods for the joint positions and velocities
 
         # Get the current position and orientation in the inertial frame
         pose = self._world.dc_interface.get_rigid_body_pose(body)