定位与位姿

机器人定位相关功能。

定位操作

设置定位

set_localization(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=[], auto_relocation=None)

Set or reset the robot’s localization (localization initialization).

定位初始化需要提供初值，按以下优先级选取其一： candidate_poses（多候选自动选优）> location_name（已注册点位名）> pose（手动位姿）。

Parameters:

location_name (str) – Name of saved location for relocation (default: “”)
pose (Pose) – Manual pose for localization (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），忽略 location_name/pose。每个元素为 geometry_msgs Pose（map 坐标系）。（默认: []）
auto_relocation (bool) – 已弃用的兼容参数，新脚本不要使用（默认: 不传）。仅为兼容导航 0731 之前编写的旧任务脚本保留：传 False 等价于不传（用调用方给的初值），仅打一条弃用 WARN；传 True 直接返回失败（自动重定位能力已下线）。

Returns:

SetLocalizationResponse.

Examples:

# 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(
    pose=pose,
    timeout=100
)

# Relocalize to a saved location
result = set_localization(location_name="start_point")
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 发送候选位姿列表，由后端自动选取最优初始位姿完成重定位，此时 location_name 与 pose 参数被忽略。
自动重定位能力已随导航 0731 版本下线（底层 /nav/trigger_relo 不再对外）；请显式提供 candidate_poses / location_name / pose 之一作为初值。
新脚本一律不要传 auto_relocation，它只为兼容 0731 之前的旧脚本保留。旧脚本 set_localization(auto_relocation=False, pose=p) 请改写为 set_localization(pose=p)；传 auto_relocation=True 会直接返回失败。
初值与真实位置偏差过大时初始化会失败；成功后仍需等待定位状态变为 NORMAL 再发起导航。

底层 ROS2 接口

按参数两分支择一下发（location_name 非空时先在本地读点位 yaml 解析出位姿，该解析不经 ROS2）。自动重定位能力已随导航 0731 版本下线（底层 /nav/trigger_relo 不再对外），定位初始化必须显式提供 candidate_poses / location_name / pose 之一作为初值。 auto_relocation 参数仅为兼容 0731 之前编写的旧任务脚本而保留，不下发：传 False 等价于不传（仅打一条弃用 WARN），传 True 在绑定层直接返回失败（口径与 /sdk/nav/set_localization 服务端一致）：

通道 1（``candidate_poses`` 非空时，优先）:

类型: Service daystar_navigation_msgs/srv/AutoSelectInitialPose
名称: /nav/autoselect_initialpose
参数映射:

封装参数	原生字段（Request）
`candidate_poses`	逐个包装为 `geometry_msgs/PoseWithCovarianceStamped`（`frame_id="map"`）填入 `request.init_poses`；`max_distance` / `max_yaw` / `fiducial_init` 等其余字段留默认
`location_name` / `pose`	此分支下被忽略，不下发
`block` / `timeout`	客户端行为，不下发（等待服务响应的时长）

通道 2（``location_name`` 非空 → 本地解析位姿；否则用 ``pose``）:

类型: Service daystar_navigation_msgs/srv/SetLocalization
名称: /nav/set_localization
参数映射:

封装参数	原生字段（Request）
`pose`（或由 `location_name` 解析出的位姿）	`request.init_pose.pose.pose`（`header.frame_id="map"`）；`max_distance` / `max_yaw` / `fiducial_init` 等其余字段留默认
`block` / `timeout`	客户端行为，不下发（等待服务响应的时长）
`auto_relocation`	已弃用的兼容参数，不下发；传 `True` 时在绑定层直接返回失败，两条通道都不会走到

获取定位状态

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

底层 ROS2 接口

类型: Topic 订阅缓存 daystar_navigation_msgs/msg/LocalizationState
名称: /nav/localization_state
说明: 本函数不发 ROS2 请求，读取订阅缓存的 loc_state 字段（0=NORMAL / 1=LACKDATA / 2=LOST）；数据未就绪时最多等待 3 秒，缓存超过 5 秒未更新视为失效、返回 UNKNOWN_LOC。本函数无入参。

获取当前定位模式

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 latched，仅模式切换时发布），读本地缓存，收到过即一直有效；从未收到返回 UNKNOWN_LOC_MODE。

底层 ROS2 接口

类型: Topic 订阅缓存 std_msgs/msg/UInt8
名称: /nav/current_localization_mode
说明: 本函数不发 ROS2 请求，读取订阅缓存（发布端 transient_local，晚订阅可补收最新值）；0=MAP / 1=SLAM / 2=REFLECTOR，缓存超过 5 秒未更新或从未收到时返回 UNKNOWN_LOC_MODE。本函数无入参。

定位与位姿

定位操作

设置定位

获取定位状态

获取当前定位模式

相关数据类型