Feat lidar (#28)

* feat: add ros2 env

* feat: add lidar sensor

* feat: add lidar sensor

* Revert "feat: add lidar sensor"

This reverts commit 6c44f99.

* feat: Add support for Lidar sensor in LoggerBackend and ROS2Backend

exts/stride.simulator/config/anymalc_cfg.yaml

@@ -0,0 +1,8 @@
+
+sensor: 
+  imu:
+    update_frequency: 250
+
+  lidar:
+    update_frequency: 25
+    prim_path: /World/AnymalC/lidar/lidar_PhysX

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

@@ -17,11 +17,11 @@ def __init__(self, config=None):
         Args:
             config (dict): A Dictionary that contains all the parameters for configuring the LoggerBackend interface
                            - it can be empty or only have some of the parameters used by this backend.
-        
+
         Examples:
             The dictionary default parameters are
 
-            >>> {"vehicle_id": 0,           
+            >>> {"vehicle_id": 0,
             >>>  "update_rate": 250.0
             >>> }
         """
@@ -70,6 +70,8 @@ def update_sensor(self, sensor_type: str, data):
 
         if sensor_type == "Imu":
             self.update_imu_data(data)
+        elif sensor_type == "Lidar":
+            self.update_lidar_data(data)
         else:
             carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.")
             pass
@@ -83,6 +85,13 @@ def update_imu_data(self, data):
         carb.log_info(f"Angular velocity: {data['angular_velocity']}")
         carb.log_info(f"Linear acceleration: {data['linear_acceleration']}")
 
+    def update_lidar_data(self, data):
+        """
+        Method that handles the receival of Lidar data. It just prints the data to the console.
+        """
+        carb.log_info(f"Received Lidar data for vehicle {self._id}")
+        carb.log_info(f"Points: {data['points']}")
+
     def update(self, dt: float):
         """
         Method that update the state of the backend and the information being sent/received from the communication

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

@@ -1,21 +1,30 @@
+
+import os
 import carb
+import numpy as np
+import struct
+
 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")
+enable_extension("omni.isaac.ros2_bridge-humble")
 
 # 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,
+    Imu, PointCloud2, PointField, MagneticField, NavSatFix, NavSatStatus
 )
 from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped  # pylint: disable=wrong-import-position
 
 
+# set environment variable to use ROS2
+os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp"
+os.environ["ROS_DOMAIN_ID"] = "15"
+
 class ROS2Backend(Backend):
     """
     A class representing the ROS2 backend for the simulation.
@@ -32,7 +41,7 @@ class ROS2Backend(Backend):
         imu_pub: Publisher for the IMU sensor data.
 
     Methods:
-        update(dt: float): Updates the state of the backend and the information being sent/received 
+        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.
@@ -53,6 +62,8 @@ def __init__(self, node_name: str):
             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)
@@ -61,6 +72,7 @@ def __init__(self, node_name: str):
 
         # Create publishers for some sensor data
         self.imu_pub = self.node.create_publisher(Imu, node_name + "/sensors/imu", 10)
+        self.point_cloud_pub = self.node.create_publisher(PointCloud2, node_name + "/sensors/points", 10)
 
     def update(self, dt: float):
         """
@@ -104,6 +116,46 @@ def update_imu_data(self, data):
         # Publish the message with the current imu state
         self.imu_pub.publish(msg)
 
+    def update_lidar_data(self, data):
+        """
+        Updates the Lidar sensor data.
+
+        Args:
+            data: The Lidar sensor data.
+        """
+
+        msg = PointCloud2()
+
+        # Flatten LiDAR data
+        points_flat = np.array(data["points"]).reshape(-1, 3)  # Adjust based on your data's structure
+
+        # Create a PointCloud2 message
+        msg = PointCloud2()
+        # Update the header
+        msg.header.stamp = self.node.get_clock().now().to_msg()
+        msg.header.frame_id = "base_link_frd"
+        msg.height = 1  # Unorganized point cloud
+        msg.width = len(points_flat)
+        msg.fields = [
+            PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)
+        ]
+        msg.is_bigendian = False
+        msg.point_step = 12  # Float32, x, y, z
+        msg.row_step = msg.point_step * msg.width
+        msg.is_dense = True  # No invalid points
+        buffer = []
+
+        # Populate the message with your LiDAR data
+        for x, y, z in points_flat:
+            buffer += [struct.pack("fff", x, y, z)]
+
+        msg.data = b"".join(buffer)
+
+        # Publish the message with the current lidar state
+        self.point_cloud_pub.publish(msg)
+
     def update_sensor(self, sensor_type: str, data):
         """
         Method that when implemented, should handle the receival of sensor data.
@@ -115,6 +167,8 @@ def update_sensor(self, sensor_type: str, data):
 
         if sensor_type == "Imu":
             self.update_imu_data(data)
+        elif sensor_type == "Lidar":
+            self.update_lidar_data(data)
         else:
             carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.")
             pass

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

@@ -4,12 +4,16 @@
 # from stride.simulator.logic.backends.mavlink_backend import MavlinkBackend
 
 # Get the location of the asset
-from stride.simulator.backends import LoggerBackend, ROS2Backend
+# from stride.simulator.backends import LoggerBackend
+from stride.simulator.backends import ROS2Backend
 from stride.simulator.params import ROBOTS
 from stride.simulator.vehicles.sensors.imu import Imu
+from stride.simulator.vehicles.sensors.lidar import Lidar
 
 from stride.simulator.vehicles.controllers.anymal_controller import AnyamlController
 
+import yaml
+import os
 
 class AnymalCConfig(QuadrupedRobotConfig):
     """
@@ -28,12 +32,17 @@ def __init__(self):
         # The USD file that describes the visual aspect of the vehicle
         self.usd_file = ROBOTS["Anymal C"]
 
+        # read config file
+        with open(os.getcwd() + "/exts/stride.simulator/config/anymalc_cfg.yaml", "r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file)
+
         # The default sensors for a Anymal C
-        self.sensors = [Imu()]  # pylint: disable=use-list-literal FIXME
+        self.sensors = [Imu(self.config["sensor"]["imu"]), Lidar(self.config["sensor"]["lidar"])]  # pylint: disable=use-list-literal FIXME
 
         # 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(), ROS2Backend(self.vehicle_name)]  # pylint: disable=use-list-literal FIXME
+        self.backends = [ROS2Backend(self.vehicle_name)]  # pylint: disable=use-list-literal
+
 
 
 class AnymalC(QuadrupedRobot):
@@ -65,7 +74,7 @@ def update_sensors(self, dt: float):
         for sensor in self._sensors:
             try:
                 sensor_data = sensor.update(self._state, dt)
-            except Exception as e:
+            except Exception as e: # pylint: disable=broad-except
                 print(f"Error updating sensor: {e}")
                 continue
 

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

@@ -89,7 +89,7 @@ def __init__(  # pylint: disable=dangerous-default-value FIXME
         # 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
+        # Height scanner
         y = np.arange(-0.5, 0.6, 0.1)
         x = np.arange(-0.8, 0.9, 0.1)
         grid_x, grid_y = np.meshgrid(x, y)
@@ -193,9 +193,9 @@ def apply_torque(self, torque):
 
     def _compute_observation(self, command):
         """[summary]
-        
+
         compute the observation vector for the policy
-        
+
         Argument:
         command {np.ndarray} -- the robot command (v_x, v_y, w_z)
 

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

@@ -0,0 +1,74 @@
+__all__ = ["Lidar"]
+
+from stride.simulator.vehicles import State
+from stride.simulator.vehicles.sensors.sensor import Sensor
+
+from omni.isaac.core import World
+from omni.isaac.sensor import RotatingLidarPhysX
+from omni.isaac.range_sensor import _range_sensor               # Imports the python bindings to interact with lidar sensor
+
+class Lidar(Sensor):
+    """
+    The class that implements the Lidar sensor. This class inherits the base class Sensor.
+    """
+
+    def __init__(self, config=None):
+        """
+        Initialize the Lidar class
+
+        Args:
+        """
+        if config is None:
+            config = {}
+        else:
+            assert isinstance(config, dict), "The config parameter must be a dictionary."
+
+        self.config = config
+
+        super().__init__(sensor_type="Lidar", update_frequency=self.config.get("update_frequency", 10.0))
+
+        # Save the state of the sensor
+        self._state = {
+            "points": [],
+        }
+
+        self.lidar_interface = _range_sensor.acquire_lidar_sensor_interface() # Used to interact with the LIDAR
+
+        self.lidar_flag_ = False
+
+
+    @property
+    def state(self):
+        return self._state
+
+    @Sensor.update_at_frequency
+    def update(self, state: State, dt: float):
+
+        if self.lidar_flag_ is True:
+            pointcloud = self.lidar_interface.get_point_cloud_data(self.config.get("prim_path"))
+            # print(pointcloud)
+            self._state["points"] = pointcloud
+
+        else:
+            my_world = World.instance()
+
+            ## check there is world
+            if my_world is None:
+                pass
+            else:
+                self._lidar = my_world.scene.add(
+                            RotatingLidarPhysX(prim_path=self.config.get("prim_path"), name="range_sensor",
+                            rotation_dt = 10
+                            )
+                )
+                self._lidar.set_fov([360, 30])
+                self._lidar.set_resolution([0.4, 0.4])
+                self._lidar.set_valid_range([0.1, 6])
+                self._lidar.enable_visualization(high_lod=True,
+                                                 draw_points=True,
+                                                 draw_lines=False)
+
+                self.lidar_flag_ = True
+
+        return self._state
+