[OdometryPublisher] Crash if initialized with zero gaussian noise standard deviation

[oysstu]

Environment

• OS Version: Archlinux
• Source or binary build? Source, tag 8.6.0

Description

Similar to the issue reported in gz-sensors and solved in this PR, the following calls to DblNormal craps out if gaussianNoise is set to zero.

gz-sim/src/systems/odometry_publisher/OdometryPublisher.cc

Lines 392 to 453 in 5f03199

	msg.mutable_twist()->mutable_linear()->set_x(
	std::get<0>(this->linearMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_linear()->set_y(
	std::get<1>(this->linearMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_linear()->set_z(
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_angular()->set_x(
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_angular()->set_y(
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	}
	// Get velocities and roll/pitch rates assuming 3D
	else if (this->dimensions == 3)
	{
	double currentRoll = pose.Rot().Roll();
	const double lastRoll = this->lastUpdatePose.Rot().Roll();
	while (currentRoll < lastRoll - GZ_PI) currentRoll += 2 * GZ_PI;
	while (currentRoll > lastRoll + GZ_PI) currentRoll -= 2 * GZ_PI;
	const float rollDiff = currentRoll - lastRoll;
	double currentPitch = pose.Rot().Pitch();
	const double lastPitch = this->lastUpdatePose.Rot().Pitch();
	while (currentPitch < lastPitch - GZ_PI) currentPitch += 2 * GZ_PI;
	while (currentPitch > lastPitch + GZ_PI) currentPitch -= 2 * GZ_PI;
	const float pitchDiff = currentPitch - lastPitch;
	double linearDisplacementZ =
	pose.Pos().Z() - this->lastUpdatePose.Pos().Z();
	math::Vector3 linearDisplacement(linearDisplacementX, linearDisplacementY,
	linearDisplacementZ);
	math::Vector3 linearVelocity =
	pose.Rot().RotateVectorReverse(linearDisplacement) / dt.count();
	std::get<0>(this->linearMean).Push(linearVelocity.X());
	std::get<1>(this->linearMean).Push(linearVelocity.Y());
	std::get<2>(this->linearMean).Push(linearVelocity.Z());
	std::get<0>(this->angularMean).Push(rollDiff / dt.count());
	std::get<1>(this->angularMean).Push(pitchDiff / dt.count());
	msg.mutable_twist()->mutable_linear()->set_x(
	std::get<0>(this->linearMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_linear()->set_y(
	std::get<1>(this->linearMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_linear()->set_z(
	std::get<2>(this->linearMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_angular()->set_x(
	std::get<0>(this->angularMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	msg.mutable_twist()->mutable_angular()->set_y(
	std::get<1>(this->angularMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));
	}
	// Set yaw rate
	std::get<2>(this->angularMean).Push(yawDiff / dt.count());
	msg.mutable_twist()->mutable_angular()->set_z(
	std::get<2>(this->angularMean).Mean() +
	gz::math::Rand::DblNormal(0, this->gaussianNoise));

This occurred when trying to launch harmonic_demo, which leaves that parameter default-initialized.

Steps to reproduce

Launch harmonic_demo (main branch) and start simulation.

Output

/usr/include/c++/14.1.1/bits/random.h:2138: std::normal_distribution<_RealType>::param_type::param_type(_RealType, _RealType) [with _RealType = double]: Assertion '_M_stddev > _RealType(0)' failed.
Stack trace (most recent call last) in thread 860836:
#10   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#9    Object "/usr/lib/libc.so.6", at 0x750b5ca3949b, in 
#8    Object "/usr/lib/libc.so.6", at 0x750b5c9b439c, in 
#7    Object "/usr/lib/libstdc++.so.6", at 0x750b58ae1cf3, in 
#6    Object "/usr/lib/libgz-sim8.so.8", at 0x750b57c3d59a, in 
#5    Object "/usr/lib/gz-sim-8/plugins/libgz-sim-odometry-publisher-system.so", at 0x750b0e742a42, in gz::sim::v8::systems::OdometryPublisherPrivate::UpdateOdometry(gz::sim::v8::UpdateInfo const&, gz::sim::v8::EntityComponentManager const&)
#4    Object "/usr/lib/libgz-math7.so.7", at 0x750b58cd929f, in gz::math::v7::Rand::DblNormal(double, double)
#3    Object "/usr/lib/libstdc++.so.6", at 0x750b58ad3baf, in std::__glibcxx_assert_fail(char const*, int, char const*, char const*)
#2    Object "/usr/lib/libc.so.6", at 0x750b5c9444c2, in abort
#1    Object "/usr/lib/libc.so.6", at 0x750b5c95d11f, in gsignal
#0    Object "/usr/lib/libc.so.6", at 0x750b5c9b63f4, in 
Aborted (Signal sent by tkill() 860305 1000)

[iche033]

added a check for 0 stddev upstream in gz-math: gazebosim/gz-math#615