Skip to content

fix: reject invalid current requests without partial updates #24

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
yeseorizi wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix: reject invalid current requests without partial updates #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions gazebo/posim_gz_world_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
1 change: 1 addition & 0 deletions gazebo/posim_gz_world_plugins/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<depend>posim_interfaces</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
Expand Down
9 changes: 8 additions & 1 deletion gazebo/posim_gz_world_plugins/src/gauss_markov_process.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <cmath>
#include <posim_gz_world_plugins/gauss_markov_process.hh>

namespace posim_gz_world_plugins
Expand All @@ -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;
}
Expand All @@ -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;
Expand Down
65 changes: 65 additions & 0 deletions gazebo/posim_gz_world_plugins/test/test_current_model.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <gtest/gtest.h>
#include <array>
#include <limits>
#include "posim_gz_world_plugins/gauss_markov_process.hh"
using posim_gz_world_plugins::GaussMarkovProcess;
TEST(CurrentModel, RejectsNonfiniteMeanWithoutMutation)
{
for (auto bad :
{std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::infinity(),
-std::numeric_limits<double>::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<double>::quiet_NaN(), std::numeric_limits<double>::infinity(),
-std::numeric_limits<double>::infinity()})
{
GaussMarkovProcess m;
ASSERT_TRUE(m.SetModel(.3, -1, 1, 0, 0));
std::array<double, 5> 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);
}
37 changes: 26 additions & 11 deletions gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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<std::chrono::nanoseconds>(_info.simTime).count(), RCL_ROS_TIME);
flowVelMsg.header.frame_id = "world";

flowVelMsg.twist.linear.x = sharedPtr->currentVelocity.X();
Expand All @@ -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<std::chrono::nanoseconds>(_info.simTime).count(), RCL_ROS_TIME);
stratCurrentVelocityMsg.header.frame_id = "world";

// Updating for stratified behaviour of Ocean Currents
Expand Down
Loading