Pr/47

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

@@ -13,6 +13,8 @@
 # Inform the user that now we are actually import the ROS2 dependencies
 # Note: we are performing the imports here to make sure that ROS2 extension was load correctly
 import rclpy  # pylint: disable=wrong-import-position
+import rclpy.logging  # pylint: disable=wrong-import-position
+
 from std_msgs.msg import Float64  # pylint: disable=unused-import, wrong-import-position
 from sensor_msgs.msg import (  # pylint: disable=unused-import, wrong-import-position
     Imu,
@@ -26,8 +28,16 @@
     PoseStamped,
     TwistStamped,
     AccelStamped,
+    TransformStamped,
+)
+from tf2_ros.static_transform_broadcaster import (  # pylint: disable=wrong-import-position
+    StaticTransformBroadcaster,
+)
+from tf2_ros.transform_broadcaster import (  # pylint: disable=wrong-import-position
+    TransformBroadcaster,
 )
 
+
 # set environment variable to use ROS2
 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp"
 os.environ["ROS_DOMAIN_ID"] = "15"
@@ -68,6 +78,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 +101,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 +311,16 @@ 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,6 @@ 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/sensors/geo_mag_utils.py

@@ -5,6 +5,7 @@
 with the PX4 stil_gazebo implementation (https://github.com/PX4/PX4-SITL_gazebo). Therefore, PX4 should behave similarly
 to a gazebo-based simulation.
 """
+
 import numpy as np
 
 # Declare which functions are visible from this file

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)