fix: stamp planner outputs

[lblommesteyn]

Problem
Planner publishes TargetPathStamped and SpeedProfileStamped messages with default/zero headers. During smoke testing, target_path echoed with sec: 0, nanosec: 0.

Downstream consumers and debugging tools should be able to trust the planner output timestamps/frame ids.

Changes

• Stamp TargetPathStamped before publishing.
• Stamp SpeedProfileStamped before publishing.
• Use the current lane frame_id for both output frame ids.

Test plan

• colcon build --symlink-install --packages-select ap1_planning --cmake-args -DCMAKE_BUILD_TYPE=Release
• git diff --check

[lblommesteyn]

Verified locally in the software smoke-test stack: /ap1/planning/target_path now publishes with a nonzero header stamp and base_link frame_id.