Skip to content

Commit 1bacecb

Browse files
g-ampog-ampo
committed
CameraZoomPlugin: support all camera-family sensor components
Extends PreUpdate's component lookup to accept BoundingBoxCamera, DepthCamera, RgbdCamera, SegmentationCamera, ThermalCamera and WideAngleCamera in addition to plain Camera. Fixes #129 and removes the same latent bug for every other camera variant.
1 parent 082a0fe commit 1bacecb

2 files changed

Lines changed: 189 additions & 5 deletions

File tree

‎src/CameraZoomPlugin.cc‎

Lines changed: 79 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,13 @@
3939
#include <gz/rendering/RenderingIface.hh>
4040
#include <gz/rendering/Scene.hh>
4141

42+
#include <gz/sim/components/BoundingBoxCamera.hh>
4243
#include <gz/sim/components/Camera.hh>
44+
#include <gz/sim/components/DepthCamera.hh>
45+
#include <gz/sim/components/RgbdCamera.hh>
46+
#include <gz/sim/components/SegmentationCamera.hh>
47+
#include <gz/sim/components/ThermalCamera.hh>
48+
#include <gz/sim/components/WideAngleCamera.hh>
4349
#include <gz/sim/components/Model.hh>
4450
#include <gz/sim/components/Name.hh>
4551
#include <gz/sim/components/ParentEntity.hh>
@@ -104,6 +110,28 @@ class CameraZoomPlugin::Impl
104110
return std::optional<sim::Entity>(parent->Data());
105111
}
106112

113+
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
114+
/// \brief Get the sdf::Sensor stored in any camera-style component
115+
/// (Camera, BoundingBoxCamera, DepthCamera, RgbdCamera,
116+
/// SegmentationCamera, ThermalCamera or WideAngleCamera) attached to
117+
/// the given sensor entity.
118+
/// \note LogicalCamera is intentionally excluded: its sdf::Sensor does
119+
/// not carry an sdf::Camera, so zoom semantics do not apply.
120+
/// \note By convention, gz-sim's SdfEntityCreator attaches exactly one
121+
/// camera-family component per sensor entity (matched to the SDF
122+
/// `<sensor type="...">`); the lookup order below is therefore
123+
/// significant only if that invariant is ever broken.
124+
/// \param[in] _ecm Entity component manager.
125+
/// \param[in] _entity Sensor entity to query.
126+
/// \param[out] _typeId Always written. Set to the matched component
127+
/// type id on success, or to kComponentTypeIdInvalid on failure.
128+
/// \return Pointer to sdf::Sensor held by the matched component, or
129+
/// nullptr if no supported camera component is attached.
130+
public: sdf::Sensor *CameraSensorSdf(
131+
EntityComponentManager &_ecm,
132+
Entity _entity,
133+
ComponentTypeId &_typeId) const;
134+
107135
/// \brief World occupied by the parent model.
108136
public: World world{kNullEntity};
109137

@@ -239,6 +267,51 @@ void CameraZoomPlugin::Impl::InitialiseCamera()
239267
}
240268
}
241269

270+
//////////////////////////////////////////////////
271+
sdf::Sensor *CameraZoomPlugin::Impl::CameraSensorSdf(
272+
EntityComponentManager &_ecm,
273+
Entity _entity,
274+
ComponentTypeId &_typeId) const
275+
{
276+
_typeId = kComponentTypeIdInvalid;
277+
if (auto *c = _ecm.Component<components::Camera>(_entity))
278+
{
279+
_typeId = components::Camera::typeId;
280+
return &c->Data();
281+
}
282+
if (auto *c = _ecm.Component<components::BoundingBoxCamera>(_entity))
283+
{
284+
_typeId = components::BoundingBoxCamera::typeId;
285+
return &c->Data();
286+
}
287+
if (auto *c = _ecm.Component<components::DepthCamera>(_entity))
288+
{
289+
_typeId = components::DepthCamera::typeId;
290+
return &c->Data();
291+
}
292+
if (auto *c = _ecm.Component<components::RgbdCamera>(_entity))
293+
{
294+
_typeId = components::RgbdCamera::typeId;
295+
return &c->Data();
296+
}
297+
if (auto *c = _ecm.Component<components::SegmentationCamera>(_entity))
298+
{
299+
_typeId = components::SegmentationCamera::typeId;
300+
return &c->Data();
301+
}
302+
if (auto *c = _ecm.Component<components::ThermalCamera>(_entity))
303+
{
304+
_typeId = components::ThermalCamera::typeId;
305+
return &c->Data();
306+
}
307+
if (auto *c = _ecm.Component<components::WideAngleCamera>(_entity))
308+
{
309+
_typeId = components::WideAngleCamera::typeId;
310+
return &c->Data();
311+
}
312+
return nullptr;
313+
}
314+
242315
//////////////////////////////////////////////////
243316
void CameraZoomPlugin::Impl::OnRenderTeardown()
244317
{
@@ -409,8 +482,10 @@ void CameraZoomPlugin::PreUpdate(
409482
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
410483
// Entity cameraEntity = this->impl->cameraSensor.Entity();
411484
Entity cameraEntity = this->impl->cameraSensorEntity;
412-
auto comp = _ecm.Component<components::Camera>(cameraEntity);
413-
if (!comp)
485+
ComponentTypeId sensorTypeId = kComponentTypeIdInvalid;
486+
sdf::Sensor *sensorSdf =
487+
this->impl->CameraSensorSdf(_ecm, cameraEntity, sensorTypeId);
488+
if (!sensorSdf)
414489
return;
415490

416491
if (this->impl->zoomChanged)
@@ -430,8 +505,7 @@ void CameraZoomPlugin::PreUpdate(
430505
}
431506

432507
// Update component.
433-
sdf::Sensor &sensor = comp->Data();
434-
sdf::Camera *cameraSdf = sensor.CameraSensor();
508+
sdf::Camera *cameraSdf = sensorSdf->CameraSensor();
435509
if (!cameraSdf)
436510
return;
437511

@@ -478,7 +552,7 @@ void CameraZoomPlugin::PreUpdate(
478552
sensorWidth, newFocalLength);
479553
// Update rendering camera with the latest focal length.
480554
cameraSdf->SetHorizontalFov(newHfov);
481-
_ecm.SetChanged(cameraEntity, components::Camera::typeId,
555+
_ecm.SetChanged(cameraEntity, sensorTypeId,
482556
ComponentState::OneTimeChange);
483557

484558
// Update rendering camera.

‎tests/worlds/test_camera_zoom_boundingbox.sdf‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.9">
3+
<world name="test_camera_zoom_boundingbox">
4+
<physics name="1ms" type="ignored">
5+
<max_step_size>0.001</max_step_size>
6+
<real_time_factor>1.0</real_time_factor>
7+
</physics>
8+
9+
<plugin name="gz::sim::systems::Physics"
10+
filename="gz-sim-physics-system">
11+
</plugin>
12+
<plugin name="gz::sim::systems::Sensors"
13+
filename="gz-sim-sensors-system">
14+
<render_engine>ogre2</render_engine>
15+
</plugin>
16+
<plugin name="gz::sim::systems::UserCommands"
17+
filename="gz-sim-user-commands-system">
18+
</plugin>
19+
<plugin name="gz::sim::systems::SceneBroadcaster"
20+
filename="gz-sim-scene-broadcaster-system">
21+
</plugin>
22+
23+
<scene>
24+
<ambient>1.0 1.0 1.0</ambient>
25+
<background>0.8 0.8 0.8</background>
26+
</scene>
27+
28+
<light type="directional" name="sun">
29+
<cast_shadows>true</cast_shadows>
30+
<pose>0 0 10 0 0 0</pose>
31+
<diffuse>0.8 0.8 0.8 1</diffuse>
32+
<specular>0.5 0.5 0.5 1</specular>
33+
<direction>-0.5 0.1 -0.9</direction>
34+
</light>
35+
36+
<model name="ground_plane">
37+
<static>true</static>
38+
<link name="link">
39+
<visual name="visual">
40+
<geometry>
41+
<plane>
42+
<normal>0 0 1</normal>
43+
<size>50 50</size>
44+
</plane>
45+
</geometry>
46+
<material>
47+
<ambient>0.7 0.7 0.7 1</ambient>
48+
<diffuse>0.7 0.7 0.7 1</diffuse>
49+
</material>
50+
</visual>
51+
</link>
52+
</model>
53+
54+
<!-- Target box for the bounding-box camera to detect. -->
55+
<model name="target">
56+
<static>true</static>
57+
<pose>5 0 0.5 0 0 0</pose>
58+
<link name="link">
59+
<visual name="visual">
60+
<geometry>
61+
<box><size>1 1 1</size></box>
62+
</geometry>
63+
<material>
64+
<ambient>1 0.2 0.2 1</ambient>
65+
<diffuse>1 0.2 0.2 1</diffuse>
66+
</material>
67+
</visual>
68+
</link>
69+
</model>
70+
71+
<!-- Sensor mount carrying a boundingbox_camera with CameraZoomPlugin.
72+
Regression fixture for ArduPilot/ardupilot_gazebo issue #129:
73+
CameraZoomPlugin must drive HFOV for non-Camera sensor components
74+
(BoundingBoxCamera here, plus DepthCamera, RgbdCamera,
75+
SegmentationCamera, ThermalCamera, WideAngleCamera).
76+
To exercise: launch this world and publish on the zoom topic, e.g.
77+
gz topic -t /model/cam_mount/sensor/camera/zoom/cmd_zoom \
78+
-m gz.msgs.Double -p 'data: 4.0'
79+
Before the fix, HFOV stays at 2.0 rad. After the fix it converges to
80+
refHfov / zoom = 0.5 rad. -->
81+
<model name="cam_mount">
82+
<static>true</static>
83+
<pose>0 0 0.5 0 0 0</pose>
84+
<link name="link">
85+
<sensor name="camera" type="boundingbox_camera">
86+
<topic>boxes</topic>
87+
<pose>0 0 0 0 0 0</pose>
88+
<camera>
89+
<box_type>2d</box_type>
90+
<horizontal_fov>2.0</horizontal_fov>
91+
<image>
92+
<width>320</width>
93+
<height>240</height>
94+
</image>
95+
<clip>
96+
<near>0.1</near>
97+
<far>100</far>
98+
</clip>
99+
</camera>
100+
<always_on>1</always_on>
101+
<update_rate>10</update_rate>
102+
103+
<plugin filename="CameraZoomPlugin" name="CameraZoomPlugin">
104+
<max_zoom>125.0</max_zoom>
105+
</plugin>
106+
</sensor>
107+
</link>
108+
</model>
109+
</world>
110+
</sdf>

0 commit comments

Comments
 (0)