Position Based Servoing

[alexlin2]

Position Based Servoing for bridging Navigation to Manipulation

Background

The nav stack (localPlanner → pathFollower) handles long-range navigation well but is too coarse for approaching an object for manipulation or servoing to a charging station.

This spec covers a new target_servoing module.

Architecture

                         ┌──────────────────────────────────┐
                         │           target_servoing         │
                         │   (target-driven controller)      │
                         │                                   │
   target   ───────────> │  State Machine:                   │
   (Detection3D,         │    IDLE → NAV_APPROACH → SERVO    │
    PoseStamped)         │                                   │
                         │  NAV_APPROACH:                    │──> /goal_pose ──> localPlanner
   /state_estimation ──> │    position filtering             │       ──> pathFollower ──> /cmd_vel
                         │    waypoint computation           │
   /terrain_map ───────> │                                   │
                         │  SERVO:                           │──> /cmd_vel (direct)
                         │    PD controller                  │
                         │    approach then orient           │
                         │                                   │──> /visual_servoing/status
                         └──────────────────────────────────┘


                         ┌──────────────────────────────────┐
                         │    local_movement                 │
                         │   (safe short-range controller)   │
                         │                                   │
   /local_movement ────> │  Collision checking (terrain)     │──> /joy (via pathFollower)
   (Pose2D, body frame)  │  Parameterized thresholds         │
                         │  Timeout / stuck detection        │
   /state_estimation ──> │  Status feedback                  │──> /local_movement/status
   /terrain_map ───────> │  PD control (optional)            │──> /way_point (on completion)
                         │                                   │
                         └──────────────────────────────────┘

The two nodes are independent. target_servoing handles perception-driven object approach. local_movement handles safe body-frame relative moves (docking, pre-manipulation positioning). They don't call each other.

target_servoing

Subscribes to target pose, robot odom, terrain map, and joystick. Publishes /goal_pose when far, /cmd_vel directly when close.

States

Code	State	Behavior
0	IDLE	No target
1	NAV_APPROACH	Publishing /goal_pose to nav stack
2	SERVO	Publishing /cmd_vel directly, PD control
3	REACHED	At standoff distance, holding
4	LOST	Tracking lost, zero velocity
5	OBSTACLE	Forward obstacle detected, paused

Transitions: IDLE → NAV_APPROACH on detection. NAV_APPROACH → SERVO when distance < approach_switch_distance. SERVO → REACHED when at goal_distance within tolerance. Any → LOST on tracking timeout. Any → IDLE on joystick override. Hysteresis band between NAV_APPROACH and SERVO prevents oscillation.

Servo Controller

Two phases in body frame:

• Approach: PD control towards target minus standoff distance. Yaw tracks heading-to-target. Linear slowdown ramp near goal.
• Hold: Zero linear, yaw correction to face target.

Position filtering: moving average with outlier rejection. Obstacle check: simple forward-cone point count from terrain map.

DIM-658

[mustafab0]

Shouldn't the local_movement (short_range_controller) also use perception driven approach for final servoing?
For example self charging alignment will be based on Aruco marker relative position.

Also I believe the short_range_controller should not worry or use the /terrain_map for servoing. The final servoing ideally should be <1m and in situations like these we should be able to navigate using simple obstacle avoidance and very low velocity.

[alexlin2]

Shouldn't the local_movement (short_range_controller) also use perception driven approach for final servoing? For example self charging alignment will be based on Aruco marker relative position.

Also I believe the short_range_controller should not worry or use the /terrain_map for servoing. The final servoing ideally should be <1m and in situations like these we should be able to navigate using simple obstacle avoidance and very low velocity.

Yes, that will be the job of the perception system to generate that pose, this is position based servoing not visual servoing.