Detect and Navigate Example

from grid.model.perception.detection.owlv2 import OWLv2
det_model = OWLv2()

def get_box_center(box, image_size):
    x_min, y_min, x_max, y_max = box
    # Check if object is too close (covers >38.5% of image)
    if image_size[0] * image_size[1] * .385 < (x_max-x_min) * (y_max-y_min):
        return -1, -1
    x_center = (x_min + x_max) / 2
    y_center = (y_min + y_max) / 2
    return x_center, y_center

def get_velocity(target, image_size):
    # Calculate normalized offsets from image center
    image_center_x = image_size[0] / 2
    image_center_y = image_size[1] / 2
    offset_x = target[0] - image_center_x
    offset_y = target[1] - image_center_y
    
    # Convert to normalized velocities
    norm_offset_x = offset_x / image_center_x
    angular_velocity = max(min(-norm_offset_x, 2), -2)
    
    return (
        Velocity(0.8, 0, 0),  # Linear velocity
        Velocity(0, 0, angular_velocity)  # Angular velocity
    )

def main_loop():
    while True:
        rgb = agent.getImage()
        if rgb is not None and rgb.data is not None:
            # Detect object
            boxes, scores, labels = det_model.detect_object(
                rgbimage=rgb.data[:, :, :3], 
                text_prompt=obj
            )
            
            if boxes is not None and len(boxes) > 0:
                # Calculate movement based on detection
                mid_x, mid_y = get_box_center(boxes[0], [rgb.data.shape[1], rgb.data.shape[0]])
                if mid_x == -1 and mid_y == -1:
                    agent.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, 0))
                    return
                linear_vel, angular_vel = get_velocity([mid_x, mid_y], [rgb.data.shape[1], rgb.data.shape[0]])
            else:
                # Search for object by rotating
                linear_vel = Velocity(0, 0, 0)
                angular_vel = Velocity(0, 0, 2)
                
            agent.moveByVelocity(linear_vel, angular_vel)
        time.sleep(.5)

from grid.robot.locomotion.isaac_locomotion import IsaacLocomotion

agent = IsaacLocomotion()
agent.run()

from grid.robot.locomotion.go2 import Go2Real
agent = Go2Real("wlp0s20f3")  # Replace with your network interface

SIM = False  # For real robot
# or
SIM = True   # For simulated robot

obj = "blue dustbin"  # For real robot (or any object in your environment)
# or
obj = "forklift"  # For simulated robot

main_loop()