lidar property change

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

@@ -106,6 +106,7 @@ def update(self, dt: float):
         rclpy.spin_once(self.node, timeout_sec=0)
 
     def update_imu_data(self, data):
+
         """
         Updates the IMU sensor data.
 
@@ -139,7 +140,7 @@ def update_lidar_data(self, data):
         Args:
             data: The Lidar sensor data.
         """
-
+        print("lidar")
         msg = PointCloud2()
 
         # Flatten LiDAR data
@@ -158,6 +159,12 @@ def update_lidar_data(self, data):
             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),
+
+            # 도영 Lidar Property 추가
+            PointField(name="intensity", offset=12, datatype=PointField.FLOAT32, count=1),
+            PointField(name="ring", offset=16, datatype=PointField.FLOAT32, count=1),
+            PointField(name="time", offset=18, datatype=PointField.FLOAT32, count=1),
+
         ]
         msg.is_bigendian = False
         msg.point_step = 12  # Float32, x, y, z
@@ -182,7 +189,8 @@ def update_sensor(self, sensor_type: str, data):
             sensor_type (str): The type of the sensor.
             data: The sensor data.
         """
-
+        print(sensor_type)
+
         if sensor_type == "Imu":
             self.update_imu_data(data)
         elif sensor_type == "Lidar":

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

@@ -89,6 +89,8 @@ def update_sensors(self, dt: float):
         """
 
         # Call the update method for the sensor to update its values internally (if applicable)
+        print("debug2")
+
         for sensor in self._sensors:
             try:
                 sensor_data = sensor.update(self._state, dt)

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

@@ -10,6 +10,7 @@
 # from stride.simulator.backends import LoggerBackend
 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.sensors.lidar import Lidar
 
@@ -50,7 +51,7 @@ def __init__(self):
         # The default sensors for a Go1
         self.sensors = [
             Imu(self.config["sensor"]["imu"]),
-            # Lidar(self.config["sensor"]["lidar"]),
+            Lidar(self.config["sensor"]["lidar"]),
         ]  # pylint: disable=use-list-literal FIXME
 
         # The backends for actually sending commands to the vehicle.
@@ -88,9 +89,13 @@ def update_sensors(self, dt: float):
         Args:
             dt (float): The time elapsed between the previous and current function calls (s).
         """
+        ## doyoung ##
+        print("debug3")
 
         # Call the update method for the sensor to update its values internally (if applicable)
         for sensor in self._sensors:
+            ## doyoung ##
+            print(sensor)
             try:
                 sensor_data = sensor.update(self._state, dt)
             except Exception as e:  # pylint: disable=broad-except

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

@@ -126,6 +126,7 @@ def update_sensors(self, dt: float):
             # If some data was updated and we have a mavlink backend or ros backend (or other), then just update it
             if sensor_data is not None:
                 for backend in self._backends:
+                    print("debug1")
                     backend.update_sensor(sensor.sensor_type, sensor_data)
 
     def update_sim_state(self, dt: float):
@@ -283,8 +284,8 @@ def _compute_observation(self, command):
             if self._query_info:
                 distance = min(self._query_info)
                 obs[48 + i] = np.clip(distance - 0.5, -1.0, 1.0)
-            else:
-                print("No hit")
+            # else:
+            #     print("No hit")
 
         return obs
 

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

@@ -12,6 +12,7 @@ class Imu(Sensor):
     """The class that implements the Imu sensor. This class inherits the base class Sensor."""
 
     def __init__(self, config=None):
+        print("???")
         """Initialize the Imu class
 
         Args:

link_app.sh

@@ -5,4 +5,4 @@ set -e
 SCRIPT_DIR=$(dirname ${BASH_SOURCE})
 cd "$SCRIPT_DIR"
 
-exec "tools/packman/python.sh" tools/scripts/link_app.py $@
+exec "tools/packman/python.sh" tools/scripts/link_app.py $@