Ocean Current Plugin

.clang-format-ignore

@@ -0,0 +1 @@
+*.proto

.pre-commit-config.yaml

@@ -79,6 +79,7 @@ repos:
     hooks:
       - id: clang-format
         args: ["-fallback-style=none", "-i"]
+        exclude: \.proto$
 
   # - repo: local
   #   hooks:

dave_interfaces/CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(gz-cmake3 REQUIRED)
+find_package(gz-msgs10 REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/UsblCommand.msg"
@@ -19,10 +21,18 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/DVL.msg"
   "msg/DVLBeam.msg"
   "msg/DVLTarget.msg"
+  "msg/StratifiedCurrentVelocity.msg"
+  "msg/StratifiedCurrentDatabase.msg"
   "srv/SetOriginSphericalCoord.srv"
   "srv/GetOriginSphericalCoord.srv"
   "srv/TransformToSphericalCoord.srv"
   "srv/TransformFromSphericalCoord.srv"
+  "srv/GetCurrentModel.srv"
+  "srv/SetCurrentModel.srv"
+  "srv/SetCurrentDirection.srv"
+  "srv/SetCurrentVelocity.srv"
+  "srv/SetStratifiedCurrentVelocity.srv"
+  "srv/SetStratifiedCurrentDirection.srv"
   DEPENDENCIES geometry_msgs
 )
 
@@ -36,4 +46,4 @@ ament_export_dependencies(rosidl_default_runtime)
 # Install CMake package configuration
 ament_export_include_directories(include)
 
-ament_package()
+ament_package()

dave_interfaces/msg/StratifiedCurrentDatabase.msg

@@ -0,0 +1,42 @@
+# Publishes depths and velocities read from the csv database
+
+# Depths
+float32[] depths
+
+# Velocities
+geometry_msgs/Vector3[] velocities
+
+# Tide time (GMT)
+int16[] time_gmt_year
+int16[] time_gmt_month
+int16[] time_gmt_day
+int16[] time_gmt_hour
+int16[] time_gmt_minute
+
+# Tide velocities
+float32[] tidevelocities
+
+# Tide constituents
+bool tideconstituents
+float32 m2_amp
+float32 m2_phase
+float32 m2_speed
+float32 s2_amp
+float32 s2_phase
+float32 s2_speed
+float32 n2_amp
+float32 n2_phase
+float32 n2_speed
+
+# Tide direction
+float32 ebb_direction
+float32 flood_direction
+
+# World start time (GMT)
+int16 world_start_time_year
+int16 world_start_time_month
+int16 world_start_time_day
+int16 world_start_time_hour
+int16 world_start_time_minute
+
+# Everything is written in lowercase according to ROS 2 conventions old(v)

dave_interfaces/msg/StratifiedCurrentVelocity.msg

@@ -0,0 +1,9 @@
+# Publishes depths and velocities based on the database contents
+
+std_msgs/Header header
+
+# Depths
+float32[] depths
+
+# Velocities
+geometry_msgs/Vector3[] velocities

dave_interfaces/package.xml

@@ -4,6 +4,7 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="helenamoyen@usp.br">lena</maintainer>
+  <maintainer email="gaurav.og.9920@gmail.com">Gaurav Kumar</maintainer>
   <license>TODO: License declaration</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>

dave_interfaces/srv/GetCurrentModel.srv

@@ -0,0 +1,21 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+---
+float64 mean
+float64 min
+float64 max
+float64 noise
+float64 mu

dave_interfaces/srv/SetCurrentDirection.srv

@@ -0,0 +1,18 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+float64 angle
+---
+bool success

dave_interfaces/srv/SetCurrentModel.srv

@@ -0,0 +1,22 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+float64 mean
+float64 min
+float64 max
+float64 noise
+float64 mu
+---
+bool success

dave_interfaces/srv/SetCurrentVelocity.srv

@@ -0,0 +1,20 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+float64 velocity
+float64 horizontal_angle
+float64 vertical_angle
+---
+bool success

dave_interfaces/srv/SetStratifiedCurrentDirection.srv

@@ -0,0 +1,19 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+uint16 layer
+float64 angle
+---
+bool success

dave_interfaces/srv/SetStratifiedCurrentVelocity.srv

@@ -0,0 +1,21 @@
+# Copyright (c) 2024 The dave Simulator Authors.
+# All rights reserved.
+#
+# 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.
+
+uint16 layer
+float64 velocity
+float64 horizontal_angle
+float64 vertical_angle
+---
+bool success

gazebo/dave_gz_model_plugins/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.8)
+project(dave_gz_model_plugins)
+
+# Find required packages
+find_package(ament_cmake REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(gz-cmake3 REQUIRED)
+find_package(gz-plugin2 REQUIRED COMPONENTS register)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(gz-common5 REQUIRED COMPONENTS profiler)
+find_package(gz-sim8 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(dave_interfaces REQUIRED)
+find_package(Protobuf REQUIRED)
+find_package(gz-msgs10 REQUIRED)
+find_package(dave_gz_world_plugins 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(OceanCurrentModelPlugin SHARED src/OceanCurrentModelPlugin.cc)
+
+target_include_directories(OceanCurrentModelPlugin PRIVATE include)
+
+target_link_libraries(OceanCurrentModelPlugin
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+)
+
+# Specify dependencies for ocean_current_plugin using ament_target_dependencies
+ament_target_dependencies(OceanCurrentModelPlugin
+  rclcpp
+  std_msgs
+  geometry_msgs
+  dave_interfaces
+  dave_gz_world_plugins
+)
+
+# Install targets
+install(TARGETS OceanCurrentModelPlugin
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install headers
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+# Environment hooks
+ament_environment_hooks(
+  "${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()
+
+ament_package()

gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in

@@ -0,0 +1 @@
+prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/

gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh

@@ -0,0 +1,74 @@
+#ifndef DAVE_GZ_MODEL_PLUGINS__OCEANCURRENTMODELPLUGIN_HH_
+#define DAVE_GZ_MODEL_PLUGINS__OCEANCURRENTMODELPLUGIN_HH_
+
+#include "dave_gz_world_plugins/gauss_markov_process.hh"
+#include "dave_gz_world_plugins/tidal_oscillation.hh"
+#include "dave_interfaces/msg/stratified_current_database.hpp"
+#include "dave_interfaces/msg/stratified_current_velocity.hpp"
+
+// OceanCurrentModelPlugin
+
+#include <gz/physics/World.hh>
+#include <gz/sim/System.hh>
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <string>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <gz/transport/Node.hh>
+#include <sdf/sdf.hh>
+
+namespace dave_gz_model_plugins
+{
+class OceanCurrentModelPlugin : public gz::sim::System,
+                                public gz::sim::ISystemConfigure,
+                                public gz::sim::ISystemUpdate,
+                                public gz::sim::ISystemPostUpdate
+{
+public:
+  OceanCurrentModelPlugin();
+  ~OceanCurrentModelPlugin() override;
+
+  // ----------------------------------------------
+
+  void Configure(
+    const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+    gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr);
+
+  void LoadCurrentVelocityParams(sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm);
+
+  void UpdateDatabase(
+    const std::shared_ptr<const dave_interfaces::msg::StratifiedCurrentDatabase> & _msg);
+
+  gz::sim::Entity GetModelEntity(
+    const std::string & modelName, gz::sim::EntityComponentManager & ecm);
+
+  gz::math::Pose3d GetModelPose(
+    const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm);
+
+  // ----------------------------------------------
+
+  void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm);
+
+  void CalculateOceanCurrent(double vehicleDepth);
+
+  // ----------------------------------------------
+
+  // Function called after the simulation state updates
+  void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm);
+
+  void PublishCurrentVelocity(const gz::sim::UpdateInfo & _info);
+
+private:
+  std::shared_ptr<rclcpp::Node> ros_node_;
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_gz_model_plugins
+
+#endif  // DAVE_GZ_MODEL_PLUGINS__OCEANCURRENTMODELPLUGIN_HH_