Skip to content

Commit 363a7c7

Browse files
GauravKumar9920GauravKumar9920
authored
Merge pull request #13 from IOES-Lab/sea_pressure_plugin
[GSOC-79] Sea Pressure Sensor Plugin
2 parents 3df20aa + d28a1f6 commit 363a7c7

17 files changed

+826
-71
lines changed

‎gazebo/dave_gz_sensor_plugins/CMakeLists.txt

Lines changed: 24 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,4 @@
11
cmake_minimum_required(VERSION 3.8)
2-
3-
# Define the project name
42
project(dave_gz_sensor_plugins)
53

64
# Find required packages
@@ -13,30 +11,42 @@ find_package(gz-common5 REQUIRED COMPONENTS profiler)
1311
find_package(gz-sim8 REQUIRED)
1412
find_package(geometry_msgs REQUIRED)
1513
find_package(dave_interfaces REQUIRED)
14+
find_package(sensor_msgs REQUIRED)
15+
find_package(Protobuf REQUIRED)
16+
find_package(gz-msgs10 REQUIRED)
1617

1718
# Set version variables
1819
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
1920
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
2021
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
22+
set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
2123

2224
message(STATUS "Compiling against Gazebo Harmonic")
2325

2426
add_library(UsblTransceiver SHARED src/UsblTransceiver.cc)
2527
add_library(UsblTransponder SHARED src/UsblTransponder.cc)
28+
add_library(sea_pressure_sensor SHARED src/sea_pressure_sensor.cc)
2629

2730
target_include_directories(UsblTransceiver PRIVATE include)
2831
target_include_directories(UsblTransponder PRIVATE include)
32+
target_include_directories(sea_pressure_sensor PRIVATE include)
2933

3034
target_link_libraries(UsblTransceiver
31-
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
35+
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
36+
)
3237

3338
target_link_libraries(UsblTransponder
34-
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
39+
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
40+
)
3541

42+
target_link_libraries(sea_pressure_sensor
43+
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
44+
)
3645

3746
# Specify dependencies for FullSystem using ament_target_dependencies
3847
ament_target_dependencies(UsblTransceiver
3948
dave_interfaces
49+
sensor_msgs
4050
rclcpp
4151
geometry_msgs
4252
std_msgs
@@ -48,8 +58,15 @@ ament_target_dependencies(UsblTransponder
4858
std_msgs
4959
)
5060

61+
ament_target_dependencies(sea_pressure_sensor
62+
rclcpp
63+
std_msgs
64+
sensor_msgs
65+
geometry_msgs
66+
)
67+
5168
# Install targets
52-
install(TARGETS UsblTransceiver UsblTransponder
69+
install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor
5370
DESTINATION lib/${PROJECT_NAME}
5471
)
5572

@@ -60,13 +77,13 @@ install(DIRECTORY include/
6077

6178
# Environment hooks
6279
ament_environment_hooks(
63-
"${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
80+
"${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in"
81+
)
6482

6583
# Testing setup
6684
if(BUILD_TESTING)
6785
find_package(ament_lint_auto REQUIRED)
6886
ament_lint_auto_find_test_dependencies()
6987
endif()
7088

71-
# Configure ament
7289
ament_package()

‎gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
#ifndef dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_
2+
#define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_
3+
4+
#include <gz/msgs/vector3d.pb.h>
5+
#include <gz/sim/System.hh>
6+
#include <memory>
7+
#include <rclcpp/rclcpp.hpp>
8+
#include <rclcpp/service.hpp>
9+
#include <sensor_msgs/msg/fluid_pressure.hpp>
10+
11+
namespace dave_gz_sensor_plugins
12+
{
13+
class SubseaPressureSensorPlugin : public gz::sim::System,
14+
public gz::sim::ISystemConfigure,
15+
public gz::sim::ISystemPreUpdate,
16+
public gz::sim::ISystemPostUpdate
17+
{
18+
public:
19+
SubseaPressureSensorPlugin();
20+
~SubseaPressureSensorPlugin();
21+
22+
/// \brief Load the plugin
23+
void Configure(
24+
const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
25+
gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr);
26+
27+
/// \brief Update sensor measurement
28+
void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm);
29+
30+
void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm);
31+
32+
gz::math::Pose3d GetWorldPose(
33+
const gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm);
34+
35+
gz::math::Pose3d GetModelPose(
36+
const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm);
37+
38+
gz::sim::Entity GetModelEntity(
39+
const std::string & modelName, gz::sim::EntityComponentManager & ecm);
40+
41+
private:
42+
std::shared_ptr<rclcpp::Node> rosNode;
43+
struct PrivateData;
44+
std::unique_ptr<PrivateData> dataPtr;
45+
};
46+
} // namespace dave_gz_sensor_plugins
47+
48+
#endif // __UUV_SUBSEA_PRESSURE_ROS_PLUGIN_HH__

‎gazebo/dave_gz_sensor_plugins/package.xml

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,15 @@
33
<name>dave_gz_sensor_plugins</name>
44
<version>0.0.0</version>
55
<description>DAVE sensor plugins</description>
6+
<maintainer email="[email protected]">Gaurav kumar</maintainer>
67
<maintainer email="[email protected]">Helena Moyen</maintainer>
78
<license>Apache 2.0</license>
9+
<depend>geometry_msgs</depend>
810
<buildtool_depend>ament_cmake</buildtool_depend>
911
<test_depend>ament_lint_auto</test_depend>
1012
<build_depend>dave_interfaces</build_depend>
13+
<build_depend>protobuf</build_depend>
14+
<exec_depend>protobuf</exec_depend>
1115
<exec_depend>dave_interfaces</exec_depend>
1216
<export>
1317
<build_type>ament_cmake</build_type>

‎gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc

Lines changed: 245 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,245 @@
1+
#include "dave_gz_sensor_plugins/sea_pressure_sensor.hh"
2+
#include <gz/msgs/fluid_pressure.pb.h>
3+
#include <chrono>
4+
#include <geometry_msgs/msg/point_stamped.hpp>
5+
#include <gz/math/Pose3.hh>
6+
#include <gz/math/Vector3.hh>
7+
#include <gz/msgs/Utility.hh>
8+
#include <gz/plugin/Register.hh>
9+
#include <gz/sim/System.hh>
10+
#include <gz/sim/World.hh>
11+
#include <gz/sim/components/CustomSensor.hh>
12+
#include <gz/sim/components/Link.hh>
13+
#include <gz/sim/components/Name.hh>
14+
#include <gz/sim/components/ParentEntity.hh>
15+
#include <gz/sim/components/Pose.hh>
16+
#include <gz/sim/components/Sensor.hh>
17+
#include <gz/sim/components/World.hh>
18+
#include <gz/transport/Node.hh>
19+
#include <memory>
20+
#include <rclcpp/rclcpp.hpp>
21+
#include <sensor_msgs/msg/fluid_pressure.hpp>
22+
23+
GZ_ADD_PLUGIN(
24+
dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System,
25+
dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemConfigure,
26+
dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemPreUpdate,
27+
dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemPostUpdate)
28+
29+
namespace dave_gz_sensor_plugins
30+
{
31+
struct SubseaPressureSensorPlugin::PrivateData
32+
{
33+
public:
34+
double saturation;
35+
gz::sim::EntityComponentManager * ecm = nullptr;
36+
std::chrono::steady_clock::duration lastMeasurementTime{0};
37+
bool estimateDepth;
38+
double standardPressure = 101.325;
39+
double kPaPerM = 9.80638;
40+
std::shared_ptr<gz::transport::Node> gazeboNode;
41+
gz::transport::Node::Publisher gz_pressure_sensor_pub;
42+
rclcpp::Publisher<sensor_msgs::msg::FluidPressure>::SharedPtr ros_pressure_sensor_pub;
43+
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr ros_depth_estimate_pub;
44+
std::shared_ptr<rclcpp::Node> rosNode;
45+
std::string robotNamespace;
46+
double noiseAmp = 0.0;
47+
double noiseSigma = 3.0;
48+
double inferredDepth = 0.0;
49+
double pressure = 0.0;
50+
std::string modelName;
51+
gz::sim::Entity modelEntity;
52+
};
53+
54+
SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique<PrivateData>())
55+
{
56+
}
57+
58+
SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {}
59+
60+
void SubseaPressureSensorPlugin::Configure(
61+
const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
62+
gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager)
63+
{
64+
if (!rclcpp::ok())
65+
{
66+
rclcpp::init(0, nullptr);
67+
}
68+
69+
gzdbg << "dave_gz_sensor_plugins::SubseaPressureSensorPlugin::Configure on entity: " << _entity
70+
<< std::endl;
71+
72+
// Initialize the ROS 2 node
73+
this->rosNode = std::make_shared<rclcpp::Node>("subsea_pressure_sensor");
74+
75+
// Initialize the Gazebo node
76+
this->dataPtr->gazeboNode = std::make_shared<gz::transport::Node>();
77+
78+
if (!_sdf->HasElement("namespace"))
79+
{
80+
gzerr << "Missing required parameter <namespace>, "
81+
<< "plugin will not be initialized." << std::endl;
82+
return;
83+
}
84+
this->dataPtr->robotNamespace = _sdf->Get<std::string>("namespace");
85+
86+
if (_sdf->HasElement("saturation"))
87+
{
88+
this->dataPtr->saturation = _sdf->Get<double>("saturation");
89+
}
90+
else
91+
{
92+
this->dataPtr->saturation = 3000;
93+
}
94+
95+
if (_sdf->HasElement("estimate_depth_on"))
96+
{
97+
this->dataPtr->estimateDepth = _sdf->Get<bool>("estimate_depth_on");
98+
}
99+
else
100+
{
101+
this->dataPtr->estimateDepth = true;
102+
}
103+
104+
if (_sdf->HasElement("standard_pressure"))
105+
{
106+
this->dataPtr->standardPressure = _sdf->Get<double>("standard_pressure");
107+
}
108+
else
109+
{
110+
this->dataPtr->standardPressure = 101.325;
111+
}
112+
113+
if (_sdf->HasElement("kPa_per_meter"))
114+
{
115+
this->dataPtr->kPaPerM = _sdf->Get<double>("kPa_per_meter");
116+
}
117+
else
118+
{
119+
this->dataPtr->kPaPerM = 9.80638;
120+
}
121+
122+
// this->dataPtr->gazeboNode->Init();
123+
this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->robotNamespace, _ecm);
124+
125+
this->dataPtr->ros_pressure_sensor_pub =
126+
this->rosNode->create_publisher<sensor_msgs::msg::FluidPressure>(
127+
this->dataPtr->robotNamespace + "/" + "Pressure", rclcpp::QoS(10).reliable());
128+
129+
if (this->dataPtr->estimateDepth)
130+
{
131+
this->dataPtr->ros_depth_estimate_pub =
132+
this->rosNode->create_publisher<geometry_msgs::msg::PointStamped>(
133+
this->dataPtr->robotNamespace + "/" + "Pressure_depth", rclcpp::QoS(10).reliable());
134+
}
135+
136+
this->dataPtr->gz_pressure_sensor_pub =
137+
this->dataPtr->gazeboNode->Advertise<gz::msgs::FluidPressure>(
138+
this->dataPtr->robotNamespace + "/" + "Pressure");
139+
}
140+
//////////////////////////////////////////
141+
142+
gz::sim::Entity SubseaPressureSensorPlugin::GetModelEntity(
143+
const std::string & modelName, gz::sim::EntityComponentManager & ecm)
144+
{
145+
gz::sim::Entity modelEntity = gz::sim::kNullEntity;
146+
147+
ecm.Each<gz::sim::components::Name>(
148+
[&](const gz::sim::Entity & entity, const gz::sim::components::Name * nameComp) -> bool
149+
{
150+
if (nameComp->Data() == modelName)
151+
{
152+
modelEntity = entity;
153+
return false; // Stop iteration
154+
}
155+
return true; // Continue iteration
156+
});
157+
158+
return modelEntity;
159+
}
160+
161+
gz::math::Pose3d SubseaPressureSensorPlugin::GetModelPose(
162+
const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm)
163+
{
164+
const auto * poseComp = ecm.Component<gz::sim::components::Pose>(modelEntity);
165+
if (poseComp)
166+
{
167+
return poseComp->Data();
168+
}
169+
else
170+
{
171+
gzerr << "Pose component not found for entity: " << modelEntity << std::endl;
172+
return gz::math::Pose3d::Zero;
173+
}
174+
}
175+
176+
void SubseaPressureSensorPlugin::PreUpdate(
177+
const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm)
178+
{
179+
// Get model pose
180+
gz::math::Pose3d sea_pressure_sensor_pos = GetModelPose(this->dataPtr->modelEntity, _ecm);
181+
double depth = std::abs(sea_pressure_sensor_pos.Z());
182+
this->dataPtr->pressure = this->dataPtr->standardPressure;
183+
if (depth >= 0)
184+
{
185+
this->dataPtr->pressure += depth * this->dataPtr->kPaPerM;
186+
}
187+
188+
// not adding gaussian noise for now, Future Work (TODO)
189+
// pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp);
190+
this->dataPtr->pressure += this->dataPtr->noiseAmp; // noiseAmp is 0.0
191+
192+
// double inferredDepth = 0.0;
193+
if (this->dataPtr->estimateDepth)
194+
{
195+
this->dataPtr->inferredDepth =
196+
(this->dataPtr->pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM;
197+
}
198+
}
199+
200+
void SubseaPressureSensorPlugin::PostUpdate(
201+
const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
202+
{
203+
this->dataPtr->lastMeasurementTime = _info.simTime;
204+
205+
// Publishing Sea_Pressure and depth estimate on gazebo topic
206+
gz::msgs::FluidPressure gzPressureMsg;
207+
gzPressureMsg.set_pressure(this->dataPtr->pressure);
208+
gzPressureMsg.set_variance(this->dataPtr->noiseSigma * this->dataPtr->noiseSigma);
209+
210+
// Publishing the pressure message
211+
this->dataPtr->gz_pressure_sensor_pub.Publish(gzPressureMsg);
212+
213+
// Publishing Sea_Pressure on Ros Topic
214+
sensor_msgs::msg::FluidPressure rosPressureMsg;
215+
rosPressureMsg.header.stamp.sec =
216+
std::chrono::duration_cast<std::chrono::seconds>(_info.simTime).count(); // Time in seconds
217+
rosPressureMsg.header.stamp.nanosec =
218+
std::chrono::duration_cast<std::chrono::nanoseconds>(_info.simTime).count() %
219+
1000000000; // Time in nanoseconds
220+
rosPressureMsg.fluid_pressure = this->dataPtr->pressure;
221+
rosPressureMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma;
222+
this->dataPtr->ros_pressure_sensor_pub->publish(rosPressureMsg);
223+
224+
// publishing depth message
225+
if (this->dataPtr->estimateDepth)
226+
{
227+
geometry_msgs::msg::PointStamped rosDepthMsg;
228+
rosDepthMsg.point.z = this->dataPtr->inferredDepth;
229+
rosDepthMsg.header.stamp.sec =
230+
std::chrono::duration_cast<std::chrono::seconds>(this->dataPtr->lastMeasurementTime).count();
231+
this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg);
232+
}
233+
234+
if (!_info.paused)
235+
{
236+
rclcpp::spin_some(this->rosNode);
237+
238+
if (_info.iterations % 1000 == 0)
239+
{
240+
gzmsg << "dave_ros_gz_plugins::SubseaPressureSensorPlugin::PostUpdate" << std::endl;
241+
}
242+
}
243+
}
244+
245+
} // namespace dave_gz_sensor_plugins

0 commit comments

Comments
 (0)