add Imu sensor to anymalc

exts/stride.simulator/stride/simulator/interfaces/__init__.py

@@ -0,0 +1 @@
+from .stride_sim_interface import StrideInterface

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

@@ -5,6 +5,8 @@
 
 # Get the location of the asset
 from stride.simulator.params import ROBOTS_ENVIRONMNETS
+from stride.simulator.vehicles.sensors import Imu
+from stride.simulator.interfaces import StrideInterface
 
 
 class AnymalCConfig(QuadrupedRobotConfig):
@@ -24,7 +26,7 @@ def __init__(self):
         self.usd_file = ROBOTS_ENVIRONMNETS["Anymal C"]
 
         # The default sensors for a quadrotor
-        self.sensors = []  # pylint: disable=use-list-literal FIXME
+        self.sensors = [Imu()]  # pylint: disable=use-list-literal FIXME
 
         # The backends for actually sending commands to the vehicle.
         # By default use mavlink (with default mavlink configurations)
@@ -34,6 +36,8 @@ def __init__(self):
 
 
 class AnymalC(QuadrupedRobot):
+    """AnymalC class - It is a child class of QuadrupedRobot class to implement a AnymalC robot in the simulator.
+    """
 
     def __init__(self, id: int, init_pos, init_orientation, config=AnymalCConfig()):
 
@@ -43,3 +47,30 @@ def __init__(self, id: int, init_pos, init_orientation, config=AnymalCConfig()):
             init_orientation = [0.0, 0.0, 0.0, 1.0]
 
         super().__init__(config.stage_prefix, config.usd_file, id, init_pos, init_orientation, config=config)
+
+
+        self._sensors = config.sensors
+        for sensor in self._sensors:
+            sensor.set_spherical_coordinate(StrideInterface().latitude, StrideInterface().longitude,
+                                            StrideInterface().altitude)
+
+        # Add callbacks to the physics engine to update each sensor at every timestep and let the sensor decide
+        # depending on its internal update rate whether to generate new data.
+        self._world.add_physics_callback(self._stage_prefix + "/Sensors", self.update_sensors)
+
+    def update_sensors(self, dt: float):
+        """Callback that is called at every physics steps and will call the sensor.update method to generate new
+        sensor data. For each data that the sensor generates, the backend.update_sensor method will also be called for
+        every backend. For example, if new data is generated for an IMU and we have a MavlinkBackend, then the
+        update_sensor method will be called for that backend so that this data can latter be sent thorugh mavlink.
+
+        Args:
+            dt (float): The time elapsed between the previous and current function calls (s).
+        """
+
+        # Call the update method for the sensor to update its values internally (if applicable)
+        for sensor in self._sensors:
+            sensor_data = sensor.update(self._state, dt)
+
+            if sensor_data is not None:
+                print("TODO: Implement backend code.")

exts/stride.simulator/stride/simulator/vehicles/sensors/__init__.py

@@ -1 +1,2 @@
 from .sensor import Sensor
+from .imu import Imu