Navigation

The navigation module provides high-level movement primitives for the 3we platform. Commands work identically in simulation and on hardware, with the planner automatically handling obstacle avoidance, path smoothing, and mecanum wheel kinematics.

Motion Primitives

move_to(x, y, theta=None)

Navigate to a goal pose in the world frame. The planner computes a collision-free path using the latest LiDAR data and executes it.

await robot.move_to(x=2.0, y=1.5, theta=0.0)

Parameters:

Parameter	Type	Default	Description
x	float	required	Goal X position in meters.
y	float	required	Goal Y position in meters.
theta	float	None	Goal heading in radians. If None, final heading is unconstrained.
timeout	float	30.0	Maximum seconds to attempt reaching the goal.
tolerance	float	0.05	Position tolerance in meters.

Returns: MoveResult with fields success, final_pose, duration, distance_traveled.

move_forward(distance)

Drive straight ahead relative to the current heading.

await robot.move_forward(0.5)  # move 50 cm forward

rotate(angle)

Rotate in place by the given angle.

import math
await robot.rotate(math.pi / 2)  # turn 90 degrees counter-clockwise

follow_path(waypoints)

Execute a sequence of waypoints without stopping between them.

path = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
result = await robot.follow_path(path)

Parameters:

Parameter	Type	Default	Description
waypoints	list[tuple]	required	List of (x, y) or (x, y, theta) tuples.
speed	float	0.3	Target speed along the path (m/s).
lookahead	float	0.2	Pure-pursuit lookahead distance (meters).

explore(duration=60.0)

Autonomously explore the environment using a frontier-based strategy. Useful for map building.

await robot.explore(duration=120.0)  # explore for 2 minutes
occupancy_grid = robot.get_map()

Parameters:

Parameter	Type	Default	Description
duration	float	60.0	Maximum exploration time in seconds.
strategy	str	"frontier"	Exploration algorithm: "frontier", "random", "coverage".

Velocity Control

For direct velocity control (e.g., teleoperation or custom controllers):

# Holonomic velocity: vx (forward), vy (strafe), omega (rotation)
robot.set_velocity(vx=0.2, vy=0.0, omega=0.1)

Call robot.stop() to halt all motion immediately.

Obstacle Avoidance

Obstacle avoidance is enabled by default for all high-level commands. Configure it with:

robot.navigation.set_avoidance(
    enabled=True,
    safety_margin=0.15,  # meters from obstacle
    reactive=True         # enable reactive dodge maneuvers
)

Map Access

After exploration or during navigation, access the built map:

grid = robot.get_map()
# OccupancyGrid with .data (numpy array), .resolution, .origin