diff --git a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt b/gazebo/posim_gz_sensor_plugins/CMakeLists.txt index e5855d10..e549f4f9 100644 --- a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/posim_gz_sensor_plugins/CMakeLists.txt @@ -110,6 +110,9 @@ if(BUILD_TESTING) ament_add_gtest(test_pressure_shutdown test/test_pressure_shutdown.cc) target_include_directories(test_pressure_shutdown PRIVATE include) target_link_libraries(test_pressure_shutdown sea_pressure_sensor) + ament_add_gtest(test_pressure_stamp test/test_pressure_stamp.cc) + target_include_directories(test_pressure_stamp PRIVATE include) + target_link_libraries(test_pressure_stamp sea_pressure_sensor) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() diff --git a/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc index 5c932dfc..944c789f 100644 --- a/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -274,9 +274,7 @@ void SubseaPressureSensorPlugin::PostUpdate( { geometry_msgs::msg::PointStamped rosDepthMsg; rosDepthMsg.point.z = this->dataPtr->inferredDepth; - rosDepthMsg.header.stamp.sec = - std::chrono::duration_cast(this->dataPtr->lastMeasurementTime) - .count(); + rosDepthMsg.header = rosPressureMsg.header; this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg); } diff --git a/gazebo/posim_gz_sensor_plugins/test/test_pressure_stamp.cc b/gazebo/posim_gz_sensor_plugins/test/test_pressure_stamp.cc new file mode 100644 index 00000000..5b3ce33c --- /dev/null +++ b/gazebo/posim_gz_sensor_plugins/test/test_pressure_stamp.cc @@ -0,0 +1,76 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "posim_gz_sensor_plugins/sea_pressure_sensor.hh" + +TEST(SeaPressure, DerivedDepthUsesTheSameFullMeasurementStamp) +{ + rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None); + { + auto node = std::make_shared("pressure_stamp_observer"); + sensor_msgs::msg::FluidPressure::SharedPtr pressure; + geometry_msgs::msg::PointStamped::SharedPtr depth; + auto ps = node->create_subscription( + "/model/pressure_stamp_test/sea_pressure", 10, + [&](sensor_msgs::msg::FluidPressure::SharedPtr msg) { pressure = msg; }); + auto ds = node->create_subscription( + "/model/pressure_stamp_test/sea_pressure_depth", 10, + [&](geometry_msgs::msg::PointStamped::SharedPtr msg) { depth = msg; }); + posim_gz_sensor_plugins::SubseaPressureSensorPlugin sensor; + gz::sim::EntityComponentManager ecm; + gz::sim::EventManager events; + auto entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, gz::sim::components::Name("pressure_stamp_test")); + ecm.CreateComponent(entity, gz::sim::components::Pose(gz::math::Pose3d(0, 0, -5, 0, 0, 0))); + auto plugin = std::make_shared(); + plugin->SetName("plugin"); + auto ns = std::make_shared(); + ns->SetName("namespace"); + ns->AddValue("string", "pressure_stamp_test", true); + plugin->InsertElement(ns); + sensor.Configure(entity, plugin, ecm, events); + const auto discovery = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while ((ps->get_publisher_count() == 0 || ds->get_publisher_count() == 0) && + std::chrono::steady_clock::now() < discovery) + { + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_GT(ps->get_publisher_count(), 0u); + EXPECT_GT(ds->get_publisher_count(), 0u); + for (int milliseconds : {125, 325}) + { + pressure.reset(); + depth.reset(); + gz::sim::UpdateInfo info; + info.simTime = std::chrono::milliseconds(milliseconds); + info.iterations = 1; + info.paused = false; + sensor.PreUpdate(info, ecm); + sensor.PostUpdate(info, ecm); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while ((!pressure || !depth) && std::chrono::steady_clock::now() < deadline) + { + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(pressure); + EXPECT_TRUE(depth); + if (pressure && depth) + { + EXPECT_EQ(pressure->header.stamp, depth->header.stamp); + EXPECT_EQ(depth->header.stamp.nanosec, static_cast(milliseconds * 1000000)); + EXPECT_NEAR(pressure->fluid_pressure, 101325.0 + 5.0 * 9806.38, 1e-6); + EXPECT_NEAR(depth->point.z, 5.0, 1e-9); + EXPECT_DOUBLE_EQ(pressure->variance, 9000000.0); + } + } + } + rclcpp::shutdown(); +}