Merge pull request #13 from IOES-Lab/sea_pressure_plugin

[GSOC-79] Sea Pressure Sensor Plugin

gazebo/dave_gz_sensor_plugins/CMakeLists.txt

@@ -1,6 +1,4 @@
 cmake_minimum_required(VERSION 3.8)
-
-# Define the project name
 project(dave_gz_sensor_plugins)
 
 # Find required packages
@@ -13,30 +11,42 @@ find_package(gz-common5 REQUIRED COMPONENTS profiler)
 find_package(gz-sim8 REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(dave_interfaces REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(Protobuf REQUIRED)
+find_package(gz-msgs10 REQUIRED)
 
 # Set version variables
 set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
 set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
 
 message(STATUS "Compiling against Gazebo Harmonic")
 
 add_library(UsblTransceiver SHARED src/UsblTransceiver.cc)
 add_library(UsblTransponder SHARED src/UsblTransponder.cc)
+add_library(sea_pressure_sensor SHARED src/sea_pressure_sensor.cc)
 
 target_include_directories(UsblTransceiver PRIVATE include)
 target_include_directories(UsblTransponder PRIVATE include)
+target_include_directories(sea_pressure_sensor PRIVATE include)
 
 target_link_libraries(UsblTransceiver
-  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+)
 
 target_link_libraries(UsblTransponder
-gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+)
 
+target_link_libraries(sea_pressure_sensor
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+)
 
 # Specify dependencies for FullSystem using ament_target_dependencies
 ament_target_dependencies(UsblTransceiver
   dave_interfaces
+  sensor_msgs
   rclcpp
   geometry_msgs
   std_msgs
@@ -48,8 +58,15 @@ ament_target_dependencies(UsblTransponder
   std_msgs
 )
 
+ament_target_dependencies(sea_pressure_sensor
+  rclcpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+)
+
 # Install targets
-install(TARGETS UsblTransceiver UsblTransponder
+install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -60,13 +77,13 @@ install(DIRECTORY include/
 
 # Environment hooks
 ament_environment_hooks(
-  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
+  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in"
+)
 
 # Testing setup
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-# Configure ament
 ament_package()

gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh

@@ -0,0 +1,48 @@
+#ifndef dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_
+#define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_
+
+#include <gz/msgs/vector3d.pb.h>
+#include <gz/sim/System.hh>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/service.hpp>
+#include <sensor_msgs/msg/fluid_pressure.hpp>
+
+namespace dave_gz_sensor_plugins
+{
+class SubseaPressureSensorPlugin : public gz::sim::System,
+                                   public gz::sim::ISystemConfigure,
+                                   public gz::sim::ISystemPreUpdate,
+                                   public gz::sim::ISystemPostUpdate
+{
+public:
+  SubseaPressureSensorPlugin();
+  ~SubseaPressureSensorPlugin();
+
+  /// \brief Load the plugin
+  void Configure(
+    const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+    gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr);
+
+  /// \brief Update sensor measurement
+  void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm);
+
+  void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm);
+
+  gz::math::Pose3d GetWorldPose(
+    const gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm);
+
+  gz::math::Pose3d GetModelPose(
+    const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm);
+
+  gz::sim::Entity GetModelEntity(
+    const std::string & modelName, gz::sim::EntityComponentManager & ecm);
+
+private:
+  std::shared_ptr<rclcpp::Node> rosNode;
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_gz_sensor_plugins
+
+#endif  // __UUV_SUBSEA_PRESSURE_ROS_PLUGIN_HH__

gazebo/dave_gz_sensor_plugins/package.xml

@@ -3,11 +3,15 @@
   <name>dave_gz_sensor_plugins</name>
   <version>0.0.0</version>
   <description>DAVE sensor plugins</description>
+  <maintainer email="[email protected]">Gaurav kumar</maintainer>
   <maintainer email="[email protected]">Helena Moyen</maintainer>
   <license>Apache 2.0</license>
+  <depend>geometry_msgs</depend>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <test_depend>ament_lint_auto</test_depend>
   <build_depend>dave_interfaces</build_depend>
+  <build_depend>protobuf</build_depend>
+  <exec_depend>protobuf</exec_depend>
   <exec_depend>dave_interfaces</exec_depend>
   <export>
     <build_type>ament_cmake</build_type>

gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc

@@ -0,0 +1,245 @@
+#include "dave_gz_sensor_plugins/sea_pressure_sensor.hh"
+#include <gz/msgs/fluid_pressure.pb.h>
+#include <chrono>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/plugin/Register.hh>
+#include <gz/sim/System.hh>
+#include <gz/sim/World.hh>
+#include <gz/sim/components/CustomSensor.hh>
+#include <gz/sim/components/Link.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/ParentEntity.hh>
+#include <gz/sim/components/Pose.hh>
+#include <gz/sim/components/Sensor.hh>
+#include <gz/sim/components/World.hh>
+#include <gz/transport/Node.hh>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/fluid_pressure.hpp>
+
+GZ_ADD_PLUGIN(
+  dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System,
+  dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemConfigure,
+  dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemPreUpdate,
+  dave_gz_sensor_plugins::SubseaPressureSensorPlugin::ISystemPostUpdate)
+
+namespace dave_gz_sensor_plugins
+{
+struct SubseaPressureSensorPlugin::PrivateData
+{
+public:
+  double saturation;
+  gz::sim::EntityComponentManager * ecm = nullptr;
+  std::chrono::steady_clock::duration lastMeasurementTime{0};
+  bool estimateDepth;
+  double standardPressure = 101.325;
+  double kPaPerM = 9.80638;
+  std::shared_ptr<gz::transport::Node> gazeboNode;
+  gz::transport::Node::Publisher gz_pressure_sensor_pub;
+  rclcpp::Publisher<sensor_msgs::msg::FluidPressure>::SharedPtr ros_pressure_sensor_pub;
+  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr ros_depth_estimate_pub;
+  std::shared_ptr<rclcpp::Node> rosNode;
+  std::string robotNamespace;
+  double noiseAmp = 0.0;
+  double noiseSigma = 3.0;
+  double inferredDepth = 0.0;
+  double pressure = 0.0;
+  std::string modelName;
+  gz::sim::Entity modelEntity;
+};
+
+SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique<PrivateData>())
+{
+}
+
+SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {}
+
+void SubseaPressureSensorPlugin::Configure(
+  const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+  gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager)
+{
+  if (!rclcpp::ok())
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  gzdbg << "dave_gz_sensor_plugins::SubseaPressureSensorPlugin::Configure on entity: " << _entity
+        << std::endl;
+
+  // Initialize the ROS 2 node
+  this->rosNode = std::make_shared<rclcpp::Node>("subsea_pressure_sensor");
+
+  // Initialize the Gazebo node
+  this->dataPtr->gazeboNode = std::make_shared<gz::transport::Node>();
+
+  if (!_sdf->HasElement("namespace"))
+  {
+    gzerr << "Missing required parameter <namespace>, "
+          << "plugin will not be initialized." << std::endl;
+    return;
+  }
+  this->dataPtr->robotNamespace = _sdf->Get<std::string>("namespace");
+
+  if (_sdf->HasElement("saturation"))
+  {
+    this->dataPtr->saturation = _sdf->Get<double>("saturation");
+  }
+  else
+  {
+    this->dataPtr->saturation = 3000;
+  }
+
+  if (_sdf->HasElement("estimate_depth_on"))
+  {
+    this->dataPtr->estimateDepth = _sdf->Get<bool>("estimate_depth_on");
+  }
+  else
+  {
+    this->dataPtr->estimateDepth = true;
+  }
+
+  if (_sdf->HasElement("standard_pressure"))
+  {
+    this->dataPtr->standardPressure = _sdf->Get<double>("standard_pressure");
+  }
+  else
+  {
+    this->dataPtr->standardPressure = 101.325;
+  }
+
+  if (_sdf->HasElement("kPa_per_meter"))
+  {
+    this->dataPtr->kPaPerM = _sdf->Get<double>("kPa_per_meter");
+  }
+  else
+  {
+    this->dataPtr->kPaPerM = 9.80638;
+  }
+
+  // this->dataPtr->gazeboNode->Init();
+  this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->robotNamespace, _ecm);
+
+  this->dataPtr->ros_pressure_sensor_pub =
+    this->rosNode->create_publisher<sensor_msgs::msg::FluidPressure>(
+      this->dataPtr->robotNamespace + "/" + "Pressure", rclcpp::QoS(10).reliable());
+
+  if (this->dataPtr->estimateDepth)
+  {
+    this->dataPtr->ros_depth_estimate_pub =
+      this->rosNode->create_publisher<geometry_msgs::msg::PointStamped>(
+        this->dataPtr->robotNamespace + "/" + "Pressure_depth", rclcpp::QoS(10).reliable());
+  }
+
+  this->dataPtr->gz_pressure_sensor_pub =
+    this->dataPtr->gazeboNode->Advertise<gz::msgs::FluidPressure>(
+      this->dataPtr->robotNamespace + "/" + "Pressure");
+}
+//////////////////////////////////////////
+
+gz::sim::Entity SubseaPressureSensorPlugin::GetModelEntity(
+  const std::string & modelName, gz::sim::EntityComponentManager & ecm)
+{
+  gz::sim::Entity modelEntity = gz::sim::kNullEntity;
+
+  ecm.Each<gz::sim::components::Name>(
+    [&](const gz::sim::Entity & entity, const gz::sim::components::Name * nameComp) -> bool
+    {
+      if (nameComp->Data() == modelName)
+      {
+        modelEntity = entity;
+        return false;  // Stop iteration
+      }
+      return true;  // Continue iteration
+    });
+
+  return modelEntity;
+}
+
+gz::math::Pose3d SubseaPressureSensorPlugin::GetModelPose(
+  const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm)
+{
+  const auto * poseComp = ecm.Component<gz::sim::components::Pose>(modelEntity);
+  if (poseComp)
+  {
+    return poseComp->Data();
+  }
+  else
+  {
+    gzerr << "Pose component not found for entity: " << modelEntity << std::endl;
+    return gz::math::Pose3d::Zero;
+  }
+}
+
+void SubseaPressureSensorPlugin::PreUpdate(
+  const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm)
+{
+  // Get model pose
+  gz::math::Pose3d sea_pressure_sensor_pos = GetModelPose(this->dataPtr->modelEntity, _ecm);
+  double depth = std::abs(sea_pressure_sensor_pos.Z());
+  this->dataPtr->pressure = this->dataPtr->standardPressure;
+  if (depth >= 0)
+  {
+    this->dataPtr->pressure += depth * this->dataPtr->kPaPerM;
+  }
+
+  // not adding gaussian noise for now, Future Work (TODO)
+  // pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp);
+  this->dataPtr->pressure += this->dataPtr->noiseAmp;  // noiseAmp is 0.0
+
+  // double inferredDepth = 0.0;
+  if (this->dataPtr->estimateDepth)
+  {
+    this->dataPtr->inferredDepth =
+      (this->dataPtr->pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM;
+  }
+}
+
+void SubseaPressureSensorPlugin::PostUpdate(
+  const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
+{
+  this->dataPtr->lastMeasurementTime = _info.simTime;
+
+  // Publishing Sea_Pressure and depth estimate on gazebo topic
+  gz::msgs::FluidPressure gzPressureMsg;
+  gzPressureMsg.set_pressure(this->dataPtr->pressure);
+  gzPressureMsg.set_variance(this->dataPtr->noiseSigma * this->dataPtr->noiseSigma);
+
+  // Publishing the pressure message
+  this->dataPtr->gz_pressure_sensor_pub.Publish(gzPressureMsg);
+
+  // Publishing Sea_Pressure on Ros Topic
+  sensor_msgs::msg::FluidPressure rosPressureMsg;
+  rosPressureMsg.header.stamp.sec =
+    std::chrono::duration_cast<std::chrono::seconds>(_info.simTime).count();  // Time in seconds
+  rosPressureMsg.header.stamp.nanosec =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(_info.simTime).count() %
+    1000000000;  // Time in nanoseconds
+  rosPressureMsg.fluid_pressure = this->dataPtr->pressure;
+  rosPressureMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma;
+  this->dataPtr->ros_pressure_sensor_pub->publish(rosPressureMsg);
+
+  // publishing depth message
+  if (this->dataPtr->estimateDepth)
+  {
+    geometry_msgs::msg::PointStamped rosDepthMsg;
+    rosDepthMsg.point.z = this->dataPtr->inferredDepth;
+    rosDepthMsg.header.stamp.sec =
+      std::chrono::duration_cast<std::chrono::seconds>(this->dataPtr->lastMeasurementTime).count();
+    this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg);
+  }
+
+  if (!_info.paused)
+  {
+    rclcpp::spin_some(this->rosNode);
+
+    if (_info.iterations % 1000 == 0)
+    {
+      gzmsg << "dave_ros_gz_plugins::SubseaPressureSensorPlugin::PostUpdate" << std::endl;
+    }
+  }
+}
+
+}  // namespace dave_gz_sensor_plugins