Harderthan/imu (#27)

* check member variable of the instance instead of the singleton instance

* implement function for backend update

* change file name of logger backend

* Implement ros2 backend

* fix: pylint error

---------

Co-authored-by: jinwon kim <[email protected]>

exts/stride.simulator/stride/simulator/backends/__init__.py

@@ -1,2 +1,3 @@
 from .backend import Backend
-from .logger import LoggerBackend
+from .logger_backend import LoggerBackend
+from .ros2_backend import ROS2Backend

exts/stride.simulator/stride/simulator/backends/logger.py → exts/stride.simulator/stride/simulator/backends/logger_backend.py

@@ -66,9 +66,13 @@ def update_sensor(self, sensor_type: str, data):
         Method that when implemented, should handle the receival of sensor data
         """
 
+        carb.log_info(f"sensor type: {sensor_type}")
+
         if sensor_type == "Imu":
             self.update_imu_data(data)
-
+        else:
+            carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.")
+            pass
         # TODO: Add support for other sensors
 
     def update_imu_data(self, data):

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

@@ -0,0 +1,196 @@
+import carb
+from stride.simulator.backends.backend import Backend
+from omni.isaac.core.utils.extensions import disable_extension, enable_extension
+
+# # Perform some checks, because Isaac Sim some times does not play nice when using ROS/ROS2
+disable_extension("omni.isaac.ros_bridge")
+enable_extension("omni.isaac.ros2_bridge")
+
+# 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
+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, MagneticField, NavSatFix, NavSatStatus,
+)
+from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped  # pylint: disable=wrong-import-position
+
+
+class ROS2Backend(Backend):
+    """
+    A class representing the ROS2 backend for the simulation.
+
+    Args:
+        node_name (str): The name of the ROS2 node.
+
+    Attributes:
+        node: The ROS2 node object.
+        pose_pub: Publisher for the state of the vehicle in ENU.
+        twist_pub: Publisher for the state of the vehicle's twist in ENU.
+        twist_inertial_pub: Publisher for the state of the vehicle's inertial twist.
+        accel_pub: Publisher for the state of the vehicle's acceleration.
+        imu_pub: Publisher for the IMU sensor data.
+
+    Methods:
+        update(dt: float): Updates the state of the backend and the information being sent/received 
+                            from the communication interface.
+        update_imu_data(data): Updates the IMU sensor data.
+        update_sensor(sensor_type: str, data): Handles the receival of sensor data.
+        update_state(state): Handles the receival of the state of the vehicle.
+    """
+
+    def __init__(self, node_name: str):
+        """
+        Initializes the ROS2Backend.
+
+        Args:
+            node_name (str): The name of the ROS2 node.
+        """
+        super().__init__()
+
+        # Start the actual ROS2 setup here
+        if not rclpy.ok():  # Check if the ROS2 context is already initialized
+            rclpy.init()
+        self.node = rclpy.create_node(node_name)
+
+        # Create publishers for the state of the vehicle in ENU
+        self.pose_pub = self.node.create_publisher(PoseStamped, node_name + "/state/pose", 10)
+        self.twist_pub = self.node.create_publisher(TwistStamped, node_name + "/state/twist", 10)
+        self.twist_inertial_pub = self.node.create_publisher(TwistStamped, node_name + "/state/twist_inertial", 10)
+        self.accel_pub = self.node.create_publisher(AccelStamped, node_name + "/state/accel", 10)
+
+        # Create publishers for some sensor data
+        self.imu_pub = self.node.create_publisher(Imu, node_name + "/sensors/imu", 10)
+
+    def update(self, dt: float):
+        """
+        Method that when implemented, should be used to update the state of the backend and the information being
+        sent/received from the communication interface. This method will be called by the simulation on every physics
+        step.
+
+        Args:
+            dt (float): The time step for the update.
+        """
+
+        # In this case, do nothing as we are sending messages as soon as new data arrives from the sensors and state
+        # and updating the reference for the thrusters as soon as receiving from ROS2 topics
+        # Just poll for new ROS2 messages in a non-blocking way
+        rclpy.spin_once(self.node, timeout_sec=0)
+
+    def update_imu_data(self, data):
+        """
+        Updates the IMU sensor data.
+
+        Args:
+            data: The IMU sensor data.
+        """
+
+        msg = Imu()
+
+        # Update the header
+        msg.header.stamp = self.node.get_clock().now().to_msg()
+        msg.header.frame_id = "base_link_frd"
+
+        # Update the angular velocity (NED + FRD)
+        msg.angular_velocity.x = data["angular_velocity"][0]
+        msg.angular_velocity.y = data["angular_velocity"][1]
+        msg.angular_velocity.z = data["angular_velocity"][2]
+
+        # Update the linear acceleration (NED)
+        msg.linear_acceleration.x = data["linear_acceleration"][0]
+        msg.linear_acceleration.y = data["linear_acceleration"][1]
+        msg.linear_acceleration.z = data["linear_acceleration"][2]
+
+        # Publish the message with the current imu state
+        self.imu_pub.publish(msg)
+
+    def update_sensor(self, sensor_type: str, data):
+        """
+        Method that when implemented, should handle the receival of sensor data.
+
+        Args:
+            sensor_type (str): The type of the sensor.
+            data: The sensor data.
+        """
+
+        if sensor_type == "Imu":
+            self.update_imu_data(data)
+        else:
+            carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.")
+            pass
+
+    def update_state(self, state):
+        """
+        Method that when implemented, should handle the receival of the state of the vehicle using this callback.
+
+        Args:
+            state: The state of the vehicle.
+        """
+
+        pose = PoseStamped()
+        twist = TwistStamped()
+        twist_inertial = TwistStamped()
+        accel = AccelStamped()
+
+        # Update the header
+        pose.header.stamp = (self.node.get_clock().now().to_msg())  # TODO: fill time when the state was measured.
+        twist.header.stamp = pose.header.stamp
+        twist_inertial.header.stamp = pose.header.stamp
+        accel.header.stamp = pose.header.stamp
+
+        pose.header.frame_id = "map"
+        twist.header.frame_id = "base_link"
+        twist_inertial.header.frame_id = "base_link"
+        accel.header.frame_id = "base_link"
+
+        # Fill the position and attitude of the vehicle in ENU.
+        pose.pose.position.x = state.position[0]
+        pose.pose.position.y = state.position[1]
+        pose.pose.position.z = state.position[2]
+
+        pose.pose.orientation.x = state.attitude[0]
+        pose.pose.orientation.y = state.attitude[1]
+        pose.pose.orientation.z = state.attitude[2]
+        pose.pose.orientation.w = state.attitude[3]
+
+        # Fill the linear and angular velocities in the body frame of the vehicle.
+        twist.twist.linear.x = state.linear_body_velocity[0]
+        twist.twist.linear.y = state.linear_body_velocity[1]
+        twist.twist.linear.z = state.linear_body_velocity[2]
+
+        twist.twist.angular.x = state.angular_velocity[0]
+        twist.twist.angular.y = state.angular_velocity[1]
+        twist.twist.angular.z = state.angular_velocity[2]
+
+        # Fill the linear velocity of the vehicle in the inertial frame.
+        twist_inertial.twist.linear.x = state.linear_velocity[0]
+        twist_inertial.twist.linear.y = state.linear_velocity[1]
+        twist_inertial.twist.linear.z = state.linear_velocity[2]
+
+        # Fill the linear acceleration in the inertial frame
+        accel.accel.linear.x = state.linear_acceleration[0]
+        accel.accel.linear.y = state.linear_acceleration[1]
+        accel.accel.linear.z = state.linear_acceleration[2]
+
+        # Publish the messages containing the state of the vehicle.
+        self.pose_pub.publish(pose)
+        self.twist_pub.publish(twist)
+        self.twist_inertial_pub.publish(twist_inertial)
+        self.accel_pub.publish(accel)
+
+    # def check_ros_extension(self):
+    #     """
+    #     Method that checks which ROS extension is installed.
+    #     """
+
+    #     # Get the handle for the extension manager
+    #     extension_manager = omni.kit.app.get_app().get_extension_manager()
+
+    #     version = ""
+
+    #     if self._ext_manager.is_extension_enabled("omni.isaac.ros_bridge"):
+    #         version = "ros"
+    #     elif self._ext_manager.is_extension_enabled("omni.isaac.ros2_bridge"):
+    #         version = "ros2"
+    #     else:
+    #         carb.log_warn("Neither extension 'omni.isaac.ros_bridge' nor 'omni.isaac.ros2_bridge' is enabled")

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

@@ -4,7 +4,7 @@
 # from stride.simulator.logic.backends.mavlink_backend import MavlinkBackend
 
 # Get the location of the asset
-from stride.simulator.backends import LoggerBackend
+from stride.simulator.backends import LoggerBackend, ROS2Backend
 from stride.simulator.params import ROBOTS
 from stride.simulator.vehicles.sensors.imu import Imu
 
@@ -20,6 +20,8 @@ def __init__(self):
 
         super().__init__()
 
+        self.vehicle_name = "anymal_c"
+
         # Stage prefix of the vehicle when spawning in the world
         self.stage_prefix = "/World/AnymalC"
 
@@ -31,7 +33,7 @@ def __init__(self):
 
         # The backends for actually sending commands to the vehicle.
         # It can also be a ROS2 backend or your own custom Backend implementation!
-        self.backends = [LoggerBackend()]  # pylint: disable=use-list-literal FIXME
+        self.backends = [LoggerBackend(), ROS2Backend(self.vehicle_name)]  # pylint: disable=use-list-literal FIXME
 
 
 class AnymalC(QuadrupedRobot):
@@ -61,15 +63,15 @@ def update_sensors(self, dt: float):
 
         # Call the update method for the sensor to update its values internally (if applicable)
         for sensor in self._sensors:
-            # TODO: sensor update 부분 구현 필요
-            # try:
-            sensor_data = sensor.update(self._state, dt)
-            # except Exception as e:
-            #     print(f"Error updating sensor: {e}")
-            #     continue
+            try:
+                sensor_data = sensor.update(self._state, dt)
+            except Exception as e:
+                print(f"Error updating sensor: {e}")
+                continue
 
             if sensor_data is not None:
-                print("TODO: Implement backend code.")
+                for backend in self._backends:
+                    backend.update_sensor(sensor.sensor_type, sensor_data)
 
     def initialize(self, physics_sim_view=None) -> None:
         """[summary]

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

@@ -86,6 +86,9 @@ def __init__(  # pylint: disable=dangerous-default-value FIXME
         for backend in self._backends:
             backend.initialize(self)
 
+        # Add a callback to the physics engine to update the state of the vehicle at every timestep.
+        self._world.add_physics_callback(self._stage_prefix + "/sim_state", self.update_sim_state)
+
         # Height scaner
         y = np.arange(-0.5, 0.6, 0.1)
         x = np.arange(-0.8, 0.9, 0.1)
@@ -124,6 +127,7 @@ def update_sim_state(self, dt: float):
         Args:
             dt (float): The time elapsed between the previous and current function calls (s).
         """
+
         for backend in self._backends:
             backend.update_state(self._state)
 
@@ -201,9 +205,9 @@ def _compute_observation(self, command):
         """
         lin_vel_I = self.state.linear_velocity  #pylint: disable=invalid-name
         ang_vel_I = self.state.angular_velocity  #pylint: disable=invalid-name
-        pos_IB = self.state.position #pylint: disable=invalid-name
+        pos_IB = self.state.position  #pylint: disable=invalid-name
         # Convert quaternion from XYZW to WXYZ format
-        q_IB = np.array( #pylint: disable=invalid-name
+        q_IB = np.array(  #pylint: disable=invalid-name
             [self.state.attitude[-1], self.state.attitude[0], self.state.attitude[1], self.state.attitude[2]])
 
         R_IB = quat_to_rot_matrix(q_IB)  #pylint: disable=invalid-name