Completing ROS Simulation Control

source/extensions/isaacsim.ros2.sim_control/python/impl/simulation_control.py

@@ -310,6 +310,7 @@ def __init__(self):
                 SetSimulationState,
                 SpawnEntity,
                 StepSimulation,
+                GetEntityBounds,
             )
 
             self.GetSimulationState = GetSimulationState
@@ -324,6 +325,7 @@ def __init__(self):
             self.GetEntitiesStates = GetEntitiesStates
             self.SetEntityState = SetEntityState
             self.GetSimulatorFeatures = GetSimulatorFeatures
+            self.GetEntityBounds = GetEntityBounds
 
             # Import action types
             from simulation_interfaces.action import SimulateSteps
@@ -344,6 +346,7 @@ def __init__(self):
             self.SetEntityState = None
             self.SimulateSteps = None
             self.GetSimulatorFeatures = None
+            self.GetEntityBounds = None
 
         # Initialize and register services
         self._initialize_ros2_services()
@@ -469,6 +472,14 @@ def _initialize_ros2_services(self):
             else:
                 carb.log_error("GetSimulatorFeatures service type not available")
 
+            # Register the GetEntityBounds service
+            if hasattr(self, "GetEntityBounds") and self.GetEntityBounds:
+                self.service_manager.register_service(
+                    f"{service_prefix}/GetEntityBounds", self.GetEntityBounds, self._handle_get_entity_bounds
+                )
+            else:
+                carb.log_error("GetEntityBounds service type not available")
+
             self.is_initialized = True
             carb.log_info("ROS 2 Simulation Control services initialized")
 
@@ -1445,6 +1456,55 @@ async def _handle_get_simulator_features(self, request, response):
             # There's no result field in GetSimulatorFeatures response, so we can only log the error
 
         return response
+
+    async def _handle_get_entity_bounds(self, request, response):
+        """Handle GetEntityBounds service request
+
+        This service retrieves the bounding box of a specific entity in the simulation.
+        The bounding box is returned as a type and a list of points representing the corners of the bounding box.
+
+        Args:
+            request: GetEntityBounds request with entity path
+            response: GetEntityBounds response with bounds type and points
+
+        Returns:
+            response: Completed GetEntityBounds response
+        """
+        try:
+            import isaacsim.core.utils.prims as prim_utils
+            from simulation_interfaces.msg import Result, Bounds
+            from geometry_msgs.msg import Vector3
+            import isaacsim.core.utils.bounds as bounds_utils
+            # Check if the entity exists
+            if not prim_utils.is_prim_path_valid(request.entity):
+                response.result = Result(
+                    result=Result.RESULT_NOT_FOUND, error_message=f"Entity '{request.entity}' does not exist"
+                )
+                return response
+
+            # Get the bounds of the entity
+            cache = bounds_utils.create_bbox_cache()
+            bounds = bounds_utils.compute_aabb(cache, prim_path=request.entity)
+
+
+            # Set the bounds in the response
+            response.bounds = bounds
+
+            # Set success result
+            response.result = Result(result=Result.RESULT_OK, error_message="")
+            response.bounds = Bounds(type=1, points=[
+                Vector3(x=bounds[0], y=bounds[1], z=bounds[2]),
+                Vector3(x=bounds[3], y=bounds[4], z=bounds[5])
+            ])
+            carb.log_info(f"Successfully retrieved bounds for entity: {request.entity}")
+
+        except Exception as e:
+            response.result = Result(
+                result=Result.RESULT_OPERATION_FAILED, error_message=f"Error getting entity bounds: {e}"
+            )
+            carb.log_error(f"Error in GetEntityBounds service handler: {e}")
+
+        return response
 
     def shutdown(self):
         """Shutdown the simulation control services"""