4.8 구현 예제

[SIM] 4.8.1 카메라 수학

python3 code/camera_geometry.py

예상 출력 예시

pixel: [420.0, 290.0]
reconstructed camera point: [0.2, 0.1, 1.0]
round-trip error: 0.000000
det(R_opt_body): 1.000

[SIM] 4.8.2 ApproximateTime ROS 2 Node

code/ros2_jazzy/sensor_pipeline/는 Python 패키지로 다음 console script를 제공한다.

• synthetic_sensors: 30 Hz Image와 50 Hz JointState를 고정된 jitter/offset과 함께 생성
• approx_sync: Image와 JointState를 근사 동기화하고 timestamp delta를 발행
• static_camera_tf: camera link와 optical frame의 고정 TF 방송

cd code/ros2_jazzy
colcon build --symlink-install --packages-select sensor_pipeline
source install/setup.bash
ros2 run sensor_pipeline synthetic_sensors

다른 터미널에서 동기화 Node를 실행한다.

cd ebook/book/04_sensor_data_pipeline/code/ros2_jazzy
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 run sensor_pipeline approx_sync --ros-args -p slop_s:=0.02

synthetic_sensors는 실물 장비 없이 slop 실험을 재현하기 위한 교육용 입력이다. image_offset_ms와 jitter_ms 파라미터로 두 stream의 timestamp 차이를 바꿀 수 있다.

[SIM] 4.8.3 독립 C++ 보간

g++ -std=c++17 -O2 code/interpolate_joint_state.cpp -o /tmp/interpolate_joint_state
/tmp/interpolate_joint_state

예상 출력 예시

t=0.033 q=[0.165, -0.066]
reversal rejected: timestamps must be strictly increasing