From 41ab49737428632839b261f6a2f4724d1ee82eeb Mon Sep 17 00:00:00 2001
From: Kwon Yeseol <172019512+yeseorizi@users.noreply.github.com>
Date: Thu, 22 Oct 2026 23:27:24 +0900
Subject: [PATCH] fix: reject invalid current requests without partial updates
Assisted-by: OpenAI Codex
Signed-off-by: Kwon Yeseol <172019512+yeseorizi@users.noreply.github.com>
---
gazebo/posim_gz_world_plugins/CMakeLists.txt | 4 ++
gazebo/posim_gz_world_plugins/package.xml | 1 +
.../src/gauss_markov_process.cc | 9 ++-
.../test/test_current_model.cc | 65 +++++++++++++++++++
.../src/OceanCurrentPlugin.cc | 37 +++++++----
5 files changed, 104 insertions(+), 12 deletions(-)
create mode 100644 gazebo/posim_gz_world_plugins/test/test_current_model.cc
diff --git a/gazebo/posim_gz_world_plugins/CMakeLists.txt b/gazebo/posim_gz_world_plugins/CMakeLists.txt
index b2f2628d..9f355575 100644
--- a/gazebo/posim_gz_world_plugins/CMakeLists.txt
+++ b/gazebo/posim_gz_world_plugins/CMakeLists.txt
@@ -158,6 +158,10 @@ ament_environment_hooks(
# Testing setup
if(BUILD_TESTING)
+ find_package(ament_cmake_gtest REQUIRED)
+ ament_add_gtest(test_current_model test/test_current_model.cc)
+ target_include_directories(test_current_model PRIVATE include)
+ target_link_libraries(test_current_model gauss_markov_process)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
diff --git a/gazebo/posim_gz_world_plugins/package.xml b/gazebo/posim_gz_world_plugins/package.xml
index f8f4ffe1..ab680e5d 100644
--- a/gazebo/posim_gz_world_plugins/package.xml
+++ b/gazebo/posim_gz_world_plugins/package.xml
@@ -19,6 +19,7 @@
posim_interfaces
sensor_msgs
geometry_msgs
+ ament_cmake_gtest
ament_lint_auto
ament_lint_common
diff --git a/gazebo/posim_gz_world_plugins/src/gauss_markov_process.cc b/gazebo/posim_gz_world_plugins/src/gauss_markov_process.cc
index e98b92cc..b8282b7e 100644
--- a/gazebo/posim_gz_world_plugins/src/gauss_markov_process.cc
+++ b/gazebo/posim_gz_world_plugins/src/gauss_markov_process.cc
@@ -1,3 +1,4 @@
+#include
#include
namespace posim_gz_world_plugins
@@ -21,7 +22,7 @@ void GaussMarkovProcess::Reset() { this->var = this->mean; }
/////////////////////////////////////////////////
bool GaussMarkovProcess::SetMean(double _mean)
{
- if (this->min > _mean || this->max < _mean)
+ if (!std::isfinite(_mean) || this->min > _mean || this->max < _mean)
{
return false;
}
@@ -34,6 +35,12 @@ bool GaussMarkovProcess::SetMean(double _mean)
/////////////////////////////////////////////////
bool GaussMarkovProcess::SetModel(double _mean, double _min, double _max, double _mu, double _noise)
{
+ if (
+ !std::isfinite(_mean) || !std::isfinite(_min) || !std::isfinite(_max) || !std::isfinite(_mu) ||
+ !std::isfinite(_noise))
+ {
+ return false;
+ }
if (_min >= _max)
{
return false;
diff --git a/gazebo/posim_gz_world_plugins/test/test_current_model.cc b/gazebo/posim_gz_world_plugins/test/test_current_model.cc
new file mode 100644
index 00000000..aa547c70
--- /dev/null
+++ b/gazebo/posim_gz_world_plugins/test/test_current_model.cc
@@ -0,0 +1,65 @@
+// Copyright 2026 Kwon Yeseol
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+#include
+#include
+#include "posim_gz_world_plugins/gauss_markov_process.hh"
+using posim_gz_world_plugins::GaussMarkovProcess;
+TEST(CurrentModel, RejectsNonfiniteMeanWithoutMutation)
+{
+ for (auto bad :
+ {std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity(),
+ -std::numeric_limits::infinity()})
+ {
+ GaussMarkovProcess m;
+ ASSERT_TRUE(m.SetModel(.3, -1, 1, 0, 0));
+ EXPECT_FALSE(m.SetMean(bad));
+ EXPECT_EQ(m.mean, .3);
+ EXPECT_EQ(m.Update(1), .3);
+ }
+}
+TEST(CurrentModel, RejectsNonfiniteParametersAtomically)
+{
+ for (int i = 0; i < 5; ++i)
+ {
+ for (auto bad :
+ {std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity(),
+ -std::numeric_limits::infinity()})
+ {
+ GaussMarkovProcess m;
+ ASSERT_TRUE(m.SetModel(.3, -1, 1, 0, 0));
+ std::array p{.5, -1., 1., .1, .1};
+ p[i] = bad;
+ EXPECT_FALSE(m.SetModel(p[0], p[1], p[2], p[3], p[4])) << "parameter=" << i;
+ EXPECT_EQ(m.mean, .3);
+ EXPECT_EQ(m.min, -1);
+ EXPECT_EQ(m.max, 1);
+ EXPECT_EQ(m.mu, 0);
+ EXPECT_EQ(m.noiseAmp, 0);
+ }
+ }
+}
+TEST(CurrentModel, ValidBoundaryValuesRetainBehavior)
+{
+ GaussMarkovProcess m;
+ ASSERT_TRUE(m.SetModel(1, -1, 1, 0, 0));
+ EXPECT_EQ(m.Update(1), 1);
+ ASSERT_TRUE(m.SetMean(-1));
+ EXPECT_EQ(m.Update(2), -1);
+ EXPECT_FALSE(m.SetModel(0, 1, 1, 0, 0));
+ EXPECT_EQ(m.mean, -1);
+ EXPECT_FALSE(m.SetModel(0, -1, 1, 0, -1));
+ EXPECT_EQ(m.mean, -1);
+}
diff --git a/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc
index ab674c52..e85c99d5 100644
--- a/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc
+++ b/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc
@@ -295,7 +295,9 @@ bool OceanCurrentPlugin::UpdateStratHorzAngle(
auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase;
auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels;
- if (_req->layer >= stratifiedDatabase.size())
+ if (
+ _req->layer >= stratifiedDatabase.size() || _req->layer >= stratifiedCurrentModels.size() ||
+ stratifiedCurrentModels[_req->layer].size() < 3)
{
_res->success = false;
return true;
@@ -345,7 +347,9 @@ bool OceanCurrentPlugin::UpdateStratVertAngle(
auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase;
auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels;
- if (_req->layer >= stratifiedDatabase.size())
+ if (
+ _req->layer >= stratifiedDatabase.size() || _req->layer >= stratifiedCurrentModels.size() ||
+ stratifiedCurrentModels[_req->layer].size() < 3)
{
_res->success = false;
return true;
@@ -371,11 +375,17 @@ bool OceanCurrentPlugin::UpdateCurrentVelocity(
auto & currentHorzAngleModel = worldPlugin->sharedDataPtr->currentHorzAngleModel;
auto & currentVertAngleModel = worldPlugin->sharedDataPtr->currentVertAngleModel;
+ // Validate the complete request on copies before changing shared state.
+ auto nextVelocity = currentVelModel;
+ auto nextHorizontal = currentHorzAngleModel;
+ auto nextVertical = currentVertAngleModel;
if (
- currentVelModel.SetMean(_req->velocity) &&
- currentHorzAngleModel.SetMean(_req->horizontal_angle) &&
- currentVertAngleModel.SetMean(_req->vertical_angle))
+ nextVelocity.SetMean(_req->velocity) && nextHorizontal.SetMean(_req->horizontal_angle) &&
+ nextVertical.SetMean(_req->vertical_angle))
{
+ currentVelModel = nextVelocity;
+ currentHorzAngleModel = nextHorizontal;
+ currentVertAngleModel = nextVertical;
gzmsg << "Current velocity [m/s] = " << _req->velocity << std::endl
<< "Current horizontal angle [rad] = " << _req->horizontal_angle << std::endl
<< "Current vertical angle [rad] = " << _req->vertical_angle << std::endl
@@ -405,16 +415,19 @@ bool OceanCurrentPlugin::UpdateStratCurrentVelocity(
auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels;
auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase;
- if (_req->layer >= stratifiedDatabase.size())
+ if (
+ _req->layer >= stratifiedDatabase.size() || _req->layer >= stratifiedCurrentModels.size() ||
+ stratifiedCurrentModels[_req->layer].size() < 3)
{
_res->success = false;
return true;
}
+ auto nextModels = stratifiedCurrentModels[_req->layer];
if (
- stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) &&
- stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) &&
- stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle))
+ nextModels[0].SetMean(_req->velocity) && nextModels[1].SetMean(_req->horizontal_angle) &&
+ nextModels[2].SetMean(_req->vertical_angle))
{
+ stratifiedCurrentModels[_req->layer] = std::move(nextModels);
// Update the database values as well
stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity;
stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity;
@@ -642,7 +655,8 @@ void OceanCurrentPlugin::PostUpdate(
{
auto flowVelMsg = geometry_msgs::msg::TwistStamped();
- flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now();
+ flowVelMsg.header.stamp = rclcpp::Time(
+ std::chrono::duration_cast(_info.simTime).count(), RCL_ROS_TIME);
flowVelMsg.header.frame_id = "world";
flowVelMsg.twist.linear.x = sharedPtr->currentVelocity.X();
@@ -655,7 +669,8 @@ void OceanCurrentPlugin::PostUpdate(
// Generate and publish stratified current velocity
auto stratCurrentVelocityMsg = posim_interfaces::msg::StratifiedCurrentVelocity();
- stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now();
+ stratCurrentVelocityMsg.header.stamp = rclcpp::Time(
+ std::chrono::duration_cast(_info.simTime).count(), RCL_ROS_TIME);
stratCurrentVelocityMsg.header.frame_id = "world";
// Updating for stratified behaviour of Ocean Currents