定位与位姿

机器人定位相关功能。

定位操作

设置定位

set_localization(auto_relocation=True, location_name='', pose={'orientation': {'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0}, 'position': {'x': 0.0, 'y': 0.0, 'z': 0.0}}, block=False, timeout=200)

Set or reset the robot’s localization.

Parameters:

• auto_relocation (bool) – If True, use automatic relocation; if False, use manual pose (default: True)

• location_name (str) – Name of saved location for relocation (default: “”)

• pose (Pose) – Manual pose for localization when auto_relocation=False (default: empty MsgPose)

• block (bool) – If True, wait up to kBlockMaxTimeoutSeconds; if False, use timeout (default: False)

• timeout (int) – Timeout in seconds for localization to complete (default: 200)

Return type:

SetLocationResponse

Returns:

SetLocalizationResponse.

Examples:

# Auto-relocation (default)
result = set_localization()

# Manual localization with specific pose
pose = MsgPose()
pose.position.x = 0.0
pose.position.y = 0.0
pose.orientation.w = 1.0
result = set_localization(
    auto_relocation=False,
    pose=pose,
    timeout=100
)

# Relocalize to a saved location
result = set_localization(
    auto_relocation=True,
    location_name="start_point"
)

# Force relocalization with longer timeout
result = set_localization(timeout=300)
if result.success:
    print("Localization successful")

获取定位状态

get_localization_state()

Get the robot’s current localization state.

This function returns the robot’s localization quality with automatic data validation. The function waits up to 3 seconds for data to become available, then validates it with a 5-second staleness threshold before returning the state.

Returns:

Response containing state and localization state

• state: State object with code and describe

• loc_state: LocalizationState enum value

  • LocalizationState.NORMAL (0): Localization is working normally

  • LocalizationState.LACKDATA (1): Insufficient localization data

  • LocalizationState.LOST (2): Localization lost

  • LocalizationState.UNKNOWN_LOC (99): Unknown or unavailable state

Return type:

GetLocalizationStateResponse

Examples:

# Get localization state
response = get_localization_state()

# Check state using enum
if response.state.code == StateCode.success:
    if response.loc_state == LocalizationState.NORMAL:
        print("Localization is normal")
    elif response.loc_state == LocalizationState.LACKDATA:
        print("Localization lacks data")
    elif response.loc_state == LocalizationState.LOST:
        print("Localization lost")
    elif response.loc_state == LocalizationState.UNKNOWN_LOC:
        print("Localization state unknown or unavailable")

# Simple usage with integer comparison
loc_state = get_localization_state().loc_state
if int(loc_state) == 0:
    print("Navigation ready")

# Wait for good localization before navigation
while True:
    resp = get_localization_state()
    if resp.loc_state == LocalizationState.NORMAL:
        break
    time.sleep(1)
navigation_to_location("target")

Note

• Data from /nav/localization_state topic subscription

• Waits up to 3 seconds for data to become available

• Validates data freshness (5 second timeout)

• Returns UNKNOWN_LOC if data is stale or not yet received

• Data validation is performed automatically following the same pattern as GetRobotState