定位与位姿

机器人定位相关功能。

定位操作

设置定位

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, candidate_poses=[])

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)

  • candidate_poses (list[Pose]) – 多候选初始位姿；非空时优先走自动选点重定位（/nav/autoselect_initialpose），忽略 auto_relocation/location_name。每个元素为 geometry_msgs Pose（map 坐标系）。（默认: []）

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")

# Multi-candidate auto-select relocalization
p1 = MsgPose()
p1.position.x = 1.0
p1.position.y = 2.0
p1.orientation.w = 1.0
p2 = MsgPose()
p2.position.x = 3.0
p2.position.y = 4.0
p2.orientation.w = 1.0
result = set_localization(candidate_poses=[p1, p2])
if result.success:
    print("Multi-candidate relocalization successful")

Note

  • 当 candidate_poses 非空时，向 /nav/autoselect_initialpose 发送候选位姿列表，由后端自动选取最优初始位姿完成重定位，此时 auto_relocation 与 location_name 参数被忽略。

获取定位状态

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

获取当前定位模式

get_current_localization_mode()

Get the robot’s current localization mode.

Parameters:

None – 此函数无参数。

Returns:

  • state: 操作状态（code 0=成功）

  • mode: LocalizationMode 枚举（MAP=0 地图定位 / SLAM=1 / REFLECTOR=2 反光柱 / UNKNOWN_LOC_MODE=99）

Return type:

GetCurrentLocalizationModeResponse

Examples:

result = get_current_localization_mode()
if result.state.code == 0:
    print(f"当前定位模式: {result.mode}")

Note

  • 来源 topic /nav/current_localization_mode（transient_local），读本地缓存，5 秒 TTL；过期/未收到返回 UNKNOWN_LOC_MODE。

相关数据类型