Skip to content

Commit 8301678

Browse files
hidmichidmic
authored
Merge 5643130 into ea7d33a 
2 parents ea7d33a + 5643130 commit 8301678

File tree

8 files changed

+2990
-0
lines changed

8 files changed

+2990
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,10 @@ set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR})
9292
gz_find_package(sdformat13 REQUIRED)
9393
set(SDF_VER ${sdformat13_VERSION_MAJOR})
9494

95+
#--------------------------------------
96+
# Find Eigen3
97+
find_package(Eigen3 REQUIRED)
98+
9599
#============================================================================
96100
# Configure the build
97101
#============================================================================

‎include/gz/sensors/DopplerVelocityLog.hh‎

Lines changed: 247 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,247 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef GZ_SENSORS_DOPPLERVELOCITYLOG_HH_
19+
#define GZ_SENSORS_DOPPLERVELOCITYLOG_HH_
20+
21+
#include <chrono>
22+
#include <memory>
23+
#include <unordered_map>
24+
#include <vector>
25+
26+
#include <gz/math/Pose3.hh>
27+
#include <gz/math/Vector3.hh>
28+
#include <gz/sensors/EnvironmentalData.hh>
29+
#include <gz/sensors/RenderingSensor.hh>
30+
31+
#include "gz/sensors/dvl/Export.hh"
32+
33+
namespace gz
34+
{
35+
namespace sensors
36+
{
37+
/// \brief Kinematic state for an entity in the world.
38+
///
39+
/// All quantities are defined w.r.t. the world frame.
40+
struct EntityKinematicState
41+
{
42+
gz::math::Pose3d pose;
43+
gz::math::Vector3d linearVelocity;
44+
gz::math::Vector3d angularVelocity;
45+
};
46+
47+
/// \brief Kinematic state for all entities in the world.
48+
using WorldKinematicState = std::unordered_map<
49+
uint64_t, EntityKinematicState>;
50+
51+
/// \brief Kinematic state for all entities in the world.
52+
struct WorldState
53+
{
54+
WorldKinematicState kinematics;
55+
gz::math::SphericalCoordinates origin;
56+
};
57+
58+
/// \brief Doppler velocity log (DVL) sensor, built as a custom
59+
/// rendering sensor to leverage GPU shaders for speed.
60+
///
61+
/// By default, this sensor uses an x-forward, y-left, z-up
62+
/// coordinate frame to report all measurements. All acoustic
63+
/// beams' lobes are expected to face downwards.
64+
///
65+
/// \verbatim
66+
/// <sensor type="custom" gz:type="dvl">
67+
/// <gz:dvl>
68+
/// <arrangement degrees="true">
69+
/// <beam id="1">
70+
/// <aperture></aperture>
71+
/// <rotation></rotation>
72+
/// <tilt></tilt>
73+
/// </beam>
74+
/// </arrangement>
75+
/// <tracking>
76+
/// <bottom_mode>
77+
/// <when></when>
78+
/// <noise type="gaussian">
79+
/// <stddev></stddev>
80+
/// </noise>
81+
/// <visualize></visualize>
82+
/// </bottom_mode>
83+
/// <water_mass_mode>
84+
/// <when></when>
85+
/// <water_velocity>
86+
/// <x></x>
87+
/// <y></y>
88+
/// <z></z>
89+
/// </water_velocity>
90+
/// <boundaries>
91+
/// <near>20.</near>
92+
/// <far>60.</far>
93+
/// </boundaries>
94+
/// <bins>10</bins>
95+
/// <noise type="gaussian">
96+
/// <stddev></stddev>
97+
/// </noise>
98+
/// <visualize></visualize>
99+
/// </water_mass_mode>
100+
/// <minimum_range></minimum_range>
101+
/// <maximum_range></maximum_range>
102+
/// <resolution></resolution>
103+
/// <reference_frame></reference_frame>
104+
/// </gz:dvl>
105+
/// </sensor>
106+
/// \endverbatim
107+
///
108+
/// - `<arrangement>` describes the arrangement of acoustic beams
109+
/// in the DVL sensor frame. It may include a `degrees` attribute
110+
/// to signal use of degrees instead of radians for all angles
111+
/// within, defaulting to radians if left unspecified.
112+
/// - `<arrangement><beam>` describes one acoustic beam in the
113+
/// arrangement. May include an `id` attribute, defaulting to the
114+
/// last specified id plus 1 if left unspecified (or 0 if it's the
115+
/// first).
116+
/// - `<arrangement><beam><aperture>` sets the aperture angle for
117+
/// the acoustic beam's main lobe (modelled as a cone). Defaults
118+
/// to 90 degrees if left unspecified.
119+
/// - `<arrangement><beam><rotation>` sets the rotation angle of
120+
/// the acoustic beam's symmetry axis about the sensor frame z
121+
/// axis. Defaults to 0 degrees if left unspecified.
122+
/// - `<arrangement><beam><tilt>` sets the inclination angle of
123+
/// the acoustic beam's symmetry axis w.r.t. the sensor frame -z
124+
/// axis (ie. rotation about the -y axis). Defaults to 0 degrees
125+
/// if left unspecified.
126+
/// - `<arrangement><visualize>` enables visual aids to evaluate
127+
/// acoustic beam arrangements. Beam lobes' are rendered and adjusted
128+
/// in size to match range measurements.
129+
/// - `<tracking>` configures velocity tracking modes for the DVL.
130+
/// - `<tracking><bottom_mode>` configures the bottom tracking mode.
131+
/// - `<tracking><bottom_mode><when>` enables (or disables) the bottom
132+
/// tracking mode. Supported values are 'never', to disable it completely
133+
/// (as if no <bottom_mode> configuration had been specified), 'always' to
134+
/// enable it at all times, and 'best' to track at all times but only
135+
/// publish estimates when it performs best among all configured modes.
136+
/// Defaults to 'always' if left unspecified.
137+
/// - `<tracking><bottom_mode><noise>` sets the noise model for velocity
138+
/// estimates. Only 'gaussian' noise is currently supported. Defaults to
139+
/// none if left unspecified.
140+
/// - `<tracking><bottom_mode><visualize>` enables visual aids to validate
141+
/// bottom tracking. Acoustic beam reflection paths are depicted, where
142+
/// the color scales linearly in hue with measured speed and low opacity
143+
/// sections depict range uncertainty (+/- 2 standard deviations).
144+
/// - `<tracking><water_mass_mode>` configures the water-mass tracking
145+
/// mode.
146+
/// - `<tracking><water_mass_mode><when>` enables (or disables) the
147+
/// water-mass tracking mode. Supported values are 'never', to disable it
148+
/// completely (as if no <water_mass_mode> configuration had been
149+
/// specified), 'always' to enable it at all times, and 'best' to track at
150+
/// all times but only publish estimates when it performs best among all
151+
/// configured modes. Defaults to 'always' if left unspecified.
152+
/// - `<tracking><water_mass_mode><water_velocity>` set the variables in
153+
/// world environmental data to be used to sample water velocity w.r.t.
154+
/// the world frame in each axis. At least one axis must be specified.
155+
/// - `<tracking><water_mass_mode><water_velocity><x>` set the variable
156+
/// in world environmental data to be used to sample water velocity w.r.t.
157+
/// the world frame along the x-axis (that is, towards east). Defaults to
158+
/// none (and thus zero water velocity in this axis) if left unspecified.
159+
/// - `<tracking><water_mass_mode><water_velocity><y>` set the variable in
160+
/// world environmental data to be used to sample water velocity w.r.t. the
161+
/// world frame along the y-axis (that is, towards north). Defaults to none
162+
/// (and thus zero water velocity in this axis) if left unspecified.
163+
/// - `<tracking><water_mass_mode><water_velocity><z>` set the variable in
164+
/// world environmental data to be used to sample water velocity w.r.t. the
165+
/// world frame along the z-axis (that is, upwards). Defaults to none (and
166+
/// thus zero water velocity in this axis) if left unspecified.
167+
/// - `<tracking><water_mass_mode><boundaries>` sets water-mass layer
168+
/// boundaries. These boundaries are planar at given z-offsets in the
169+
/// sensor frame.
170+
/// - `<tracking><water_mass_mode><boundaries><near>` sets the water-mass
171+
/// layer boundary that is the closest to the sensor.
172+
/// - `<tracking><water_mass_mode><boundaries><far>` sets the water-mass
173+
/// layer boundary that is the farthest to the sensor.
174+
/// - `<tracking><water_mass_mode><bins>` sets the number of bins to use
175+
/// for water-mass velocity sampling. Each bin is a slab of water between
176+
/// boundaries.
177+
/// - `<tracking><water_mass_mode><noise>` sets the noise model for
178+
/// velocity estimates. Only 'gaussian' noise is currently supported.
179+
/// Defaults to none if left unspecified.
180+
/// - `<tracking><water_mass_mode><visualize>` enables visual aids to
181+
/// validate bottom tracking. Acoustic beam reflection paths are depicted,
182+
/// where the color scales linearly in hue with measured speed and low
183+
/// opacity sections depict range uncertainty (+/- 2 standard deviations).
184+
/// - `<type>` sets the sensor type, either 'piston' or 'phased_array'.
185+
/// Defaults to unspecified.
186+
/// - `<resolution>` sets the resolution of the beam for bottom
187+
/// tracking at a 1 m distance. Defaults to 1 cm if left unspecified.
188+
/// - `<minimum_range>` sets a lower bound for range measurements.
189+
/// Defaults to 1 cm if left unspecified.
190+
/// - `<maximum_range>` sets an upper bound for range measurements.
191+
/// Defaults to 100 m if left unspecified.
192+
/// - `<reference_frame>` sets a transform from the sensor frame to the
193+
/// reference frame in which all measurements are reported. Defaults to
194+
/// the identity transform.
195+
///
196+
/// Note the tethys::DopplerVelocityLogSystem plugin must be
197+
/// loaded for these custom sensors to be picked up and setup.
198+
class GZ_SENSORS_DVL_VISIBLE DopplerVelocityLog
199+
: public gz::sensors::RenderingSensor
200+
{
201+
public: DopplerVelocityLog();
202+
203+
public: ~DopplerVelocityLog();
204+
205+
/// Inherits documentation from parent class
206+
public: virtual bool Load(const sdf::Sensor &_sdf) override;
207+
208+
/// Inherits documentation from parent class
209+
public: virtual bool Load(sdf::ElementPtr _sdf) override;
210+
211+
/// Inherits documentation from parent class
212+
public: virtual bool Update(
213+
const std::chrono::steady_clock::duration &_now) override;
214+
215+
/// Perform any sensor updates after the rendering pass
216+
public: virtual void PostUpdate(
217+
const std::chrono::steady_clock::duration &_now);
218+
219+
/// Inherits documentation from parent class
220+
public: void SetScene(gz::rendering::ScenePtr _scene) override;
221+
222+
/// \brief Set this sensor's entity ID (for world state lookup).
223+
public: void SetEntity(uint64_t entity);
224+
225+
/// \brief Set world `_state` to support DVL water and bottom-tracking.
226+
public: void SetWorldState(const WorldState &_state);
227+
228+
/// \brief Set environmental `_data` to support DVL water-tracking.
229+
public: void SetEnvironmentalData(const EnvironmentalData &_data);
230+
231+
/// \brief Inherits documentation from parent class
232+
public: virtual bool HasConnections() const override;
233+
234+
/// \brief Yield rendering sensors that underpin the implementation.
235+
///
236+
/// \internal
237+
public: std::vector<gz::rendering::SensorPtr> RenderingSensors() const;
238+
239+
private: class Implementation;
240+
241+
private: std::unique_ptr<Implementation> dataPtr;
242+
};
243+
244+
} // namespace sensors
245+
} // namespace gz
246+
247+
#endif // GZ_SENSORS_DOPPLERVELOCITYLOG_HH_

‎include/gz/sensors/EnvironmentalData.hh‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
#ifndef GZ_SENSORS_ENVIRONMENTAL_DATA_HH_
18+
#define GZ_SENSORS_ENVIRONMENTAL_DATA_HH_
19+
20+
#include <memory>
21+
#include <string>
22+
23+
#include <gz/common/DataFrame.hh>
24+
#include <gz/math/SphericalCoordinates.hh>
25+
#include <gz/math/TimeVaryingVolumetricGrid.hh>
26+
27+
#include <gz/sensors/config.hh>
28+
#include <gz/sensors/Export.hh>
29+
30+
namespace gz
31+
{
32+
namespace sensors
33+
{
34+
// Inline bracket to help doxygen filtering.
35+
inline namespace GZ_SENSORS_VERSION_NAMESPACE
36+
{
37+
/// \brief Environment data across time and space. This is useful to
38+
/// introduce physical quantities that may be of interest even if not
39+
/// modelled in simulation.
40+
struct GZ_SENSORS_VISIBLE EnvironmentalData
41+
{
42+
using T = math::InMemoryTimeVaryingVolumetricGrid<double>;
43+
using FrameT = common::DataFrame<std::string, T>;
44+
using ReferenceT = math::SphericalCoordinates::CoordinateType;
45+
46+
/// \brief Reference units
47+
enum class ReferenceUnits {
48+
RADIANS = 0,
49+
DEGREES
50+
};
51+
52+
/// \brief Instantiate environmental data.
53+
///
54+
/// An std::make_shared equivalent that ensures
55+
/// dynamically loaded call sites use a template
56+
/// instantiation that is guaranteed to outlive
57+
/// them.
58+
static std::shared_ptr<EnvironmentalData>
59+
MakeShared(FrameT _frame, ReferenceT _reference,
60+
ReferenceUnits _units = ReferenceUnits::RADIANS,
61+
bool _ignoreTimeStep = false);
62+
63+
/// \brief Environmental data frame.
64+
FrameT frame;
65+
66+
/// \brief Spatial reference for data coordinates.
67+
ReferenceT reference;
68+
69+
/// \brief The units to be used (only for spherical coordinates)
70+
ReferenceUnits units;
71+
72+
/// \brief Use time axis or not.
73+
bool staticTime;
74+
};
75+
} // namespace GZ_SENSORS_VERSION_NAMESPACE
76+
} // namespace sensors
77+
} // namespace gz
78+
79+
#endif

‎src/CMakeLists.txt‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
set (sources
22
BrownDistortionModel.cc
33
Distortion.cc
4+
EnvironmentalData.cc
45
GaussianNoiseModel.cc
56
Manager.cc
67
Noise.cc
@@ -88,6 +89,22 @@ target_link_libraries(${depth_camera_target}
8889
gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER}
8990
)
9091

92+
set(dvl_sources DopplerVelocityLog.cc)
93+
gz_add_component(dvl
94+
SOURCES ${dvl_sources}
95+
DEPENDS_ON_COMPONENTS rendering
96+
GET_TARGET_NAME dvl_target)
97+
target_compile_definitions(${dvl_target} PUBLIC DopplerVelocityLog_EXPORTS)
98+
target_link_libraries(${dvl_target}
99+
PUBLIC
100+
${rendering_target}
101+
Eigen3::Eigen
102+
PRIVATE
103+
gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
104+
gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER}
105+
gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}
106+
gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER})
107+
91108
set(lidar_sources Lidar.cc)
92109
gz_add_component(lidar
93110
SOURCES ${lidar_sources}

0 commit comments

Comments
 (0)