webots: use real inertial unit for camera imu#1
Merged
Conversation
Reviewer's GuideRefactors WebotsCamera to use real Webots IMU devices (Gyro, Accelerometer, InertialUnit) and align their sampling/timestamping and quaternion conventions with libxr, while adjusting camera sampling to target FPS and updating the README for the new IMU wiring and semantics. Sequence diagram for WebotsCamera capture loop with real IMU and image schedulingsequenceDiagram
participant CaptureThread
participant WebotsCamera
participant Webots_Camera_device as Webots_Camera
participant Webots_Gyro as Webots_Gyro
participant Webots_Accelerometer as Webots_Accelerometer
participant Webots_InertialUnit as Webots_InertialUnit
participant LibXR_Topics as LibXR_Topics
CaptureThread->>WebotsCamera: StartCaptureThread()
loop while running
CaptureThread->>WebotsCamera: ThreadLoopTick()
WebotsCamera->>WebotsCamera: LibXR_Thread_Sleep(time_step_ms_)
WebotsCamera->>WebotsCamera: clock = ReadSimClockSample()
WebotsCamera->>WebotsCamera: InitializeImageScheduleIfNeeded(clock.step)
WebotsCamera->>WebotsCamera: PollSensorSyncProbeCommand()
WebotsCamera->>Webots_InertialUnit: getQuaternion()
Webots_InertialUnit-->>WebotsCamera: raw_xyzw
WebotsCamera->>WebotsCamera: ReadCameraPoseSample(clock.timestamp, pose) bool
alt inertial_unit_available
WebotsCamera->>LibXR_Topics: PublishGimbalRotation(pose)
WebotsCamera->>Webots_Gyro: getValues()
Webots_Gyro-->>WebotsCamera: angular_velocity_xyz
WebotsCamera->>Webots_Accelerometer: getValues()
Webots_Accelerometer-->>WebotsCamera: linear_acceleration_xyz
WebotsCamera->>LibXR_Topics: Publish raw_gyro_topic_
WebotsCamera->>LibXR_Topics: Publish raw_accl_topic_
WebotsCamera->>LibXR_Topics: Publish raw_quat_topic_
WebotsCamera->>WebotsCamera: ShouldPublishImageAtStep(clock.step)?
alt should_publish_image
WebotsCamera->>Webots_Camera_device: getImage()
Webots_Camera_device-->>WebotsCamera: BGRA_image
WebotsCamera->>WebotsCamera: PublishImageFrame(clock)
WebotsCamera->>LibXR_Topics: PublishImageEvent(timestamp, step)
WebotsCamera->>LibXR_Topics: PublishImagePayload(timestamp)
WebotsCamera->>WebotsCamera: AdvanceImageScheduleAfterPublish()
else skip_image
WebotsCamera->>WebotsCamera: AdvanceImageScheduleWithoutPublish()
end
WebotsCamera->>WebotsCamera: ReportRepeatedFailureIfNeeded()
else inertial_unit_missing_or_invalid
WebotsCamera-->>CaptureThread: skip_this_step
end
end
Updated class diagram for WebotsCamera and WebotsCameraNamesclassDiagram
class WebotsCameraNames {
-string device_name_
-string pose_def_name_
-string image_topic_name_
-string imu_topic_name_
+WebotsCameraNames(device_name, pose_def_name, image_topic_name, imu_topic_name)
+DeviceNameOwned() string
+PoseDefNameOwned() string
+ImageTopicNameOwned() string
+ImuTopicNameOwned() string
}
class LibXR_Application {
<<interface>>
}
class CameraBase {
<<template CameraInfoV>>
}
class WebotsCamera {
<<template CameraInfoV>>
-RuntimeParam runtime_
-string gyro_topic_name_
-string accl_topic_name_
-string quat_topic_name_
-string image_event_topic_name_
-LibXR_Topic raw_gyro_topic_
-LibXR_Topic raw_accl_topic_
-LibXR_Topic raw_quat_topic_
-LibXR_Topic image_event_topic_
-LibXR_Topic sensor_sync_cmd_topic_
-webots_Robot* robot_
-webots_Camera* cam_
-webots_Gyro* gyro_
-webots_Accelerometer* accelerometer_
-webots_InertialUnit* inertial_unit_
-ImuSensorNames imu_sensor_names_
-int time_step_ms_
-int base_image_interval_steps_
+WebotsCamera(hw, app, runtime)
+OnStart() void
+OnStop() void
+OnMonitor() void
-InitRobot() void
-InitCamera() void
-InitImuSensors() void
-ConfigureSamplingOnStartup() void
-ReadSimClockSample() SimClockSample
-CaptureOnce(clock) void
-ReadCameraPoseSample(timestamp, pose) bool
-PublishGimbalRotation(pose) void
-PublishImageEvent(timestamp, step) void
-PublishImagePayload(timestamp) void
-PublishImageFrame(clock) void
}
class RuntimeParam {
+string device_name
+int fps
+double exposure
+double gain
+string pose_def_name
+string image_topic_name
+string imu_topic_name
}
class ImuSensorNames {
+string gyro
+string accelerometer
+string inertial_unit
}
class SimClockSample {
+uint64_t step
+LibXR_MicrosecondTimestamp timestamp
}
class LibXR_Topic {
}
class webots_Robot {
}
class webots_Camera {
}
class webots_Gyro {
}
class webots_Accelerometer {
}
class webots_InertialUnit {
}
WebotsCameraNames <|-- WebotsCamera
LibXR_Application <|.. WebotsCamera
CameraBase <|.. WebotsCamera
WebotsCamera "1" o-- "1" RuntimeParam
WebotsCamera "1" o-- "1" ImuSensorNames
WebotsCamera "1" o-- "1" SimClockSample
WebotsCamera "1" --> "1" webots_Robot
WebotsCamera "1" --> "1" webots_Camera
WebotsCamera "1" --> "1" webots_Gyro
WebotsCamera "1" --> "1" webots_Accelerometer
WebotsCamera "1" --> "1" webots_InertialUnit
WebotsCamera "1" --> "*" LibXR_Topic
RuntimeParam "1" --> "1" WebotsCameraNames : values_init
File-Level Changes
Tips and commandsInteracting with Sourcery
Customizing Your ExperienceAccess your dashboard to:
Getting Help
|
![sourcery-ai[bot]](https://avatars.githubusercontent.com/in/48477?s=60&v=4){kind=small}
There was a problem hiding this comment.
Hey - I've left some high level feedback:
- In
ReadCameraPoseSample, returningfalsewheninertial_unit_is null orgetQuaternion()returns null will silently drop pose/IMU publishing for that step; consider emitting a throttled log or error path so that misconfigured or failing inertial units are easier to diagnose at runtime. - The destructor no longer disables the camera or IMU devices, whereas
ConfigureSamplingOnStartupandInitImuSensorscallenableon them; if these controllers are reloaded or reused in the same Webots process, consider reintroducing symmetricdisable()calls or clarifying that lifetime is single-shot to avoid subtle resource or timing issues.
Prompt for AI Agents
Please address the comments from this code review:
## Overall Comments
- In `ReadCameraPoseSample`, returning `false` when `inertial_unit_` is null or `getQuaternion()` returns null will silently drop pose/IMU publishing for that step; consider emitting a throttled log or error path so that misconfigured or failing inertial units are easier to diagnose at runtime.
- The destructor no longer disables the camera or IMU devices, whereas `ConfigureSamplingOnStartup` and `InitImuSensors` call `enable` on them; if these controllers are reloaded or reused in the same Webots process, consider reintroducing symmetric `disable()` calls or clarifying that lifetime is single-shot to avoid subtle resource or timing issues.Help me be more useful! Please click 👍 or 👎 on each comment and I'll use the feedback to improve your reviews.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
摘要
Gyro、Accelerometer、InertialUnit传感器。InertialUnit::getQuaternion()的xyzw输出转换为 libxr 使用的wxyz。basicTimeStep误跑到 1kHz。验证
flow=0.1 / 1.0、latest_imu / raw_probe：PASS。边界
Summary by Sourcery
Switch Webots camera IMU handling to use the simulator’s real inertial unit sensors and align camera sampling with the configured frame rate.
New Features:
Enhancements:
Documentation: