From 48b556f8f72d730a653f767d2c97f5ec8ce67ab1 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 10 Jul 2024 00:39:31 +0530 Subject: [PATCH 01/79] Added ocean_Current_Plugin at dave_ros_gz_plugins --- .../ocean_current_model_plugin/CMakeLists.txt | 0 .../ocean_current_model_plugin.cpp | 0 .../ocean_current_model_plugin.hh | 249 ++++++++ .../gauss_markov_process/CMakeLists.txt | 0 .../gauss_markov_process.cpp | 0 .../gauss_markov_process.hh | 0 .../msgs/StratifiedCurrentVelocity.proto | 20 + .../ocean_current_world_plugin/CMakeLists.txt | 0 .../ocean_current_world_plugin.cpp | 0 .../ocean_current_world_plugin.hh | 0 .../msg/StratifiedCurrentDatabase.msg | 40 ++ .../msg/StratifiedCurrentVelocity.msg | 10 + .../ocean_current_plugin/CMakeLists.txt | 158 +++++ .../ocean_current_plugin.cpp | 585 ++++++++++++++++++ .../ocean_current_plugin.hh | 107 ++++ .../srv/GetCurrentModel.srv | 21 + .../srv/GetOriginSphericalCoord.srv | 22 + .../srv/SetCurrentDirection.srv | 18 + .../srv/SetCurrentModel.srv | 22 + .../srv/SetCurrentVelocity.srv | 20 + .../srv/SetOriginSphericalCoord.srv | 23 + .../srv/SetStratifiedCurrentDirection.srv | 19 + .../srv/SetStratifiedCurrentVelocity.srv | 21 + .../srv/TransformFromSphericalCoord.srv | 23 + .../srv/TransformToSphericalCoord.srv | 23 + 25 files changed, 1381 insertions(+) create mode 100644 gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt create mode 100644 gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp create mode 100644 gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh create mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt create mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp create mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh create mode 100644 gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto create mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt create mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp create mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh create mode 100644 gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg create mode 100644 gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg create mode 100644 gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt create mode 100755 gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp create mode 100755 gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh create mode 100644 gazebo/dave_ros_gz_plugins/srv/GetCurrentModel.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetCurrentDirection.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetCurrentModel.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetCurrentVelocity.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentDirection.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv create mode 100644 gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh new file mode 100644 index 00000000..853dca51 --- /dev/null +++ b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh @@ -0,0 +1,249 @@ +namespace gz +{ +namespace sim +{ +namespace systems +{ +namespace dave_simulator_ros +{ +class TransientCurrentPlugin : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate +{ + /// \brief Class constructor +public: + TransientCurrentPlugin(); + + /// \brief Class destructor +public: + ~TransientCurrentPlugin(); + + // Documentation inherited +public: + void Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + + // Documentation inherited +public: + void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + + // Documentation inherited +public: + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + + // Documentation inherited. +public: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + // Documentation inherited. +public: + virtual void Init(); + + /// \brief Update the simulation state. + /// \param[in] _info Information used in the update event. +public: + void Update(const common::UpdateInfo & _info); + + /// \brief Update event +protected: + event::ConnectionPtr updateConnection; + + /// \brief Pointer to world +protected: + physics::WorldPtr world; + + /// \brief Pointer to model +protected: + gz::sim::Entity entity{gz::sim::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; + gz::sim::Entity modelLink{gz::sim::kNullEntity}; + + /// \brief Pointer to sdf +protected: + sdf::ElementPtr sdf; + + /// \brief Namespace for topics and services +protected: + std::string ns; + + /// \brief Pointer to this ROS node's handle. +private: + boost::shared_ptr rosNode; + + /// \brief Connects the update event callback +protected: + virtual void Connect(); + + /// \brief Pointer to a node for communication +protected: + transport::NodePtr node; + + /// \brief Map of publishers +protected: + std::map publishers; + + /// \brief Current velocity topic +protected: + std::string currentVelocityTopic; + + /// \brief Publisher for the flow velocity in the world frame +private: + ros::Publisher flowVelocityPub; + + /// \brief transient Ocean current topic +protected: + std::string transientCurrentVelocityTopic; + + /// \brief Subscriber for the transient ocean current database +private: + ros::Subscriber databaseSub; + + /// \brief A ROS callbackqueue that helps process messages +private: + ros::CallbackQueue databaseSubQueue; + + /// \brief A thread the keeps running the rosQueue +private: + std::thread databaseSubQueueThread; + + /// \brief A thread mutex to lock +private: + std::mutex lock_; + + /// \brief Period after which we should publish a message via ROS. +private: + std::chrono::steady_clock::duration rosPublishPeriod{0}; + + /// \brief Last time we published a message via ROS. +private: + std::chrono::steady_clock::duration lastRosPublishTime{0}; + + /// \brief Convey model state from gazebo topic to outside +protected: + virtual void UpdateDatabase( + const dave_gazebo_ros_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); + + /// \brief ROS helper function that processes messages +private: + void databaseSubThread(); + + /// \brief Last update time stamp +protected: + common::Time lastUpdate; + + /// \brief Last depth index +protected: + int lastDepthIndex; + + /// \brief Calculate ocean current using database and vehicle state +private: + void CalculateOceanCurrent(); + + /// \brief Current linear velocity vector +protected: + ignition::math::Vector3d currentVelocity; + + /// \brief Gauss-Markov process instance for the velocity north +protected: + GaussMarkovProcess currentVelNorthModel; + + /// \brief Gauss-Markov process instance for the velocity east +protected: + GaussMarkovProcess currentVelEastModel; + + /// \brief Gauss-Markov process instance for the velocity down +protected: + GaussMarkovProcess currentVelDownModel; + + /// \brief Gauss-Markov noise +protected: + double noiseAmp_North; + +protected: + double noiseAmp_East; + +protected: + double noiseAmp_Down; + +protected: + double noiseFreq_North; + +protected: + double noiseFreq_East; + +protected: + double noiseFreq_Down; + + /// \brief Publish ocean current +private: + void PublishCurrentVelocity(); + + /// \brief Vector to read database +protected: + std::vector database; + + /// \brief Tidal Oscillation interpolation model +protected: + TidalOscillation tide; + + /// \brief Tidal Oscillation flag +protected: + bool tideFlag; + + /// \brief Vector to read timeGMT +protected: + std::vector> timeGMT; + + /// \brief Vector to read tideVelocities +protected: + std::vector tideVelocities; + + /// \brief Tidal current harmonic constituents +protected: + bool tide_Constituents; + +protected: + double M2_amp; + +protected: + double M2_phase; + +protected: + double M2_speed; + +protected: + double S2_amp; + +protected: + double S2_phase; + +protected: + double S2_speed; + +protected: + double N2_amp; + +protected: + double N2_phase; + +protected: + double N2_speed; + + /// \brief Tidal oscillation mean ebb direction +protected: + double ebbDirection; + + /// \brief Tidal oscillation mean flood direction +protected: + double floodDirection; + + /// \brief Tidal oscillation world start time (GMT) +protected: + std::array world_start_time; +} +} // namespace dave_simulator_ros +} // namespace systems +} // namespace sim +} // namespace gz diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt b/gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp b/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh b/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto b/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto new file mode 100644 index 00000000..dc744084 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto @@ -0,0 +1,20 @@ +syntax = "proto3"; +package dave_gazebo_world_plugins_msgs.msgs; +import "vector3d.proto"; // Ensure vector3d.proto is compatible with proto3 + +message StratifiedCurrentVelocity { + repeated gazebo.msgs.Vector3d velocity = 1; + repeated double depth = 2; +} + + +// syntax = "proto2"; +// package dave_gazebo_world_plugins_msgs.msgs; +// import "vector3d.proto"; +// import "any.proto"; + +// message StratifiedCurrentVelocity +// { +// repeated gazebo.msgs.Vector3d velocity = 1; +// repeated double depth = 2; +// } \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh new file mode 100644 index 00000000..e69de29b diff --git a/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg b/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg new file mode 100644 index 00000000..97c8eac4 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg @@ -0,0 +1,40 @@ +# Publishes depths and velocities read from the csv database + +# Depths +float32[] depths + +# Velocities +geometry_msgs/Vector3[] velocities + +# Tide time (GMT) +int16[] timeGMTYear +int16[] timeGMTMonth +int16[] timeGMTDay +int16[] timeGMTHour +int16[] timeGMTMinute + +# Tide velocities +float32[] tideVelocities + +# Tide constituents +bool tideConstituents +float32 M2amp +float32 M2phase +float32 M2speed +float32 S2amp +float32 S2phase +float32 S2speed +float32 N2amp +float32 N2phase +float32 N2speed + +# Tide direction +float32 ebbDirection +float32 floodDirection + +# World start time (GMT) +int16 worldStartTimeYear +int16 worldStartTimeMonth +int16 worldStartTimeDay +int16 worldStartTimeHour +int16 worldStartTimeMinute \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg b/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg new file mode 100644 index 00000000..997787bd --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg @@ -0,0 +1,10 @@ +# Publishes depths and velocities based on the database contents + +std_msgs/Header header + +# Depths +float32[] depths + +# Velocities +geometry_msgs/Vector3[] velocities + diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt new file mode 100644 index 00000000..a0e3be04 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt @@ -0,0 +1,158 @@ +# might remove this from here and put it in the main CMakeLists.txt +cmake_minimum_required(VERSION 3.0.2) +project(dave_gazebo_ros_plugins) + +if(NOT "${CMAKE_VERSION}" VERSION_LESS "3.16") + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +else() + add_compile_options(-std=c++11) +endif() + +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + roscpp + gazebo_msgs + geometry_msgs + dave_gz_world_plugins + message_generation + std_msgs +) +find_package(Boost REQUIRED COMPONENTS system) + +set(DAVE_GAZEBO_ROS_PLUGINS_LIST "") + +# ocean current msg and services +add_service_files( + FILES + SetCurrentModel.srv + GetCurrentModel.srv + SetCurrentVelocity.srv + SetCurrentDirection.srv + SetStratifiedCurrentVelocity.srv + SetStratifiedCurrentDirection.srv + SetOriginSphericalCoord.srv + GetOriginSphericalCoord.srv + TransformToSphericalCoord.srv + TransformFromSphericalCoord.srv +) + +add_message_files( + FILES + StratifiedCurrentDatabase.msg + StratifiedCurrentVelocity.msg +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + dave_ocean_current_plugin + dave_sc_coordinates_interface + CATKIN_DEPENDS + roscpp + gazebo_msgs + geometry_msgs + dave_gazebo_world_plugins + gazebo_dev + std_msgs + geometry_msgs + message_runtime +) + +include_directories(${PROJECT_SOURCE_DIR}/include + ${Boost_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ${GAZEBO_MSG_INCLUDE_DIRS} +# ... +) + +link_directories(${catkin_LIBRARY_DIRS} + ${GAXEBO_LIBRARY_DIRS} +# ... +) + +# ocean current plugin +add_library(dave_ocean_current_plugin + SHARED + src/ocean_current_plugin.cpp +) +add_dependencies(dave_ocean_current_plugin + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(dave_ocean_current_plugin + ${catkin_LIBRARIES} +) +list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_ocean_current_plugin) + +# pulse lidar plugin +add_library(dave_pulse_lidar_plugin + SHARED + src/pulse_lidar_plugin.cpp +) +add_dependencies(dave_pulse_lidar_plugin + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(dave_pulse_lidar_plugin + ${catkin_LIBRARIES} +) +list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_pulse_lidar_plugin) + +# spherical coordinate interfafce +add_library(dave_sc_coordinates_interface + SHARED + src/spherical_coordinates_interface.cpp +) +add_dependencies(dave_sc_coordinates_interface + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(dave_sc_coordinates_interface + ${catkin_LIBRARIES} +) +list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_sc_coordinates_interface) + +# Plugin compile directions + +#add_library(plugin_name +# SHARED +# src/plugin_source_file1.cc +# src/plugin_source_file2.cc +# ... +#) +#add_dependencies(plugin_name +# ${catkin_EXPORTED_TARGETS} +# ... +#) +#target_link_libraries(plugin_name +# ${catkin_LIBRARIES} +# ... +#) +#list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST plugin_name) + +# Plugin install directions + +install(TARGETS ${DAVE_GAZEBO_ROS_PLUGINS_LIST} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN "*~" EXCLUDE +) + +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN ".h" +) + diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp new file mode 100755 index 00000000..4b142e4c --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp @@ -0,0 +1,585 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +// #include "dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.hpp" +// #include "dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.hpp" +// #include "dave_ros_gz_plugins/srv/SetCurrentModel.hpp" +// #include "dave_ros_gz_plugins/srv/GetCurrentModel.hpp" +// #include "dave_ros_gz_plugins/srv/SetCurrentVelocity.hpp" +// #include "dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.hpp" +// #include "dave_ros_gz_plugins/srv/SetCurrentAngle.hpp" +// #include "dave_ros_gz_plugins/srv/SetStratifiedCurrentAngle.hpp" + +// Things to consider/decide for now - +// 1. Is consistency being maintained while the use of "gzerr" and "RCLCPP_ERROR" for error logging +// ? +// 2. Do the Headers included in the code exist ? +// 3. use of gz_bridge and message types. +// 4. Integration with the rest of the system with respect to the use of smart pointers and other +// parameters like behaviour. + +using namespace gz; +using namespace sim; +using namespace systems; +using namespace dave_simulator_ros; + +class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin +{ +public: + UnderwaterCurrentROSPlugin(); + ~UnderwaterCurrentROSPlugin(); + +private: + rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating + // communication with other ROS nodes. + rclcpp::TimerBase::SharedPtr rosPublishTimer; + gz::common::Time rosPublishPeriod; + gz::common::Time lastRosPublishTime; + std::string db_path; +}; + +///////////////////////////////////////////////// +UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() +{ + this->rosPublishPeriod = gz::common::Time(0.05); + this->lastRosPublishTime = gz::common::Time(0.0); + this->db_path = + ament_index_cpp::get_package_share_directory("dave_worlds"); // might need to be (updated) + + // Initialize the ROS 2 node + this->rosNode = std::make_shared("underwater_current_ros_plugin"); + + // Setting up a timer in ROS 2 + this->rosPublishTimer = this->rosNode->create_wall_timer( + std::chrono::milliseconds(static_cast(this->rosPublishPeriod.Double() * 1000)), + [this]() { this->PublishCurrentInfo(); }); +} + +///////////////////////////////////////////////// +// this can be removed as the connections are now handled by samrt pointers (to be (updated)) +UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() +{ + this->rosNode.reset(); // Properly handle ROS 2 node shutdown +} + +///////////////////////////////////////////////// +void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr _sdf) +{ // make sure this is closed + try + { + UnderwaterCurrentPlugin::Load(_world, _sdf); + this->stratifiedCurrentVelocityDatabaseTopic = + this->stratifiedCurrentVelocityTopic + "_database"; + } + catch (const gz::common::Exception & e) // this is a gazebo error hence -gzerr, check for "_e" or + // "e" (to be (updated)) + // findings- we can use both "_e" and "e" but "e" is now + // the conventional way to display errors in C++. further + // writing "&e" or "& e" is a personal preference. + { + gzerr << "Error loading plugin: " << e.what() << '\n'; + return; + } + + if (!rclcpp::ok()) // this is a ROS 2 error hence -RCLCPP_ERROR to be (considered) (updated) + { + gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your " + "ROS 2 environment.\n"; + return; + } + + this->ns = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + this->rosNode = std::make_shared("underwater_current_ros_plugin", this->ns); + + // Create and advertise Messages + // Advertise the flow velocity as a stamped twist message + this->flowVelocityPub = this->rosNode->create_publisher( + this->currentVelocityTopic, rclcpp::QoS(10)); + + // Advertise the stratified ocean current message + this->stratifiedCurrentVelocityPub = + this->rosNode->create_publisher( + this->stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); + + // Advertise the stratified ocean current database message + this->stratifiedCurrentDatabasePub = + this->rosNode->create_publisher( + this->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); + + // Create and advertise services + // In this setup : std::placeholders::_1 and std::placeholders::_2 are used to represent the + // request and response objects that the service callback expects to receive. This allows the + // serviceCallback method of to be used directly as a ROS 2 service handler without + // having to wrap it in another function that manually passes the request and response. + + // check for the number of arg being passed to the function + + // Advertise the service to update the current velocity model + this->worldServices["set_current_velocity_model"] = + this->rosNode->create_service( + "set_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to get the current velocity model + this->worldServices["get_current_velocity_model"] = + this->rosNode->create_service( + "get_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle model + this->worldServices["set_current_horz_angle_model"] = + this->rosNode->create_service( + "set_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to get the current horizontal angle model + this->worldServices["get_current_horz_angle_model"] = + this->rosNode->create_service( + "get_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current vertical angle model + this->worldServices["set_current_vert_angle_model"] = + this->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to get the current vertical angle model + this->worldServices["get_current_vert_angle_model"] = + this->rosNode->create_service( + "get_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current velocity mean value + this->worldServices["set_current_velocity"] = + this->rosNode->create_service( + "set_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current velocity + this->worldServices["set_stratified_current_velocity"] = + this->rosNode->create_service( + "set_stratified_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle + this->worldServices["set_current_horz_angle"] = + this->rosNode->create_service( + "set_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current horizontal angle + this->worldServices["set_stratified_current_horz_angle"] = + this->rosNode->create_service( + "set_stratified_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, + this, std::placeholders::_1, std::placeholders::_2)); + // Advertise the service to update the current vertical angle + this->worldServices["set_current_vert_angle"] = + this->rosNode->create_service( + "set_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current vertical angle + this->worldServices["set_stratified_current_vert_angle"] = + this->rosNode->create_service( + "set_stratified_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, + this, std::placeholders::_1, std::placeholders::_2)); + + // Connect to the Gazebo Harmonic update event + this->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & info) + { this->OnUpdateCurrentVel(info); }); +} + +///////////////////////////////////////////////// +void UnderwaterCurrentROSPlugin::OnUpdateCurrentVel() +{ + if (this->lastUpdate - this->lastRosPublishTime >= this->rosPublishPeriod) + { + // Update the time tracking for publication + this->lastRosPublishTime = this->lastUpdate; + + // Generate and publish current velocity according to the vehicle depth + geometry_msgs::msg::TwistStamped flowVelMsg; + flowVelMsg.header.stamp = this->rosNode->get_clock()->now(); + flowVelMsg.header.frame_id = + "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions + + flowVelMsg.twist.linear.x = this->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->currentVelocity.Z(); + + this->flowVelocityPub->publish(flowVelMsg); + + // Generate and publish stratified current velocity + dave_ros_gz_plugins::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; + stratCurrentVelocityMsg.header.stamp = this->rosNode->get_clock()->now(); + stratCurrentVelocityMsg.header.frame_id = "world"; + + // Updating for stratified behaviour of Ocean Currents + // What is the .size value over here, to be (updated)(checked) + for (size_t i = 0; i < this->currentStratifiedVelocity.size(); i++) + { + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->currentStratifiedVelocity[i].X(); + velocity.y = this->currentStratifiedVelocity[i].Y(); + velocity.z = this->currentStratifiedVelocity[i].Z(); + stratCurrentVelocityMsg.velocities.push_back(velocity); + stratCurrentVelocityMsg.depths.push_back(this->currentStratifiedVelocity[i].W()); + } + // updated till here + this->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); + + // Generate and publish stratified current database + dave_ros_gz_plugins::msg::StratifiedCurrentDatabase currentDatabaseMsg; + for (int i = 0; i < this->stratifiedDatabase.size(); i++) + { + // Stratified current database entry preparation + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->stratifiedDatabase[i].X(); + velocity.y = this->stratifiedDatabase[i].Y(); + velocity.z = 0.0; // Assuming z is intentionally set to 0.0 + currentDatabaseMsg.velocities.push_back(velocity); + currentDatabaseMsg.depths.push_back(this->stratifiedDatabase[i].Z()); + } + + if (this->tidalHarmonicFlag) + { + // Tidal harmonic constituents + currentDatabaseMsg.M2amp = this->M2_amp; + currentDatabaseMsg.M2phase = this->M2_phase; + currentDatabaseMsg.M2speed = this->M2_speed; + currentDatabaseMsg.S2amp = this->S2_amp; + currentDatabaseMsg.S2phase = this->S2_phase; + currentDatabaseMsg.S2speed = this->S2_speed; + currentDatabaseMsg.N2amp = this->N2_amp; + currentDatabaseMsg.N2phase = this->N2_phase; + currentDatabaseMsg.N2speed = this->N2_speed; + currentDatabaseMsg.tideConstituents = true; + } + else + { + for (int i = 0; i < this->dateGMT.size(); i++) + { + // Tidal oscillation database + currentDatabaseMsg.timeGMTYear.push_back(this->dateGMT[i][0]); + currentDatabaseMsg.timeGMTMonth.push_back(this->dateGMT[i][1]); + currentDatabaseMsg.timeGMTDay.push_back(this->dateGMT[i][2]); + currentDatabaseMsg.timeGMTHour.push_back(this->dateGMT[i][3]); + currentDatabaseMsg.timeGMTMinute.push_back(this->dateGMT[i][4]); + + currentDatabaseMsg.tideVelocities.push_back(this->speedcmsec[i]); + } + currentDatabaseMsg.tideConstituents = false; + } + + currentDatabaseMsg.ebbDirection = this->ebbDirection; + currentDatabaseMsg.floodDirection = this->floodDirection; + + currentDatabaseMsg.worldStartTimeYear = this->world_start_time_year; + currentDatabaseMsg.worldStartTimeMonth = this->world_start_time_month; + currentDatabaseMsg.worldStartTimeDay = this->world_start_time_day; + currentDatabaseMsg.worldStartTimeHour = this->world_start_time_hour; + currentDatabaseMsg.worldStartTimeMinute = this->world_start_time_minute; + + this->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); + } +} +// updated till here + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->currentHorzAngleModel.SetMean(_req->angle); + + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + + _res->success = this->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + if (_res->success) + { + // Update the database values (new angle, unchanged velocity) + double velocity = + hypot(this->stratifiedDatabase[_req->layer].X(), this->stratifiedDatabase[_req->layer].Y()); + this->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + this->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + } + return true; +} +// -updated till here td +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->currentVertAngleModel.SetMean(_req->angle); + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + _res->success = this->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if ( + this->currentVelModel.SetMean(_req->velocity) && + this->currentHorzAngleModel.SetMean(_req->horizontal_angle) && + this->currentVertAngleModel.SetMean(_req->vertical_angle)) + { + RCLCPP_INFO( + this->rosNode->get_logger(), + "Current velocity [m/s] = %f\nCurrent horizontal angle [rad] = %f\nCurrent vertical angle " + "[rad] = %f\n\tWARNING: Current velocity calculated in the ENU frame", + _req->velocity, _req->horizontal_angle, _req->vertical_angle); + _res->success = true; + } + else + { + RCLCPP_ERROR(this->rosNode->get_logger(), "Error while updating the current velocity"); + _res->success = false; + } + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + if ( + this->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + this->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + this->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + { + // Update the database values as well + this->stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; + this->stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; + RCLCPP_INFO( + this->rosNode->get_logger(), + "Layer %d current velocity [m/s] = %f\n Horizontal angle [rad] = %f\n Vertical angle [rad] " + "= %f\n\tWARNING: Current velocity calculated in the ENU frame", + _req->layer, _req->velocity, _req->horizontal_angle, _req->vertical_angle); + _res->success = true; + } + else + { + RCLCPP_ERROR(this->rosNode->get_logger(), "Error while updating the current velocity"); + _res->success = false; + } + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->currentVelModel.mean; + _res->min = this->currentVelModel.min; + _res->max = this->currentVelModel.max; + _res->noise = this->currentVelModel.noiseAmp; + _res->mu = this->currentVelModel.mu; + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->currentHorzAngleModel.mean; + _res->min = this->currentHorzAngleModel.min; + _res->max = this->currentHorzAngleModel.max; + _res->noise = this->currentHorzAngleModel.noiseAmp; + _res->mu = this->currentHorzAngleModel.mu; + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->currentVertAngleModel.mean; + _res->min = this->currentVertAngleModel.min; + _res->max = this->currentVertAngleModel.max; + _res->noise = this->currentVertAngleModel.noiseAmp; + _res->mu = this->currentVertAngleModel.mu; + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->currentVelModel.SetModel( + std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, + _req->noise); + + for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) + { + gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][0]; + model.SetModel( + model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); + } + + RCLCPP_INFO( + this->rosNode->get_logger(), + "Current velocity model updated\n\tWARNING: Current velocity calculated in the ENU frame"); + + // Assuming currentVelModel.Print() prints model details, you would instead log these details + // For instance, you could log mean, min, max etc. + // Example of logging additional model details: + RCLCPP_INFO( + this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f", + this->currentVelModel.mean, this->currentVelModel.min, this->currentVelModel.max); + + return true; + + // gzmsg << "Current velocity model updated" << std::endl + // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + // this->currentVelModel.Print(); + // return true; +} +// -updated till here td +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = + this->currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + + for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) + { + gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][1]; + model.SetModel( + model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); + } + + RCLCPP_INFO( + this->rosNode->get_logger(), + "Horizontal angle model updated\n\tWARNING: Current velocity calculated in the ENU frame"); + + // Assuming currentHorzAngleModel.Print() prints model details, you would instead log these + // details Example of logging additional model details: + RCLCPP_INFO( + this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f", + this->currentHorzAngleModel.mean, this->currentHorzAngleModel.min, + this->currentHorzAngleModel.max); + + return true; + + // gzmsg << "Horizontal angle model updated" << std::endl + // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + // this->currentHorzAngleModel.Print(); +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = + this->currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + + RCLCPP_INFO( + this->rosNode->get_logger(), + "Vertical angle model updated\n\tWARNING: Current velocity calculated in the ENU frame"); + + // Assuming currentVertAngleModel.Print() prints model details, you would instead log these + // details Example of logging additional model details: + RCLCPP_INFO( + this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f, Mu: %f, Noise: %f", + this->currentVertAngleModel.mean, this->currentVertAngleModel.min, + this->currentVertAngleModel.max, this->currentVertAngleModel.mu, + this->currentVertAngleModel.noiseAmp); + return true; + // gzmsg << "Vertical angle model updated" << std::endl + // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + // this->currentVertAngleModel.Print(); +} + +///////////////////////////////////////////////// +GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) + +// #endif \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh new file mode 100755 index 00000000..c9d31f21 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh @@ -0,0 +1,107 @@ +// 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. + +/// \file ocean_current_plugin.hh +/// \brief Publishes the ocean current velocity in ROS messages and cxreates a +/// service to alter the flow model in runtime + +#ifndef OCEAN_CURRENT_PLUGIN_H_ +#define OCEAN_CURRENT_PLUGIN_H_ + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +// using namespace gz; +// using namespace sim; +// using namespace systems; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE // can remove this line if gazebo. +{ +namespace systems +{ +namespace dave_simulator_ros +{ + +class UnderwaterCurrentROSPlugin : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate +{ + /// \brief Class constructor +public: + explicit UnderwaterCurrentROSPlugin(); + + /// \brief Class destructor +public: + ~UnderwaterCurrentROSPlugin(); + + // Documentation inherited +public: + void Configure( + const Entity & _entity, const std::shared_ptr & _sdf, + EntityComponentManager & _ecm, EventManager & _eventMgr) override; + + // Documentation inherited +public: + void PreUpdate(const UpdateInfo & _info, EntityComponentManager & _ecm) override; + + // Documentation inherited +public: + void PostUpdate(const UpdateInfo & _info, const EntityComponentManager & _ecm) override; +} +} // namespace dave_simulator_ros +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +// Register plugin +GZ_ADD_PLUGIN( + UnderwaterCurrentROSPlugin, gz::sim::System, UnderwaterCurrentROSPlugin::ISystemConfigure, + UnderwaterCurrentROSPlugin::ISystemPostUpdate) + +// Add plugin alias so that we can refer to the plugin without the version +// namespace +GZ_ADD_PLUGIN_ALIAS(UnderwaterCurrentROSPlugin, "gz::sim::systems::UnderwaterCurrentROSPlugin") + +#endif // OCEAN_CURRENT_PLUGIN_H_ diff --git a/gazebo/dave_ros_gz_plugins/srv/GetCurrentModel.srv b/gazebo/dave_ros_gz_plugins/srv/GetCurrentModel.srv new file mode 100644 index 00000000..2f6d7d7e --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv new file mode 100644 index 00000000..70f3aafc --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.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. + +--- +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentDirection.srv b/gazebo/dave_ros_gz_plugins/srv/SetCurrentDirection.srv new file mode 100644 index 00000000..03c09e88 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentModel.srv b/gazebo/dave_ros_gz_plugins/srv/SetCurrentModel.srv new file mode 100644 index 00000000..b9538295 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentVelocity.srv b/gazebo/dave_ros_gz_plugins/srv/SetCurrentVelocity.srv new file mode 100644 index 00000000..52ee6088 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv new file mode 100644 index 00000000..80791954 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv @@ -0,0 +1,23 @@ +# 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. + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude +--- +bool success diff --git a/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentDirection.srv b/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentDirection.srv new file mode 100644 index 00000000..a6a5957e --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.srv b/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.srv new file mode 100644 index 00000000..c10a8030 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/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 diff --git a/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv new file mode 100644 index 00000000..6f4da710 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv @@ -0,0 +1,23 @@ +# 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. + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude +--- +geometry_msgs/Vector3 output diff --git a/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv new file mode 100644 index 00000000..3ad501f5 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv @@ -0,0 +1,23 @@ +# 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. + +geometry_msgs/Vector3 input +--- +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude From 9a802425bb8559bd2b2030160c7c7338ff253495 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 19 Jul 2024 20:14:48 +0530 Subject: [PATCH 02/79] added the world, model and general plugin for water_current along with updated Tidal oscillations and gauss markov process --- gazebo/dave_gz_model_plugin/CMakeLists.txt | 64 ++ .../hooks/dave_ros_gz_plugins.dsv.in | 1 + .../ocean_current_model_plugin.hh | 227 +++--- .../ocean_current_model_plugin/CMakeLists.txt | 0 .../ocean_current_model_plugin.cpp | 0 gazebo/dave_gz_model_plugin/package.xml | 30 + .../src/ocean_current_model_plugin.cpp | 542 +++++++++++++++ gazebo/dave_gz_world_plugins/CMakeLists.txt | 53 ++ .../gauss_markov_process/CMakeLists.txt | 0 .../gauss_markov_process.cpp | 0 .../gauss_markov_process.hh | 0 .../hooks/dave_ros_gz_plugins.dsv.in | 1 + .../include/gauss_markov_process.hh | 104 +++ .../include/ocean_current_world_plugin.hh | 212 ++++++ .../include/tidal_oscillation.hh | 122 ++++ .../msgs/StratifiedCurrentVelocity.proto | 20 - .../ocean_current_world_plugin/CMakeLists.txt | 0 .../ocean_current_world_plugin.cpp | 0 .../ocean_current_world_plugin.hh | 0 gazebo/dave_gz_world_plugins/package.xml | 24 + .../src/gauss_markov_process.cpp | 108 +++ .../src/ocean_current_world_plugin.cpp | 647 ++++++++++++++++++ .../src/tidal_oscillation.cpp | 182 +++++ gazebo/dave_ros_gz_plugins/CMakeLists.txt | 33 +- .../ocean_current_plugin.hh | 37 +- .../ocean_current_plugin/CMakeLists.txt | 158 ----- gazebo/dave_ros_gz_plugins/package.xml | 23 +- .../ocean_current_plugin.cpp | 336 +++++---- 28 files changed, 2392 insertions(+), 532 deletions(-) create mode 100644 gazebo/dave_gz_model_plugin/CMakeLists.txt create mode 100644 gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in rename gazebo/dave_gz_model_plugin/{ocean_current_model_plugin => include}/ocean_current_model_plugin.hh (62%) delete mode 100644 gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt delete mode 100644 gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp create mode 100644 gazebo/dave_gz_model_plugin/package.xml create mode 100644 gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp create mode 100644 gazebo/dave_gz_world_plugins/CMakeLists.txt delete mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt delete mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp delete mode 100644 gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh create mode 100644 gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in create mode 100644 gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh create mode 100644 gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh create mode 100644 gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh delete mode 100644 gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto delete mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt delete mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp delete mode 100644 gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh create mode 100644 gazebo/dave_gz_world_plugins/package.xml create mode 100644 gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp create mode 100644 gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp create mode 100644 gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp rename gazebo/dave_ros_gz_plugins/{ocean_current_plugin => include}/ocean_current_plugin.hh (67%) mode change 100755 => 100644 delete mode 100644 gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt rename gazebo/dave_ros_gz_plugins/{ocean_current_plugin => src}/ocean_current_plugin.cpp (76%) diff --git a/gazebo/dave_gz_model_plugin/CMakeLists.txt b/gazebo/dave_gz_model_plugin/CMakeLists.txt new file mode 100644 index 00000000..1dfb8f2e --- /dev/null +++ b/gazebo/dave_gz_model_plugin/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) + +project(dave_gz_model_plugins) + +find_package(ament_cmake REQUIRED) +find_package(dave_gz_model_plugins REQUIRED) + +find_package(ros_gz_example_description 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) + +# Set version variables with gazebo Harmonic +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) +set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + +message(STATUS "Compiling against Gazebo Harmonic") + +add_library(dave_gz_model_plugins SHARED + src/ocean_current_model_plugin.cpp +) + +target_include_directories(ocean_current_model_plugin PRIVATE include) + +target_link_libraries(ocean_current_model_plugin + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + + +# Specify dependencies for ocean_current_plugin using ament_target_dependencies +ament_target_dependencies(ocean_current_model_plugin + "rclcpp" + "std_msgs" + "geometry_msgs" + "dave_interfaces" +) + +# Install targets +install(TARGETS ocean_current_model_plugin + DESTINATION lib/${PROJECT_NAME} +) + +# Install headers +install(DIRECTORY include/ + DESTINATION include/ +) + +# # Following directives are used when testing. +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +# endif() + +# Environment Hooks +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") + +ament_package() + diff --git a/gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in new file mode 100644 index 00000000..5e60e0e3 --- /dev/null +++ b/gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh similarity index 62% rename from gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh rename to gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh index 853dca51..f189447e 100644 --- a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh @@ -1,249 +1,196 @@ +#ifndef OCEAN_CURRENT_MODEL_PLUGIN_H_ +#define OCEAN_CURRENT_MODEL_PLUGIN_H_ + +#include +#include +// #include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + namespace gz { namespace sim { namespace systems { -namespace dave_simulator_ros +namespace dave_gz_sensor_plugins { class TransientCurrentPlugin : public System, public ISystemConfigure, public ISystemPreUpdate, - public ISystemPostUpdate + public ISystemUpdate, + public ISystemPostUpdate, + public ModelPlugin { /// \brief Class constructor public: - TransientCurrentPlugin(); - - /// \brief Class destructor -public: - ~TransientCurrentPlugin(); + TransientCurrentPlugin(); // constructor + ~TransientCurrentPlugin() override; // Destructor - // Documentation inherited -public: + // Configure the plugin with necessary entities and component managers void Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - // Documentation inherited -public: + // Function called before the simulation state updates void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - // Documentation inherited -public: + // Function called after the simulation state updates void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - // Documentation inherited. -public: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; - // Documentation inherited. -public: + // Initialize any necessary states before the plugin starts virtual void Init(); - /// \brief Update the simulation state. - /// \param[in] _info Information used in the update event. -public: - void Update(const common::UpdateInfo & _info); - - /// \brief Update event -protected: - event::ConnectionPtr updateConnection; - /// \brief Pointer to world protected: physics::WorldPtr world; /// \brief Pointer to model -protected: gz::sim::Entity entity{gz::sim::kNullEntity}; gz::sim::Model model{gz::sim::kNullEntity}; gz::sim::Entity modelLink{gz::sim::kNullEntity}; - /// \brief Pointer to sdf -protected: - sdf::ElementPtr sdf; - /// \brief Namespace for topics and services -protected: std::string ns; - /// \brief Pointer to this ROS node's handle. -private: - boost::shared_ptr rosNode; - /// \brief Connects the update event callback -protected: virtual void Connect(); /// \brief Pointer to a node for communication -protected: - transport::NodePtr node; + gz::transport::NodePtr node; /// \brief Map of publishers -protected: std::map publishers; - /// \brief Current velocity topic -protected: + /// \brief Current velocity topic and transient current velocity topic std::string currentVelocityTopic; - - /// \brief Publisher for the flow velocity in the world frame -private: - ros::Publisher flowVelocityPub; - - /// \brief transient Ocean current topic -protected: std::string transientCurrentVelocityTopic; - /// \brief Subscriber for the transient ocean current database -private: - ros::Subscriber databaseSub; - - /// \brief A ROS callbackqueue that helps process messages -private: - ros::CallbackQueue databaseSubQueue; - - /// \brief A thread the keeps running the rosQueue -private: - std::thread databaseSubQueueThread; - - /// \brief A thread mutex to lock -private: - std::mutex lock_; - - /// \brief Period after which we should publish a message via ROS. -private: - std::chrono::steady_clock::duration rosPublishPeriod{0}; - - /// \brief Last time we published a message via ROS. -private: - std::chrono::steady_clock::duration lastRosPublishTime{0}; - /// \brief Convey model state from gazebo topic to outside -protected: - virtual void UpdateDatabase( - const dave_gazebo_ros_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); - /// \brief ROS helper function that processes messages -private: - void databaseSubThread(); + virtual void UpdateDatabase( + const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); /// \brief Last update time stamp -protected: common::Time lastUpdate; /// \brief Last depth index -protected: int lastDepthIndex; - /// \brief Calculate ocean current using database and vehicle state -private: - void CalculateOceanCurrent(); - /// \brief Current linear velocity vector -protected: - ignition::math::Vector3d currentVelocity; + gz::math::Vector3d currentVelocity; - /// \brief Gauss-Markov process instance for the velocity north -protected: + /// \brief Gauss-Markov process instance for the velocity components GaussMarkovProcess currentVelNorthModel; - - /// \brief Gauss-Markov process instance for the velocity east -protected: GaussMarkovProcess currentVelEastModel; - - /// \brief Gauss-Markov process instance for the velocity down -protected: GaussMarkovProcess currentVelDownModel; /// \brief Gauss-Markov noise -protected: double noiseAmp_North; - -protected: double noiseAmp_East; - -protected: double noiseAmp_Down; - -protected: double noiseFreq_North; - -protected: double noiseFreq_East; - -protected: double noiseFreq_Down; - /// \brief Publish ocean current -private: - void PublishCurrentVelocity(); - /// \brief Vector to read database -protected: - std::vector database; + std::vector database; /// \brief Tidal Oscillation interpolation model -protected: TidalOscillation tide; /// \brief Tidal Oscillation flag -protected: bool tideFlag; /// \brief Vector to read timeGMT -protected: std::vector> timeGMT; /// \brief Vector to read tideVelocities -protected: std::vector tideVelocities; /// \brief Tidal current harmonic constituents -protected: bool tide_Constituents; - -protected: double M2_amp; - -protected: double M2_phase; - -protected: double M2_speed; - -protected: double S2_amp; - -protected: double S2_phase; - -protected: double S2_speed; - -protected: double N2_amp; - -protected: double N2_phase; - -protected: double N2_speed; /// \brief Tidal oscillation mean ebb direction -protected: double ebbDirection; /// \brief Tidal oscillation mean flood direction -protected: double floodDirection; /// \brief Tidal oscillation world start time (GMT) -protected: std::array world_start_time; + + /// \brief Private data pointer +private: + std::unique_ptr dataPtr; + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr flowVelocityPub; + +private: + /// \brief Pointer to this ROS node's handle. + std::shared_ptr rosNode; + + /// \brief Publisher for the flow velocity in the world frame + rclcpp::Publisher flowVelocityPub; + + /// \brief Subscriber for the transient ocean current database + rclcpp::Subscriber databaseSub; + + /// \brief A ROS callbackqueue that helps process messages + rclcpp::CallbackQueue databaseSubQueue; + + /// \brief A thread the keeps running the rosQueue + std::thread databaseSubQueueThread; + + /// \brief A thread mutex to lock + std::mutex lock_; + + /// \brief Period after which we should publish a message via ROS. + std::chrono::steady_clock::duration rosPublishPeriod{0}; + + /// \brief Last time we published a message via ROS. + std::chrono::steady_clock::duration lastRosPublishTime{0}; + + /// \brief ROS helper function that processes messages + void databaseSubThread(); + + /// \brief Calculate ocean current using database and vehicle state + void CalculateOceanCurrent(); + + /// \brief Publish ocean current + void PublishCurrentVelocity(); } -} // namespace dave_simulator_ros +} // namespace dave_gz_sensor_plugins } // namespace systems } // namespace sim } // namespace gz diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/CMakeLists.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugin/ocean_current_model_plugin/ocean_current_model_plugin.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_model_plugin/package.xml b/gazebo/dave_gz_model_plugin/package.xml new file mode 100644 index 00000000..2c12b233 --- /dev/null +++ b/gazebo/dave_gz_model_plugin/package.xml @@ -0,0 +1,30 @@ + + + dave_gz_model_plugins + 3.1.1 + The dave_gz_model_plugins package (gz model plugin definitions) + Gaurav Kumar and woensug + Gaurav Kumar and woensug + Apache 2.0 + catkin + protobuf-dev + gz_ros + rclcpp + protobuf + message_generation + gz_ros + protobuf + message_generation + gz_ros + rclcpp + protobuf + message_runtime + + gz_dev + gz_msgs + dave_gz_world_plugins + dave_gz_ros_plugins + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp new file mode 100644 index 00000000..122e8c06 --- /dev/null +++ b/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp @@ -0,0 +1,542 @@ +// Copyright (c) 2016 The UUV 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. + +/// \file ocean_current_model_plugin.cc + +#include "ocean_current_model_plugin.hh" +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gz/common/StringUtils.hh" +#include "gz/plugin/Register.hh" + +#include +#include +#include +#include +#include +#include + +// Register plugin +GZ_ADD_PLUGIN( + gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin, gz::sim::System, + gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemConfigure, + gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemUpdate, + gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemPostUpdate) + +// Add plugin alias so that we can refer to the plugin without the version +// namespace +GZ_ADD_PLUGIN_ALIAS( + gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin, "TransientCurrentPlugin") + +using namespace gz; +using namespace sim; +using namespace systems; +using namespace dave_gz_sensor_plugins; + +class gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin // dave_model_systems +{ +public: + TransientCurrentPlugin(); /// \brief Class constructor + ~TransientCurrentPlugin(); /// \brief Class destructor + +private: + std::shared_ptr rosNode; // This is a smart pointer to a ROS 2 node, facilitating + // communication with other ROS nodes. + std::string transientCurrentVelocityTopic; // Declare the variable (updated) +} + +//////////////////////////////////// + +TransientCurrentPlugin::TransientCurrentPlugin() +{ + this->Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr flowVelocityPub; + std::chrono::steady_clock::duration lastUpdate{0}; +}; + +TransientCurrentPlugin::~TransientCurrentPlugin() +{ + // Shutdown the ROS 2 node + this->rosNode.reset(); +} + +void TransientCurrentPlugin::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) +{ + this->dataPtr->world = _entity; + this->dataPtr->entity = _entity; + this->dataPtr->model = gz::sim::Model(_entity); + this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); + this->dataPtr->sdf = _sdf; + auto lastUpdate = gz::sim::simTime; + + gzmsg << "Loading transient ocean current model plugin..." << std::endl; + + if (_sdf->HasElement("flow_velocity_topic")) + { + this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic"); + } + else + { + this->dataPtr->currentVelocityTopic = + "hydrodynamics/current_velocity/" + this->dataPtr->model.Name(); + gz::sim::log::error() << "Empty flow_velocity_topic for transient_current model plugin. " + << "Default topicName definition is used" << std::endl; + } + gz::sim::log::info() << "Transient velocity topic name for " << this->dataPtr->model.Name() + << " : " << this->dataPtr->currentVelocityTopic << std::endl; + // Read the namespace for topics and services + this->dataPtr->ns = _sdf->Get("namespace"); + + // Initialize the ROS 2 node + this->dataPtr->ros_node_ = std::make_shared("TransirentCurrentPlugin"); + + // Advertise the ROS flow velocity as a stamped twist message + this->dataPtr->flowVelocityPub = + this->dataPtr->ros_node_->create_publisher( + this->dataPtr->currentVelocityTopic, rclcpp::QOS(10)); + + // Initializing the Gazebo transport node + this->dataPtr->node = transport::NodePtr(new transport::Node()); + + // Advertise the current velocity topic in ROS 2 + this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = + this->dataPtr->node->create_publisher( + this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); + + // Read topic name of stratified ocean current from SDF + if (_sdf->HasElement("transient_current")) + { + sdf::ElementPtr currentVelocityParams = _sdf->GetElement("transient_current"); + if (currentVelocityParams->HasElement("topic_stratified")) + { + this->dataPtr->transientCurrentVelocityTopic = + "/hydrodynamics/" + currentVelocityParams->Get("topic_stratified") + + "_database"; + } + else + { + this->dataPtr->transientCurrentVelocityTopic = + "/hydrodynamics/stratified_current_velocity_database"; + } + + // Tidal Oscillation + if ( + this->dataPtr->sdf->HasElement("tide_oscillation") && + this->dataPtr->sdf->Get("tide_oscillation") == true) + { + this->dataPtr->tideFlag = true; + } + else + { + this->dataPtr->tideFlag = false; + } + + this->dataPtr->lastDepthIndex = 0; + } + + // this->dataPtr->lastUpdate = _info.simTime +} +///////////////////////////////////////////////// +void TransientCurrentPlugin::Init() +{ + // Doing nothing for now +} +///////////////////////////////////////////////// +void TransientCurrentPlugin::CalculateOceanCurrent() +{ + this->dataPtr->lock_.lock(); + + // Update vehicle position + double vehicleDepth = -this->dataPtr->model->WorldPose().Pos().Z(); + + if (this->dataPtr->database.size() == 0) + { + // skip for next time (waiting for valid database subscrition) + } + else + { + double northCurrent = 0.0; + double eastCurrent = 0.0; + + //--- Interpolate velocity from database ---// + // find current depth index from database + // (X: north-direction, Y: east-direction, Z: depth) + int depthIndex = 0; + for (int i = 1; i < this->dataPtr->database.size(); i++) + { + if (this->dataPtr->database[i].Z() > vehicleDepth) + { + depthIndex = i; + break; + } + } + + // If sudden change found, use the one before + if (this->dataPtr->lastDepthIndex == 0) + { + this->dataPtr->lastDepthIndex = depthIndex; + } + else + { + if (abs(depthIndex - this->dataPtr->lastDepthIndex) > 2) + { + depthIndex = this->dataPtr->lastDepthIndex; + } + this->dataPtr->lastDepthIndex = depthIndex; + } + + // interpolate + if (depthIndex == 0) + { // Deeper than database use deepest value + northCurrent = this->dataPtr->database[this->dataPtr->database.size() - 1].X(); + eastCurrent = this->dataPtr->database[this->dataPtr->database.size() - 1].Y(); + } + else + { + double rate = + (vehicleDepth - this->dataPtr->database[depthIndex - 1].Z()) / + (this->dataPtr->database[depthIndex].Z() - this->dataPtr->database[depthIndex - 1].Z()); + northCurrent = + (this->dataPtr->database[depthIndex].X() - this->dataPtr->database[depthIndex - 1].X()) * + rate + + this->dataPtr->database[depthIndex - 1].X(); + eastCurrent = + (this->dataPtr->database[depthIndex].Y() - this->dataPtr->database[depthIndex - 1].Y()) * + rate + + this->dataPtr->database[depthIndex - 1].Y(); + } + this->dataPtr->currentVelNorthModel.mean = northCurrent; + this->dataPtr->currentVelEastModel.mean = eastCurrent; + this->dataPtr->currentVelDownModel.mean = 0.0; + + // Tidal oscillation + if (this->dataPtr->tideFlag) + { + // Update tide oscillation + this->dataPtr->time = _info.simTime; + + if (this->dataPtr->tide_Constituents) + { + this->dathis->dataPtr->tide.M2_amp = this->dathis->dataPtr->M2_amp; + this->dathis->dataPtr->tide.M2_phase = this->dathis->dataPtr->M2_phase; + this->dathis->dataPtr->tide.M2_speed = this->dathis->dataPtr->M2_speed; + this->dathis->dataPtr->tide.S2_amp = this->dathis->dataPtr->S2_amp; + this->dathis->dataPtr->tide.S2_phase = this->dathis->dataPtr->S2_phase; + this->dathis->dataPtr->tide.S2_speed = this->dathis->dataPtr->S2_speed; + this->dathis->dataPtr->tide.N2_amp = this->dathis->dataPtr->N2_amp; + this->dathis->dataPtr->tide.N2_phase = this->dathis->dataPtr->N2_phase; + this->dathis->dataPtr->tide.N2_speed = this->dathis->dataPtr->N2_speed; + } + else + { + this->dataPtr->tide.dateGMT = this->dataPtr->timeGMT; + this->dataPtr->tide.speedcmsec = this->dataPtr->tideVelocities; + } + this->dataPtr->tide.ebbDirection = this->dataPtr->ebbDirection; + this->dataPtr->tide.floodDirection = this->dataPtr->floodDirection; + this->dataPtr->tide.worldStartTime = this->dataPtr->world_start_time; + this->dataPtr->tide.Initiate(this->dataPtr->tide_Constituents); + std::pair currents = + this->dataPtr->tide.Update((this->dataPtr->time).Double(), northCurrent); + this->dataPtr->currentVelNorthModel.mean = currents.first; + this->dataPtr->currentVelEastModel.mean = currents.second; + this->dataPtr->currentVelDownModel.mean = 0.0; + } + else + { + this->dataPtr->currentVelNorthModel.mean = northCurrent; + this->dataPtr->currentVelEastModel.mean = eastCurrent; + this->dataPtr->currentVelDownModel.mean = 0.0; + } + + // Change min max accordingly + currentVelNorthModel.max = currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; + currentVelNorthModel.min = currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; + currentVelEastModel.max = currentVelEastModel.mean + this->dataPtr->noiseAmp_East; + currentVelEastModel.min = currentVelEastModel.mean - this->dataPtr->noiseAmp_East; + currentVelDownModel.max = currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; + currentVelDownModel.min = currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; + + // Assign values to the model + this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; + this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; + this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; + + // Update time + this->dataPtr->time = _info.simTime; + + // Update current velocity + double velocityNorth = + this->dataPtr->currentVelNorthModel.Update((this->dataPtr->time).Double()); + + // Update current horizontal direction around z axis of flow frame + double velocityEast = this->dataPtr->currentVelEastModel.Update((this->dataPtr->time).Double()); + + // Update current horizontal direction around z axis of flow frame + double velocityDown = this->dataPtr->currentVelDownModel.Update((this->dataPtr->time).Double()); + + // Update current Velocity + this->dataPtr->currentVelocity = gz::math::Vector3d(velocityNorth, velocityEast, velocityDown); + + // Update time stamp + this->dataPtr->lastUpdate = (this->dataPtr->time).Double(); + } + + this->dataPtr->lock_.unlock(); +} +///////////////////////////////////////////////// +void TransientCurrentPlugin::PublishCurrentVelocity() +{ + // Generate and publish ROS topic according to the vehicle depth + if (_info.simeTime > this->dataPtr->lastRosPublishTime) + { + geometry_msgs::TwistStamped flowVelMsg; + flowVelMsg.header.stamp = rclcpp::Time().now(); + flowVelMsg.header.frame_id = "/world"; + flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + this->dataPtr->flowVelocityPub.publish(flowVelMsg); + } + // Generate and publish Gazebo topic according to the vehicle depth + msgs::Vector3d currentVel; + msgs::Set( + ¤tVel, gz::math::Vector3d( + this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), + this->dataPtr->currentVelocity.Z())); + this->dataPtr->publishers[this->dataPtr->currentVelocityTopic]->Publish(currentVel); +} + +///////////////////////////////////////////////// +void TransientCurrentPlugin::PostPublishCurrentVelocity() +{ + this->dataPtr->lastRosPublishTime = _info.simeTime; +} + +///////////////////////////////////////////////// +TransientCurrentPlugin::UpdateDatabase( + const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) +{ + this->dataPtr->lock_.lock(); + + this->dataPtr->database.clear(); + for (int i = 0; i < _msg->depths.size(); i++) + { + gz::math::Vector3d data(_msg->velocities[i].x, _msg->velocities[i].y, _msg->depths[i]); + this->dataPtr->database.push_back(data); + } + if (this->dataPtr->tideFlag) + { + this->dataPtr->timeGMT.clear(); + this->dataPtr->tideVelocities.clear(); + if (_msg->tideConstituents == true) + { + this->dataPtr->M2_amp = _msg->M2amp; + this->dataPtr->M2_phase = _msg->M2phase; + this->dataPtr->M2_speed = _msg->M2speed; + this->dataPtr->S2_amp = _msg->S2amp; + this->dataPtr->S2_phase = _msg->S2phase; + this->dataPtr->S2_speed = _msg->S2speed; + this->dataPtr->N2_amp = _msg->N2amp; + this->dataPtr->N2_phase = _msg->N2phase; + this->dataPtr->N2_speed = _msg->N2speed; + this->dataPtr->tide_Constituents = true; + } + else + { + std::array tmpDateVals; + for (int i = 0; i < _msg->timeGMTYear.size(); i++) + { + tmpDateVals[0] = _msg->timeGMTYear[i]; + tmpDateVals[1] = _msg->timeGMTMonth[i]; + tmpDateVals[2] = _msg->timeGMTDay[i]; + tmpDateVals[3] = _msg->timeGMTHour[i]; + tmpDateVals[4] = _msg->timeGMTMinute[i]; + + this->dataPtr->timeGMT.push_back(tmpDateVals); + this->dataPtr->tideVelocities.push_back(_msg->tideVelocities[i]); + } + this->dataPtr->tide_Constituents = false; + } + this->dataPtr->ebbDirection = _msg->ebbDirection; + this->dataPtr->floodDirection = _msg->floodDirection; + this->dataPtr->world_start_time[0] = _msg->worldStartTimeYear; + this->dataPtr->world_start_time[1] = _msg->worldStartTimeMonth; + this->dataPtr->world_start_time[2] = _msg->worldStartTimeDay; + this->dataPtr->world_start_time[3] = _msg->worldStartTimeHour; + this->dataPtr->world_start_time[4] = _msg->worldStartTimeMinute; + } + + this->dataPtr->lock_.unlock(); +} + +///////////////////////////////////////////////// +void TransientCurrentPlugin::Gauss_Markov_process_initialize( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + // Read Gauss-Markov parameters + + // initialize velocity_north_model parameters + if (currentVelocityParams->HasElement("velocity_north")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); + if (elem->HasElement("mean")) + { + this->currentVelNorthModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->currentVelNorthModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->noiseAmp_North = elem->Get("noiseAmp"); + } + this->currentVelNorthModel.min = this->currentVelNorthModel.mean - this->noiseAmp_North; + this->currentVelNorthModel.max = this->currentVelNorthModel.mean + this->noiseAmp_North; + if (elem->HasElement("noiseFreq")) + { + this->noiseFreq_North = elem->Get("noiseFreq"); + } + this->currentVelNorthModel.noiseAmp = this->noiseFreq_North; + } + + this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; + gzmsg << "For vehicle " << this->dataPtr->model->GetName() + << " -> Current north-direction velocity [m/s] " + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelNorthModel.Print(); + + // initialize velocity_east_model parameters + if (currentVelocityParams->HasElement("velocity_east")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); + if (elem->HasElement("mean")) + { + this->currentVelEastModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->currentVelEastModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->noiseAmp_East = elem->Get("noiseAmp"); + } + this->currentVelEastModel.min = this->currentVelEastModel.mean - this->noiseAmp_East; + this->currentVelEastModel.max = this->currentVelEastModel.mean + this->noiseAmp_East; + if (elem->HasElement("noiseFreq")) + { + this->noiseFreq_East = elem->Get("noiseFreq"); + } + this->currentVelEastModel.noiseAmp = this->noiseFreq_East; + } + + this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; + gzmsg << "For vehicle " << this->dataPtr->model->GetName() + << " -> Current east-direction velocity [m/s] " + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelEastModel.Print(); + + // initialize velocity_down_model parameters + if (currentVelocityParams->HasElement("velocity_down")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_down"); + if (elem->HasElement("mean")) + { + this->currentVelDownModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->currentVelDownModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->noiseAmp_Down = elem->Get("noiseAmp"); + } + this->currentVelDownModel.min = this->currentVelDownModel.mean - this->noiseAmp_Down; + this->currentVelDownModel.max = this->currentVelDownModel.mean + this->noiseAmp_Down; + if (elem->HasElement("noiseFreq")) + { + this->noiseFreq_Down = elem->Get("noiseFreq"); + } + this->currentVelDownModel.noiseAmp = this->noiseFreq_Down; + } + + this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; + gzmsg << "For vehicle " << this->dataPtr->modelName << " -> Current down-direction velocity [m/s]" + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelDownModel.Print(); + + this->dataPtr->lastUpdate = _info + .simTime +#endif // need to look into this + this->dataPtr->currentVelNorthModel.lastUpdate = + this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.Double(); +} +///////////////////////////////////////////////// +void TransientCurrentPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + this->dataPtr->Gauss_Markov_process_initialize(); // something is wrong here + // Subscribe stratified ocean current database + this->dataPtr->databaseSub = + this->dataPtr->rosNode + ->create_subscription( + this->dataPtr->transientCurrentVelocityTopic, 10, + std::bind(&TransientCurrentPlugin::UpdateDatabase, this, _1)); + + // Connect the update event callback for ROS and ocean current calculation + this->dataPtr->Connect(); + + gzmsg << "Transient current model plugin loaded!" << std::endl; + this->dataPtr->lastUpdate = this->dataPtr->time; +} + +///////////////////////////////////////////////// +void TransientCurrentPlugin::Update( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + this->dataPtr->calculateOceanCurrent(); +} +///////////////////////////////////////////////// +void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + PostPublishCurrentVelocity(); + this->dataPtr->lastRosPublishTime = _info.simeTime; +} diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt new file mode 100644 index 00000000..4b283dfa --- /dev/null +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) + +#Define the project name +project(dave_gz_world_plugins) + +find_package(ament_cmake REQUIRED) +find_package(ros_gz_example_description 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) + +# Set version variables with gazebo Harmonic +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) +set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + +message(STATUS "compiling against Gazebo Harmonic") + +add_library(dave_gz_world_plugins SHARED +src/ocean_current_world_plugins.cpp) + +target_include_directories(ocean_current_world_plugins PRIVATE include) + +target_link_libraries(ocean_current_world_plugins +gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) + +ament_target_dependencies(ocean_current_world_plugin + "rclcpp" + "std_msgs" + "geometry_msgs" +# "dave_interfaces" +) + +# Install targets +install(TARGETS ocean_current_world_plugin + 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") + +ament_package() \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt b/gazebo/dave_gz_world_plugins/gauss_markov_process/CMakeLists.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp b/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh b/gazebo/dave_gz_world_plugins/gauss_markov_process/gauss_markov_process.hh deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in new file mode 100644 index 00000000..5e60e0e3 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh b/gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh new file mode 100644 index 00000000..01503ef4 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh @@ -0,0 +1,104 @@ +// Copyright (c) 2016 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. + +/// \file gauss_markov_process.h +/// \brief Implementation of a Gauss-Markov process model + +#ifndef GAUSS_MARKOV_PROCESS_H_ +#define GAUSS_MARKOV_PROCESS_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace gz +{ +/// \brief Implementation of a Gauss-Markov process to model the current +/// velocity and direction according to [1] +/// [1] Fossen, Thor I. Handbook of marine craft hydrodynamics and motion +/// control. John Wiley & Sons, 2011. +class GaussMarkovProcess +{ + /// \brief Class constructor +public: + GaussMarkovProcess(); + + /// \brief Resets the process parameters +public: + void Reset(); + /// \brief Sets all the necessary parameters for the computation + /// \param _mean Mean value + /// \param _min Minimum limit + /// \param _max Maximum limit + /// \param _mu Process constant + /// \param _noise Amplitude for the Gaussian white noise + /// \return True, if all parameters were valid +public: + bool SetModel(double _mean, double _min, double _max, double _mu = 0, double _noise = 0); + + /// \brief Set mean process value + /// \param _mean New mean value + /// \return True, if value inside the limit range +public: + bool SetMean(double _mean); + + /// \brief Process variable +public: + double var; + + /// \brief Mean process value +public: + double mean; + + /// \brief Minimum limit for the process variable +public: + double min; + + /// \brief Maximum limit for the process variable +public: + double max; + + /// \brief Process constant, if zero, process becomes a random walk +public: + double mu; + + /// \brief Gaussian white noise amplitude +public: + double noiseAmp; + + /// \brief Timestamp for the last update +public: + double lastUpdate; + + /// \brief State storage for the random number generator +private: + unsigned int randSeed; + + /// \brief Update function for a new time stamp + /// \param _time Current time stamp +public: + double Update(double _time); + + /// \brief Print current model parameters +public: + void Print(); +}; +} // namespace gz + +#endif // GAUSS_MARKOV_PROCESS_H_ diff --git a/gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh new file mode 100644 index 00000000..ab643375 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh @@ -0,0 +1,212 @@ +// Copyright (c) 2016 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. + +/// \file ocean_current_world_plugin.hh +/// \brief Plugin that for the underwater world + +#ifndef OCEAN_CURRENT_WORLD_PLUGIN_H_ +#define OCEAN_CURRENT_WORLD_PLUGIN_H_ + +#include +#include + +#include +#include + +// #include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +namespace systems +{ +namespace dave_simulator_ros +{ + +/// \brief Class for the underwater current plugin + +// typedef const boost::shared_ptr AnyPtr; + +// public WorldPlugin, +class UnderwaterCurrentPlugin : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemUpdate, + public ISystemPostUpdate, + public WorldPlugin + +{ + /// \brief Class constructor +public: + UnderwaterCurrentPlugin(); + + /// \brief Class destructor + virtual ~UnderwaterCurrentPlugin(); + + // Documentation inherited. + void Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + + // Documentation inherited. + virtual void Init(); + + /// \brief Update the simulation state. + /// \param[in] _info Information used in the update event. + // void Update(const common::UpdateInfo & _info); (check if this is needed) + + void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + + /// \brief Load global current velocity configuration +protected: + virtual void LoadGlobalCurrentConfig(); + + /// \brief Load stratified current velocity database + virtual void LoadStratifiedCurrentDatabase(); + + /// \brief Load tidal oscillation database + virtual void LoadTidalOscillationDatabase(); + + /// \brief Publish current velocity and the pose of its frame + void PublishCurrentVelocity(); + + /// \brief Publish stratified oceqan current velocity + void PublishStratifiedCurrentVelocity(); + + /// \brief Update event + event::ConnectionPtr updateConnection; + + /// \brief Pointer to world + physics::WorldPtr world; + + /// \brief Pointer to sdf + sdf::ElementPtr sdf; + + /// \brief True if the sea surface is present + bool hasSurface; + + /// \brief Pointer to a node for communication + transport::NodePtr node; + + /// \brief Map of publishers + std::map publishers; + + /// \brief Vehicle Depth Subscriber + transport::SubscriberPtr subscriber; + + /// \brief Current velocity topic + std::string currentVelocityTopic; + + /// \brief Stratified Ocean current topic + std::string stratifiedCurrentVelocityTopic; + + /// \brief Vehicle depth topic + std::string vehicleDepthTopic; + + /// \brief Ocean Current Database file path for csv file + std::string databaseFilePath; + + /// \brief Tidal Oscillation Database file path for txt file + std::string tidalFilePath; + + /// \brief Vector for read stratified current database values + std::vector stratifiedDatabase; + + /// \brief Namespace for topics and services + std::string ns; + + /// \brief Gauss-Markov process instance for the current velocity + GaussMarkovProcess currentVelModel; + + /// \brief Gauss-Markov process instance for horizontal angle model + GaussMarkovProcess currentHorzAngleModel; + + /// \brief Gauss-Markov process instance for vertical angle model + GaussMarkovProcess currentVertAngleModel; + + /// \brief Vector of Gauss-Markov process instances for stratified velocity + std::vector> stratifiedCurrentModels; + + /// \brief Vector of dateGMT for tidal oscillation + std::vector> dateGMT; + + /// \brief Vector of speedcmsec for tidal oscillation + std::vector speedcmsec; + + /// \brief Tidal current harmonic constituents + bool tidalHarmonicFlag; + + double M2_amp; + double M2_phase; + double M2_speed; + double S2_amp; + double S2_phase; + double S2_speed; + double N2_amp; + double N2_phase; + double N2_speed; + + /// \brief Tidal oscillation mean ebb direction + double ebbDirection; + + /// \brief Tidal oscillation mean flood direction + double floodDirection; + + /// \brief Tidal oscillation world start time (GMT) + int world_start_time_day; + int world_start_time_month; + int world_start_time_year; + int world_start_time_hour; + int world_start_time_minute; + + /// \brief Tidal Oscillation flag + bool tideFlag; + + /// \brief Tidal Oscillation interpolation model + TidalOscillation tide; + + /// \brief Last update time stamp + common::Time lastUpdate; + + /// \brief Current linear velocity vector + gz::math::Vector3d currentVelocity; + + /// \brief Vector of current depth-specific linear velocity vectors + std::vector currentStratifiedVelocity; + + /// \brief File path for stratified current database + std::string db_path; +}; +} // namespace dave_simulator_ros +} // namespace systems +} // namespace sim +} // namespace gz + +#endif // OCEAN_CURRENT_WORLD_PLUGIN_H_ diff --git a/gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh b/gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh new file mode 100644 index 00000000..34bf8379 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh @@ -0,0 +1,122 @@ +// Copyright (c) 2016 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. + +/// \file tidal_oscillation.h +/// \brief Interpolation of NOAA data for Tidal Oscillation feature + +#ifndef TIDAL_OSCILLATION_H_ +#define TIDAL_OSCILLATION_H_ + +#include +#include +#include +#include +#include +#include +#include + +// #include + +namespace gz +{ +/// \brief Interpolation of NOAA data for Tidal Oscillation feature +class TidalOscillation +{ + /// \brief Class constructor +public: + TidalOscillation(); + + /// \brief Resets the process parameters +public: + void Reset(); + + /// \brief Prepare the data for interpolation +public: + void Initiate(bool _harmonicConstituents); + + /// \brief Translate datetime string to datenum +public: + double TranslateDate(std::array _datetime); + + /// \brief Input Datenum data +public: + std::vector> dateGMT; + + /// \brief Input Tidal data +public: + std::vector speedcmsec; + + /// \brief Input Datenum data +public: + std::vector datenum; + + /// \brief Bool for method type +public: + bool harmonicConstituent; + + /// \brief Tidal current harmonic constituents +public: + double M2_amp; + +public: + double M2_phase; + +public: + double M2_speed; + +public: + double S2_amp; + +public: + double S2_phase; + +public: + double S2_speed; + +public: + double N2_amp; + +public: + double N2_phase; + +public: + double N2_speed; + + /// \brief Input Tidal direction +public: + double ebbDirection; + +public: + double floodDirection; + + /// \brief Input world start time +public: + std::array worldStartTime; + +public: + double worldStartTime_num; + + /// \brief Update function for a new time stamp + /// \param _time Current time stamp +public: + std::pair Update(double _time, double _currentDepthRatio); + + /// \brief save current state (Flood: true, Ebb: false) +public: + bool currentType; +}; +} // namespace gz + +#endif // TIDAL_OSCILLATION_H_ diff --git a/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto b/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto deleted file mode 100644 index dc744084..00000000 --- a/gazebo/dave_gz_world_plugins/msgs/StratifiedCurrentVelocity.proto +++ /dev/null @@ -1,20 +0,0 @@ -syntax = "proto3"; -package dave_gazebo_world_plugins_msgs.msgs; -import "vector3d.proto"; // Ensure vector3d.proto is compatible with proto3 - -message StratifiedCurrentVelocity { - repeated gazebo.msgs.Vector3d velocity = 1; - repeated double depth = 2; -} - - -// syntax = "proto2"; -// package dave_gazebo_world_plugins_msgs.msgs; -// import "vector3d.proto"; -// import "any.proto"; - -// message StratifiedCurrentVelocity -// { -// repeated gazebo.msgs.Vector3d velocity = 1; -// repeated double depth = 2; -// } \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/CMakeLists.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/ocean_current_world_plugin/ocean_current_world_plugin.hh deleted file mode 100644 index e69de29b..00000000 diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml new file mode 100644 index 00000000..b11a0a67 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -0,0 +1,24 @@ + + + dave_gazebo_world_plugins + 3.1.1 + The dave_gazebo_world_plugins package (Gazebo world plugin definitions) + Gaurav Kumar and woensug + Gaurav Kumar and woensug + Apache 2.0 + catkin + protobuf-dev + gazebo_ros + roscpp + protobuf + gazebo_ros + protobuf + gazebo_ros + roscpp + protobuf + gazebo_dev + gazebo_msgs + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp new file mode 100644 index 00000000..94497431 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2016 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. + +/// \file gauss_markov_process.cc + +#include + +namespace gz +{ +///////////////////////////////////////////////// +GaussMarkovProcess::GaussMarkovProcess() +{ + this->mean = 0; + this->min = -1; + this->max = 1; + this->mu = 0; + this->noiseAmp = 0; + this->lastUpdate = 0; + this->Reset(); + this->randSeed = std::time(NULL); +} + +///////////////////////////////////////////////// +void GaussMarkovProcess::Reset() { this->var = this->mean; } + +///////////////////////////////////////////////// +bool GaussMarkovProcess::SetMean(double _mean) +{ + if (this->min > _mean || this->max < _mean) + { + return false; + } + + this->mean = _mean; + this->Reset(); + return true; +} + +///////////////////////////////////////////////// +bool GaussMarkovProcess::SetModel(double _mean, double _min, double _max, double _mu, double _noise) +{ + if (_min >= _max) + { + return false; + } + if (_min > _mean || _max < _mean) + { + return false; + } + if (_noise < 0) + { + return false; + } + if (_mu < 0 || _mu > 1) + { + return false; + } + this->mean = _mean; + this->min = _min; + this->max = _max; + this->mu = _mu; + this->noiseAmp = _noise; + + this->Reset(); + return true; +} + +///////////////////////////////////////////////// +double GaussMarkovProcess::Update(double _time) +{ + double step = _time - this->lastUpdate; + double random = + static_cast(static_cast(rand_r(&this->randSeed)) / RAND_MAX) - 0.5; + this->var = (1 - step * this->mu) * this->var + this->noiseAmp * random; + if (this->var >= this->max) + { + this->var = this->max; + } + if (this->var <= this->min) + { + this->var = this->min; + } + this->lastUpdate = _time; + return this->var; +} + +///////////////////////////////////////////////// +void GaussMarkovProcess::Print() +{ + gzmsg << "\tMean = " << this->mean << std::endl + << "\tMin. Limit = " << this->min << std::endl + << "\tMax. Limit = " << this->max << std::endl + << "\tMu = " << this->mu << std::endl + << "\tNoise Amp. = " << this->noiseAmp << std::endl; +} +} // namespace gz diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp new file mode 100644 index 00000000..93bb5411 --- /dev/null +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp @@ -0,0 +1,647 @@ +// Copyright (c) 2016 The UUV 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. + +/// \file ocean_current_world_plugin.cc + +#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gz/common/StringUtils.hh" +#include "gz/plugin/Register.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/World.hh" + +using namespace gz; +using namespace sim; +using namespace systems; +using namespace dave_simulator_ros; + +class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentPlugin +{ +public: + UnderwaterCurrentPlugin(); + virtual ~UnderwaterCurrentPlugin(); + +private: + void Load(sdf::ElementPtr _sdf); + // void OnUpdate(const common::UpdateInfo & _info); + std::shared_ptr rosNode; +} + +///////////////////////////////////////////////// +UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() +{ + // Doing nothing for now + void configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); +} + +///////////////////////////////////////////////// +UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->rosNode.reset(); } + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) +{ + GZ_ASSERT(_entity != NULL, "World pointer is invalid"); + GZ_ASSERT(_sdf != NULL, "SDF pointer is invalid"); + + this->dataPtr->world = _entity; + this->dataPtr->sdf = _sdf; + + // Read the namespace for topics and services + this->dataPtr->ns = _sdf->Get("namespace"); + + gzmsg << "Loading underwater world..." << std::endl; + // Initializing the transport node + this->node = transport::NodePtr(new transport::Node()); + + this->dataPtr->node->Init(this->dataPtr->world->Name(_ecm)); // check if correct + + // Initialize the time update + this->lastUpdate = _info.simeTime; // should it be zero ? (check) + std::chrono::steady_clock::duration lastUpdate{0}; // better ? (check) + + this->LoadGlobalCurrentConfig(); + this->LoadStratifiedCurrentDatabase(); + if (this->sdf->HasElement("tidal_oscillation")) + { + this->LoadTidalOscillationDatabase(); + } + + // Connect the update event. This isn't needed it seems (check) + // this->updateConnection = event::Events::ConnectWorldUpdateBegin( + // boost::bind(&UnderwaterCurrentPlugin::Update, + // this, _1)); + + gzmsg << "Underwater current plugin loaded!" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() +{ + this->tideFlag = true; + this->tidalHarmonicFlag = false; + + sdf::ElementPtr tidalOscillationParams = this->sdf->GetElement("tidal_oscillation"); + sdf::ElementPtr tidalHarmonicParams; + + // Read the tidal oscillation parameter from the SDF file + if (tidalOscillationParams->HasElement("databasefilePath")) + { + this->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); + gzmsg << "Tidal current database configuration found" << std::endl; + } + else + { + if (tidalOscillationParams->HasElement("harmonic_constituents")) + { + tidalHarmonicParams = tidalOscillationParams->GetElement("harmonic_constituents"); + gzmsg << "Tidal harmonic constituents " << "configuration found" << std::endl; + tidalHarmonicFlag = true; + } + else + { + this->tidalFilePath = + ros::package::getPath("dave_worlds") + "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; + } + } + + // Read the tidal oscillation direction from the SDF file + GZ_ASSERT( + tidalOscillationParams->HasElement("mean_direction"), "Tidal mean direction not defined"); + if (tidalOscillationParams->HasElement("mean_direction")) + { + sdf::ElementPtr elem = tidalOscillationParams->GetElement("mean_direction"); + GZ_ASSERT(elem->HasElement("ebb"), "Tidal mean ebb direction not defined"); + this->ebbDirection = elem->Get("ebb"); + this->floodDirection = elem->Get("flood"); + GZ_ASSERT(elem->HasElement("flood"), "Tidal mean flood direction not defined"); + } + + // Read the world start time (GMT) from the SDF file + GZ_ASSERT( + tidalOscillationParams->HasElement("world_start_time_GMT"), + "World start time (GMT) not defined"); + if (tidalOscillationParams->HasElement("world_start_time_GMT")) + { + sdf::ElementPtr elem = tidalOscillationParams->GetElement("world_start_time_GMT"); + GZ_ASSERT(elem->HasElement("day"), "World start time (day) not defined"); + this->world_start_time_day = elem->Get("day"); + GZ_ASSERT(elem->HasElement("month"), "World start time (month) not defined"); + this->world_start_time_month = elem->Get("month"); + GZ_ASSERT(elem->HasElement("year"), "World start time (year) not defined"); + this->world_start_time_year = elem->Get("year"); + GZ_ASSERT(elem->HasElement("hour"), "World start time (hour) not defined"); + this->world_start_time_hour = elem->Get("hour"); + if (elem->HasElement("minute")) + { + this->world_start_time_minute = elem->Get("minute"); + } + else + { + this->world_start_time_minute = 0; + } + } + + if (tidalHarmonicFlag) + { + // Read harmonic constituents + GZ_ASSERT(tidalHarmonicParams->HasElement("M2"), "Harcomnic constituents M2 not found"); + sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2"); + this->M2_amp = M2Params->Get("amp"); + this->M2_phase = M2Params->Get("phase"); + this->M2_speed = M2Params->Get("speed"); + GZ_ASSERT(tidalHarmonicParams->HasElement("S2"), "Harcomnic constituents S2 not found"); + sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2"); + this->S2_amp = S2Params->Get("amp"); + this->S2_phase = S2Params->Get("phase"); + this->S2_speed = S2Params->Get("speed"); + GZ_ASSERT(tidalHarmonicParams->HasElement("N2"), "Harcomnic constituents N2 not found"); + sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2"); + this->N2_amp = N2Params->Get("amp"); + this->N2_phase = N2Params->Get("phase"); + this->N2_speed = N2Params->Get("speed"); + gzmsg << "Tidal harmonic constituents loaded : " << std::endl; + gzmsg << "M2 amp: " << this->M2_amp << " phase: " << this->M2_phase + << " speed: " << this->M2_speed << std::endl; + gzmsg << "S2 amp: " << this->S2_amp << " phase: " << this->S2_phase + << " speed: " << this->S2_speed << std::endl; + gzmsg << "N2 amp: " << this->N2_amp << " phase: " << this->N2_phase + << " speed: " << this->N2_speed << std::endl; + } + else + { + // Read database + std::ifstream csvFile; + std::string line; + csvFile.open(this->tidalFilePath); + if (!csvFile) + { + common::SystemPaths * paths = common::SystemPaths::Instance(); + this->tidalFilePath = paths->FindFile(this->tidalFilePath, true); + csvFile.open(this->tidalFilePath); + } + GZ_ASSERT(csvFile, "Tidal Oscillation database file does not exist"); + + gzmsg << "Tidal Oscillation Database loaded : " << this->tidalFilePath << std::endl; + + // skip the first line + getline(csvFile, line); + while (getline(csvFile, line)) + { + if (line.empty()) // skip empty lines: + { + continue; + } + std::istringstream iss(line); + std::string lineStream; + std::string::size_type sz; + std::vector row; + std::array tmpDateArray; + while (getline(iss, lineStream, ',')) + { + row.push_back(lineStream); + } + if (strcmp(row[1].c_str(), " slack")) // skip 'slack' category + { + tmpDateArray[0] = std::stoi(row[0].substr(0, 4)); + tmpDateArray[1] = std::stoi(row[0].substr(5, 7)); + tmpDateArray[2] = std::stoi(row[0].substr(8, 10)); + tmpDateArray[3] = std::stoi(row[0].substr(11, 13)); + tmpDateArray[4] = std::stoi(row[0].substr(14, 16)); + this->dateGMT.push_back(tmpDateArray); + + this->speedcmsec.push_back(stold(row[2], &sz)); + } + } + csvFile.close(); + + // Eliminate data with same consecutive type + std::vector duplicated; + for (int i = 0; i < this->dateGMT.size() - 1; i++) + { + // delete latter if same sign + if ( + ((this->speedcmsec[i] > 0) - (this->speedcmsec[i] < 0)) == + ((this->speedcmsec[i + 1] > 0) - (this->speedcmsec[i + 1] < 0))) + { + duplicated.push_back(i + 1); + } + } + int eraseCount = 0; + for (int i = 0; i < duplicated.size(); i++) + { + this->dateGMT.erase(this->dateGMT.begin() + duplicated[i] - eraseCount); + this->speedcmsec.erase(this->speedcmsec.begin() + duplicated[i] - eraseCount); + eraseCount++; + } + } +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() +{ + GZ_ASSERT( + this->sdf->HasElement("transient_current"), "Transient current configuration not available"); + sdf::ElementPtr transientCurrentParams = this->sdf->GetElement("transient_current"); + + if (transientCurrentParams->HasElement("topic_stratified")) + { + this->stratifiedCurrentVelocityTopic = + transientCurrentParams->Get("topic_stratified"); + } + else + { + this->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; + } + + GZ_ASSERT( + !this->stratifiedCurrentVelocityTopic.empty(), "Empty stratified ocean current velocity topic"); + + // Read the depth dependent ocean current file path from the SDF file + if (transientCurrentParams->HasElement("databasefilePath")) + { + this->databaseFilePath = transientCurrentParams->Get("databasefilePath"); + } + else + { + this->databaseFilePath = "transientOceanCurrentDatabase.csv"; + } + + GZ_ASSERT(!this->databaseFilePath.empty(), "Empty stratified ocean current database file path"); + + gzmsg << this->databaseFilePath << std::endl; + + // #if GAZEBO_MAJOR_VERSION >= 8 + // this->lastUpdate = this->world->SimTime(); + // #else + // this->lastUpdate = this->world->GetSimTime(); + // #endif + + // Read database + std::ifstream csvFile; + std::string line; + csvFile.open(this->databaseFilePath); + if (!csvFile) + { + common::SystemPaths * paths = common::SystemPaths::Instance(); + this->databaseFilePath = paths->FindFile(this->databaseFilePath, true); + csvFile.open(this->databaseFilePath); + } + GZ_ASSERT(csvFile, "Stratified Ocean database file does not exist"); + + gzmsg << "Statified Ocean Current Database loaded : " << this->databaseFilePath << std::endl; + + // skip the 3 lines + getline(csvFile, line); + getline(csvFile, line); + getline(csvFile, line); + while (getline(csvFile, line)) + { + if (line.empty()) // skip empty lines: + { + continue; + } + std::istringstream iss(line); + std::string lineStream; + std::string::size_type sz; + std::vector row; + while (getline(iss, lineStream, ',')) + { + row.push_back(stold(lineStream, &sz)); // convert to double + } + gz::math::Vector3d read; + read.X() = row[0]; + read.Y() = row[1]; + read.Z() = row[2]; + this->stratifiedDatabase.push_back(read); + + // Create Gauss-Markov processes for the stratified currents + // Means are the database-specified magnitudes & angles, and + // the other values come from the constant current models + // TODO: Vertical angle currently set to 0 (not in database) + GaussMarkovProcess magnitudeModel; + magnitudeModel.mean = hypot(row[1], row[0]); + magnitudeModel.var = magnitudeModel.mean; + magnitudeModel.max = this->currentVelModel.max; + magnitudeModel.min = 0.0; + magnitudeModel.mu = this->currentVelModel.mu; + magnitudeModel.noiseAmp = this->currentVelModel.noiseAmp; + magnitudeModel.lastUpdate = this->lastUpdate.Double(); + + GaussMarkovProcess hAngleModel; + hAngleModel.mean = atan2(row[1], row[0]); + hAngleModel.var = hAngleModel.mean; + hAngleModel.max = M_PI; + hAngleModel.min = -M_PI; + hAngleModel.mu = this->currentHorzAngleModel.mu; + hAngleModel.noiseAmp = this->currentHorzAngleModel.noiseAmp; + hAngleModel.lastUpdate = this->lastUpdate.Double(); + + GaussMarkovProcess vAngleModel; + vAngleModel.mean = 0.0; + vAngleModel.var = vAngleModel.mean; + vAngleModel.max = M_PI / 2.0; + vAngleModel.min = -M_PI / 2.0; + vAngleModel.mu = this->currentVertAngleModel.mu; + vAngleModel.noiseAmp = this->currentVertAngleModel.noiseAmp; + vAngleModel.lastUpdate = this->lastUpdate.Double(); + + std::vector depthModels; + depthModels.push_back(magnitudeModel); + depthModels.push_back(hAngleModel); + depthModels.push_back(vAngleModel); + this->stratifiedCurrentModels.push_back(depthModels); + } + csvFile.close(); + + this->publishers[this->stratifiedCurrentVelocityTopic] = + this->node->create_publisher( + this->ns + "/" + this->stratifiedCurrentVelocityTopic); + gzmsg << "Stratified current velocity topic name: " + << this->ns + "/" + this->stratifiedCurrentVelocityTopic << std::endl; +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() +{ + // NOTE: The plugin currently requires stratified current, so the + // global current set up in this method is potentially + // inconsistent or redundant. + // Consider setting it up as one or the other, but not both? + + // Retrieve the velocity configuration, if existent + GZ_ASSERT(this->sdf->HasElement("constant_current"), "Current configuration not available"); + sdf::ElementPtr currentVelocityParams = this->sdf->GetElement("constant_current"); + + // Read the topic names from the SDF file + if (currentVelocityParams->HasElement("topic")) + { + this->currentVelocityTopic = currentVelocityParams->Get("topic"); + } + else + { + this->currentVelocityTopic = "current_velocity"; + } + + GZ_ASSERT(!this->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); + + if (currentVelocityParams->HasElement("velocity")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity"); + if (elem->HasElement("mean")) + { + this->currentVelModel.mean = elem->Get("mean"); + } + if (elem->HasElement("min")) + { + this->currentVelModel.min = elem->Get("min"); + } + if (elem->HasElement("max")) + { + this->currentVelModel.max = elem->Get("max"); + } + if (elem->HasElement("mu")) + { + this->currentVelModel.mu = elem->Get("mu"); + } + if (elem->HasElement("noiseAmp")) + { + this->currentVelModel.noiseAmp = elem->Get("noiseAmp"); + } + + GZ_ASSERT( + this->currentVelModel.min < this->currentVelModel.max, "Invalid current velocity limits"); + GZ_ASSERT( + this->currentVelModel.mean >= this->currentVelModel.min, + "Mean velocity must be greater than minimum"); + GZ_ASSERT( + this->currentVelModel.mean <= this->currentVelModel.max, + "Mean velocity must be smaller than maximum"); + GZ_ASSERT( + this->currentVelModel.mu >= 0 && this->currentVelModel.mu < 1, "Invalid process constant"); + GZ_ASSERT( + this->currentVelModel.noiseAmp < 1 && this->currentVelModel.noiseAmp >= 0, + "Noise amplitude has to be smaller than 1"); + } + + this->currentVelModel.var = this->currentVelModel.mean; + gzmsg << "Current velocity [m/s] Gauss-Markov process model:" << std::endl; + this->currentVelModel.Print(); + + if (currentVelocityParams->HasElement("horizontal_angle")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("horizontal_angle"); + + if (elem->HasElement("mean")) + { + this->currentHorzAngleModel.mean = elem->Get("mean"); + } + if (elem->HasElement("min")) + { + this->currentHorzAngleModel.min = elem->Get("min"); + } + if (elem->HasElement("max")) + { + this->currentHorzAngleModel.max = elem->Get("max"); + } + if (elem->HasElement("mu")) + { + this->currentHorzAngleModel.mu = elem->Get("mu"); + } + if (elem->HasElement("noiseAmp")) + { + this->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); + } + + GZ_ASSERT( + this->currentHorzAngleModel.min < this->currentHorzAngleModel.max, + "Invalid current horizontal angle limits"); + GZ_ASSERT( + this->currentHorzAngleModel.mean >= this->currentHorzAngleModel.min, + "Mean horizontal angle must be greater than minimum"); + GZ_ASSERT( + this->currentHorzAngleModel.mean <= this->currentHorzAngleModel.max, + "Mean horizontal angle must be smaller than maximum"); + GZ_ASSERT( + this->currentHorzAngleModel.mu >= 0 && this->currentHorzAngleModel.mu < 1, + "Invalid process constant"); + GZ_ASSERT( + this->currentHorzAngleModel.noiseAmp < 1 && this->currentHorzAngleModel.noiseAmp >= 0, + "Noise amplitude for horizontal angle has to be between 0 and 1"); + } + + this->currentHorzAngleModel.var = this->currentHorzAngleModel.mean; + gzmsg << "Current velocity horizontal angle [rad] Gauss-Markov process model:" << std::endl; + this->currentHorzAngleModel.Print(); + + if (currentVelocityParams->HasElement("vertical_angle")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("vertical_angle"); + + if (elem->HasElement("mean")) + { + this->currentVertAngleModel.mean = elem->Get("mean"); + } + if (elem->HasElement("min")) + { + this->currentVertAngleModel.min = elem->Get("min"); + } + if (elem->HasElement("max")) + { + this->currentVertAngleModel.max = elem->Get("max"); + } + if (elem->HasElement("mu")) + { + this->currentVertAngleModel.mu = elem->Get("mu"); + } + if (elem->HasElement("noiseAmp")) + { + this->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); + } + + GZ_ASSERT( + this->currentVertAngleModel.min < this->currentVertAngleModel.max, + "Invalid current vertical angle limits"); + GZ_ASSERT( + this->currentVertAngleModel.mean >= this->currentVertAngleModel.min, + "Mean vertical angle must be greater than minimum"); + GZ_ASSERT( + this->currentVertAngleModel.mean <= this->currentVertAngleModel.max, + "Mean vertical angle must be smaller than maximum"); + GZ_ASSERT( + this->currentVertAngleModel.mu >= 0 && this->currentVertAngleModel.mu < 1, + "Invalid process constant"); + GZ_ASSERT( + this->currentVertAngleModel.noiseAmp < 1 && this->currentVertAngleModel.noiseAmp >= 0, + "Noise amplitude for vertical angle has to be between 0 and 1"); + } + + this->currentVertAngleModel.var = this->currentVertAngleModel.mean; + gzmsg << "Current velocity vertical angle [rad] Gauss-Markov process model:" << std::endl; + this->currentVertAngleModel.Print(); + + this->currentVelModel.lastUpdate = this->lastUpdate.Double(); + this->currentHorzAngleModel.lastUpdate = this->lastUpdate.Double(); + this->currentVertAngleModel.lastUpdate = this->lastUpdate.Double(); + + // Advertise the current velocity & stratified current velocity topics + this->publishers[this->currentVelocityTopic] = + this->node->create_publisher(this->ns + "/" + this->currentVelocityTopic); + gzmsg << "Current velocity topic name: " << this->ns + "/" + this->currentVelocityTopic + << std::endl; +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::PublishCurrentVelocity() +{ + msgs::Vector3d currentVel; + msgs::Set( + ¤tVel, + gz::math::Vector3d( + this->currentVelocity.X(), this->currentVelocity.Y(), this->currentVelocity.Z())); + this->publishers[this->currentVelocityTopic]->Publish(currentVel); +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() +{ + dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; + for (std::vector::iterator it = this->currentStratifiedVelocity.begin(); + it != this->currentStratifiedVelocity.end(); ++it) + { + msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); + currentVel.add_depth(it->W()); + } + if (currentVel.velocity_size() == 0) + { + return; + } + this->publishers[this->stratifiedCurrentVelocityTopic]->Publish(currentVel); +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ +} + +///////////////////////////////////////////////// +void UnderwaterCurrentPlugin::Update( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + // Update the time + this->dataPtr->time = _info.simTime; + + // Calculate the flow velocity and the direction using the Gauss-Markov + // model + + // Update current velocity + double currentVelMag = this->currentVelModel.Update(time.Double()); + // Update current horizontal direction around z axis of flow frame + double horzAngle = this->currentHorzAngleModel.Update(time.Double()); + + // Update current horizontal direction around z axis of flow frame + double vertAngle = this->currentVertAngleModel.Update(time.Double()); + + // Generating the current velocity vector as in the North-East-Down frame + this->currentVelocity = gz::math::Vector3d( + currentVelMag * cos(horzAngle) * cos(vertAngle), + currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle)); + + // Generate the depth-specific velocities + this->currentStratifiedVelocity.clear(); + for (int i = 0; i < this->stratifiedDatabase.size(); i++) + { + double depth = this->stratifiedDatabase[i].Z(); + currentVelMag = this->stratifiedCurrentModels[i][0].Update(time.Double()); + horzAngle = this->stratifiedCurrentModels[i][1].Update(time.Double()); + vertAngle = this->stratifiedCurrentModels[i][2].Update(time.Double()); + gz::math::Vector4d depthVel( + currentVelMag * cos(horzAngle) * cos(vertAngle), + currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); + this->currentStratifiedVelocity.push_back(depthVel); + } +} + +///////////////////////////////////////////////// +void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + // Update time stamp + this->lastUpdate = _info.simTime; + this->PublishCurrentVelocity(); + this->PublishStratifiedCurrentVelocity(); +} \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp new file mode 100644 index 00000000..5d065feb --- /dev/null +++ b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2016 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. + +/// \file tidal_oscillation.cc + +#include + +#include + +namespace std +{ + +static int cumdays[] = {0, 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; +double calcdatenum(int year, int mon, int day, int hour, int imin, int sec, int mil) +{ + int tmp1, tmp2, tmp3; + double tmp4, tmp5; + double dNum; + + /* Calculate the serial date number:*/ + tmp1 = 365 * year + cumdays[mon] + day; + tmp2 = year / 4 - year / 100 + year / 400; + tmp3 = (year % 4 != 0) - (year % 100 != 0) + (year % 400 != 0); + tmp4 = static_cast(tmp1 + tmp2 + tmp3); + tmp5 = (hour * 3600000 + imin * 60000 + sec * 1000 + mil) / 86400000.0; + + dNum = tmp4 + tmp5; + + if (mon > 2) + { + if (((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0)) + { + dNum += 1.0; + } + } + + return (dNum); +} +} // namespace std + +namespace gz +{ +///////////////////////////////////////////////// +TidalOscillation::TidalOscillation() {} + +///////////////////////////////////////////////// +void TidalOscillation::Reset() {} + +///////////////////////////////////////////////// +void TidalOscillation::Initiate(bool _harmonicConstituent) +{ + if (_harmonicConstituent) + { + this->harmonicConstituent = true; + } + else + { + int nData = this->speedcmsec.size(); + // Calculate datenum + if (this->dateGMT.size() != this->datenum.size()) + { + for (size_t i = 0; i < nData; i++) + { + this->datenum.push_back(TranslateDate(this->dateGMT[i])); + } + } + this->harmonicConstituent = false; + } + this->worldStartTime_num = TranslateDate(this->worldStartTime); + + // Make sure worldStarTime is in the range of database + // GZ_ASSERT((this->worldStartTime_num > this->datenum[0] + // || this->worldStartTime_num < this->datenum[this->datenum.size()]), + // "World Start Time (GMT) is not in the range of the database"); +} + +///////////////////////////////////////////////// +double TidalOscillation::TranslateDate(std::array _datetime) +{ + double datenumReturn; + + // Calculate datenum + datenumReturn = + std::calcdatenum(_datetime[0], _datetime[1], _datetime[2], _datetime[3], _datetime[4], 0, 0); + + return datenumReturn; +} + +///////////////////////////////////////////////// +std::pair TidalOscillation::Update(double _time, double _currentDepthRatio) +{ + std::pair currents; + currents.first = 0.5; + currents.second = 0.5; + double currentVelocity; + + // Calculate current time + double simTimePassed = (0 * 3600000 + 0 * 60000 + _time * 1000 + 0) / 86400000.0; + double currentTime = worldStartTime_num + simTimePassed; + + if (this->harmonicConstituent) + { + // Harmonic Constituents calculated in meters and GMT + // speed [deg/hour], phase [degrees], amplitude [meters] + double h_0 = 0.0; // mean height of water level above the datum + // Approx tidal current with 90 deg shift to height of the tide + double h_M2 = + M2_amp * cos( + (M2_speed / 180.0 * M_PI / 3600) * (currentTime * 86400000.0 / 1000.0) + + (M2_phase / 180 * M_PI) - (M_PI / 2.0)); + double h_S2 = + S2_amp * cos( + (S2_speed / 180.0 * M_PI / 3600) * (currentTime * 86400000.0 / 1000.0) + + (S2_phase / 180 * M_PI) - (M_PI / 2.0)); + double h_N2 = + N2_amp * cos( + (N2_speed / 180.0 * M_PI / 3600) * (currentTime * 86400000.0 / 1000.0) + + (N2_phase / 180 * M_PI) - (M_PI / 2.0)); + currentVelocity = h_0 + (h_M2 + h_S2 + h_N2); + } + else + { + // Search current time index from database + int currentI = 0; + for (size_t i = 0; i < this->datenum.size(); i++) + { + if (this->datenum[i] > currentTime) + { + currentI = i; + break; + } + } + + // Interpolate from database (linear) + // boost::math::barycentric_rational + // interp(datenum.data(), speedcmsec.data(), datenum.size()); + currentVelocity = (this->speedcmsec[currentI] - this->speedcmsec[currentI - 1]) / + (this->datenum[currentI] - this->datenum[currentI - 1]) * + (currentTime - this->datenum[currentI - 1]) + + this->speedcmsec[currentI - 1]; + + // Change units to m/s + currentVelocity = currentVelocity / 100.0; + } + + // Calculate current + if (currentVelocity > 0) // flood + { + currentType = true; + // north current velocity + currents.first = -cos(this->floodDirection / 180.0 * M_PI) * currentVelocity; + // east current velocity + currents.second = -sin(this->floodDirection / 180.0 * M_PI) * currentVelocity; + } + else // ebb + { + currentType = false; + // north current velocity + currents.first = -cos(this->ebbDirection / 180.0 * M_PI) * currentVelocity; + // east current velocity + currents.second = -sin(this->ebbDirection / 180.0 * M_PI) * currentVelocity; + } + + // Apply stratified current ratio + currents.first = currents.first * _currentDepthRatio; + currents.second = currents.second * _currentDepthRatio; + + return currents; // in m/s +} +} // namespace gz diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 7c33aafb..7f32fcd0 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(gz-common5 REQUIRED COMPONENTS profiler) find_package(gz-sim8 REQUIRED) find_package(geometry_msgs REQUIRED) find_package(dave_interfaces REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) # Set version variables set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) @@ -19,14 +20,19 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") +# Libraries add_library(SphericalCoords SHARED src/SphericalCoords.cc) +add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cpp) +# Include directories target_include_directories(SphericalCoords PRIVATE include) +target_include_directories(ocean_current_plugin PRIVATE include) -target_link_libraries(SphericalCoords - gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) +# Linking +target_link_libraries(SphericalCoords gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) +target_link_libraries(ocean_current_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) -# Specify dependencies for FullSystem using ament_target_dependencies +# Specify dependencies using ament_target_dependencies ament_target_dependencies(SphericalCoords dave_interfaces rclcpp @@ -34,8 +40,20 @@ ament_target_dependencies(SphericalCoords std_msgs ) +ament_target_dependencies(ocean_current_plugin + rclcpp + std_msgs + geometry_msgs + dave_interfaces +) + +# Install targets +install(TARGETS SphericalCoords dave_ros_gz_plugins + DESTINATION lib/${PROJECT_NAME} +) + # Install targets -install(TARGETS SphericalCoords +install(TARGETS ocean_current_plugin DESTINATION lib/${PROJECT_NAME} ) @@ -45,14 +63,13 @@ install(DIRECTORY include/ ) # Environment hooks -ament_environment_hooks( -"${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") -# Testing setup +# Optional: Testing setup if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() # Configure ament -ament_package() +ament_package() \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/ocean_current_plugin.hh old mode 100755 new mode 100644 similarity index 67% rename from gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh rename to gazebo/dave_ros_gz_plugins/include/ocean_current_plugin.hh index c9d31f21..cc5b10c0 --- a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/ocean_current_plugin.hh @@ -20,24 +20,22 @@ #ifndef OCEAN_CURRENT_PLUGIN_H_ #define OCEAN_CURRENT_PLUGIN_H_ -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include @@ -55,8 +53,6 @@ namespace gz { namespace sim { -inline namespace GZ_SIM_VERSION_NAMESPACE // can remove this line if gazebo. -{ namespace systems { namespace dave_simulator_ros @@ -91,7 +87,6 @@ public: } } // namespace dave_simulator_ros } // namespace systems -} // namespace GZ_SIM_VERSION_NAMESPACE } // namespace sim } // namespace gz @@ -100,8 +95,4 @@ GZ_ADD_PLUGIN( UnderwaterCurrentROSPlugin, gz::sim::System, UnderwaterCurrentROSPlugin::ISystemConfigure, UnderwaterCurrentROSPlugin::ISystemPostUpdate) -// Add plugin alias so that we can refer to the plugin without the version -// namespace -GZ_ADD_PLUGIN_ALIAS(UnderwaterCurrentROSPlugin, "gz::sim::systems::UnderwaterCurrentROSPlugin") - #endif // OCEAN_CURRENT_PLUGIN_H_ diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt deleted file mode 100644 index a0e3be04..00000000 --- a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/CMakeLists.txt +++ /dev/null @@ -1,158 +0,0 @@ -# might remove this from here and put it in the main CMakeLists.txt -cmake_minimum_required(VERSION 3.0.2) -project(dave_gazebo_ros_plugins) - -if(NOT "${CMAKE_VERSION}" VERSION_LESS "3.16") - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -else() - add_compile_options(-std=c++11) -endif() - -find_package(catkin REQUIRED COMPONENTS - gazebo_dev - roscpp - gazebo_msgs - geometry_msgs - dave_gz_world_plugins - message_generation - std_msgs -) -find_package(Boost REQUIRED COMPONENTS system) - -set(DAVE_GAZEBO_ROS_PLUGINS_LIST "") - -# ocean current msg and services -add_service_files( - FILES - SetCurrentModel.srv - GetCurrentModel.srv - SetCurrentVelocity.srv - SetCurrentDirection.srv - SetStratifiedCurrentVelocity.srv - SetStratifiedCurrentDirection.srv - SetOriginSphericalCoord.srv - GetOriginSphericalCoord.srv - TransformToSphericalCoord.srv - TransformFromSphericalCoord.srv -) - -add_message_files( - FILES - StratifiedCurrentDatabase.msg - StratifiedCurrentVelocity.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES - dave_ocean_current_plugin - dave_sc_coordinates_interface - CATKIN_DEPENDS - roscpp - gazebo_msgs - geometry_msgs - dave_gazebo_world_plugins - gazebo_dev - std_msgs - geometry_msgs - message_runtime -) - -include_directories(${PROJECT_SOURCE_DIR}/include - ${Boost_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} - ${GAZEBO_MSG_INCLUDE_DIRS} -# ... -) - -link_directories(${catkin_LIBRARY_DIRS} - ${GAXEBO_LIBRARY_DIRS} -# ... -) - -# ocean current plugin -add_library(dave_ocean_current_plugin - SHARED - src/ocean_current_plugin.cpp -) -add_dependencies(dave_ocean_current_plugin - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(dave_ocean_current_plugin - ${catkin_LIBRARIES} -) -list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_ocean_current_plugin) - -# pulse lidar plugin -add_library(dave_pulse_lidar_plugin - SHARED - src/pulse_lidar_plugin.cpp -) -add_dependencies(dave_pulse_lidar_plugin - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(dave_pulse_lidar_plugin - ${catkin_LIBRARIES} -) -list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_pulse_lidar_plugin) - -# spherical coordinate interfafce -add_library(dave_sc_coordinates_interface - SHARED - src/spherical_coordinates_interface.cpp -) -add_dependencies(dave_sc_coordinates_interface - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(dave_sc_coordinates_interface - ${catkin_LIBRARIES} -) -list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST dave_sc_coordinates_interface) - -# Plugin compile directions - -#add_library(plugin_name -# SHARED -# src/plugin_source_file1.cc -# src/plugin_source_file2.cc -# ... -#) -#add_dependencies(plugin_name -# ${catkin_EXPORTED_TARGETS} -# ... -#) -#target_link_libraries(plugin_name -# ${catkin_LIBRARIES} -# ... -#) -#list(APPEND DAVE_GAZEBO_ROS_PLUGINS_LIST plugin_name) - -# Plugin install directions - -install(TARGETS ${DAVE_GAZEBO_ROS_PLUGINS_LIST} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN "*~" EXCLUDE -) - -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN ".h" -) - diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml index b240b498..037c1ab9 100644 --- a/gazebo/dave_ros_gz_plugins/package.xml +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -1,14 +1,35 @@ - + dave_ros_gz_plugins 0.0.0 TODO: Package description lena + Gaurav Kumar TODO: License declaration ament_cmake + protobuf-dev rclcpp std_msgs dave_interfaces + sensor_msgs + geometry_msgs + ros_gz_bridge + ros_gz_sim + gz_msgs + gz_dev + dave_gz_world_plugins + gz_ros + protobuf + message_generation + rclcpp + dave_interfaces + gz_ros + protobuf + gz_ros + protobuf + message_runtime + rclcpp + dave_interfaces ament_lint_auto ament_lint_common diff --git a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cpp similarity index 76% rename from gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp rename to gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cpp index 4b142e4c..2d6a8698 100755 --- a/gazebo/dave_ros_gz_plugins/ocean_current_plugin/ocean_current_plugin.cpp +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cpp @@ -1,50 +1,23 @@ -#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include -#include -#include +#include +#include #include #include -#include -#include #include +#include +#include #include -#include -#include +#include #include #include #include -// #include "dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.hpp" -// #include "dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.hpp" -// #include "dave_ros_gz_plugins/srv/SetCurrentModel.hpp" -// #include "dave_ros_gz_plugins/srv/GetCurrentModel.hpp" -// #include "dave_ros_gz_plugins/srv/SetCurrentVelocity.hpp" -// #include "dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.hpp" -// #include "dave_ros_gz_plugins/srv/SetCurrentAngle.hpp" -// #include "dave_ros_gz_plugins/srv/SetStratifiedCurrentAngle.hpp" - // Things to consider/decide for now - // 1. Is consistency being maintained while the use of "gzerr" and "RCLCPP_ERROR" for error logging // ? @@ -67,27 +40,24 @@ class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin private: rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating // communication with other ROS nodes. - rclcpp::TimerBase::SharedPtr rosPublishTimer; - gz::common::Time rosPublishPeriod; - gz::common::Time lastRosPublishTime; std::string db_path; }; ///////////////////////////////////////////////// UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() { - this->rosPublishPeriod = gz::common::Time(0.05); - this->lastRosPublishTime = gz::common::Time(0.0); - this->db_path = - ament_index_cpp::get_package_share_directory("dave_worlds"); // might need to be (updated) + // this->rosPublishPeriod = gz::common::Time(0.05); + // this->lastRosPublishTime = gz::common::Time(0.0); + this->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); // Initialize the ROS 2 node this->rosNode = std::make_shared("underwater_current_ros_plugin"); + std::chrono::steady_clock::duration lastUpdate{0}; - // Setting up a timer in ROS 2 - this->rosPublishTimer = this->rosNode->create_wall_timer( - std::chrono::milliseconds(static_cast(this->rosPublishPeriod.Double() * 1000)), - [this]() { this->PublishCurrentInfo(); }); + // Setting up a timer in ROS 2 Do we need this ? (to be (updated)) + // this->rosPublishTimer = this->rosNode->create_wall_timer( + // std::chrono::milliseconds(static_cast(this->rosPublishPeriod.Double() * 1000)), + // [this]() { this->PublishCurrentInfo(); }); } ///////////////////////////////////////////////// @@ -98,25 +68,31 @@ UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() } ///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr _sdf) -{ // make sure this is closed +void UnderwaterCurrentPlugin::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) +{ + this->dataPtr->_world = _entity; + this->dataPtr->entity = _entity; + this->dataPtr->model = gz::sim::Model(_entity); + this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); + this->dataPtr->sdf = _sdf; + std::chrono::steady_clock::duration lastUpdate{0}; + // auto lastUpdate = gz::sim::simTime; try { - UnderwaterCurrentPlugin::Load(_world, _sdf); this->stratifiedCurrentVelocityDatabaseTopic = this->stratifiedCurrentVelocityTopic + "_database"; } - catch (const gz::common::Exception & e) // this is a gazebo error hence -gzerr, check for "_e" or - // "e" (to be (updated)) - // findings- we can use both "_e" and "e" but "e" is now - // the conventional way to display errors in C++. further - // writing "&e" or "& e" is a personal preference. + catch (const gz::common::Exception & e) + // we can use both "_e" and "e" but "e" is now + // the conventional way to display errors in C++. { gzerr << "Error loading plugin: " << e.what() << '\n'; return; } - if (!rclcpp::ok()) // this is a ROS 2 error hence -RCLCPP_ERROR to be (considered) (updated) + if (!rclcpp::ok()) { gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your " "ROS 2 environment.\n"; @@ -124,6 +100,8 @@ void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr } this->ns = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + + // Initialising ROS 2 node this->rosNode = std::make_shared("underwater_current_ros_plugin", this->ns); // Create and advertise Messages @@ -149,13 +127,6 @@ void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr // check for the number of arg being passed to the function - // Advertise the service to update the current velocity model - this->worldServices["set_current_velocity_model"] = - this->rosNode->create_service( - "set_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, - std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to get the current velocity model this->worldServices["get_current_velocity_model"] = this->rosNode->create_service( @@ -163,13 +134,6 @@ void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current horizontal angle model - this->worldServices["set_current_horz_angle_model"] = - this->rosNode->create_service( - "set_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to get the current horizontal angle model this->worldServices["get_current_horz_angle_model"] = this->rosNode->create_service( @@ -177,13 +141,6 @@ void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current vertical angle model - this->worldServices["set_current_vert_angle_model"] = - this->rosNode->create_service( - "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to get the current vertical angle model this->worldServices["get_current_vert_angle_model"] = this->rosNode->create_service( @@ -191,60 +148,42 @@ void UnderwaterCurrentROSPlugin::Load(gz::sim::WorldPtr _world, sdf::ElementPtr &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current velocity mean value - this->worldServices["set_current_velocity"] = - this->rosNode->create_service( - "set_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the stratified current velocity - this->worldServices["set_stratified_current_velocity"] = - this->rosNode->create_service( - "set_stratified_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, - this, std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current horizontal angle - this->worldServices["set_current_horz_angle"] = - this->rosNode->create_service( - "set_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, - std::placeholders::_1, std::placeholders::_2)); + // Connect to the Gazebo Harmonic update event + // this->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & info) + // { this->OnUpdateCurrentVel(info); }); +} - // Advertise the service to update the stratified current horizontal angle - this->worldServices["set_stratified_current_horz_angle"] = - this->rosNode->create_service( - "set_stratified_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, - this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current vertical angle - this->worldServices["set_current_vert_angle"] = - this->rosNode->create_service( - "set_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, - std::placeholders::_1, std::placeholders::_2)); +///////////////////////////////////////////////// +void gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + // Advertise the service to get the current velocity model + this->worldServices["get_current_velocity_model"] = + this->rosNode->create_service( + "get_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, + std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current vertical angle - this->worldServices["set_stratified_current_vert_angle"] = - this->rosNode->create_service( - "set_stratified_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, - this, std::placeholders::_1, std::placeholders::_2)); + // Advertise the service to get the current horizontal angle model + this->worldServices["get_current_horz_angle_model"] = + this->rosNode->create_service( + "get_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); - // Connect to the Gazebo Harmonic update event - this->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & info) - { this->OnUpdateCurrentVel(info); }); + // Advertise the service to get the current vertical angle model + this->worldServices["get_current_vert_angle_model"] = + this->rosNode->create_service( + "get_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); } - ///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::OnUpdateCurrentVel() +void UnderwaterCurrentROSPlugin::Update( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - if (this->lastUpdate - this->lastRosPublishTime >= this->rosPublishPeriod) + if (_info.simTime > this->lastUpdate) { - // Update the time tracking for publication - this->lastRosPublishTime = this->lastUpdate; - // Generate and publish current velocity according to the vehicle depth geometry_msgs::msg::TwistStamped flowVelMsg; flowVelMsg.header.stamp = this->rosNode->get_clock()->now(); @@ -263,7 +202,7 @@ void UnderwaterCurrentROSPlugin::OnUpdateCurrentVel() stratCurrentVelocityMsg.header.frame_id = "world"; // Updating for stratified behaviour of Ocean Currents - // What is the .size value over here, to be (updated)(checked) + // What is the .size value over here, to be (checked) for (size_t i = 0; i < this->currentStratifiedVelocity.size(); i++) { geometry_msgs::msg::Vector3 velocity; @@ -273,7 +212,7 @@ void UnderwaterCurrentROSPlugin::OnUpdateCurrentVel() stratCurrentVelocityMsg.velocities.push_back(velocity); stratCurrentVelocityMsg.depths.push_back(this->currentStratifiedVelocity[i].W()); } - // updated till here + this->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database @@ -331,8 +270,75 @@ void UnderwaterCurrentROSPlugin::OnUpdateCurrentVel() this->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); } } -// updated till here +///////////////////////////////////////////////// +void gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + // Advertise the service to update the current velocity model + this->worldServices["set_current_velocity_model"] = + this->rosNode->create_service( + "set_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, + std::placeholders::_1, std::placeholders::_2)); + // Advertise the service to update the current horizontal angle model + this->worldServices["set_current_horz_angle_model"] = + this->rosNode->create_service( + "set_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current vertical angle model + this->worldServices["set_current_vert_angle_model"] = + this->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current velocity mean value + this->worldServices["set_current_velocity"] = + this->rosNode->create_service( + "set_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current velocity + this->worldServices["set_stratified_current_velocity"] = + this->rosNode->create_service( + "set_stratified_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle + this->worldServices["set_current_horz_angle"] = + this->rosNode->create_service( + "set_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, + std::placeholders::_1, std::placeholders::_2)); + // Advertise the service to update the stratified current horizontal angle + this->worldServices["set_stratified_current_horz_angle"] = + this->rosNode->create_service( + "set_stratified_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current vertical angle + this->worldServices["set_current_vert_angle"] = + this->rosNode->create_service( + "set_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current vertical angle + this->worldServices["set_stratified_current_vert_angle"] = + this->rosNode->create_service( + "set_stratified_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, + this, std::placeholders::_1, std::placeholders::_2)); + + // Update the time tracking for publication + this->lastUpdate = _info.simTime; +} ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( const std::shared_ptr _req, @@ -365,7 +371,7 @@ bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( } return true; } -// -updated till here td + ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateVertAngle( const std::shared_ptr _req, @@ -399,17 +405,16 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( this->currentHorzAngleModel.SetMean(_req->horizontal_angle) && this->currentVertAngleModel.SetMean(_req->vertical_angle)) { - RCLCPP_INFO( - this->rosNode->get_logger(), - "Current velocity [m/s] = %f\nCurrent horizontal angle [rad] = %f\nCurrent vertical angle " - "[rad] = %f\n\tWARNING: Current velocity calculated in the ENU frame", - _req->velocity, _req->horizontal_angle, _req->vertical_angle); - _res->success = true; + 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 + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; } else { - RCLCPP_ERROR(this->rosNode->get_logger(), "Error while updating the current velocity"); - _res->success = false; + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; } return true; } @@ -432,17 +437,16 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( // Update the database values as well this->stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; this->stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; - RCLCPP_INFO( - this->rosNode->get_logger(), - "Layer %d current velocity [m/s] = %f\n Horizontal angle [rad] = %f\n Vertical angle [rad] " - "= %f\n\tWARNING: Current velocity calculated in the ENU frame", - _req->layer, _req->velocity, _req->horizontal_angle, _req->vertical_angle); - _res->success = true; + gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl + << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl + << " Vertical angle [rad] = " << _req.vertical_angle << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; } else { - RCLCPP_ERROR(this->rosNode->get_logger(), "Error while updating the current velocity"); - _res->success = false; + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; } return true; } @@ -497,30 +501,17 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) { - gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][0]; + gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][0]; //(updated) model.SetModel( model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); } - RCLCPP_INFO( - this->rosNode->get_logger(), - "Current velocity model updated\n\tWARNING: Current velocity calculated in the ENU frame"); - - // Assuming currentVelModel.Print() prints model details, you would instead log these details - // For instance, you could log mean, min, max etc. - // Example of logging additional model details: - RCLCPP_INFO( - this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f", - this->currentVelModel.mean, this->currentVelModel.min, this->currentVelModel.max); + gzmsg << "Current velocity model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->currentVelModel.Print(); return true; - - // gzmsg << "Current velocity model updated" << std::endl - // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->currentVelModel.Print(); - // return true; } -// -updated till here td ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( const std::shared_ptr _req, @@ -536,17 +527,9 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); } - RCLCPP_INFO( - this->rosNode->get_logger(), - "Horizontal angle model updated\n\tWARNING: Current velocity calculated in the ENU frame"); - - // Assuming currentHorzAngleModel.Print() prints model details, you would instead log these - // details Example of logging additional model details: - RCLCPP_INFO( - this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f", - this->currentHorzAngleModel.mean, this->currentHorzAngleModel.min, - this->currentHorzAngleModel.max); - + gzmsg << "Horizontal angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->currentHorzAngleModel.Print(); return true; // gzmsg << "Horizontal angle model updated" << std::endl @@ -562,21 +545,10 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( _res->success = this->currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); - RCLCPP_INFO( - this->rosNode->get_logger(), - "Vertical angle model updated\n\tWARNING: Current velocity calculated in the ENU frame"); - - // Assuming currentVertAngleModel.Print() prints model details, you would instead log these - // details Example of logging additional model details: - RCLCPP_INFO( - this->rosNode->get_logger(), "Model details: Mean: %f, Min: %f, Max: %f, Mu: %f, Noise: %f", - this->currentVertAngleModel.mean, this->currentVertAngleModel.min, - this->currentVertAngleModel.max, this->currentVertAngleModel.mu, - this->currentVertAngleModel.noiseAmp); + gzmsg << "Vertical angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->currentVertAngleModel.Print(); return true; - // gzmsg << "Vertical angle model updated" << std::endl - // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->currentVertAngleModel.Print(); } ///////////////////////////////////////////////// From 286a21d575f5ab9d52a3a11aede5c0288758745d Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 21 Jul 2024 00:30:01 +0530 Subject: [PATCH 03/79] updated the ocean current model plugin --- .../include/ocean_current_model_plugin.hh | 158 ++--------- .../src/ocean_current_model_plugin.cpp | 265 ++++++++++++++---- 2 files changed, 230 insertions(+), 193 deletions(-) diff --git a/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh index f189447e..c255cac8 100644 --- a/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh @@ -28,19 +28,18 @@ namespace sim { namespace systems { -namespace dave_gz_sensor_plugins +namespace dave_gz_model_plugins { -class TransientCurrentPlugin : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemUpdate, - public ISystemPostUpdate, +class TransientCurrentPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemUpdate, + public gz::sim::ISystemPostUpdate, public ModelPlugin { - /// \brief Class constructor public: - TransientCurrentPlugin(); // constructor - ~TransientCurrentPlugin() override; // Destructor + TransientCurrentPlugin(); // constructor + ~TransientCurrentPlugin(); // Destructor // Configure the plugin with necessary entities and component managers void Configure( @@ -50,6 +49,9 @@ public: // Function called before the simulation state updates void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void TransientCurrentPlugin::Update( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + // Function called after the simulation state updates void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); @@ -62,135 +64,27 @@ public: // Initialize any necessary states before the plugin starts virtual void Init(); - /// \brief Pointer to world -protected: - physics::WorldPtr world; - - /// \brief Pointer to model - gz::sim::Entity entity{gz::sim::kNullEntity}; - gz::sim::Model model{gz::sim::kNullEntity}; - gz::sim::Entity modelLink{gz::sim::kNullEntity}; - - /// \brief Namespace for topics and services - std::string ns; - - /// \brief Connects the update event callback - virtual void Connect(); - - /// \brief Pointer to a node for communication - gz::transport::NodePtr node; - - /// \brief Map of publishers - std::map publishers; - - /// \brief Current velocity topic and transient current velocity topic - std::string currentVelocityTopic; - std::string transientCurrentVelocityTopic; - - /// \brief Convey model state from gazebo topic to outside - - virtual void UpdateDatabase( - const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); - - /// \brief Last update time stamp - common::Time lastUpdate; - - /// \brief Last depth index - int lastDepthIndex; - - /// \brief Current linear velocity vector - gz::math::Vector3d currentVelocity; - - /// \brief Gauss-Markov process instance for the velocity components - GaussMarkovProcess currentVelNorthModel; - GaussMarkovProcess currentVelEastModel; - GaussMarkovProcess currentVelDownModel; - - /// \brief Gauss-Markov noise - double noiseAmp_North; - double noiseAmp_East; - double noiseAmp_Down; - double noiseFreq_North; - double noiseFreq_East; - double noiseFreq_Down; - - /// \brief Vector to read database - std::vector database; - - /// \brief Tidal Oscillation interpolation model - TidalOscillation tide; - - /// \brief Tidal Oscillation flag - bool tideFlag; - - /// \brief Vector to read timeGMT - std::vector> timeGMT; - - /// \brief Vector to read tideVelocities - std::vector tideVelocities; - - /// \brief Tidal current harmonic constituents - bool tide_Constituents; - double M2_amp; - double M2_phase; - double M2_speed; - double S2_amp; - double S2_phase; - double S2_speed; - double N2_amp; - double N2_phase; - double N2_speed; - - /// \brief Tidal oscillation mean ebb direction - double ebbDirection; - - /// \brief Tidal oscillation mean flood direction - double floodDirection; - - /// \brief Tidal oscillation world start time (GMT) - std::array world_start_time; - - /// \brief Private data pointer -private: - std::unique_ptr dataPtr; - std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr flowVelocityPub; - -private: - /// \brief Pointer to this ROS node's handle. - std::shared_ptr rosNode; - - /// \brief Publisher for the flow velocity in the world frame - rclcpp::Publisher flowVelocityPub; - - /// \brief Subscriber for the transient ocean current database - rclcpp::Subscriber databaseSub; - - /// \brief A ROS callbackqueue that helps process messages - rclcpp::CallbackQueue databaseSubQueue; - - /// \brief A thread the keeps running the rosQueue - std::thread databaseSubQueueThread; - - /// \brief A thread mutex to lock - std::mutex lock_; - - /// \brief Period after which we should publish a message via ROS. - std::chrono::steady_clock::duration rosPublishPeriod{0}; - - /// \brief Last time we published a message via ROS. - std::chrono::steady_clock::duration lastRosPublishTime{0}; - /// \brief ROS helper function that processes messages void databaseSubThread(); /// \brief Calculate ocean current using database and vehicle state - void CalculateOceanCurrent(); + void TransientCurrentPlugin::CalculateOceanCurrent( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + + void TransientCurrentPlugin::PublishCurrentVelocity( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + + void TransientCurrentPlugin::Gauss_Markov_process_initialize( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + + TransientCurrentPlugin::UpdateDatabase( + const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) - /// \brief Publish ocean current - void PublishCurrentVelocity(); + /// \brief Publish ocean current + void TransientCurrentPlugin::PublishCurrentVelocity( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) } -} // namespace dave_gz_sensor_plugins +} // namespace dave_gz_model_plugins } // namespace systems } // namespace sim } // namespace gz diff --git a/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp index 122e8c06..e51e3c65 100644 --- a/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp +++ b/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp @@ -46,22 +46,20 @@ // Register plugin GZ_ADD_PLUGIN( - gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin, gz::sim::System, - gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemConfigure, - gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemUpdate, - gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin::ISystemPostUpdate) + dave_gz_model_plugins::TransientCurrentPlugin, gz::sim::System, + dave_gz_model_plugins::TransientCurrentPlugin::ISystemConfigure, + dave_gz_model_plugins::TransientCurrentPlugin::ISystemPreUpdate, + dave_gz_model_plugins::TransientCurrentPlugin::ISystemUpdate, + dave_gz_model_plugins::TransientCurrentPlugin::ISystemPostUpdate) // Add plugin alias so that we can refer to the plugin without the version // namespace -GZ_ADD_PLUGIN_ALIAS( - gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin, "TransientCurrentPlugin") +GZ_ADD_PLUGIN_ALIAS(dave_gz_model_plugins::TransientCurrentPlugin, "TransientCurrentPlugin") -using namespace gz; -using namespace sim; -using namespace systems; -using namespace dave_gz_sensor_plugins; +namespace dave_gz_model_plugins +{ -class gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin // dave_model_systems +class TransientCurrentPlugin // dave_model_systems { public: TransientCurrentPlugin(); /// \brief Class constructor @@ -71,20 +69,161 @@ class gz::sim::systems::dave_gz_sensor_plugins::TransientCurrentPlugin // dave_ std::shared_ptr rosNode; // This is a smart pointer to a ROS 2 node, facilitating // communication with other ROS nodes. std::string transientCurrentVelocityTopic; // Declare the variable (updated) -} - -//////////////////////////////////// +}; -TransientCurrentPlugin::TransientCurrentPlugin() +struct TransientCurrentPlugin::PrivateData { - this->Configure( - const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + // starts here + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; + + // Initialize any necessary states before the plugin starts + virtual void Init(); + + /// \brief Pointer to world +protected: + physics::WorldPtr world; + + /// \brief Pointer to model + gz::sim::Entity entity{gz::sim::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; + gz::sim::Entity modelLink{gz::sim::kNullEntity}; + + /// \brief Namespace for topics and services + std::string ns; + + /// \brief Connects the update event callback + virtual void Connect(); + + /// \brief Pointer to a node for communication + gz::transport::NodePtr node; + + /// \brief Map of publishers + std::map publishers; + + /// \brief Current velocity topic and transient current velocity topic + std::string currentVelocityTopic; + std::string transientCurrentVelocityTopic; + + /// \brief Convey model state from gazebo topic to outside + + virtual void UpdateDatabase( + const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); + + /// \brief Last update time stamp + common::Time lastUpdate; + + /// \brief Last depth index + int lastDepthIndex; + + /// \brief Current linear velocity vector + gz::math::Vector3d currentVelocity; + + /// \brief Gauss-Markov process instance for the velocity components + GaussMarkovProcess currentVelNorthModel; + GaussMarkovProcess currentVelEastModel; + GaussMarkovProcess currentVelDownModel; + + /// \brief Gauss-Markov noise + double noiseAmp_North; + double noiseAmp_East; + double noiseAmp_Down; + double noiseFreq_North; + double noiseFreq_East; + double noiseFreq_Down; + + /// \brief Vector to read database + std::vector database; + + /// \brief Tidal Oscillation interpolation model + TidalOscillation tide; + + /// \brief Tidal Oscillation flag + bool tideFlag; + + /// \brief Vector to read timeGMT + std::vector> timeGMT; + + /// \brief Vector to read tideVelocities + std::vector tideVelocities; + + /// \brief Tidal current harmonic constituents + bool tide_Constituents; + double M2_amp; + double M2_phase; + double M2_speed; + double S2_amp; + double S2_phase; + double S2_speed; + double N2_amp; + double N2_phase; + double N2_speed; + + /// \brief Tidal oscillation mean ebb direction + double ebbDirection; + + /// \brief Tidal oscillation mean flood direction + double floodDirection; + + /// \brief Tidal oscillation world start time (GMT) + std::array world_start_time; + + /// \brief Private data pointer +private: + std::unique_ptr dataPtr; + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr flowVelocityPub; + +private: + /// \brief Pointer to this ROS node's handle. + std::shared_ptr rosNode; + + /// \brief Publisher for the flow velocity in the world frame + rclcpp::Publisher flowVelocityPub; + + /// \brief Subscriber for the transient ocean current database + rclcpp::Subscriber databaseSub; + + /// \brief A ROS callbackqueue that helps process messages + rclcpp::CallbackQueue databaseSubQueue; + + /// \brief A thread the keeps running the rosQueue + std::thread databaseSubQueueThread; + + /// \brief A thread mutex to lock + std::mutex lock_; + + /// \brief Period after which we should publish a message via ROS. + std::chrono::steady_clock::duration rosPublishPeriod{0}; + + /// \brief Last time we published a message via ROS. + std::chrono::steady_clock::duration lastRosPublishTime{0}; + + /// \brief ROS helper function that processes messages + void databaseSubThread(); + + /// \brief Calculate ocean current using database and vehicle state + void CalculateOceanCurrent(); + + /// \brief Publish ocean current + void PublishCurrentVelocity(); + std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr flowVelocityPub; std::chrono::steady_clock::duration lastUpdate{0}; + // std::chrono::steady_clock::duration lastRosPublishTime{0}; + std::chrono::steady_clock::duration currentTime{0}; + this->dataPtr->rosPublishPeriod = 0.05; + // ends here }; +// constructor +TransientCurrentPlugin::TransientCurrentPlugin() : dataPtr(std::make_unique()) {} + TransientCurrentPlugin::~TransientCurrentPlugin() { // Shutdown the ROS 2 node @@ -100,7 +239,6 @@ void TransientCurrentPlugin::Configure( this->dataPtr->model = gz::sim::Model(_entity); this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); this->dataPtr->sdf = _sdf; - auto lastUpdate = gz::sim::simTime; gzmsg << "Loading transient ocean current model plugin..." << std::endl; @@ -166,8 +304,6 @@ void TransientCurrentPlugin::Configure( this->dataPtr->lastDepthIndex = 0; } - - // this->dataPtr->lastUpdate = _info.simTime } ///////////////////////////////////////////////// void TransientCurrentPlugin::Init() @@ -175,7 +311,8 @@ void TransientCurrentPlugin::Init() // Doing nothing for now } ///////////////////////////////////////////////// -void TransientCurrentPlugin::CalculateOceanCurrent() +void TransientCurrentPlugin::CalculateOceanCurrent( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { this->dataPtr->lock_.lock(); @@ -246,19 +383,19 @@ void TransientCurrentPlugin::CalculateOceanCurrent() if (this->dataPtr->tideFlag) { // Update tide oscillation - this->dataPtr->time = _info.simTime; + this->dataPtr->time = this->dataPtr->currentTime; if (this->dataPtr->tide_Constituents) { - this->dathis->dataPtr->tide.M2_amp = this->dathis->dataPtr->M2_amp; - this->dathis->dataPtr->tide.M2_phase = this->dathis->dataPtr->M2_phase; - this->dathis->dataPtr->tide.M2_speed = this->dathis->dataPtr->M2_speed; - this->dathis->dataPtr->tide.S2_amp = this->dathis->dataPtr->S2_amp; - this->dathis->dataPtr->tide.S2_phase = this->dathis->dataPtr->S2_phase; - this->dathis->dataPtr->tide.S2_speed = this->dathis->dataPtr->S2_speed; - this->dathis->dataPtr->tide.N2_amp = this->dathis->dataPtr->N2_amp; - this->dathis->dataPtr->tide.N2_phase = this->dathis->dataPtr->N2_phase; - this->dathis->dataPtr->tide.N2_speed = this->dathis->dataPtr->N2_speed; + this->dataPtr->tide.M2_amp = this->dataPtr->M2_amp; + this->dataPtr->tide.M2_phase = this->dataPtr->M2_phase; + this->dataPtr->tide.M2_speed = this->dataPtr->M2_speed; + this->dataPtr->tide.S2_amp = this->dataPtr->S2_amp; + this->dataPtr->tide.S2_phase = this->dataPtr->S2_phase; + this->dataPtr->tide.S2_speed = this->dataPtr->S2_speed; + this->dataPtr->tide.N2_amp = this->dataPtr->N2_amp; + this->dataPtr->tide.N2_phase = this->dataPtr->N2_phase; + this->dataPtr->tide.N2_speed = this->dataPtr->N2_speed; } else { @@ -296,7 +433,7 @@ void TransientCurrentPlugin::CalculateOceanCurrent() this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; // Update time - this->dataPtr->time = _info.simTime; + this->dataPtr->time = this->dataPtr->lastUpdate; // Update current velocity double velocityNorth = @@ -318,19 +455,17 @@ void TransientCurrentPlugin::CalculateOceanCurrent() this->dataPtr->lock_.unlock(); } ///////////////////////////////////////////////// -void TransientCurrentPlugin::PublishCurrentVelocity() +void TransientCurrentPlugin::PublishCurrentVelocity( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - // Generate and publish ROS topic according to the vehicle depth - if (_info.simeTime > this->dataPtr->lastRosPublishTime) - { - geometry_msgs::TwistStamped flowVelMsg; - flowVelMsg.header.stamp = rclcpp::Time().now(); - flowVelMsg.header.frame_id = "/world"; - flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); - flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); - flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); - this->dataPtr->flowVelocityPub.publish(flowVelMsg); - } + geometry_msgs::TwistStamped flowVelMsg; + flowVelMsg.header.stamp = rclcpp::Time().now(); + flowVelMsg.header.frame_id = "/world"; + flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + this->dataPtr->flowVelocityPub.publish(flowVelMsg); + // Generate and publish Gazebo topic according to the vehicle depth msgs::Vector3d currentVel; msgs::Set( @@ -340,12 +475,6 @@ void TransientCurrentPlugin::PublishCurrentVelocity() this->dataPtr->publishers[this->dataPtr->currentVelocityTopic]->Publish(currentVel); } -///////////////////////////////////////////////// -void TransientCurrentPlugin::PostPublishCurrentVelocity() -{ - this->dataPtr->lastRosPublishTime = _info.simeTime; -} - ///////////////////////////////////////////////// TransientCurrentPlugin::UpdateDatabase( const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) @@ -501,11 +630,9 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize( << "Gauss-Markov process model:" << std::endl; this->dataPtr->currentVelDownModel.Print(); - this->dataPtr->lastUpdate = _info - .simTime -#endif // need to look into this - this->dataPtr->currentVelNorthModel.lastUpdate = - this->dataPtr->lastUpdate.Double(); + // this->dataPtr->lastUpdate = _info.simTime; This isn't needed because the last update is being + // initialised to 0 and is being updated at postupdate + this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.Double(); this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.Double(); this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.Double(); } @@ -513,7 +640,10 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize( void TransientCurrentPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - this->dataPtr->Gauss_Markov_process_initialize(); // something is wrong here + this->dataPtr->currentTime = _info.simTime; + this->dataPtr->time = this->dataPtr->currentTime; + this->dataPtr->Gauss_Markov_process_initialize(); // something is wrong here (check) + // Subscribe stratified ocean current database this->dataPtr->databaseSub = this->dataPtr->rosNode @@ -525,7 +655,6 @@ void TransientCurrentPlugin::PreUpdate( this->dataPtr->Connect(); gzmsg << "Transient current model plugin loaded!" << std::endl; - this->dataPtr->lastUpdate = this->dataPtr->time; } ///////////////////////////////////////////////// @@ -535,8 +664,22 @@ void TransientCurrentPlugin::Update( this->dataPtr->calculateOceanCurrent(); } ///////////////////////////////////////////////// -void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +void TransientCurrentPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - PostPublishCurrentVelocity(); - this->dataPtr->lastRosPublishTime = _info.simeTime; + // Publish the Current Velocity. + // if ((!_info.paused && _info.simTime > this->dataPtr->lastUpdate) && (_info.simTime - + // this->dataPtr->lastUpdate >= this->dataPtr->rosPublishPeriod)) + // { + // this->dataPtr->lastUpdate = _info.simTime; + // PostPublishCurrentVelocity(); + // } + + // Publish the Current Velocity. + if (!_info.paused && _info.simTime > this->dataPtr->lastUpdate) + { + this->dataPtr->lastUpdate = _info.simTime; + PostPublishCurrentVelocity(); + } } +} // namespace dave_gz_model_plugins \ No newline at end of file From 8bf29631249efa5d3ac448b373a5f9e244dece90 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 21 Jul 2024 22:24:20 +0530 Subject: [PATCH 04/79] updated and adjusted the head node --- .../dave_gz_model_plugin/include/ocean_current_model_plugin.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh index c255cac8..8e462d34 100644 --- a/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh @@ -77,6 +77,7 @@ public: void TransientCurrentPlugin::Gauss_Markov_process_initialize( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + /// \brief Convey model state from gazebo topic to outside TransientCurrentPlugin::UpdateDatabase( const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) From 81f4d94715c2508981ffaacf06b6167eebea6f37 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 25 Jul 2024 14:37:43 +0530 Subject: [PATCH 05/79] addidng service and msg, updated cmake --- dave_interfaces/CMakeLists.txt | 8 +++++++ .../msg/StratifiedCurrentDatabase.msg | 0 .../msg/StratifiedCurrentVelocity.msg | 0 .../srv/GetCurrentModel.srv | 0 .../srv/SetCurrentDirection.srv | 0 .../srv/SetCurrentModel.srv | 0 .../srv/SetCurrentVelocity.srv | 0 .../srv/SetStratifiedCurrentDirection.srv | 0 .../srv/SetStratifiedCurrentVelocity.srv | 0 .../srv/GetOriginSphericalCoord.srv | 22 ------------------ .../srv/SetOriginSphericalCoord.srv | 23 ------------------- .../srv/TransformFromSphericalCoord.srv | 23 ------------------- .../srv/TransformToSphericalCoord.srv | 23 ------------------- 13 files changed, 8 insertions(+), 91 deletions(-) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/msg/StratifiedCurrentDatabase.msg (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/msg/StratifiedCurrentVelocity.msg (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/GetCurrentModel.srv (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/SetCurrentDirection.srv (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/SetCurrentModel.srv (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/SetCurrentVelocity.srv (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/SetStratifiedCurrentDirection.srv (100%) rename {gazebo/dave_ros_gz_plugins => dave_interfaces}/srv/SetStratifiedCurrentVelocity.srv (100%) delete mode 100644 gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv delete mode 100644 gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv delete mode 100644 gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv delete mode 100644 gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 117faaff..eacbe332 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -16,10 +16,18 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" "msg/UsblResponse.msg" "msg/Location.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 ) diff --git a/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg b/dave_interfaces/msg/StratifiedCurrentDatabase.msg similarity index 100% rename from gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentDatabase.msg rename to dave_interfaces/msg/StratifiedCurrentDatabase.msg diff --git a/gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg b/dave_interfaces/msg/StratifiedCurrentVelocity.msg similarity index 100% rename from gazebo/dave_ros_gz_plugins/msg/StratifiedCurrentVelocity.msg rename to dave_interfaces/msg/StratifiedCurrentVelocity.msg diff --git a/gazebo/dave_ros_gz_plugins/srv/GetCurrentModel.srv b/dave_interfaces/srv/GetCurrentModel.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/GetCurrentModel.srv rename to dave_interfaces/srv/GetCurrentModel.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentDirection.srv b/dave_interfaces/srv/SetCurrentDirection.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/SetCurrentDirection.srv rename to dave_interfaces/srv/SetCurrentDirection.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentModel.srv b/dave_interfaces/srv/SetCurrentModel.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/SetCurrentModel.srv rename to dave_interfaces/srv/SetCurrentModel.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/SetCurrentVelocity.srv b/dave_interfaces/srv/SetCurrentVelocity.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/SetCurrentVelocity.srv rename to dave_interfaces/srv/SetCurrentVelocity.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentDirection.srv b/dave_interfaces/srv/SetStratifiedCurrentDirection.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentDirection.srv rename to dave_interfaces/srv/SetStratifiedCurrentDirection.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.srv b/dave_interfaces/srv/SetStratifiedCurrentVelocity.srv similarity index 100% rename from gazebo/dave_ros_gz_plugins/srv/SetStratifiedCurrentVelocity.srv rename to dave_interfaces/srv/SetStratifiedCurrentVelocity.srv diff --git a/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv deleted file mode 100644 index 70f3aafc..00000000 --- a/gazebo/dave_ros_gz_plugins/srv/GetOriginSphericalCoord.srv +++ /dev/null @@ -1,22 +0,0 @@ -# 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. - ---- -# Latitude [degrees]. Positive is north of equator; negative is south. -float64 latitude_deg -# Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude_deg -# Altitude [m]. Positive is above the WGS 84 ellipsoid -float64 altitude diff --git a/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv deleted file mode 100644 index 80791954..00000000 --- a/gazebo/dave_ros_gz_plugins/srv/SetOriginSphericalCoord.srv +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -# Latitude [degrees]. Positive is north of equator; negative is south. -float64 latitude_deg -# Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude_deg -# Altitude [m]. Positive is above the WGS 84 ellipsoid -float64 altitude ---- -bool success diff --git a/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv deleted file mode 100644 index 6f4da710..00000000 --- a/gazebo/dave_ros_gz_plugins/srv/TransformFromSphericalCoord.srv +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -# Latitude [degrees]. Positive is north of equator; negative is south. -float64 latitude_deg -# Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude_deg -# Altitude [m]. Positive is above the WGS 84 ellipsoid -float64 altitude ---- -geometry_msgs/Vector3 output diff --git a/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv b/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv deleted file mode 100644 index 3ad501f5..00000000 --- a/gazebo/dave_ros_gz_plugins/srv/TransformToSphericalCoord.srv +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -geometry_msgs/Vector3 input ---- -# Latitude [degrees]. Positive is north of equator; negative is south. -float64 latitude_deg -# Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude_deg -# Altitude [m]. Positive is above the WGS 84 ellipsoid -float64 altitude From 9a2d810852627808794068ae4a0ccc30d859007c Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 25 Jul 2024 14:56:20 +0530 Subject: [PATCH 06/79] updated include file directory --- .../{ => dave_gz_model_plugin}/ocean_current_model_plugin.hh | 0 .../include/{ => dave_gz_world_plugins}/gauss_markov_process.hh | 0 .../{ => dave_gz_world_plugins}/ocean_current_world_plugin.hh | 0 .../include/{ => dave_gz_world_plugins}/tidal_oscillation.hh | 0 .../include/{ => dave_ros_gz_plugins}/ocean_current_plugin.hh | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename gazebo/dave_gz_model_plugin/include/{ => dave_gz_model_plugin}/ocean_current_model_plugin.hh (100%) rename gazebo/dave_gz_world_plugins/include/{ => dave_gz_world_plugins}/gauss_markov_process.hh (100%) rename gazebo/dave_gz_world_plugins/include/{ => dave_gz_world_plugins}/ocean_current_world_plugin.hh (100%) rename gazebo/dave_gz_world_plugins/include/{ => dave_gz_world_plugins}/tidal_oscillation.hh (100%) rename gazebo/dave_ros_gz_plugins/include/{ => dave_ros_gz_plugins}/ocean_current_plugin.hh (100%) diff --git a/gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugin/include/dave_gz_model_plugin/ocean_current_model_plugin.hh similarity index 100% rename from gazebo/dave_gz_model_plugin/include/ocean_current_model_plugin.hh rename to gazebo/dave_gz_model_plugin/include/dave_gz_model_plugin/ocean_current_model_plugin.hh diff --git a/gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh similarity index 100% rename from gazebo/dave_gz_world_plugins/include/gauss_markov_process.hh rename to gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh diff --git a/gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh similarity index 100% rename from gazebo/dave_gz_world_plugins/include/ocean_current_world_plugin.hh rename to gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh diff --git a/gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh similarity index 100% rename from gazebo/dave_gz_world_plugins/include/tidal_oscillation.hh rename to gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh diff --git a/gazebo/dave_ros_gz_plugins/include/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh similarity index 100% rename from gazebo/dave_ros_gz_plugins/include/ocean_current_plugin.hh rename to gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh From 425bb6831fcbc0a2aff14c476f6a7266c4751d77 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 25 Jul 2024 16:41:39 +0530 Subject: [PATCH 07/79] updated a few things and resolved bugs for dave_interfaces --- dave_interfaces/CMakeLists.txt | 2 +- .../msg/StratifiedCurrentDatabase.msg | 89 ++++++++++++++----- .../CMakeLists.txt | 13 ++- .../hooks/dave_ros_gz_plugins.dsv.in | 0 .../ocean_current_model_plugin.hh | 0 .../package.xml | 0 .../src/ocean_current_model_plugin.cpp | 1 - gazebo/dave_gz_world_plugins/CMakeLists.txt | 3 +- 8 files changed, 73 insertions(+), 35 deletions(-) rename gazebo/{dave_gz_model_plugin => dave_gz_model_plugins}/CMakeLists.txt (84%) rename gazebo/{dave_gz_model_plugin => dave_gz_model_plugins}/hooks/dave_ros_gz_plugins.dsv.in (100%) rename gazebo/{dave_gz_model_plugin => dave_gz_model_plugins}/include/dave_gz_model_plugin/ocean_current_model_plugin.hh (100%) rename gazebo/{dave_gz_model_plugin => dave_gz_model_plugins}/package.xml (100%) rename gazebo/{dave_gz_model_plugin => dave_gz_model_plugins}/src/ocean_current_model_plugin.cpp (99%) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index eacbe332..da8c93f6 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -25,7 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetCurrentModel.srv" "srv/SetCurrentModel.srv" "srv/SetCurrentDirection.srv" - "srv/setCurrentVelocity.srv" + "srv/SetCurrentVelocity.srv" "srv/SetStratifiedCurrentVelocity.srv" "srv/SetStratifiedCurrentDirection.srv" DEPENDENCIES geometry_msgs diff --git a/dave_interfaces/msg/StratifiedCurrentDatabase.msg b/dave_interfaces/msg/StratifiedCurrentDatabase.msg index 97c8eac4..1cfba138 100644 --- a/dave_interfaces/msg/StratifiedCurrentDatabase.msg +++ b/dave_interfaces/msg/StratifiedCurrentDatabase.msg @@ -7,34 +7,77 @@ float32[] depths geometry_msgs/Vector3[] velocities # Tide time (GMT) -int16[] timeGMTYear -int16[] timeGMTMonth -int16[] timeGMTDay -int16[] timeGMTHour -int16[] timeGMTMinute +int16[] gmtyear +int16[] timegmtmonth +int16[] timegmtday +int16[] timegmthour +int16[] timegmtminute # Tide velocities -float32[] tideVelocities +float32[] tidevelocities # Tide constituents -bool tideConstituents -float32 M2amp -float32 M2phase -float32 M2speed -float32 S2amp -float32 S2phase -float32 S2speed -float32 N2amp -float32 N2phase -float32 N2speed +bool tideconstituents +float32 m2amp +float32 m2phase +float32 m2speed +float32 s2amp +float32 s2phase +float32 s2speed +float32 n2amp +float32 n2phase +float32 n2speed # Tide direction -float32 ebbDirection -float32 floodDirection +float32 ebbdirection +float32 flooddirection # World start time (GMT) -int16 worldStartTimeYear -int16 worldStartTimeMonth -int16 worldStartTimeDay -int16 worldStartTimeHour -int16 worldStartTimeMinute \ No newline at end of file +int16 worldstarttimeyear +int16 worldstarttimemonth +int16 worldstarttimeday +int16 worldstarttimehour +int16 worldstarttimeminute + +# Everything is written in lowercase according to ROS 2 conventions old(v) + +# # Publishes depths and velocities read from the csv database + +# # Depths +# float32[] depths + +# # Velocities +# geometry_msgs/Vector3[] velocities + +# # Tide time (GMT) +# int16[] timeGMTYear +# int16[] timeGMTMonth +# int16[] timeGMTDay +# int16[] timeGMTHour +# int16[] timeGMTMinute + +# # Tide velocities +# float32[] tideVelocities + +# # Tide constituents +# bool tideConstituents +# float32 M2amp +# float32 M2phase +# float32 M2speed +# float32 S2amp +# float32 S2phase +# float32 S2speed +# float32 N2amp +# float32 N2phase +# float32 N2speed + +# # Tide direction +# float32 ebbDirection +# float32 floodDirection + +# # World start time (GMT) +# int16 worldStartTimeYear +# int16 worldStartTimeMonth +# int16 worldStartTimeDay +# int16 worldStartTimeHour +# int16 worldStartTimeMinute diff --git a/gazebo/dave_gz_model_plugin/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt similarity index 84% rename from gazebo/dave_gz_model_plugin/CMakeLists.txt rename to gazebo/dave_gz_model_plugins/CMakeLists.txt index 1dfb8f2e..4cef975c 100644 --- a/gazebo/dave_gz_model_plugin/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -3,9 +3,6 @@ cmake_minimum_required(VERSION 3.8) project(dave_gz_model_plugins) find_package(ament_cmake REQUIRED) -find_package(dave_gz_model_plugins REQUIRED) - -find_package(ros_gz_example_description REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(gz-cmake3 REQUIRED) find_package(gz-plugin2 REQUIRED COMPONENTS register) @@ -51,11 +48,11 @@ install(DIRECTORY include/ DESTINATION include/ ) -# # Following directives are used when testing. -# if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -# endif() +# Following directives are used when testing. +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() # Environment Hooks ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") diff --git a/gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_gz_model_plugins/hooks/dave_ros_gz_plugins.dsv.in similarity index 100% rename from gazebo/dave_gz_model_plugin/hooks/dave_ros_gz_plugins.dsv.in rename to gazebo/dave_gz_model_plugins/hooks/dave_ros_gz_plugins.dsv.in diff --git a/gazebo/dave_gz_model_plugin/include/dave_gz_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh similarity index 100% rename from gazebo/dave_gz_model_plugin/include/dave_gz_model_plugin/ocean_current_model_plugin.hh rename to gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh diff --git a/gazebo/dave_gz_model_plugin/package.xml b/gazebo/dave_gz_model_plugins/package.xml similarity index 100% rename from gazebo/dave_gz_model_plugin/package.xml rename to gazebo/dave_gz_model_plugins/package.xml diff --git a/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp similarity index 99% rename from gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp rename to gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp index e51e3c65..ed7777df 100644 --- a/gazebo/dave_gz_model_plugin/src/ocean_current_model_plugin.cpp +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp @@ -178,7 +178,6 @@ struct TransientCurrentPlugin::PrivateData std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr flowVelocityPub; -private: /// \brief Pointer to this ROS node's handle. std::shared_ptr rosNode; diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 4b283dfa..ae263a2e 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -4,7 +4,6 @@ cmake_minimum_required(VERSION 3.8) project(dave_gz_world_plugins) find_package(ament_cmake REQUIRED) -find_package(ros_gz_example_description REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(gz-cmake3 REQUIRED) find_package(gz-plugin2 REQUIRED COMPONENTS register) @@ -34,7 +33,7 @@ ament_target_dependencies(ocean_current_world_plugin "rclcpp" "std_msgs" "geometry_msgs" -# "dave_interfaces" + "dave_interfaces" ) # Install targets From e33faf43250eb0d0a2e2b23f11dca9bfcada75a7 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 26 Jul 2024 01:02:55 +0530 Subject: [PATCH 08/79] updates in code after fixing bugs --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 4 ++-- ...ns.dsv.in => dave_gz_model_plugins.dsv.in} | 0 .../ocean_current_model_plugin.hh | 1 - gazebo/dave_gz_model_plugins/package.xml | 2 ++ ...ugin.cpp => ocean_current_model_plugin.cc} | 2 +- gazebo/dave_gz_world_plugins/CMakeLists.txt | 21 +++++++++++++------ ...ns.dsv.in => dave_gz_world_plugins.dsv.in} | 0 gazebo/dave_gz_world_plugins/package.xml | 4 +++- ...ov_process.cpp => gauss_markov_process.cc} | 0 ...ugin.cpp => ocean_current_world_plugin.cc} | 0 ...l_oscillation.cpp => tidal_oscillation.cc} | 0 gazebo/dave_ros_gz_plugins/CMakeLists.txt | 2 +- gazebo/dave_ros_gz_plugins/package.xml | 1 - ...ent_plugin.cpp => ocean_current_plugin.cc} | 0 14 files changed, 24 insertions(+), 13 deletions(-) rename gazebo/dave_gz_model_plugins/hooks/{dave_ros_gz_plugins.dsv.in => dave_gz_model_plugins.dsv.in} (100%) rename gazebo/dave_gz_model_plugins/src/{ocean_current_model_plugin.cpp => ocean_current_model_plugin.cc} (99%) rename gazebo/dave_gz_world_plugins/hooks/{dave_ros_gz_plugins.dsv.in => dave_gz_world_plugins.dsv.in} (100%) rename gazebo/dave_gz_world_plugins/src/{gauss_markov_process.cpp => gauss_markov_process.cc} (100%) rename gazebo/dave_gz_world_plugins/src/{ocean_current_world_plugin.cpp => ocean_current_world_plugin.cc} (100%) rename gazebo/dave_gz_world_plugins/src/{tidal_oscillation.cpp => tidal_oscillation.cc} (100%) rename gazebo/dave_ros_gz_plugins/src/{ocean_current_plugin.cpp => ocean_current_plugin.cc} (100%) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index 4cef975c..d0c9f72f 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -20,8 +20,8 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") -add_library(dave_gz_model_plugins SHARED - src/ocean_current_model_plugin.cpp +add_library(ocean_current_model_plugin SHARED + src/ocean_current_model_plugin.cc ) target_include_directories(ocean_current_model_plugin PRIVATE include) diff --git a/gazebo/dave_gz_model_plugins/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in similarity index 100% rename from gazebo/dave_gz_model_plugins/hooks/dave_ros_gz_plugins.dsv.in rename to gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh index 8e462d34..da4b4803 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh @@ -1,7 +1,6 @@ #ifndef OCEAN_CURRENT_MODEL_PLUGIN_H_ #define OCEAN_CURRENT_MODEL_PLUGIN_H_ -#include #include // #include diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index 2c12b233..4e49d8eb 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -11,6 +11,7 @@ gz_ros rclcpp protobuf + dave_interfaces message_generation gz_ros protobuf @@ -19,6 +20,7 @@ rclcpp protobuf message_runtime + dave_interfaces gz_dev gz_msgs diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc similarity index 99% rename from gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp rename to gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index ed7777df..99e27688 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cpp +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -15,7 +15,7 @@ /// \file ocean_current_model_plugin.cc -#include "ocean_current_model_plugin.hh" +#include "dave_gz_model_plugin/ocean_current_model_plugin.hh" #include #include #include diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index ae263a2e..6be7cdca 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -21,12 +21,12 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) message(STATUS "compiling against Gazebo Harmonic") -add_library(dave_gz_world_plugins SHARED -src/ocean_current_world_plugins.cpp) +add_library(ocean_current_world_plugin SHARED +src/ocean_current_world_plugin.cc) -target_include_directories(ocean_current_world_plugins PRIVATE include) +target_include_directories(ocean_current_world_plugin PRIVATE include) -target_link_libraries(ocean_current_world_plugins +target_link_libraries(ocean_current_world_plugin gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) ament_target_dependencies(ocean_current_world_plugin @@ -46,7 +46,16 @@ install(DIRECTORY include/ DESTINATION include/ ) -# Environment Hooks -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") +# 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() + +# Configure ament ament_package() \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_gz_world_plugins/hooks/dave_gz_world_plugins.dsv.in similarity index 100% rename from gazebo/dave_gz_world_plugins/hooks/dave_ros_gz_plugins.dsv.in rename to gazebo/dave_gz_world_plugins/hooks/dave_gz_world_plugins.dsv.in diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml index b11a0a67..bacbe6db 100644 --- a/gazebo/dave_gz_world_plugins/package.xml +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -1,6 +1,6 @@ - dave_gazebo_world_plugins + dave_gazebo_world_plugin 3.1.1 The dave_gazebo_world_plugins package (Gazebo world plugin definitions) Gaurav Kumar and woensug @@ -11,11 +11,13 @@ gazebo_ros roscpp protobuf + dave_interfaces gazebo_ros protobuf gazebo_ros roscpp protobuf + dave_interfaces gazebo_dev gazebo_msgs diff --git a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc similarity index 100% rename from gazebo/dave_gz_world_plugins/src/gauss_markov_process.cpp rename to gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc similarity index 100% rename from gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cpp rename to gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc diff --git a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc similarity index 100% rename from gazebo/dave_gz_world_plugins/src/tidal_oscillation.cpp rename to gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 7f32fcd0..9233b75a 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -22,7 +22,7 @@ message(STATUS "Compiling against Gazebo Harmonic") # Libraries add_library(SphericalCoords SHARED src/SphericalCoords.cc) -add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cpp) +add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cc) # Include directories target_include_directories(SphericalCoords PRIVATE include) diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml index 037c1ab9..a0db5374 100644 --- a/gazebo/dave_ros_gz_plugins/package.xml +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -17,7 +17,6 @@ ros_gz_sim gz_msgs gz_dev - dave_gz_world_plugins gz_ros protobuf message_generation diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cpp b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc similarity index 100% rename from gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cpp rename to gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc From 2df7a8a4632701bcd516e940fd7fd0ae0bbfb19e Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 29 Jul 2024 00:47:06 +0530 Subject: [PATCH 09/79] minor updates to the ocean current world plugin --- .../ocean_current_world_plugin.hh | 142 +----- .../src/gauss_markov_process.cc | 2 +- .../src/ocean_current_world_plugin.cc | 450 +++++++++++------- 3 files changed, 308 insertions(+), 286 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index ab643375..b8b3c49d 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -39,13 +39,7 @@ #include #include -namespace gz -{ -namespace sim -{ -namespace systems -{ -namespace dave_simulator_ros +namespace dave_gz_world_plugins { /// \brief Class for the underwater current plugin @@ -53,20 +47,18 @@ namespace dave_simulator_ros // typedef const boost::shared_ptr AnyPtr; // public WorldPlugin, -class UnderwaterCurrentPlugin : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemUpdate, - public ISystemPostUpdate, - public WorldPlugin +class UnderwaterCurrentPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemUpdate, + public gz::sim::ISystemPostUpdate +// public gz::sim::WorldPlugin { /// \brief Class constructor public: UnderwaterCurrentPlugin(); - - /// \brief Class destructor - virtual ~UnderwaterCurrentPlugin(); + ~UnderwaterCurrentPlugin(); // Documentation inherited. void Configure( @@ -80,12 +72,15 @@ public: /// \param[in] _info Information used in the update event. // void Update(const common::UpdateInfo & _info); (check if this is needed) - void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void UnderwaterCurrentPlugin::Update( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + + void UnderwaterCurrentPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); /// \brief Load global current velocity configuration -protected: virtual void LoadGlobalCurrentConfig(); /// \brief Load stratified current velocity database @@ -100,113 +95,12 @@ protected: /// \brief Publish stratified oceqan current velocity void PublishStratifiedCurrentVelocity(); - /// \brief Update event - event::ConnectionPtr updateConnection; - - /// \brief Pointer to world - physics::WorldPtr world; - - /// \brief Pointer to sdf - sdf::ElementPtr sdf; - - /// \brief True if the sea surface is present - bool hasSurface; - - /// \brief Pointer to a node for communication - transport::NodePtr node; - - /// \brief Map of publishers - std::map publishers; - - /// \brief Vehicle Depth Subscriber - transport::SubscriberPtr subscriber; - - /// \brief Current velocity topic - std::string currentVelocityTopic; - - /// \brief Stratified Ocean current topic - std::string stratifiedCurrentVelocityTopic; - - /// \brief Vehicle depth topic - std::string vehicleDepthTopic; - - /// \brief Ocean Current Database file path for csv file - std::string databaseFilePath; - - /// \brief Tidal Oscillation Database file path for txt file - std::string tidalFilePath; - - /// \brief Vector for read stratified current database values - std::vector stratifiedDatabase; - - /// \brief Namespace for topics and services - std::string ns; - - /// \brief Gauss-Markov process instance for the current velocity - GaussMarkovProcess currentVelModel; - - /// \brief Gauss-Markov process instance for horizontal angle model - GaussMarkovProcess currentHorzAngleModel; - - /// \brief Gauss-Markov process instance for vertical angle model - GaussMarkovProcess currentVertAngleModel; - - /// \brief Vector of Gauss-Markov process instances for stratified velocity - std::vector> stratifiedCurrentModels; - - /// \brief Vector of dateGMT for tidal oscillation - std::vector> dateGMT; - - /// \brief Vector of speedcmsec for tidal oscillation - std::vector speedcmsec; - - /// \brief Tidal current harmonic constituents - bool tidalHarmonicFlag; - - double M2_amp; - double M2_phase; - double M2_speed; - double S2_amp; - double S2_phase; - double S2_speed; - double N2_amp; - double N2_phase; - double N2_speed; - - /// \brief Tidal oscillation mean ebb direction - double ebbDirection; - - /// \brief Tidal oscillation mean flood direction - double floodDirection; - - /// \brief Tidal oscillation world start time (GMT) - int world_start_time_day; - int world_start_time_month; - int world_start_time_year; - int world_start_time_hour; - int world_start_time_minute; - - /// \brief Tidal Oscillation flag - bool tideFlag; - - /// \brief Tidal Oscillation interpolation model - TidalOscillation tide; - - /// \brief Last update time stamp - common::Time lastUpdate; - - /// \brief Current linear velocity vector - gz::math::Vector3d currentVelocity; - - /// \brief Vector of current depth-specific linear velocity vectors - std::vector currentStratifiedVelocity; +private: + // std::shared_ptr ros_node_; - /// \brief File path for stratified current database - std::string db_path; + struct PrivateData; + std::unique_ptr dataPtr; }; -} // namespace dave_simulator_ros -} // namespace systems -} // namespace sim -} // namespace gz +} // namespace dave_gz_world_plugins #endif // OCEAN_CURRENT_WORLD_PLUGIN_H_ diff --git a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc index 94497431..aab94da1 100644 --- a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc +++ b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc @@ -15,7 +15,7 @@ /// \file gauss_markov_process.cc -#include +#include namespace gz { diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 93bb5411..131c0225 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -18,6 +18,8 @@ #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include #include +#include +#include #include #include @@ -40,10 +42,125 @@ #include "gz/sim/components/Model.hh" #include "gz/sim/components/World.hh" -using namespace gz; -using namespace sim; -using namespace systems; -using namespace dave_simulator_ros; +// using namespace gz; +// using namespace sim; +// using namespace systems; +// using namespace dave_gz_world_plugins; + +GZ_ADD_PLUGIN( + dave_gz_world_plugins::UnderwaterCurrentPlugin, gz::sim::System, + dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemConfigure, + dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemPreUpdate, + dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemPostUpdate) + +namespace dave_gz_world_plugins +{ +struct UnderwaterCurrentPlugin::PrivateData +{ + /// \brief Update event + event::ConnectionPtr updateConnection; + + /// \brief Pointer to world + physics::WorldPtr world; + + /// \brief Pointer to sdf + sdf::ElementPtr sdf; + + /// \brief True if the sea surface is present + bool hasSurface; + + /// \brief Pointer to a node for communication + transport::NodePtr node; + + /// \brief Map of publishers + std::map publishers; + + /// \brief Vehicle Depth Subscriber + transport::SubscriberPtr subscriber; + + /// \brief Current velocity topic + std::string currentVelocityTopic; + + /// \brief Stratified Ocean current topic + std::string stratifiedCurrentVelocityTopic; + + /// \brief Vehicle depth topic + std::string vehicleDepthTopic; + + /// \brief Ocean Current Database file path for csv file + std::string databaseFilePath; + + /// \brief Tidal Oscillation Database file path for txt file + std::string tidalFilePath; + + /// \brief Vector for read stratified current database values + std::vector stratifiedDatabase; + + /// \brief Namespace for topics and services + std::string ns; + + /// \brief Gauss-Markov process instance for the current velocity + GaussMarkovProcess currentVelModel; + + /// \brief Gauss-Markov process instance for horizontal angle model + GaussMarkovProcess currentHorzAngleModel; + + /// \brief Gauss-Markov process instance for vertical angle model + GaussMarkovProcess currentVertAngleModel; + + /// \brief Vector of Gauss-Markov process instances for stratified velocity + std::vector> stratifiedCurrentModels; + + /// \brief Vector of dateGMT for tidal oscillation + std::vector> dateGMT; + + /// \brief Vector of speedcmsec for tidal oscillation + std::vector speedcmsec; + + /// \brief Tidal current harmonic constituents + bool tidalHarmonicFlag; + + double M2_amp; + double M2_phase; + double M2_speed; + double S2_amp; + double S2_phase; + double S2_speed; + double N2_amp; + double N2_phase; + double N2_speed; + + /// \brief Tidal oscillation mean ebb direction + double ebbDirection; + + /// \brief Tidal oscillation mean flood direction + double floodDirection; + + /// \brief Tidal oscillation world start time (GMT) + int world_start_time_day; + int world_start_time_month; + int world_start_time_year; + int world_start_time_hour; + int world_start_time_minute; + + /// \brief Tidal Oscillation flag + bool tideFlag; + + /// \brief Tidal Oscillation interpolation model + TidalOscillation tide; + + /// \brief Last update time stamp + std::chrono::steady_clock::duration lastUpdate{0}; + + /// \brief Current linear velocity vector + gz::math::Vector3d currentVelocity; + + /// \brief Vector of current depth-specific linear velocity vectors + std::vector currentStratifiedVelocity; + + /// \brief File path for stratified current database + std::string db_path; +}; class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentPlugin { @@ -60,14 +177,10 @@ class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentPlugin ///////////////////////////////////////////////// UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() { - // Doing nothing for now - void configure( - const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); } ///////////////////////////////////////////////// -UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->rosNode.reset(); } +UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->dataPtr->rosNode.reset(); } ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::Configure( @@ -77,7 +190,12 @@ void UnderwaterCurrentPlugin::Configure( GZ_ASSERT(_entity != NULL, "World pointer is invalid"); GZ_ASSERT(_sdf != NULL, "SDF pointer is invalid"); - this->dataPtr->world = _entity; + this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(components::World())); + if (!this->dataPtr->world.Valid(_ecm)) // check + { + gzerr << "World entity not found" << std::endl; + return; + } this->dataPtr->sdf = _sdf; // Read the namespace for topics and services @@ -85,23 +203,19 @@ void UnderwaterCurrentPlugin::Configure( gzmsg << "Loading underwater world..." << std::endl; // Initializing the transport node - this->node = transport::NodePtr(new transport::Node()); - - this->dataPtr->node->Init(this->dataPtr->world->Name(_ecm)); // check if correct + this->dataPtr->node = transport::NodePtr(new transport::Node()); - // Initialize the time update - this->lastUpdate = _info.simeTime; // should it be zero ? (check) - std::chrono::steady_clock::duration lastUpdate{0}; // better ? (check) + this->dataPtr->node->Init(this->dataPtr->world.Name(_ecm)); // check if correct - this->LoadGlobalCurrentConfig(); - this->LoadStratifiedCurrentDatabase(); - if (this->sdf->HasElement("tidal_oscillation")) + this->dataPtr->LoadGlobalCurrentConfig(); + this->dataPtr->LoadStratifiedCurrentDatabase(); + if (this->dataPtr->sdf->HasElement("tidal_oscillation")) { - this->LoadTidalOscillationDatabase(); + this->dataPtr->LoadTidalOscillationDatabase(); } // Connect the update event. This isn't needed it seems (check) - // this->updateConnection = event::Events::ConnectWorldUpdateBegin( + // this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin( // boost::bind(&UnderwaterCurrentPlugin::Update, // this, _1)); @@ -112,16 +226,16 @@ void UnderwaterCurrentPlugin::Configure( ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { - this->tideFlag = true; - this->tidalHarmonicFlag = false; + this->dataPtr->tideFlag = true; + this->dataPtr->tidalHarmonicFlag = false; - sdf::ElementPtr tidalOscillationParams = this->sdf->GetElement("tidal_oscillation"); + sdf::ElementPtr tidalOscillationParams = this->dataPtr->sdf->GetElement("tidal_oscillation"); sdf::ElementPtr tidalHarmonicParams; // Read the tidal oscillation parameter from the SDF file if (tidalOscillationParams->HasElement("databasefilePath")) { - this->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); + this->dataPtr->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); gzmsg << "Tidal current database configuration found" << std::endl; } else @@ -134,7 +248,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() } else { - this->tidalFilePath = + this->dataPtr->tidalFilePath = ros::package::getPath("dave_worlds") + "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; } } @@ -146,8 +260,8 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { sdf::ElementPtr elem = tidalOscillationParams->GetElement("mean_direction"); GZ_ASSERT(elem->HasElement("ebb"), "Tidal mean ebb direction not defined"); - this->ebbDirection = elem->Get("ebb"); - this->floodDirection = elem->Get("flood"); + this->dataPtr->ebbDirection = elem->Get("ebb"); + this->dataPtr->floodDirection = elem->Get("flood"); GZ_ASSERT(elem->HasElement("flood"), "Tidal mean flood direction not defined"); } @@ -159,20 +273,20 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { sdf::ElementPtr elem = tidalOscillationParams->GetElement("world_start_time_GMT"); GZ_ASSERT(elem->HasElement("day"), "World start time (day) not defined"); - this->world_start_time_day = elem->Get("day"); + this->dataPtr->world_start_time_day = elem->Get("day"); GZ_ASSERT(elem->HasElement("month"), "World start time (month) not defined"); - this->world_start_time_month = elem->Get("month"); + this->dataPtr->world_start_time_month = elem->Get("month"); GZ_ASSERT(elem->HasElement("year"), "World start time (year) not defined"); - this->world_start_time_year = elem->Get("year"); + this->dataPtr->world_start_time_year = elem->Get("year"); GZ_ASSERT(elem->HasElement("hour"), "World start time (hour) not defined"); - this->world_start_time_hour = elem->Get("hour"); + this->dataPtr->world_start_time_hour = elem->Get("hour"); if (elem->HasElement("minute")) { - this->world_start_time_minute = elem->Get("minute"); + this->dataPtr->world_start_time_minute = elem->Get("minute"); } else { - this->world_start_time_minute = 0; + this->dataPtr->world_start_time_minute = 0; } } @@ -181,42 +295,42 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() // Read harmonic constituents GZ_ASSERT(tidalHarmonicParams->HasElement("M2"), "Harcomnic constituents M2 not found"); sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2"); - this->M2_amp = M2Params->Get("amp"); - this->M2_phase = M2Params->Get("phase"); - this->M2_speed = M2Params->Get("speed"); + this->dataPtr->M2_amp = M2Params->Get("amp"); + this->dataPtr->M2_phase = M2Params->Get("phase"); + this->dataPtr->M2_speed = M2Params->Get("speed"); GZ_ASSERT(tidalHarmonicParams->HasElement("S2"), "Harcomnic constituents S2 not found"); sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2"); - this->S2_amp = S2Params->Get("amp"); - this->S2_phase = S2Params->Get("phase"); - this->S2_speed = S2Params->Get("speed"); + this->dataPtr->S2_amp = S2Params->Get("amp"); + this->dataPtr->S2_phase = S2Params->Get("phase"); + this->dataPtr->S2_speed = S2Params->Get("speed"); GZ_ASSERT(tidalHarmonicParams->HasElement("N2"), "Harcomnic constituents N2 not found"); sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2"); - this->N2_amp = N2Params->Get("amp"); - this->N2_phase = N2Params->Get("phase"); - this->N2_speed = N2Params->Get("speed"); + this->dataPtr->N2_amp = N2Params->Get("amp"); + this->dataPtr->N2_phase = N2Params->Get("phase"); + this->dataPtr->N2_speed = N2Params->Get("speed"); gzmsg << "Tidal harmonic constituents loaded : " << std::endl; - gzmsg << "M2 amp: " << this->M2_amp << " phase: " << this->M2_phase - << " speed: " << this->M2_speed << std::endl; - gzmsg << "S2 amp: " << this->S2_amp << " phase: " << this->S2_phase - << " speed: " << this->S2_speed << std::endl; - gzmsg << "N2 amp: " << this->N2_amp << " phase: " << this->N2_phase - << " speed: " << this->N2_speed << std::endl; + gzmsg << "M2 amp: " << this->dataPtr->M2_amp << " phase: " << this->dataPtr->M2_phase + << " speed: " << this->dataPtr->M2_speed << std::endl; + gzmsg << "S2 amp: " << this->dataPtr->S2_amp << " phase: " << this->dataPtr->S2_phase + << " speed: " << this->dataPtr->S2_speed << std::endl; + gzmsg << "N2 amp: " << this->dataPtr->N2_amp << " phase: " << this->dataPtr->N2_phase + << " speed: " << this->dataPtr->N2_speed << std::endl; } else { // Read database std::ifstream csvFile; std::string line; - csvFile.open(this->tidalFilePath); + csvFile.open(this->dataPtr->tidalFilePath); if (!csvFile) { common::SystemPaths * paths = common::SystemPaths::Instance(); - this->tidalFilePath = paths->FindFile(this->tidalFilePath, true); - csvFile.open(this->tidalFilePath); + this->dataPtr->tidalFilePath = paths->FindFile(this->dataPtr->tidalFilePath, true); + csvFile.open(this->dataPtr->tidalFilePath); } GZ_ASSERT(csvFile, "Tidal Oscillation database file does not exist"); - gzmsg << "Tidal Oscillation Database loaded : " << this->tidalFilePath << std::endl; + gzmsg << "Tidal Oscillation Database loaded : " << this->dataPtr->tidalFilePath << std::endl; // skip the first line getline(csvFile, line); @@ -242,21 +356,21 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() tmpDateArray[2] = std::stoi(row[0].substr(8, 10)); tmpDateArray[3] = std::stoi(row[0].substr(11, 13)); tmpDateArray[4] = std::stoi(row[0].substr(14, 16)); - this->dateGMT.push_back(tmpDateArray); + this->dataPtr->dateGMT.push_back(tmpDateArray); - this->speedcmsec.push_back(stold(row[2], &sz)); + this->dataPtr->speedcmsec.push_back(stold(row[2], &sz)); } } csvFile.close(); // Eliminate data with same consecutive type std::vector duplicated; - for (int i = 0; i < this->dateGMT.size() - 1; i++) + for (int i = 0; i < this->dataPtr->dateGMT.size() - 1; i++) { // delete latter if same sign if ( - ((this->speedcmsec[i] > 0) - (this->speedcmsec[i] < 0)) == - ((this->speedcmsec[i + 1] > 0) - (this->speedcmsec[i + 1] < 0))) + ((this->dataPtr->speedcmsec[i] > 0) - (this->dataPtr->speedcmsec[i] < 0)) == + ((this->dataPtr->speedcmsec[i + 1] > 0) - (this->dataPtr->speedcmsec[i + 1] < 0))) { duplicated.push_back(i + 1); } @@ -264,8 +378,9 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() int eraseCount = 0; for (int i = 0; i < duplicated.size(); i++) { - this->dateGMT.erase(this->dateGMT.begin() + duplicated[i] - eraseCount); - this->speedcmsec.erase(this->speedcmsec.begin() + duplicated[i] - eraseCount); + this->dataPtr->dateGMT.erase(this->dataPtr->dateGMT.begin() + duplicated[i] - eraseCount); + this->dataPtr->speedcmsec.erase( + this->dataPtr->speedcmsec.begin() + duplicated[i] - eraseCount); eraseCount++; } } @@ -275,55 +390,59 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() { GZ_ASSERT( - this->sdf->HasElement("transient_current"), "Transient current configuration not available"); - sdf::ElementPtr transientCurrentParams = this->sdf->GetElement("transient_current"); + this->dataPtr->sdf->HasElement("transient_current"), + "Transient current configuration not available"); + sdf::ElementPtr transientCurrentParams = this->dataPtr->sdf->GetElement("transient_current"); if (transientCurrentParams->HasElement("topic_stratified")) { - this->stratifiedCurrentVelocityTopic = + this->dataPtr->stratifiedCurrentVelocityTopic = transientCurrentParams->Get("topic_stratified"); } else { - this->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; + this->dataPtr->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; } GZ_ASSERT( - !this->stratifiedCurrentVelocityTopic.empty(), "Empty stratified ocean current velocity topic"); + !this->dataPtr->stratifiedCurrentVelocityTopic.empty(), + "Empty stratified ocean current velocity topic"); // Read the depth dependent ocean current file path from the SDF file if (transientCurrentParams->HasElement("databasefilePath")) { - this->databaseFilePath = transientCurrentParams->Get("databasefilePath"); + this->dataPtr->databaseFilePath = transientCurrentParams->Get("databasefilePath"); } else { - this->databaseFilePath = "transientOceanCurrentDatabase.csv"; + this->dataPtr->databaseFilePath = "transientOceanCurrentDatabase.csv"; } - GZ_ASSERT(!this->databaseFilePath.empty(), "Empty stratified ocean current database file path"); + GZ_ASSERT( + !this->dataPtr->databaseFilePath.empty(), "Empty stratified ocean current database file path"); - gzmsg << this->databaseFilePath << std::endl; + gzmsg << this->dataPtr->databaseFilePath << std::endl; // #if GAZEBO_MAJOR_VERSION >= 8 - // this->lastUpdate = this->world->SimTime(); + // this->dataPtr->lastUpdate = this->dataPtr->world->SimTime(); // #else - // this->lastUpdate = this->world->GetSimTime(); + // this->dataPtr->lastUpdate = this->dataPtr->world->GetSimTime(); // #endif // Read database std::ifstream csvFile; std::string line; - csvFile.open(this->databaseFilePath); + csvFile.open(this->dataPtr->databaseFilePath); if (!csvFile) { common::SystemPaths * paths = common::SystemPaths::Instance(); - this->databaseFilePath = paths->FindFile(this->databaseFilePath, true); - csvFile.open(this->databaseFilePath); + this->dataPtr->databaseFilePath = paths->FindFile(this->dataPtr->databaseFilePath, true); + csvFile.open(this->dataPtr->databaseFilePath); } GZ_ASSERT(csvFile, "Stratified Ocean database file does not exist"); - gzmsg << "Statified Ocean Current Database loaded : " << this->databaseFilePath << std::endl; + gzmsg << "Statified Ocean Current Database loaded : " << this->dataPtr->databaseFilePath + << std::endl; // skip the 3 lines getline(csvFile, line); @@ -347,7 +466,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() read.X() = row[0]; read.Y() = row[1]; read.Z() = row[2]; - this->stratifiedDatabase.push_back(read); + this->dataPtr->stratifiedDatabase.push_back(read); // Create Gauss-Markov processes for the stratified currents // Means are the database-specified magnitudes & angles, and @@ -356,43 +475,44 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() GaussMarkovProcess magnitudeModel; magnitudeModel.mean = hypot(row[1], row[0]); magnitudeModel.var = magnitudeModel.mean; - magnitudeModel.max = this->currentVelModel.max; + magnitudeModel.max = this->dataPtr->currentVelModel.max; magnitudeModel.min = 0.0; - magnitudeModel.mu = this->currentVelModel.mu; - magnitudeModel.noiseAmp = this->currentVelModel.noiseAmp; - magnitudeModel.lastUpdate = this->lastUpdate.Double(); + magnitudeModel.mu = this->dataPtr->currentVelModel.mu; + magnitudeModel.noiseAmp = this->dataPtr->currentVelModel.noiseAmp; + magnitudeModel.lastUpdate = this->dataPtr->lastUpdate.Double(); GaussMarkovProcess hAngleModel; hAngleModel.mean = atan2(row[1], row[0]); hAngleModel.var = hAngleModel.mean; hAngleModel.max = M_PI; hAngleModel.min = -M_PI; - hAngleModel.mu = this->currentHorzAngleModel.mu; - hAngleModel.noiseAmp = this->currentHorzAngleModel.noiseAmp; - hAngleModel.lastUpdate = this->lastUpdate.Double(); + hAngleModel.mu = this->dataPtr->currentHorzAngleModel.mu; + hAngleModel.noiseAmp = this->dataPtr->currentHorzAngleModel.noiseAmp; + hAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); GaussMarkovProcess vAngleModel; vAngleModel.mean = 0.0; vAngleModel.var = vAngleModel.mean; vAngleModel.max = M_PI / 2.0; vAngleModel.min = -M_PI / 2.0; - vAngleModel.mu = this->currentVertAngleModel.mu; - vAngleModel.noiseAmp = this->currentVertAngleModel.noiseAmp; - vAngleModel.lastUpdate = this->lastUpdate.Double(); + vAngleModel.mu = this->dataPtr->currentVertAngleModel.mu; + vAngleModel.noiseAmp = this->dataPtr->currentVertAngleModel.noiseAmp; + vAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); std::vector depthModels; depthModels.push_back(magnitudeModel); depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); - this->stratifiedCurrentModels.push_back(depthModels); + this->dataPtr->stratifiedCurrentModels.push_back(depthModels); } csvFile.close(); - this->publishers[this->stratifiedCurrentVelocityTopic] = - this->node->create_publisher( - this->ns + "/" + this->stratifiedCurrentVelocityTopic); + this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic] = + this->dataPtr->node + ->create_publisher( + this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); gzmsg << "Stratified current velocity topic name: " - << this->ns + "/" + this->stratifiedCurrentVelocityTopic << std::endl; + << this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic << std::endl; } ///////////////////////////////////////////////// @@ -404,63 +524,66 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() // Consider setting it up as one or the other, but not both? // Retrieve the velocity configuration, if existent - GZ_ASSERT(this->sdf->HasElement("constant_current"), "Current configuration not available"); - sdf::ElementPtr currentVelocityParams = this->sdf->GetElement("constant_current"); + GZ_ASSERT( + this->dataPtr->sdf->HasElement("constant_current"), "Current configuration not available"); + sdf::ElementPtr currentVelocityParams = this->dataPtr->sdf->GetElement("constant_current"); // Read the topic names from the SDF file if (currentVelocityParams->HasElement("topic")) { - this->currentVelocityTopic = currentVelocityParams->Get("topic"); + this->dataPtr->currentVelocityTopic = currentVelocityParams->Get("topic"); } else { - this->currentVelocityTopic = "current_velocity"; + this->dataPtr->currentVelocityTopic = "current_velocity"; } - GZ_ASSERT(!this->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); + GZ_ASSERT(!this->dataPtr->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); if (currentVelocityParams->HasElement("velocity")) { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity"); if (elem->HasElement("mean")) { - this->currentVelModel.mean = elem->Get("mean"); + this->dataPtr->currentVelModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->currentVelModel.min = elem->Get("min"); + this->dataPtr->currentVelModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->currentVelModel.max = elem->Get("max"); + this->dataPtr->currentVelModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->currentVelModel.mu = elem->Get("mu"); + this->dataPtr->currentVelModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->currentVelModel.noiseAmp = elem->Get("noiseAmp"); + this->dataPtr->currentVelModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->currentVelModel.min < this->currentVelModel.max, "Invalid current velocity limits"); + this->dataPtr->currentVelModel.min < this->dataPtr->currentVelModel.max, + "Invalid current velocity limits"); GZ_ASSERT( - this->currentVelModel.mean >= this->currentVelModel.min, + this->dataPtr->currentVelModel.mean >= this->dataPtr->currentVelModel.min, "Mean velocity must be greater than minimum"); GZ_ASSERT( - this->currentVelModel.mean <= this->currentVelModel.max, + this->dataPtr->currentVelModel.mean <= this->dataPtr->currentVelModel.max, "Mean velocity must be smaller than maximum"); GZ_ASSERT( - this->currentVelModel.mu >= 0 && this->currentVelModel.mu < 1, "Invalid process constant"); + this->dataPtr->currentVelModel.mu >= 0 && this->dataPtr->currentVelModel.mu < 1, + "Invalid process constant"); GZ_ASSERT( - this->currentVelModel.noiseAmp < 1 && this->currentVelModel.noiseAmp >= 0, + this->dataPtr->currentVelModel.noiseAmp < 1 && this->dataPtr->currentVelModel.noiseAmp >= 0, "Noise amplitude has to be smaller than 1"); } - this->currentVelModel.var = this->currentVelModel.mean; + this->dataPtr->currentVelModel.var = this->dataPtr->currentVelModel.mean; gzmsg << "Current velocity [m/s] Gauss-Markov process model:" << std::endl; - this->currentVelModel.Print(); + this->dataPtr->currentVelModel.Print(); if (currentVelocityParams->HasElement("horizontal_angle")) { @@ -468,45 +591,46 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() if (elem->HasElement("mean")) { - this->currentHorzAngleModel.mean = elem->Get("mean"); + this->dataPtr->currentHorzAngleModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->currentHorzAngleModel.min = elem->Get("min"); + this->dataPtr->currentHorzAngleModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->currentHorzAngleModel.max = elem->Get("max"); + this->dataPtr->currentHorzAngleModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->currentHorzAngleModel.mu = elem->Get("mu"); + this->dataPtr->currentHorzAngleModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); + this->dataPtr->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->currentHorzAngleModel.min < this->currentHorzAngleModel.max, + this->dataPtr->currentHorzAngleModel.min < this->dataPtr->currentHorzAngleModel.max, "Invalid current horizontal angle limits"); GZ_ASSERT( - this->currentHorzAngleModel.mean >= this->currentHorzAngleModel.min, + this->dataPtr->currentHorzAngleModel.mean >= this->dataPtr->currentHorzAngleModel.min, "Mean horizontal angle must be greater than minimum"); GZ_ASSERT( - this->currentHorzAngleModel.mean <= this->currentHorzAngleModel.max, + this->dataPtr->currentHorzAngleModel.mean <= this->dataPtr->currentHorzAngleModel.max, "Mean horizontal angle must be smaller than maximum"); GZ_ASSERT( - this->currentHorzAngleModel.mu >= 0 && this->currentHorzAngleModel.mu < 1, + this->dataPtr->currentHorzAngleModel.mu >= 0 && this->dataPtr->currentHorzAngleModel.mu < 1, "Invalid process constant"); GZ_ASSERT( - this->currentHorzAngleModel.noiseAmp < 1 && this->currentHorzAngleModel.noiseAmp >= 0, + this->dataPtr->currentHorzAngleModel.noiseAmp < 1 && + this->dataPtr->currentHorzAngleModel.noiseAmp >= 0, "Noise amplitude for horizontal angle has to be between 0 and 1"); } - this->currentHorzAngleModel.var = this->currentHorzAngleModel.mean; + this->dataPtr->currentHorzAngleModel.var = this->dataPtr->currentHorzAngleModel.mean; gzmsg << "Current velocity horizontal angle [rad] Gauss-Markov process model:" << std::endl; - this->currentHorzAngleModel.Print(); + this->dataPtr->currentHorzAngleModel.Print(); if (currentVelocityParams->HasElement("vertical_angle")) { @@ -514,55 +638,57 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() if (elem->HasElement("mean")) { - this->currentVertAngleModel.mean = elem->Get("mean"); + this->dataPtr->currentVertAngleModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->currentVertAngleModel.min = elem->Get("min"); + this->dataPtr->currentVertAngleModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->currentVertAngleModel.max = elem->Get("max"); + this->dataPtr->currentVertAngleModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->currentVertAngleModel.mu = elem->Get("mu"); + this->dataPtr->currentVertAngleModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); + this->dataPtr->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->currentVertAngleModel.min < this->currentVertAngleModel.max, + this->dataPtr->currentVertAngleModel.min < this->dataPtr->currentVertAngleModel.max, "Invalid current vertical angle limits"); GZ_ASSERT( - this->currentVertAngleModel.mean >= this->currentVertAngleModel.min, + this->dataPtr->currentVertAngleModel.mean >= this->dataPtr->currentVertAngleModel.min, "Mean vertical angle must be greater than minimum"); GZ_ASSERT( - this->currentVertAngleModel.mean <= this->currentVertAngleModel.max, + this->dataPtr->currentVertAngleModel.mean <= this->dataPtr->currentVertAngleModel.max, "Mean vertical angle must be smaller than maximum"); GZ_ASSERT( - this->currentVertAngleModel.mu >= 0 && this->currentVertAngleModel.mu < 1, + this->dataPtr->currentVertAngleModel.mu >= 0 && this->dataPtr->currentVertAngleModel.mu < 1, "Invalid process constant"); GZ_ASSERT( - this->currentVertAngleModel.noiseAmp < 1 && this->currentVertAngleModel.noiseAmp >= 0, + this->dataPtr->currentVertAngleModel.noiseAmp < 1 && + this->dataPtr->currentVertAngleModel.noiseAmp >= 0, "Noise amplitude for vertical angle has to be between 0 and 1"); } - this->currentVertAngleModel.var = this->currentVertAngleModel.mean; + this->dataPtr->currentVertAngleModel.var = this->dataPtr->currentVertAngleModel.mean; gzmsg << "Current velocity vertical angle [rad] Gauss-Markov process model:" << std::endl; - this->currentVertAngleModel.Print(); + this->dataPtr->currentVertAngleModel.Print(); - this->currentVelModel.lastUpdate = this->lastUpdate.Double(); - this->currentHorzAngleModel.lastUpdate = this->lastUpdate.Double(); - this->currentVertAngleModel.lastUpdate = this->lastUpdate.Double(); + this->dataPtr->currentVelModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentHorzAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentVertAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); // Advertise the current velocity & stratified current velocity topics - this->publishers[this->currentVelocityTopic] = - this->node->create_publisher(this->ns + "/" + this->currentVelocityTopic); - gzmsg << "Current velocity topic name: " << this->ns + "/" + this->currentVelocityTopic - << std::endl; + this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = + this->dataPtr->node->create_publisher( + this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); + gzmsg << "Current velocity topic name: " + << this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic << std::endl; } ///////////////////////////////////////////////// @@ -570,18 +696,19 @@ void UnderwaterCurrentPlugin::PublishCurrentVelocity() { msgs::Vector3d currentVel; msgs::Set( - ¤tVel, - gz::math::Vector3d( - this->currentVelocity.X(), this->currentVelocity.Y(), this->currentVelocity.Z())); - this->publishers[this->currentVelocityTopic]->Publish(currentVel); + ¤tVel, gz::math::Vector3d( + this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), + this->dataPtr->currentVelocity.Z())); + this->dataPtr->publishers[this->dataPtr->currentVelocityTopic]->Publish(currentVel); } ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() { dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; - for (std::vector::iterator it = this->currentStratifiedVelocity.begin(); - it != this->currentStratifiedVelocity.end(); ++it) + for (std::vector::iterator it = + this->dataPtr->currentStratifiedVelocity.begin(); + it != this->dataPtr->currentStratifiedVelocity.end(); ++it) { msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); currentVel.add_depth(it->W()); @@ -590,7 +717,7 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() { return; } - this->publishers[this->stratifiedCurrentVelocityTopic]->Publish(currentVel); + this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic]->Publish(currentVel); } ///////////////////////////////////////////////// @@ -610,30 +737,30 @@ void UnderwaterCurrentPlugin::Update( // model // Update current velocity - double currentVelMag = this->currentVelModel.Update(time.Double()); + double currentVelMag = this->dataPtr->currentVelModel.Update(time.Double()); // Update current horizontal direction around z axis of flow frame - double horzAngle = this->currentHorzAngleModel.Update(time.Double()); + double horzAngle = this->dataPtr->currentHorzAngleModel.Update(time.Double()); // Update current horizontal direction around z axis of flow frame - double vertAngle = this->currentVertAngleModel.Update(time.Double()); + double vertAngle = this->dataPtr->currentVertAngleModel.Update(time.Double()); // Generating the current velocity vector as in the North-East-Down frame - this->currentVelocity = gz::math::Vector3d( + this->dataPtr->currentVelocity = gz::math::Vector3d( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle)); // Generate the depth-specific velocities - this->currentStratifiedVelocity.clear(); - for (int i = 0; i < this->stratifiedDatabase.size(); i++) + this->dataPtr->currentStratifiedVelocity.clear(); + for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) { - double depth = this->stratifiedDatabase[i].Z(); - currentVelMag = this->stratifiedCurrentModels[i][0].Update(time.Double()); - horzAngle = this->stratifiedCurrentModels[i][1].Update(time.Double()); - vertAngle = this->stratifiedCurrentModels[i][2].Update(time.Double()); + double depth = this->dataPtr->stratifiedDatabase[i].Z(); + currentVelMag = this->dataPtr->stratifiedCurrentModels[i][0].Update(time.Double()); + horzAngle = this->dataPtr->stratifiedCurrentModels[i][1].Update(time.Double()); + vertAngle = this->dataPtr->stratifiedCurrentModels[i][2].Update(time.Double()); gz::math::Vector4d depthVel( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); - this->currentStratifiedVelocity.push_back(depthVel); + this->dataPtr->currentStratifiedVelocity.push_back(depthVel); } } @@ -641,7 +768,8 @@ void UnderwaterCurrentPlugin::Update( void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Update time stamp - this->lastUpdate = _info.simTime; - this->PublishCurrentVelocity(); - this->PublishStratifiedCurrentVelocity(); -} \ No newline at end of file + this->dataPtr->lastUpdate = _info.simTime; + this->dataPtr->PublishCurrentVelocity(); + this->dataPtr->PublishStratifiedCurrentVelocity(); +} +} // namespace dave_gz_world_plugins \ No newline at end of file From 1b7a606b227a52c23a81f42e52592a91ca8b95cb Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 29 Jul 2024 01:06:10 +0530 Subject: [PATCH 10/79] added proto files for oc_world --- dave_interfaces/CMakeLists.txt | 35 +++++++++++++++++++ .../msgs/StratifiedCurrentVelocity.proto | 10 ++++++ 2 files changed, 45 insertions(+) create mode 100644 dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index da8c93f6..6f5eafc3 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/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" @@ -37,6 +39,39 @@ install(FILES DESTINATION share/${PROJECT_NAME} ) +# Define a variable 'GZ_MSGS_VER' holding the version number +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +# Define a variable 'MSGS_PROTOS' listing the .proto files +set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) + +# Define the GZ_DATA_INSTALL_DIR variable +# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos +install( + DIRECTORY gz + DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto + COMPONENT proto + FILES_MATCHING PATTERN "*.proto") + +# Example of custom messages that depend on gz.msgs +set(MSGS_PROTOS + ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) +gz_msgs_generate_messages( + # The cmake target to be generated for libraries/executables to link + TARGET msgs # pressure_sensor_msgs_gen + # The protobuf package to generate (Typically based on the path) + PROTO_PACKAGE "pressure_sensor_msgs.msgs" + # The path to the base directory of the proto files + # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) + MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + # List of proto files to generate + MSGS_PROTOS ${MSGS_PROTOS} + # List of message targets this library imports from + # DEPENDENCIES gz_msgs_gen + # Dependency on gz-msgs + DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} +) + ament_export_dependencies(rosidl_default_runtime) # Install CMake package configuration ament_export_include_directories(include) diff --git a/dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto b/dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto new file mode 100644 index 00000000..7a4fba1a --- /dev/null +++ b/dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto @@ -0,0 +1,10 @@ +syntax = "proto3"; +package dave_gz_world_plugins_msgs.msgs; +import "gz/msgs/vector3d.proto"; +// import "any.proto"; + +message StratifiedCurrentVelocity +{ + repeated gz.msgs.Vector3d velocity = 1; + repeated double depth = 2; +} From 44bc5076fd42613bdf6052295b2d9488607029a0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 30 Jul 2024 01:47:17 +0530 Subject: [PATCH 11/79] minor fixes --- .../ocean_current_model_plugin.hh | 12 +------ gazebo/dave_gz_model_plugins/package.xml | 6 ++-- .../src/ocean_current_model_plugin.cc | 28 ++++----------- gazebo/dave_gz_world_plugins/CMakeLists.txt | 29 ++++++++++++++- gazebo/dave_gz_world_plugins/package.xml | 6 ++-- .../src/ocean_current_world_plugin.cc | 20 +++-------- .../ocean_current_plugin.hh | 4 +-- gazebo/dave_ros_gz_plugins/package.xml | 4 +-- .../src/ocean_current_plugin.cc | 36 ++++++++----------- 9 files changed, 63 insertions(+), 82 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh index da4b4803..3856790e 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh @@ -21,20 +21,13 @@ #include #include -namespace gz -{ -namespace sim -{ -namespace systems -{ namespace dave_gz_model_plugins { class TransientCurrentPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPreUpdate, public gz::sim::ISystemUpdate, - public gz::sim::ISystemPostUpdate, - public ModelPlugin + public gz::sim::ISystemPostUpdate { public: TransientCurrentPlugin(); // constructor @@ -85,6 +78,3 @@ public: const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) } } // namespace dave_gz_model_plugins -} // namespace systems -} // namespace sim -} // namespace gz diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index 4e49d8eb..75c5c1ec 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -6,15 +6,15 @@ Gaurav Kumar and woensug Gaurav Kumar and woensug Apache 2.0 - catkin - protobuf-dev + ament_cmake + gz_ros rclcpp protobuf dave_interfaces message_generation gz_ros - protobuf + message_generation gz_ros rclcpp diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index 99e27688..f0c6720c 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -59,18 +59,6 @@ GZ_ADD_PLUGIN_ALIAS(dave_gz_model_plugins::TransientCurrentPlugin, "TransientCur namespace dave_gz_model_plugins { -class TransientCurrentPlugin // dave_model_systems -{ -public: - TransientCurrentPlugin(); /// \brief Class constructor - ~TransientCurrentPlugin(); /// \brief Class destructor - -private: - std::shared_ptr rosNode; // This is a smart pointer to a ROS 2 node, facilitating - // communication with other ROS nodes. - std::string transientCurrentVelocityTopic; // Declare the variable (updated) -}; - struct TransientCurrentPlugin::PrivateData { // starts here @@ -84,8 +72,12 @@ struct TransientCurrentPlugin::PrivateData // Initialize any necessary states before the plugin starts virtual void Init(); - /// \brief Pointer to world + std::shared_ptr rosNode; // This is a smart pointer to a ROS 2 node, facilitating + // communication with other ROS nodes. + std::string transientCurrentVelocityTopic; // Declare the variable (updated) + protected: + /// \brief Pointer to world physics::WorldPtr world; /// \brief Pointer to model @@ -531,7 +523,7 @@ TransientCurrentPlugin::UpdateDatabase( this->dataPtr->lock_.unlock(); } -///////////////////////////////////////////////// +///////////////////////////////////////////////// (check this,dpr) void TransientCurrentPlugin::Gauss_Markov_process_initialize( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { @@ -666,14 +658,6 @@ void TransientCurrentPlugin::Update( void TransientCurrentPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - // Publish the Current Velocity. - // if ((!_info.paused && _info.simTime > this->dataPtr->lastUpdate) && (_info.simTime - - // this->dataPtr->lastUpdate >= this->dataPtr->rosPublishPeriod)) - // { - // this->dataPtr->lastUpdate = _info.simTime; - // PostPublishCurrentVelocity(); - // } - // Publish the Current Velocity. if (!_info.paused && _info.simTime > this->dataPtr->lastUpdate) { diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 6be7cdca..a7e467eb 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -13,21 +13,34 @@ 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) # Set version variables with gazebo Harmonic 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(ocean_current_world_plugin SHARED src/ocean_current_world_plugin.cc) +add_library(gauss_markov_process SHARED +src/gauss_markov_process.cc) +add_library(tidal_oscillation SHARED +src/tidal_oscillation.cc) target_include_directories(ocean_current_world_plugin PRIVATE include) +target_include_directories(gauss_markov_process PRIVATE include) +target_include_directories(tidal_oscillation PRIVATE include) target_link_libraries(ocean_current_world_plugin gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) +target_link_libraries(gauss_markov_process +gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) +target_link_libraries(tidal_oscillation +gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) ament_target_dependencies(ocean_current_world_plugin "rclcpp" @@ -36,8 +49,22 @@ ament_target_dependencies(ocean_current_world_plugin "dave_interfaces" ) +ament_target_dependencies(gauss_markov_process + "rclcpp" + "std_msgs" + "geometry_msgs" + "dave_interfaces" +) + +ament_target_dependencies(tidal_oscillation + "rclcpp" + "std_msgs" + "geometry_msgs" + "dave_interfaces" +) + # Install targets -install(TARGETS ocean_current_world_plugin +install(TARGETS ocean_current_world_plugin gauss_markov_process tidal_oscillation DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml index bacbe6db..d88bc6b8 100644 --- a/gazebo/dave_gz_world_plugins/package.xml +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -6,14 +6,14 @@ Gaurav Kumar and woensug Gaurav Kumar and woensug Apache 2.0 - catkin - protobuf-dev + ament_cmake + gazebo_ros roscpp protobuf dave_interfaces gazebo_ros - protobuf + gazebo_ros roscpp protobuf diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 131c0225..e51bdb78 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -16,7 +16,7 @@ /// \file ocean_current_world_plugin.cc #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" -#include +#include "dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.pb.h #include #include #include @@ -160,24 +160,12 @@ struct UnderwaterCurrentPlugin::PrivateData /// \brief File path for stratified current database std::string db_path; -}; - -class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentPlugin -{ -public: - UnderwaterCurrentPlugin(); - virtual ~UnderwaterCurrentPlugin(); -private: - void Load(sdf::ElementPtr _sdf); - // void OnUpdate(const common::UpdateInfo & _info); std::shared_ptr rosNode; -} +}; ///////////////////////////////////////////////// -UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() -{ -} +UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() : dataPtr(std::make_unique()) {} ///////////////////////////////////////////////// UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->dataPtr->rosNode.reset(); } @@ -705,7 +693,7 @@ void UnderwaterCurrentPlugin::PublishCurrentVelocity() ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() { - dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; + dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check for (std::vector::iterator it = this->dataPtr->currentStratifiedVelocity.begin(); it != this->dataPtr->currentStratifiedVelocity.end(); ++it) diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index cc5b10c0..d8d754bb 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -55,7 +55,7 @@ namespace sim { namespace systems { -namespace dave_simulator_ros +namespace dave_ros_gz_pluginsulator_ros { class UnderwaterCurrentROSPlugin : public System, @@ -85,7 +85,7 @@ public: public: void PostUpdate(const UpdateInfo & _info, const EntityComponentManager & _ecm) override; } -} // namespace dave_simulator_ros +} // namespace dave_ros_gz_pluginsulator_ros } // namespace systems } // namespace sim } // namespace gz diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml index a0db5374..25fce8f1 100644 --- a/gazebo/dave_ros_gz_plugins/package.xml +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -1,5 +1,5 @@ - + dave_ros_gz_plugins 0.0.0 TODO: Package description @@ -23,7 +23,7 @@ rclcpp dave_interfaces gz_ros - protobuf + gz_ros protobuf message_runtime diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 2d6a8698..f9f6bb70 100755 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -26,38 +26,29 @@ // 4. Integration with the rest of the system with respect to the use of smart pointers and other // parameters like behaviour. -using namespace gz; -using namespace sim; -using namespace systems; -using namespace dave_simulator_ros; +GZ_ADD_PLUGIN( + dave_ros_gz_plugins::UnderwaterCurrentROSPlugin, gz::sim::System, + dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::ISystemConfigure, + dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::ISystemPostUpdate) -class gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin +namespace dave_ros_gz_plugins { -public: - UnderwaterCurrentROSPlugin(); - ~UnderwaterCurrentROSPlugin(); -private: +struct UnderwaterCurrentROSPlugin::PrivateData +{ rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating // communication with other ROS nodes. std::string db_path; -}; - -///////////////////////////////////////////////// -UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() -{ - // this->rosPublishPeriod = gz::common::Time(0.05); - // this->lastRosPublishTime = gz::common::Time(0.0); this->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); // Initialize the ROS 2 node this->rosNode = std::make_shared("underwater_current_ros_plugin"); std::chrono::steady_clock::duration lastUpdate{0}; +}; - // Setting up a timer in ROS 2 Do we need this ? (to be (updated)) - // this->rosPublishTimer = this->rosNode->create_wall_timer( - // std::chrono::milliseconds(static_cast(this->rosPublishPeriod.Double() * 1000)), - // [this]() { this->PublishCurrentInfo(); }); +///////////////////////////////////////////////// +UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() : dataPtr(std::make_unique()) +{ } ///////////////////////////////////////////////// @@ -154,7 +145,7 @@ void UnderwaterCurrentPlugin::Configure( } ///////////////////////////////////////////////// -void gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin::PreUpdate( +void gz::sim::systems::dave_ros_gz_pluginsulator_ros::UnderwaterCurrentROSPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { // Advertise the service to get the current velocity model @@ -271,7 +262,7 @@ void UnderwaterCurrentROSPlugin::Update( } } ///////////////////////////////////////////////// -void gz::sim::systems::dave_simulator_ros::UnderwaterCurrentROSPlugin::PostUpdate( +void gz::sim::systems::dave_ros_gz_pluginsulator_ros::UnderwaterCurrentROSPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Advertise the service to update the current velocity model @@ -553,5 +544,6 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( ///////////////////////////////////////////////// GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) +} // namespace dave_ros_gz_plugins // #endif \ No newline at end of file From 5f88f0b92346483f1a1ac548f5781d5fb52ff872 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 1 Aug 2024 12:30:00 +0530 Subject: [PATCH 12/79] minor updates --- .../ocean_current_plugin.hh | 27 +------------------ .../src/ocean_current_plugin.cc | 0 2 files changed, 1 insertion(+), 26 deletions(-) mode change 100755 => 100644 gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index d8d754bb..046b7f64 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -45,16 +45,6 @@ #include #include -// using namespace gz; -// using namespace sim; -// using namespace systems; - -namespace gz -{ -namespace sim -{ -namespace systems -{ namespace dave_ros_gz_pluginsulator_ros { @@ -63,36 +53,21 @@ class UnderwaterCurrentROSPlugin : public System, public ISystemPreUpdate, public ISystemPostUpdate { - /// \brief Class constructor -public: - explicit UnderwaterCurrentROSPlugin(); - - /// \brief Class destructor public: + UnderwaterCurrentROSPlugin(); ~UnderwaterCurrentROSPlugin(); // Documentation inherited -public: void Configure( const Entity & _entity, const std::shared_ptr & _sdf, EntityComponentManager & _ecm, EventManager & _eventMgr) override; // Documentation inherited -public: void PreUpdate(const UpdateInfo & _info, EntityComponentManager & _ecm) override; // Documentation inherited -public: void PostUpdate(const UpdateInfo & _info, const EntityComponentManager & _ecm) override; } } // namespace dave_ros_gz_pluginsulator_ros -} // namespace systems -} // namespace sim -} // namespace gz - -// Register plugin -GZ_ADD_PLUGIN( - UnderwaterCurrentROSPlugin, gz::sim::System, UnderwaterCurrentROSPlugin::ISystemConfigure, - UnderwaterCurrentROSPlugin::ISystemPostUpdate) #endif // OCEAN_CURRENT_PLUGIN_H_ diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc old mode 100755 new mode 100644 From e601dc1e88b9abcf2ce4b718850a5bd4b4dc8afa Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 2 Aug 2024 01:44:41 +0530 Subject: [PATCH 13/79] added service support on ocean_current plugin --- dave_interfaces/CMakeLists.txt | 2 +- .../msg/StratifiedCurrentDatabase.msg | 2 +- gazebo/dave_gz_model_plugins/package.xml | 2 +- gazebo/dave_gz_world_plugins/CMakeLists.txt | 34 +-- .../ocean_current_world_plugin.hh | 18 +- gazebo/dave_gz_world_plugins/package.xml | 29 +-- .../src/ocean_current_world_plugin.cc | 9 +- .../src/tidal_oscillation.cc | 2 +- gazebo/dave_ros_gz_plugins/CMakeLists.txt | 2 +- .../ocean_current_plugin.hh | 120 ++++++++--- gazebo/dave_ros_gz_plugins/package.xml | 13 +- .../src/ocean_current_plugin.cc | 198 ++++++++++-------- 12 files changed, 248 insertions(+), 183 deletions(-) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 6f5eafc3..d32bb9b7 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -60,7 +60,7 @@ gz_msgs_generate_messages( # The cmake target to be generated for libraries/executables to link TARGET msgs # pressure_sensor_msgs_gen # The protobuf package to generate (Typically based on the path) - PROTO_PACKAGE "pressure_sensor_msgs.msgs" + PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" # The path to the base directory of the proto files # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto diff --git a/dave_interfaces/msg/StratifiedCurrentDatabase.msg b/dave_interfaces/msg/StratifiedCurrentDatabase.msg index 1cfba138..97143aad 100644 --- a/dave_interfaces/msg/StratifiedCurrentDatabase.msg +++ b/dave_interfaces/msg/StratifiedCurrentDatabase.msg @@ -7,7 +7,7 @@ float32[] depths geometry_msgs/Vector3[] velocities # Tide time (GMT) -int16[] gmtyear +int16[] timegmtyear int16[] timegmtmonth int16[] timegmtday int16[] timegmthour diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index 75c5c1ec..ba5ea320 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -1,7 +1,7 @@ dave_gz_model_plugins - 3.1.1 + 0.0.0 The dave_gz_model_plugins package (gz model plugin definitions) Gaurav Kumar and woensug Gaurav Kumar and woensug diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index a7e467eb..e84ec1b7 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -13,6 +13,7 @@ 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) @@ -31,36 +32,36 @@ src/gauss_markov_process.cc) add_library(tidal_oscillation SHARED src/tidal_oscillation.cc) -target_include_directories(ocean_current_world_plugin PRIVATE include) +target_include_directories(ocean_current_world_plugin PRIVATE include ${dave_interfaces_INCLUDE_DIRS}) target_include_directories(gauss_markov_process PRIVATE include) target_include_directories(tidal_oscillation PRIVATE include) target_link_libraries(ocean_current_world_plugin -gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) +gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) target_link_libraries(gauss_markov_process -gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) +gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) target_link_libraries(tidal_oscillation -gz-sim${GZ_SIM_VER}::GZ-SIM${GZ_SIM_VER}) +gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) ament_target_dependencies(ocean_current_world_plugin - "rclcpp" - "std_msgs" - "geometry_msgs" - "dave_interfaces" + rclcpp + std_msgs + geometry_msgs + dave_interfaces ) ament_target_dependencies(gauss_markov_process - "rclcpp" - "std_msgs" - "geometry_msgs" - "dave_interfaces" + rclcpp + std_msgs + geometry_msgs + dave_interfaces ) ament_target_dependencies(tidal_oscillation - "rclcpp" - "std_msgs" - "geometry_msgs" - "dave_interfaces" + rclcpp + std_msgs + geometry_msgs + dave_interfaces ) # Install targets @@ -77,7 +78,6 @@ install(DIRECTORY include/ ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") - # Testing setup if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index b8b3c49d..f0220806 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -21,21 +21,17 @@ #include #include - -#include #include +#include "dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.pb.h" // #include -#include -#include -#include +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -72,11 +68,9 @@ public: /// \param[in] _info Information used in the update event. // void Update(const common::UpdateInfo & _info); (check if this is needed) - void UnderwaterCurrentPlugin::Update( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - void UnderwaterCurrentPlugin::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml index d88bc6b8..04d455cc 100644 --- a/gazebo/dave_gz_world_plugins/package.xml +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -1,25 +1,28 @@ - dave_gazebo_world_plugin - 3.1.1 - The dave_gazebo_world_plugins package (Gazebo world plugin definitions) + dave_gz_world_plugins + 0.0.0 + The dave_gz_world_plugins package (gz world plugin definitions) Gaurav Kumar and woensug Gaurav Kumar and woensug - Apache 2.0 + TODO: License declaration ament_cmake - gazebo_ros - roscpp protobuf - dave_interfaces - gazebo_ros + rosidl_default_generators + gz_ros - gazebo_ros - roscpp protobuf - dave_interfaces - gazebo_dev - gazebo_msgs + rosidl_default_runtime + gz_dev + gz_msgs + rclcpp + std_msgs + dave_interfaces + sensor_msgs + geometry_msgs + ament_lint_auto + ament_lint_common ament_cmake diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index e51bdb78..7e92de00 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -16,7 +16,8 @@ /// \file ocean_current_world_plugin.cc #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" -#include "dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.pb.h +#include + #include #include #include @@ -28,12 +29,6 @@ #include #include -#include -#include -#include -#include -#include -#include #include #include diff --git a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc index 5d065feb..517b0cef 100644 --- a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc +++ b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc @@ -15,7 +15,7 @@ /// \file tidal_oscillation.cc -#include +#include #include diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 9233b75a..2f0082ae 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -48,7 +48,7 @@ ament_target_dependencies(ocean_current_plugin ) # Install targets -install(TARGETS SphericalCoords dave_ros_gz_plugins +install(TARGETS SphericalCoords DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index 046b7f64..7d733a0b 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -20,38 +20,42 @@ #ifndef OCEAN_CURRENT_PLUGIN_H_ #define OCEAN_CURRENT_PLUGIN_H_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include #include -#include - -#include -#include -#include +#include +#include #include #include #include -namespace dave_ros_gz_pluginsulator_ros -{ +#include "dave_interfaces/srv/GetCurrentModel.hpp" +#include "dave_interfaces/srv/GetOriginSphericalCoord.hpp" +#include "dave_interfaces/srv/SetCurrentDirection.hpp" +#include "dave_interfaces/srv/SetCurrentModel.hpp" +#include "dave_interfaces/srv/SetCurrentVelocity.hpp" +#include "dave_interfaces/srv/SetOriginSphericalCoord.hpp" +#include "dave_interfaces/srv/SetStratifiedCurrentDirection.hpp" +#include "dave_interfaces/srv/SetStratifiedCurrentVelocity.hpp" +#include "dave_interfaces/srv/StratifiedCurrentDatabase.hpp" +#include "dave_interfaces/srv/StratifiedCurrentVelocity.hpp" -class UnderwaterCurrentROSPlugin : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate +namespace dave_ros_gz_plugins +{ +class UnderwaterCurrentROSPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { public: UnderwaterCurrentROSPlugin(); @@ -59,15 +63,71 @@ public: // Documentation inherited void Configure( - const Entity & _entity, const std::shared_ptr & _sdf, - EntityComponentManager & _ecm, EventManager & _eventMgr) override; + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + + void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); // Documentation inherited - void PreUpdate(const UpdateInfo & _info, EntityComponentManager & _ecm) override; + void PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; // Documentation inherited - void PostUpdate(const UpdateInfo & _info, const EntityComponentManager & _ecm) override; -} -} // namespace dave_ros_gz_pluginsulator_ros + void PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; + + void UpdateHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateStratHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateStratVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateStratCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool GetCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool GetCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool GetCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res); + + bool UpdateCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res); + +private: + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_ros_gz_plugins #endif // OCEAN_CURRENT_PLUGIN_H_ diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml index 25fce8f1..218f37f3 100644 --- a/gazebo/dave_ros_gz_plugins/package.xml +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -9,26 +9,15 @@ ament_cmake protobuf-dev rclcpp - std_msgs dave_interfaces sensor_msgs geometry_msgs - ros_gz_bridge - ros_gz_sim - gz_msgs - gz_dev - gz_ros + std_msgs protobuf message_generation - rclcpp - dave_interfaces - gz_ros - gz_ros protobuf message_runtime - rclcpp - dave_interfaces ament_lint_auto ament_lint_common diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index f9f6bb70..a39e8784 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,23 +1,23 @@ -#include +#include "dave_ros_gz_plugins/ocean_current_plugin.hh" #include #include -#include #include -#include #include #include -#include -#include -#include -#include +#include #include #include #include +#include +#include +#include "gz/plugin/Register.hh" +#include "gz/sim/components/World.hh" + // Things to consider/decide for now - // 1. Is consistency being maintained while the use of "gzerr" and "RCLCPP_ERROR" for error logging // ? @@ -33,7 +33,6 @@ GZ_ADD_PLUGIN( namespace dave_ros_gz_plugins { - struct UnderwaterCurrentROSPlugin::PrivateData { rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating @@ -44,6 +43,31 @@ struct UnderwaterCurrentROSPlugin::PrivateData // Initialize the ROS 2 node this->rosNode = std::make_shared("underwater_current_ros_plugin"); std::chrono::steady_clock::duration lastUpdate{0}; + rclcpp::Service::SharedPtr getOriginSrv; + rclcpp::Service::SharedPtr setOriginSrv; + rclcpp::Service::SharedPtr get_current_velocity_model; + rclcpp::Service::SharedPtr get_current_horz_angle_model; + rclcpp::Service::SharedPtr get_current_vert_angle_model; + rclcpp::Service::SharedPtr; + rclcpp::Service::SharedPtr; + rclcpp::Service::SharedPtr; + rclcpp::Service::SharedPtr; + rclcpp::Service::SharedPtr get_current_velocity_model; + rclcpp::Service::SharedPtr get_current_horz_angle_model; + rclcpp::Service::SharedPtr get_current_vert_angle_model; + rclcpp::Service::SharedPtr set_current_velocity_model; + rclcpp::Service::SharedPtr set_current_horz_angle_model; + rclcpp::Service::SharedPtr set_current_vert_angle_model; + rclcpp::Service::SharedPtr set_current_velocity; + rclcpp::Service::SharedPtr + set_stratified_current_velocity; + rclcpp::Service::SharedPtr set_current_horz_angle; + rclcpp::Service::SharedPtr + set_stratified_current_horz_angle; + rclcpp::Service::SharedPtr set_current_vert_angle; + rclcpp::Service::SharedPtr + set_stratified_current_vert_angle; +}; }; ///////////////////////////////////////////////// @@ -119,22 +143,22 @@ void UnderwaterCurrentPlugin::Configure( // check for the number of arg being passed to the function // Advertise the service to get the current velocity model - this->worldServices["get_current_velocity_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_velocity_model = + this->rosNode->create_service( "get_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current horizontal angle model - this->worldServices["get_current_horz_angle_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_horz_angle_model = + this->rosNode->create_service( "get_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current vertical angle model - this->worldServices["get_current_vert_angle_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_vert_angle_model = + this->rosNode->create_service( "get_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); @@ -145,28 +169,28 @@ void UnderwaterCurrentPlugin::Configure( } ///////////////////////////////////////////////// -void gz::sim::systems::dave_ros_gz_pluginsulator_ros::UnderwaterCurrentROSPlugin::PreUpdate( +void gz::sim::systems::dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { // Advertise the service to get the current velocity model - this->worldServices["get_current_velocity_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_velocity_model = + this->rosNode->create_service( "get_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current horizontal angle model - this->worldServices["get_current_horz_angle_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_horz_angle_model = + this->rosNode->create_service( "get_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current vertical angle model - this->worldServices["get_current_vert_angle_model"] = - this->rosNode->create_service( + this->dataPtr->get_current_vert_angle_model = + this->rosNode->create_service( "get_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + &UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); } ///////////////////////////////////////////////// @@ -222,15 +246,15 @@ void UnderwaterCurrentROSPlugin::Update( if (this->tidalHarmonicFlag) { // Tidal harmonic constituents - currentDatabaseMsg.M2amp = this->M2_amp; - currentDatabaseMsg.M2phase = this->M2_phase; - currentDatabaseMsg.M2speed = this->M2_speed; - currentDatabaseMsg.S2amp = this->S2_amp; - currentDatabaseMsg.S2phase = this->S2_phase; - currentDatabaseMsg.S2speed = this->S2_speed; - currentDatabaseMsg.N2amp = this->N2_amp; - currentDatabaseMsg.N2phase = this->N2_phase; - currentDatabaseMsg.N2speed = this->N2_speed; + currentDatabaseMsg.m2amp = this->M2_amp; + currentDatabaseMsg.m2phase = this->M2_phase; + currentDatabaseMsg.m2speed = this->M2_speed; + currentDatabaseMsg.s2amp = this->S2_amp; + currentDatabaseMsg.s2phase = this->S2_phase; + currentDatabaseMsg.s2speed = this->S2_speed; + currentDatabaseMsg.n2amp = this->N2_amp; + currentDatabaseMsg.n2phase = this->N2_phase; + currentDatabaseMsg.n2speed = this->N2_speed; currentDatabaseMsg.tideConstituents = true; } else @@ -238,91 +262,91 @@ void UnderwaterCurrentROSPlugin::Update( for (int i = 0; i < this->dateGMT.size(); i++) { // Tidal oscillation database - currentDatabaseMsg.timeGMTYear.push_back(this->dateGMT[i][0]); - currentDatabaseMsg.timeGMTMonth.push_back(this->dateGMT[i][1]); - currentDatabaseMsg.timeGMTDay.push_back(this->dateGMT[i][2]); - currentDatabaseMsg.timeGMTHour.push_back(this->dateGMT[i][3]); - currentDatabaseMsg.timeGMTMinute.push_back(this->dateGMT[i][4]); + currentDatabaseMsg.timegmtyear.push_back(this->dateGMT[i][0]); + currentDatabaseMsg.timegmtmonth.push_back(this->dateGMT[i][1]); + currentDatabaseMsg.timegmtday.push_back(this->dateGMT[i][2]); + currentDatabaseMsg.timegmthour.push_back(this->dateGMT[i][3]); + currentDatabaseMsg.timegmtminute.push_back(this->dateGMT[i][4]); - currentDatabaseMsg.tideVelocities.push_back(this->speedcmsec[i]); + currentDatabaseMsg.tidevelocities.push_back(this->speedcmsec[i]); } currentDatabaseMsg.tideConstituents = false; } - currentDatabaseMsg.ebbDirection = this->ebbDirection; - currentDatabaseMsg.floodDirection = this->floodDirection; + currentDatabaseMsg.ebbdirection = this->ebbDirection; + currentDatabaseMsg.flooddirection = this->floodDirection; - currentDatabaseMsg.worldStartTimeYear = this->world_start_time_year; - currentDatabaseMsg.worldStartTimeMonth = this->world_start_time_month; - currentDatabaseMsg.worldStartTimeDay = this->world_start_time_day; - currentDatabaseMsg.worldStartTimeHour = this->world_start_time_hour; - currentDatabaseMsg.worldStartTimeMinute = this->world_start_time_minute; + currentDatabaseMsg.worldstarttimeyear = this->world_start_time_year; + currentDatabaseMsg.worldstarttimemonth = this->world_start_time_month; + currentDatabaseMsg.worldstarttimeday = this->world_start_time_day; + currentDatabaseMsg.worldstarttimehour = this->world_start_time_hour; + currentDatabaseMsg.worldstarttimeminute = this->world_start_time_minute; this->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); } } ///////////////////////////////////////////////// -void gz::sim::systems::dave_ros_gz_pluginsulator_ros::UnderwaterCurrentROSPlugin::PostUpdate( +void gz::sim::systems::dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Advertise the service to update the current velocity model - this->worldServices["set_current_velocity_model"] = - this->rosNode->create_service( + this->dataPtr->set_current_velocity_model = + this->rosNode->create_service( "set_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle model - this->worldServices["set_current_horz_angle_model"] = - this->rosNode->create_service( + this->dataPtr->set_current_horz_angle_model = + this->rosNode->create_service( "set_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current vertical angle model - this->worldServices["set_current_vert_angle_model"] = - this->rosNode->create_service( + this->dataPtr->set_current_vert_angle_model = + this->rosNode->create_service( "set_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current velocity mean value - this->worldServices["set_current_velocity"] = - this->rosNode->create_service( + this->dataPtr->set_current_velocity = + this->rosNode->create_service( "set_current_velocity", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current velocity - this->worldServices["set_stratified_current_velocity"] = - this->rosNode->create_service( + this->dataPtr->set_stratified_current_velocity = + this->rosNode->create_service( "set_stratified_current_velocity", std::bind( &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle - this->worldServices["set_current_horz_angle"] = - this->rosNode->create_service( + this->dataPtr->set_current_horz_angle = + this->rosNode->create_service( "set_current_horz_angle", std::bind( &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current horizontal angle - this->worldServices["set_stratified_current_horz_angle"] = - this->rosNode->create_service( + this->dataPtr->set_stratified_current_horz_angle = + this->rosNode->create_service( "set_stratified_current_horz_angle", std::bind( &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current vertical angle - this->worldServices["set_current_vert_angle"] = - this->rosNode->create_service( + this->dataPtr->set_current_vert_angle = + this->rosNode->create_service( "set_current_vert_angle", std::bind( &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current vertical angle - this->worldServices["set_stratified_current_vert_angle"] = - this->rosNode->create_service( + this->dataPtr->set_stratified_current_vert_angle = + this->rosNode->create_service( "set_stratified_current_vert_angle", std::bind( &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, this, std::placeholders::_1, std::placeholders::_2)); @@ -332,8 +356,8 @@ void gz::sim::systems::dave_ros_gz_pluginsulator_ros::UnderwaterCurrentROSPlugin } ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->success = this->currentHorzAngleModel.SetMean(_req->angle); @@ -342,8 +366,8 @@ bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { if (_req->layer >= this->stratifiedDatabase.size()) { @@ -365,8 +389,8 @@ bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->success = this->currentVertAngleModel.SetMean(_req->angle); return true; @@ -374,8 +398,8 @@ bool UnderwaterCurrentROSPlugin::UpdateVertAngle( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { if (_req->layer >= this->stratifiedDatabase.size()) { @@ -388,8 +412,8 @@ bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { if ( this->currentVelModel.SetMean(_req->velocity) && @@ -412,8 +436,8 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { if (_req->layer >= this->stratifiedDatabase.size()) { @@ -444,8 +468,8 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->mean = this->currentVelModel.mean; _res->min = this->currentVelModel.min; @@ -457,8 +481,8 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->mean = this->currentHorzAngleModel.mean; _res->min = this->currentHorzAngleModel.min; @@ -470,8 +494,8 @@ bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->mean = this->currentVertAngleModel.mean; _res->min = this->currentVertAngleModel.min; @@ -483,8 +507,8 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->success = this->currentVelModel.SetModel( std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, @@ -505,8 +529,8 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( } ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->success = this->currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); @@ -530,8 +554,8 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + const std::shared_ptr _req, + std::shared_ptr _res) { _res->success = this->currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); From fd37ffd3c6efaf3c3848608b1f18af68358595e3 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 3 Aug 2024 01:00:59 +0530 Subject: [PATCH 14/79] updates --- gazebo/dave_gz_world_plugins/CMakeLists.txt | 1 + gazebo/dave_gz_world_plugins/package.xml | 4 + .../src/ocean_current_world_plugin.cc | 110 +++--- .../ocean_current_plugin.hh | 68 ++-- .../src/ocean_current_plugin.cc | 335 +++++++++--------- 5 files changed, 270 insertions(+), 248 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index e84ec1b7..49d7b559 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(dave_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(Protobuf REQUIRED) find_package(gz-msgs10 REQUIRED) +find_package(ament_index_cpp REQUIRED) # Set version variables with gazebo Harmonic set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml index 04d455cc..b0d5d322 100644 --- a/gazebo/dave_gz_world_plugins/package.xml +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -12,6 +12,10 @@ rosidl_default_generators gz_ros + + ament_index_cpp + ament_index_cpp + protobuf rosidl_default_runtime gz_dev diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 7e92de00..d6f72d2d 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -19,23 +19,27 @@ #include #include -#include -#include - -#include -#include - #include #include +#include #include - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include - #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/World.hh" +// #include TODO (235-239) // using namespace gz; // using namespace sim; @@ -52,11 +56,8 @@ namespace dave_gz_world_plugins { struct UnderwaterCurrentPlugin::PrivateData { - /// \brief Update event - event::ConnectionPtr updateConnection; - /// \brief Pointer to world - physics::WorldPtr world; + gz::sim::World world{gz::sim::kNullEntity}; /// \brief Pointer to sdf sdf::ElementPtr sdf; @@ -65,13 +66,13 @@ struct UnderwaterCurrentPlugin::PrivateData bool hasSurface; /// \brief Pointer to a node for communication - transport::NodePtr node; + std::shared_ptr gz_node; /// \brief Map of publishers - std::map publishers; + gz::transport::Node::Publisher publishers; /// \brief Vehicle Depth Subscriber - transport::SubscriberPtr subscriber; + gz::transport::Node subscriber; /// \brief Current velocity topic std::string currentVelocityTopic; @@ -95,16 +96,16 @@ struct UnderwaterCurrentPlugin::PrivateData std::string ns; /// \brief Gauss-Markov process instance for the current velocity - GaussMarkovProcess currentVelModel; + gz::GaussMarkovProcess currentVelModel; /// \brief Gauss-Markov process instance for horizontal angle model - GaussMarkovProcess currentHorzAngleModel; + gz::GaussMarkovProcess currentHorzAngleModel; /// \brief Gauss-Markov process instance for vertical angle model - GaussMarkovProcess currentVertAngleModel; + gz::GaussMarkovProcess currentVertAngleModel; /// \brief Vector of Gauss-Markov process instances for stratified velocity - std::vector> stratifiedCurrentModels; + std::vector> stratifiedCurrentModels; /// \brief Vector of dateGMT for tidal oscillation std::vector> dateGMT; @@ -142,7 +143,7 @@ struct UnderwaterCurrentPlugin::PrivateData bool tideFlag; /// \brief Tidal Oscillation interpolation model - TidalOscillation tide; + gz::TidalOscillation tide; /// \brief Last update time stamp std::chrono::steady_clock::duration lastUpdate{0}; @@ -173,28 +174,28 @@ void UnderwaterCurrentPlugin::Configure( GZ_ASSERT(_entity != NULL, "World pointer is invalid"); GZ_ASSERT(_sdf != NULL, "SDF pointer is invalid"); - this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(components::World())); + this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); if (!this->dataPtr->world.Valid(_ecm)) // check { gzerr << "World entity not found" << std::endl; return; } - this->dataPtr->sdf = _sdf; + this->dataPtr->sdf = std::const_pointer_cast(_sdf); // Read the namespace for topics and services this->dataPtr->ns = _sdf->Get("namespace"); gzmsg << "Loading underwater world..." << std::endl; // Initializing the transport node - this->dataPtr->node = transport::NodePtr(new transport::Node()); + this->dataPtr->gz_node = std::make_shared(); - this->dataPtr->node->Init(this->dataPtr->world.Name(_ecm)); // check if correct + // this->dataPtr->gz_node->Init(this->dataPtr->world.Name(_ecm)); // check if correct - this->dataPtr->LoadGlobalCurrentConfig(); - this->dataPtr->LoadStratifiedCurrentDatabase(); + LoadGlobalCurrentConfig(); + LoadStratifiedCurrentDatabase(); if (this->dataPtr->sdf->HasElement("tidal_oscillation")) { - this->dataPtr->LoadTidalOscillationDatabase(); + LoadTidalOscillationDatabase(); } // Connect the update event. This isn't needed it seems (check) @@ -215,7 +216,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() sdf::ElementPtr tidalOscillationParams = this->dataPtr->sdf->GetElement("tidal_oscillation"); sdf::ElementPtr tidalHarmonicParams; - // Read the tidal oscillation parameter from the SDF file + // Read the tidal oscillation parameter from the SDF file // (TO DO)(TO UNDERSTAND) if (tidalOscillationParams->HasElement("databasefilePath")) { this->dataPtr->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); @@ -227,13 +228,14 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { tidalHarmonicParams = tidalOscillationParams->GetElement("harmonic_constituents"); gzmsg << "Tidal harmonic constituents " << "configuration found" << std::endl; - tidalHarmonicFlag = true; - } - else - { - this->dataPtr->tidalFilePath = - ros::package::getPath("dave_worlds") + "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; + this->dataPtr->tidalHarmonicFlag = true; } + // else + // { + // this->dataPtr->tidalFilePath = + // ament_index_cpp::get_package_share_directory("dave_worlds") + + // "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; + // } TODO } // Read the tidal oscillation direction from the SDF file @@ -273,7 +275,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() } } - if (tidalHarmonicFlag) + if (this->dataPtr->tidalHarmonicFlag) { // Read harmonic constituents GZ_ASSERT(tidalHarmonicParams->HasElement("M2"), "Harcomnic constituents M2 not found"); @@ -307,7 +309,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() csvFile.open(this->dataPtr->tidalFilePath); if (!csvFile) { - common::SystemPaths * paths = common::SystemPaths::Instance(); + gz::common::SystemPaths * paths = new gz::common::SystemPaths(); this->dataPtr->tidalFilePath = paths->FindFile(this->dataPtr->tidalFilePath, true); csvFile.open(this->dataPtr->tidalFilePath); } @@ -418,7 +420,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() csvFile.open(this->dataPtr->databaseFilePath); if (!csvFile) { - common::SystemPaths * paths = common::SystemPaths::Instance(); + gz::common::SystemPaths * paths = new gz::common::SystemPaths(); this->dataPtr->databaseFilePath = paths->FindFile(this->dataPtr->databaseFilePath, true); csvFile.open(this->dataPtr->databaseFilePath); } @@ -455,34 +457,35 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() // Means are the database-specified magnitudes & angles, and // the other values come from the constant current models // TODO: Vertical angle currently set to 0 (not in database) - GaussMarkovProcess magnitudeModel; + gz::GaussMarkovProcess magnitudeModel; magnitudeModel.mean = hypot(row[1], row[0]); magnitudeModel.var = magnitudeModel.mean; magnitudeModel.max = this->dataPtr->currentVelModel.max; magnitudeModel.min = 0.0; magnitudeModel.mu = this->dataPtr->currentVelModel.mu; magnitudeModel.noiseAmp = this->dataPtr->currentVelModel.noiseAmp; - magnitudeModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + // magnitudeModel.lastUpdate = this->dataPtr->lastUpdate; + magnitudeModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - GaussMarkovProcess hAngleModel; + gz::GaussMarkovProcess hAngleModel; hAngleModel.mean = atan2(row[1], row[0]); hAngleModel.var = hAngleModel.mean; hAngleModel.max = M_PI; hAngleModel.min = -M_PI; hAngleModel.mu = this->dataPtr->currentHorzAngleModel.mu; hAngleModel.noiseAmp = this->dataPtr->currentHorzAngleModel.noiseAmp; - hAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + hAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - GaussMarkovProcess vAngleModel; + gz::GaussMarkovProcess vAngleModel; vAngleModel.mean = 0.0; vAngleModel.var = vAngleModel.mean; vAngleModel.max = M_PI / 2.0; vAngleModel.min = -M_PI / 2.0; vAngleModel.mu = this->dataPtr->currentVertAngleModel.mu; vAngleModel.noiseAmp = this->dataPtr->currentVertAngleModel.noiseAmp; - vAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + vAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - std::vector depthModels; + std::vector depthModels; depthModels.push_back(magnitudeModel); depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); @@ -491,9 +494,8 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() csvFile.close(); this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic] = - this->dataPtr->node - ->create_publisher( - this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); + this->dataPtr->gz_node->Advertise( + this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); gzmsg << "Stratified current velocity topic name: " << this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic << std::endl; } @@ -662,13 +664,16 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() gzmsg << "Current velocity vertical angle [rad] Gauss-Markov process model:" << std::endl; this->dataPtr->currentVertAngleModel.Print(); - this->dataPtr->currentVelModel.lastUpdate = this->dataPtr->lastUpdate.Double(); - this->dataPtr->currentHorzAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); - this->dataPtr->currentVertAngleModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentVelModel.lastUpdate = + std::chrono::duration(this->dataPtr->lastUpdate).count(); + this->dataPtr->currentHorzAngleModel.lastUpdate = + std::chrono::duration(this->dataPtr->lastUpdate).count(); + this->dataPtr->currentVertAngleModel.lastUpdate = + std::chrono::duration(this->dataPtr->lastUpdate).count(); // Advertise the current velocity & stratified current velocity topics this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = - this->dataPtr->node->create_publisher( + this->dataPtr->gz_node->Advertise( this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); gzmsg << "Current velocity topic name: " << this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic << std::endl; @@ -748,7 +753,8 @@ void UnderwaterCurrentPlugin::Update( } ///////////////////////////////////////////////// -void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +void UnderwaterCurrentPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Update time stamp this->dataPtr->lastUpdate = _info.simTime; diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index 7d733a0b..3efa067e 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -39,16 +39,16 @@ #include #include -#include "dave_interfaces/srv/GetCurrentModel.hpp" -#include "dave_interfaces/srv/GetOriginSphericalCoord.hpp" -#include "dave_interfaces/srv/SetCurrentDirection.hpp" -#include "dave_interfaces/srv/SetCurrentModel.hpp" -#include "dave_interfaces/srv/SetCurrentVelocity.hpp" -#include "dave_interfaces/srv/SetOriginSphericalCoord.hpp" -#include "dave_interfaces/srv/SetStratifiedCurrentDirection.hpp" -#include "dave_interfaces/srv/SetStratifiedCurrentVelocity.hpp" -#include "dave_interfaces/srv/StratifiedCurrentDatabase.hpp" -#include "dave_interfaces/srv/StratifiedCurrentVelocity.hpp" +#include "dave_interfaces/srv/Get_Current_Model.hpp" +#include "dave_interfaces/srv/Get_Origin_Spherical_Coord.hpp" +#include "dave_interfaces/srv/Set_Current_Direction.hpp" +#include "dave_interfaces/srv/Set_Current_Model.hpp" +#include "dave_interfaces/srv/Set_Current_Velocity.hpp" +#include "dave_interfaces/srv/Set_Origin_Spherical_Coord.hpp" +#include "dave_interfaces/srv/Set_Stratified_Current_Direction.hpp" +#include "dave_interfaces/srv/Set_Stratified_Current_Velocity.hpp" +// #include "dave_interfaces/srv/Stratified_Current_Database.hpp" +// #include "dave_interfaces/srv/Stratified_Current_Velocity.hpp" namespace dave_ros_gz_plugins { @@ -77,52 +77,52 @@ public: const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; void UpdateHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateStratHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateStratVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateStratCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool GetCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool GetCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool GetCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); bool UpdateCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res); + const std::shared_ptr _req, + std::shared_ptr _res); private: struct PrivateData; diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index a39e8784..4d830731 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -37,21 +37,10 @@ struct UnderwaterCurrentROSPlugin::PrivateData { rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating // communication with other ROS nodes. - std::string db_path; - this->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); - - // Initialize the ROS 2 node - this->rosNode = std::make_shared("underwater_current_ros_plugin"); + // std::string db_path; (check)(TODO) + // this->dataPtr->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); std::chrono::steady_clock::duration lastUpdate{0}; - rclcpp::Service::SharedPtr getOriginSrv; - rclcpp::Service::SharedPtr setOriginSrv; - rclcpp::Service::SharedPtr get_current_velocity_model; - rclcpp::Service::SharedPtr get_current_horz_angle_model; - rclcpp::Service::SharedPtr get_current_vert_angle_model; - rclcpp::Service::SharedPtr; - rclcpp::Service::SharedPtr; - rclcpp::Service::SharedPtr; - rclcpp::Service::SharedPtr; + // // rclcpp::Service::SharedPtr; //Template rclcpp::Service::SharedPtr get_current_velocity_model; rclcpp::Service::SharedPtr get_current_horz_angle_model; rclcpp::Service::SharedPtr get_current_vert_angle_model; @@ -61,43 +50,56 @@ struct UnderwaterCurrentROSPlugin::PrivateData rclcpp::Service::SharedPtr set_current_velocity; rclcpp::Service::SharedPtr set_stratified_current_velocity; - rclcpp::Service::SharedPtr set_current_horz_angle; - rclcpp::Service::SharedPtr - set_stratified_current_horz_angle; - rclcpp::Service::SharedPtr set_current_vert_angle; - rclcpp::Service::SharedPtr - set_stratified_current_vert_angle; -}; + rclcpp::Service::SharedPtr + set_stratified_current_horz_angle_model; + rclcpp::Service::SharedPtr + set_stratified_current_vert_angle_model; + + std::shared_ptr rosNode; + gz::sim::World world; + gz::sim::Model model; + gz::sim::Entity entity; // Added entity member + std::string modelName; + std::shared_ptr sdf; + std::string stratifiedCurrentVelocityTopic; + std::string stratifiedCurrentVelocityDatabaseTopic; + std::string currentVelocityTopic; + std::string ns; + rclcpp::Publisher::SharedPtr flowVelocityPub; + rclcpp::Publisher::SharedPtr + stratifiedCurrentVelocityPub; + rclcpp::Publisher::SharedPtr + stratifiedCurrentDatabasePub; }; ///////////////////////////////////////////////// UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() : dataPtr(std::make_unique()) { } - -///////////////////////////////////////////////// -// this can be removed as the connections are now handled by samrt pointers (to be (updated)) UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() { - this->rosNode.reset(); // Properly handle ROS 2 node shutdown + this->dataPtr->rosNode.reset(); // Properly handle ROS 2 node shutdown } ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::Configure( +void UnderwaterCurrentROSPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { - this->dataPtr->_world = _entity; + this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin"); + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); + this->dataPtr->world = gz::sim::World(worldEntity); this->dataPtr->entity = _entity; this->dataPtr->model = gz::sim::Model(_entity); this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); this->dataPtr->sdf = _sdf; std::chrono::steady_clock::duration lastUpdate{0}; + // auto lastUpdate = gz::sim::simTime; try { - this->stratifiedCurrentVelocityDatabaseTopic = - this->stratifiedCurrentVelocityTopic + "_database"; + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = + this->dataPtr->stratifiedCurrentVelocityTopic + "_database"; } catch (const gz::common::Exception & e) // we can use both "_e" and "e" but "e" is now @@ -114,25 +116,27 @@ void UnderwaterCurrentPlugin::Configure( return; } - this->ns = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + this->dataPtr->ns = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; // Initialising ROS 2 node - this->rosNode = std::make_shared("underwater_current_ros_plugin", this->ns); + this->dataPtr->rosNode = + std::make_shared("underwater_current_ros_plugin", this->dataPtr->ns); // Create and advertise Messages // Advertise the flow velocity as a stamped twist message - this->flowVelocityPub = this->rosNode->create_publisher( - this->currentVelocityTopic, rclcpp::QoS(10)); + this->dataPtr->flowVelocityPub = + this->dataPtr->rosNode->create_publisher( + this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); // Advertise the stratified ocean current message - this->stratifiedCurrentVelocityPub = - this->rosNode->create_publisher( - this->stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); + this->dataPtr->stratifiedCurrentVelocityPub = + this->dataPtr->rosNode->create_publisher( + this->dataPtr->stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); // Advertise the stratified ocean current database message - this->stratifiedCurrentDatabasePub = - this->rosNode->create_publisher( - this->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); + this->dataPtr->stratifiedCurrentDatabasePub = + this->dataPtr->rosNode->create_publisher( + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); // Create and advertise services // In this setup : std::placeholders::_1 and std::placeholders::_2 are used to represent the @@ -144,51 +148,53 @@ void UnderwaterCurrentPlugin::Configure( // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current horizontal angle model this->dataPtr->get_current_horz_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current vertical angle model this->dataPtr->get_current_vert_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Connect to the Gazebo Harmonic update event - // this->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & info) - // { this->OnUpdateCurrentVel(info); }); + // this->dataPtr->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & + // info) + // { this->dataPtr->OnUpdateCurrentVel(info); + // }); } ///////////////////////////////////////////////// -void gz::sim::systems::dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::PreUpdate( +void UnderwaterCurrentROSPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current horizontal angle model this->dataPtr->get_current_horz_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current vertical angle model this->dataPtr->get_current_vert_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "get_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); @@ -197,192 +203,195 @@ void gz::sim::systems::dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::PreUpdat void UnderwaterCurrentROSPlugin::Update( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - if (_info.simTime > this->lastUpdate) + if (_info.simTime > this->dataPtr->lastUpdate) { // Generate and publish current velocity according to the vehicle depth geometry_msgs::msg::TwistStamped flowVelMsg; - flowVelMsg.header.stamp = this->rosNode->get_clock()->now(); + flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); flowVelMsg.header.frame_id = "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions - flowVelMsg.twist.linear.x = this->currentVelocity.X(); - flowVelMsg.twist.linear.y = this->currentVelocity.Y(); - flowVelMsg.twist.linear.z = this->currentVelocity.Z(); + flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); - this->flowVelocityPub->publish(flowVelMsg); + this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish stratified current velocity dave_ros_gz_plugins::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; - stratCurrentVelocityMsg.header.stamp = this->rosNode->get_clock()->now(); + stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); stratCurrentVelocityMsg.header.frame_id = "world"; // Updating for stratified behaviour of Ocean Currents // What is the .size value over here, to be (checked) - for (size_t i = 0; i < this->currentStratifiedVelocity.size(); i++) + for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); i++) { geometry_msgs::msg::Vector3 velocity; - velocity.x = this->currentStratifiedVelocity[i].X(); - velocity.y = this->currentStratifiedVelocity[i].Y(); - velocity.z = this->currentStratifiedVelocity[i].Z(); + velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); + velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); + velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); stratCurrentVelocityMsg.velocities.push_back(velocity); - stratCurrentVelocityMsg.depths.push_back(this->currentStratifiedVelocity[i].W()); + stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); } - this->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); + this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database dave_ros_gz_plugins::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; i < this->stratifiedDatabase.size(); i++) + for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) { // Stratified current database entry preparation geometry_msgs::msg::Vector3 velocity; - velocity.x = this->stratifiedDatabase[i].X(); - velocity.y = this->stratifiedDatabase[i].Y(); + velocity.x = this->dataPtr->stratifiedDatabase[i].X(); + velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); velocity.z = 0.0; // Assuming z is intentionally set to 0.0 currentDatabaseMsg.velocities.push_back(velocity); - currentDatabaseMsg.depths.push_back(this->stratifiedDatabase[i].Z()); + currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); } - if (this->tidalHarmonicFlag) + if (this->dataPtr->tidalHarmonicFlag) { // Tidal harmonic constituents - currentDatabaseMsg.m2amp = this->M2_amp; - currentDatabaseMsg.m2phase = this->M2_phase; - currentDatabaseMsg.m2speed = this->M2_speed; - currentDatabaseMsg.s2amp = this->S2_amp; - currentDatabaseMsg.s2phase = this->S2_phase; - currentDatabaseMsg.s2speed = this->S2_speed; - currentDatabaseMsg.n2amp = this->N2_amp; - currentDatabaseMsg.n2phase = this->N2_phase; - currentDatabaseMsg.n2speed = this->N2_speed; + currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; + currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; + currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; + currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; + currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; + currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; + currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; + currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; + currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; currentDatabaseMsg.tideConstituents = true; } else { - for (int i = 0; i < this->dateGMT.size(); i++) + for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) { // Tidal oscillation database - currentDatabaseMsg.timegmtyear.push_back(this->dateGMT[i][0]); - currentDatabaseMsg.timegmtmonth.push_back(this->dateGMT[i][1]); - currentDatabaseMsg.timegmtday.push_back(this->dateGMT[i][2]); - currentDatabaseMsg.timegmthour.push_back(this->dateGMT[i][3]); - currentDatabaseMsg.timegmtminute.push_back(this->dateGMT[i][4]); + currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); + currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); + currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); + currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); + currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); - currentDatabaseMsg.tidevelocities.push_back(this->speedcmsec[i]); + currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); } currentDatabaseMsg.tideConstituents = false; } - currentDatabaseMsg.ebbdirection = this->ebbDirection; - currentDatabaseMsg.flooddirection = this->floodDirection; + currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; + currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; - currentDatabaseMsg.worldstarttimeyear = this->world_start_time_year; - currentDatabaseMsg.worldstarttimemonth = this->world_start_time_month; - currentDatabaseMsg.worldstarttimeday = this->world_start_time_day; - currentDatabaseMsg.worldstarttimehour = this->world_start_time_hour; - currentDatabaseMsg.worldstarttimeminute = this->world_start_time_minute; + currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; + currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; + currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; + currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; + currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; - this->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); + this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); } } ///////////////////////////////////////////////// -void gz::sim::systems::dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::PostUpdate( +void UnderwaterCurrentROSPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Advertise the service to update the current velocity model this->dataPtr->set_current_velocity_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_current_velocity_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle model this->dataPtr->set_current_horz_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_current_horz_angle_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current vertical angle model this->dataPtr->set_current_vert_angle_model = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current velocity mean value this->dataPtr->set_current_velocity = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_current_velocity", std::bind( &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current velocity this->dataPtr->set_stratified_current_velocity = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_stratified_current_velocity", std::bind( &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle this->dataPtr->set_current_horz_angle = - this->rosNode->create_service( + this->dataPtr->rosNode->create_service( "set_current_horz_angle", std::bind( &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current horizontal angle - this->dataPtr->set_stratified_current_horz_angle = - this->rosNode->create_service( - "set_stratified_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, - this, std::placeholders::_1, std::placeholders::_2)); + this->dataPtr->set_stratified_current_horz_angle_model = + this->dataPtr->rosNode->create_service( + "set_stratified_current_horz_angle_model", + std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel, this, std::placeholders::_1, + std::placeholders::_2)); // Advertise the service to update the current vertical angle - this->dataPtr->set_current_vert_angle = - this->rosNode->create_service( - "set_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, - std::placeholders::_1, std::placeholders::_2)); + this->dataPtr->set_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current vertical angle - this->dataPtr->set_stratified_current_vert_angle = - this->rosNode->create_service( - "set_stratified_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, - this, std::placeholders::_1, std::placeholders::_2)); + this->dataPtr->set_stratified_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_stratified_current_vert_angle_model", + std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel, this, std::placeholders::_1, + std::placeholders::_2)); // Update the time tracking for publication - this->lastUpdate = _info.simTime; + this->dataPtr->lastUpdate = _info.simTime; } ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->currentHorzAngleModel.SetMean(_req->angle); + _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); return true; } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( +bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->stratifiedDatabase.size()) + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = this->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); if (_res->success) { // Update the database values (new angle, unchanged velocity) - double velocity = - hypot(this->stratifiedDatabase[_req->layer].X(), this->stratifiedDatabase[_req->layer].Y()); - this->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; - this->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + double velocity = hypot( + this->dataPtr->stratifiedDatabase[_req->layer].X(), + this->dataPtr->stratifiedDatabase[_req->layer].Y()); + this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; } return true; } @@ -392,21 +401,21 @@ bool UnderwaterCurrentROSPlugin::UpdateVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->currentVertAngleModel.SetMean(_req->angle); + _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); return true; } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( +bool UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->stratifiedDatabase.size()) + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = this->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); return true; } @@ -416,9 +425,9 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( std::shared_ptr _res) { if ( - this->currentVelModel.SetMean(_req->velocity) && - this->currentHorzAngleModel.SetMean(_req->horizontal_angle) && - this->currentVertAngleModel.SetMean(_req->vertical_angle)) + this->dataPtr->currentVelModel.SetMean(_req->velocity) && + this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && + this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) { gzmsg << "Current velocity [m/s] = " << _req.velocity << std::endl << "Current horizontal angle [rad] = " << _req.horizontal_angle << std::endl @@ -439,19 +448,21 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->stratifiedDatabase.size()) + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) { _res->success = false; return true; } if ( - this->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && - this->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && - this->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) { // Update the database values as well - this->stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; - this->stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; + this->dataPtr->stratifiedDatabase[_req->layer].X() = + cos(_req->horizontal_angle) * _req->velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = + sin(_req->horizontal_angle) * _req->velocity; gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl << " Vertical angle [rad] = " << _req.vertical_angle << std::endl @@ -471,11 +482,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->currentVelModel.mean; - _res->min = this->currentVelModel.min; - _res->max = this->currentVelModel.max; - _res->noise = this->currentVelModel.noiseAmp; - _res->mu = this->currentVelModel.mu; + _res->mean = this->dataPtr->currentVelModel.mean; + _res->min = this->dataPtr->currentVelModel.min; + _res->max = this->dataPtr->currentVelModel.max; + _res->noise = this->dataPtr->currentVelModel.noiseAmp; + _res->mu = this->dataPtr->currentVelModel.mu; return true; } @@ -484,11 +495,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->currentHorzAngleModel.mean; - _res->min = this->currentHorzAngleModel.min; - _res->max = this->currentHorzAngleModel.max; - _res->noise = this->currentHorzAngleModel.noiseAmp; - _res->mu = this->currentHorzAngleModel.mu; + _res->mean = this->dataPtr->currentHorzAngleModel.mean; + _res->min = this->dataPtr->currentHorzAngleModel.min; + _res->max = this->dataPtr->currentHorzAngleModel.max; + _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentHorzAngleModel.mu; return true; } @@ -497,11 +508,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->currentVertAngleModel.mean; - _res->min = this->currentVertAngleModel.min; - _res->max = this->currentVertAngleModel.max; - _res->noise = this->currentVertAngleModel.noiseAmp; - _res->mu = this->currentVertAngleModel.mu; + _res->mean = this->dataPtr->currentVertAngleModel.mean; + _res->min = this->dataPtr->currentVertAngleModel.min; + _res->max = this->dataPtr->currentVertAngleModel.max; + _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentVertAngleModel.mu; return true; } @@ -510,20 +521,20 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->currentVelModel.SetModel( + _res->success = this->dataPtr->currentVelModel.SetModel( std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); - for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) { - gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][0]; //(updated) + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) model.SetModel( model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); } gzmsg << "Current velocity model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->currentVelModel.Print(); + this->dataPtr->currentVelModel.Print(); return true; } @@ -532,24 +543,24 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = - this->currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + _res->success = this->dataPtr->currentHorzAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); - for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) { - gz::GaussMarkovProcess & model = this->stratifiedCurrentModels[i][1]; + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; model.SetModel( model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); } gzmsg << "Horizontal angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->currentHorzAngleModel.Print(); + this->dataPtr->currentHorzAngleModel.Print(); return true; // gzmsg << "Horizontal angle model updated" << std::endl // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->currentHorzAngleModel.Print(); + // this->dataPtr->currentHorzAngleModel.Print(); } ///////////////////////////////////////////////// @@ -557,12 +568,12 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = - this->currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + _res->success = this->dataPtr->currentVertAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); gzmsg << "Vertical angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->currentVertAngleModel.Print(); + this->dataPtr->currentVertAngleModel.Print(); return true; } From 206963f55d45c7c0d8afb4f1974515dbcc5060af Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 3 Aug 2024 21:31:50 +0530 Subject: [PATCH 15/79] updated code fixed bugs --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 12 +-- .../ocean_current_model_plugin.hh | 59 ++++++------- gazebo/dave_gz_model_plugins/package.xml | 1 + .../src/ocean_current_model_plugin.cc | 87 +++++++------------ .../src/ocean_current_world_plugin.cc | 38 ++++---- .../ocean_current_plugin.hh | 2 + .../src/ocean_current_plugin.cc | 16 ++-- 7 files changed, 95 insertions(+), 120 deletions(-) rename gazebo/dave_gz_model_plugins/include/{dave_gz_model_plugin => dave_gz_model_plugins}/ocean_current_model_plugin.hh (53%) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index d0c9f72f..17727dae 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(gz-common5 REQUIRED COMPONENTS profiler) find_package(gz-sim8 REQUIRED) find_package(geometry_msgs REQUIRED) find_package(dave_interfaces REQUIRED) +find_package(dave_gz_world_plugins REQUIRED) # Set version variables with gazebo Harmonic set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) @@ -24,7 +25,7 @@ add_library(ocean_current_model_plugin SHARED src/ocean_current_model_plugin.cc ) -target_include_directories(ocean_current_model_plugin PRIVATE include) +target_include_directories(ocean_current_model_plugin PRIVATE include ${dave_gz_world_plugins_INCLUDE_DIRS}) target_link_libraries(ocean_current_model_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -32,10 +33,11 @@ target_link_libraries(ocean_current_model_plugin # Specify dependencies for ocean_current_plugin using ament_target_dependencies ament_target_dependencies(ocean_current_model_plugin - "rclcpp" - "std_msgs" - "geometry_msgs" - "dave_interfaces" + rclcpp + std_msgs + geometry_msgs + dave_interfaces + dave_gz_world_plugins ) # Install targets diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh similarity index 53% rename from gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh rename to gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 3856790e..a309af2c 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugin/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -1,25 +1,22 @@ -#ifndef OCEAN_CURRENT_MODEL_PLUGIN_H_ -#define OCEAN_CURRENT_MODEL_PLUGIN_H_ +#ifndef OCEAN_CURRENT_MODEL_PLUGIN_HH_ +#define OCEAN_CURRENT_MODEL_PLUGIN_HH_ -#include +// #include "dave_gz_world_plugins/gauss_markov_process.hh" // #include #include -#include - -#include +// #include #include -#include +#include #include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include namespace dave_gz_model_plugins { @@ -30,8 +27,8 @@ class TransientCurrentPlugin : public gz::sim::System, public gz::sim::ISystemPostUpdate { public: - TransientCurrentPlugin(); // constructor - ~TransientCurrentPlugin(); // Destructor + TransientCurrentPlugin(); // constructor + ~TransientCurrentPlugin() override = default; // Destructor // Configure the plugin with necessary entities and component managers void Configure( @@ -41,8 +38,7 @@ public: // Function called before the simulation state updates void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - void TransientCurrentPlugin::Update( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); // Function called after the simulation state updates void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); @@ -51,7 +47,7 @@ public: /// \param[in] _entity Target entity /// \param[in] _ecm Entity component manager /// \return True if buoyancy should be applied. - bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; + // bool IsEnabled(gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm) const; // Initialize any necessary states before the plugin starts virtual void Init(); @@ -60,21 +56,20 @@ public: void databaseSubThread(); /// \brief Calculate ocean current using database and vehicle state - void TransientCurrentPlugin::CalculateOceanCurrent( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - - void TransientCurrentPlugin::PublishCurrentVelocity( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + void CalculateOceanCurrent(); - void TransientCurrentPlugin::Gauss_Markov_process_initialize( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + void Gauss_Markov_process_initialize(); /// \brief Convey model state from gazebo topic to outside - TransientCurrentPlugin::UpdateDatabase( - const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) + void UpdateDatabase(); - /// \brief Publish ocean current - void TransientCurrentPlugin::PublishCurrentVelocity( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -} + /// \brief Publish ocean current + void PublishCurrentVelocity(); + +private: + struct PrivateData; + std::unique_ptr dataPtr; +}; } // namespace dave_gz_model_plugins + +#endif // OCEAN_CURRENT_MODEL_PLUGIN_HH_ \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index ba5ea320..7ffbefbf 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -12,6 +12,7 @@ rclcpp protobuf dave_interfaces + message_generation gz_ros diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index f0c6720c..9c1f2f6d 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -14,16 +14,11 @@ // limitations under the License. /// \file ocean_current_model_plugin.cc - -#include "dave_gz_model_plugin/ocean_current_model_plugin.hh" +#include "dave_gz_model_plugins/ocean_current_model_plugin.hh" +// #include +// #include #include #include -#include -#include -#include -#include -#include - #include #include #include @@ -33,15 +28,20 @@ #include #include #include +#include +#include #include +#include +#include +#include +#include +#include #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -#include +// #include //TODO #include #include -#include -#include #include // Register plugin @@ -52,13 +52,8 @@ GZ_ADD_PLUGIN( dave_gz_model_plugins::TransientCurrentPlugin::ISystemUpdate, dave_gz_model_plugins::TransientCurrentPlugin::ISystemPostUpdate) -// Add plugin alias so that we can refer to the plugin without the version -// namespace -GZ_ADD_PLUGIN_ALIAS(dave_gz_model_plugins::TransientCurrentPlugin, "TransientCurrentPlugin") - namespace dave_gz_model_plugins { - struct TransientCurrentPlugin::PrivateData { // starts here @@ -67,7 +62,7 @@ struct TransientCurrentPlugin::PrivateData /// \param[in] _entity Target entity /// \param[in] _ecm Entity component manager /// \return True if buoyancy should be applied. - bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; + // bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; // Initialize any necessary states before the plugin starts virtual void Init(); @@ -78,7 +73,7 @@ struct TransientCurrentPlugin::PrivateData protected: /// \brief Pointer to world - physics::WorldPtr world; + gz::sim::World world{gz::sim::kNullEntity}; /// \brief Pointer to model gz::sim::Entity entity{gz::sim::kNullEntity}; @@ -92,22 +87,16 @@ struct TransientCurrentPlugin::PrivateData virtual void Connect(); /// \brief Pointer to a node for communication - gz::transport::NodePtr node; + std::shared_ptr gz_node; /// \brief Map of publishers - std::map publishers; + std::map publishers; /// \brief Current velocity topic and transient current velocity topic std::string currentVelocityTopic; - std::string transientCurrentVelocityTopic; - - /// \brief Convey model state from gazebo topic to outside - - virtual void UpdateDatabase( - const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg); /// \brief Last update time stamp - common::Time lastUpdate; + std::chrono::steady_clock::duration lastUpdate{0}; /// \brief Last depth index int lastDepthIndex; @@ -115,10 +104,10 @@ struct TransientCurrentPlugin::PrivateData /// \brief Current linear velocity vector gz::math::Vector3d currentVelocity; - /// \brief Gauss-Markov process instance for the velocity components - GaussMarkovProcess currentVelNorthModel; - GaussMarkovProcess currentVelEastModel; - GaussMarkovProcess currentVelDownModel; + /// \brief Gauss-Markov process instance for the velocity components // TODO + // gz::GaussMarkovProcess currentVelNorthModel; // TODO + // gz::GaussMarkovProcess currentVelEastModel; // TODO + // gz::GaussMarkovProcess currentVelDownModel; // TODO /// \brief Gauss-Markov noise double noiseAmp_North; @@ -132,7 +121,7 @@ struct TransientCurrentPlugin::PrivateData std::vector database; /// \brief Tidal Oscillation interpolation model - TidalOscillation tide; + // gz::TidalOscillation tide; /// \brief Tidal Oscillation flag bool tideFlag; @@ -166,7 +155,6 @@ struct TransientCurrentPlugin::PrivateData /// \brief Private data pointer private: - std::unique_ptr dataPtr; std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr flowVelocityPub; @@ -174,7 +162,6 @@ struct TransientCurrentPlugin::PrivateData std::shared_ptr rosNode; /// \brief Publisher for the flow velocity in the world frame - rclcpp::Publisher flowVelocityPub; /// \brief Subscriber for the transient ocean current database rclcpp::Subscriber databaseSub; @@ -192,22 +179,10 @@ struct TransientCurrentPlugin::PrivateData std::chrono::steady_clock::duration rosPublishPeriod{0}; /// \brief Last time we published a message via ROS. - std::chrono::steady_clock::duration lastRosPublishTime{0}; - - /// \brief ROS helper function that processes messages - void databaseSubThread(); - - /// \brief Calculate ocean current using database and vehicle state - void CalculateOceanCurrent(); - - /// \brief Publish ocean current - void PublishCurrentVelocity(); + // std::chrono::steady_clock::duration lastRosPublishTime{0}; std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr flowVelocityPub; - std::chrono::steady_clock::duration lastUpdate{0}; - // std::chrono::steady_clock::duration lastRosPublishTime{0}; - std::chrono::steady_clock::duration currentTime{0}; + // std::chrono::steady_clock::duration currentTime{0}; this->dataPtr->rosPublishPeriod = 0.05; // ends here }; @@ -258,11 +233,11 @@ void TransientCurrentPlugin::Configure( this->dataPtr->currentVelocityTopic, rclcpp::QOS(10)); // Initializing the Gazebo transport node - this->dataPtr->node = transport::NodePtr(new transport::Node()); + this->dataPtr->gz_node = transport::NodePtr(new transport::Node()); // check // Advertise the current velocity topic in ROS 2 this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = - this->dataPtr->node->create_publisher( + this->dataPtr->gz_node->create_publisher( this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); // Read topic name of stratified ocean current from SDF @@ -302,8 +277,7 @@ void TransientCurrentPlugin::Init() // Doing nothing for now } ///////////////////////////////////////////////// -void TransientCurrentPlugin::CalculateOceanCurrent( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +void TransientCurrentPlugin::CalculateOceanCurrent() { this->dataPtr->lock_.lock(); @@ -446,8 +420,7 @@ void TransientCurrentPlugin::CalculateOceanCurrent( this->dataPtr->lock_.unlock(); } ///////////////////////////////////////////////// -void TransientCurrentPlugin::PublishCurrentVelocity( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +void TransientCurrentPlugin::PublishCurrentVelocity() { geometry_msgs::TwistStamped flowVelMsg; flowVelMsg.header.stamp = rclcpp::Time().now(); @@ -467,8 +440,7 @@ void TransientCurrentPlugin::PublishCurrentVelocity( } ///////////////////////////////////////////////// -TransientCurrentPlugin::UpdateDatabase( - const dave_ros_gz_plugins::StratifiedCurrentDatabase::ConstPtr & _msg) +TransientCurrentPlugin::UpdateDatabase() { this->dataPtr->lock_.lock(); @@ -524,8 +496,7 @@ TransientCurrentPlugin::UpdateDatabase( } ///////////////////////////////////////////////// (check this,dpr) -void TransientCurrentPlugin::Gauss_Markov_process_initialize( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +void TransientCurrentPlugin::Gauss_Markov_process_initialize() { // Read Gauss-Markov parameters diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index d6f72d2d..24a886af 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -18,6 +18,7 @@ #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include +#include #include #include #include @@ -34,8 +35,9 @@ #include #include #include +#include #include -#include "gz/common/StringUtils.hh" +#include #include "gz/plugin/Register.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/World.hh" @@ -69,7 +71,8 @@ struct UnderwaterCurrentPlugin::PrivateData std::shared_ptr gz_node; /// \brief Map of publishers - gz::transport::Node::Publisher publishers; + // gz::transport::Node::Publisher publishers; + std::map publishers; /// \brief Vehicle Depth Subscriber gz::transport::Node subscriber; @@ -496,6 +499,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic] = this->dataPtr->gz_node->Advertise( this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); + gzmsg << "Stratified current velocity topic name: " << this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic << std::endl; } @@ -673,7 +677,7 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() // Advertise the current velocity & stratified current velocity topics this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = - this->dataPtr->gz_node->Advertise( + this->dataPtr->gz_node->Advertise( this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); gzmsg << "Current velocity topic name: " << this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic << std::endl; @@ -682,12 +686,12 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::PublishCurrentVelocity() { - msgs::Vector3d currentVel; - msgs::Set( + gz::msgs::Vector3d currentVel; + gz::msgs::Set( ¤tVel, gz::math::Vector3d( this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), this->dataPtr->currentVelocity.Z())); - this->dataPtr->publishers[this->dataPtr->currentVelocityTopic]->Publish(currentVel); + this->dataPtr->publishers[this->dataPtr->currentVelocityTopic].Publish(currentVel); } ///////////////////////////////////////////////// @@ -698,14 +702,14 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() this->dataPtr->currentStratifiedVelocity.begin(); it != this->dataPtr->currentStratifiedVelocity.end(); ++it) { - msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); + gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); currentVel.add_depth(it->W()); } if (currentVel.velocity_size() == 0) { return; } - this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic]->Publish(currentVel); + this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic].Publish(currentVel); } ///////////////////////////////////////////////// @@ -719,18 +723,18 @@ void UnderwaterCurrentPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { // Update the time - this->dataPtr->time = _info.simTime; + auto time = std::chrono::duration(_info.simTime).count(); // Calculate the flow velocity and the direction using the Gauss-Markov // model // Update current velocity - double currentVelMag = this->dataPtr->currentVelModel.Update(time.Double()); + double currentVelMag = this->dataPtr->currentVelModel.Update(time); // Update current horizontal direction around z axis of flow frame - double horzAngle = this->dataPtr->currentHorzAngleModel.Update(time.Double()); + double horzAngle = this->dataPtr->currentHorzAngleModel.Update(time); // Update current horizontal direction around z axis of flow frame - double vertAngle = this->dataPtr->currentVertAngleModel.Update(time.Double()); + double vertAngle = this->dataPtr->currentVertAngleModel.Update(time); // Generating the current velocity vector as in the North-East-Down frame this->dataPtr->currentVelocity = gz::math::Vector3d( @@ -742,9 +746,9 @@ void UnderwaterCurrentPlugin::Update( for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) { double depth = this->dataPtr->stratifiedDatabase[i].Z(); - currentVelMag = this->dataPtr->stratifiedCurrentModels[i][0].Update(time.Double()); - horzAngle = this->dataPtr->stratifiedCurrentModels[i][1].Update(time.Double()); - vertAngle = this->dataPtr->stratifiedCurrentModels[i][2].Update(time.Double()); + currentVelMag = this->dataPtr->stratifiedCurrentModels[i][0].Update(time); + horzAngle = this->dataPtr->stratifiedCurrentModels[i][1].Update(time); + vertAngle = this->dataPtr->stratifiedCurrentModels[i][2].Update(time); gz::math::Vector4d depthVel( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); @@ -758,7 +762,7 @@ void UnderwaterCurrentPlugin::PostUpdate( { // Update time stamp this->dataPtr->lastUpdate = _info.simTime; - this->dataPtr->PublishCurrentVelocity(); - this->dataPtr->PublishStratifiedCurrentVelocity(); + PublishCurrentVelocity(); + PublishStratifiedCurrentVelocity(); } } // namespace dave_gz_world_plugins \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index 3efa067e..84b39c61 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -39,6 +39,8 @@ #include #include +#include "dave_interfaces/msg/Stratified_Current_Database.hpp" +#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" #include "dave_interfaces/srv/Get_Current_Model.hpp" #include "dave_interfaces/srv/Get_Origin_Spherical_Coord.hpp" #include "dave_interfaces/srv/Set_Current_Direction.hpp" diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 4d830731..b6c6e9c9 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -13,6 +13,8 @@ #include #include +#include +#include #include #include #include "gz/plugin/Register.hh" @@ -35,8 +37,6 @@ namespace dave_ros_gz_plugins { struct UnderwaterCurrentROSPlugin::PrivateData { - rclcpp::Node::SharedPtr rosNode; // This is a smart pointer to a ROS 2 node, facilitating - // communication with other ROS nodes. // std::string db_path; (check)(TODO) // this->dataPtr->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); std::chrono::steady_clock::duration lastUpdate{0}; @@ -66,9 +66,9 @@ struct UnderwaterCurrentROSPlugin::PrivateData std::string currentVelocityTopic; std::string ns; rclcpp::Publisher::SharedPtr flowVelocityPub; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr stratifiedCurrentVelocityPub; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr stratifiedCurrentDatabasePub; }; @@ -130,12 +130,12 @@ void UnderwaterCurrentROSPlugin::Configure( // Advertise the stratified ocean current message this->dataPtr->stratifiedCurrentVelocityPub = - this->dataPtr->rosNode->create_publisher( + this->dataPtr->rosNode->create_publisher( this->dataPtr->stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); // Advertise the stratified ocean current database message this->dataPtr->stratifiedCurrentDatabasePub = - this->dataPtr->rosNode->create_publisher( + this->dataPtr->rosNode->create_publisher( this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); // Create and advertise services @@ -218,7 +218,7 @@ void UnderwaterCurrentROSPlugin::Update( this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish stratified current velocity - dave_ros_gz_plugins::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; + dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); stratCurrentVelocityMsg.header.frame_id = "world"; @@ -237,7 +237,7 @@ void UnderwaterCurrentROSPlugin::Update( this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database - dave_ros_gz_plugins::msg::StratifiedCurrentDatabase currentDatabaseMsg; + dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) { // Stratified current database entry preparation From 596458b22ba134026215dd3fa2707072b6fd03a8 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:43:24 +0530 Subject: [PATCH 16/79] ocean_current CSV Update/support added --- .../worlds/ACT1951_1_Annual_2021.csv | 8501 +++++++++++++++++ .../worlds/transientOceanCurrentDatabase.csv | 15 + 2 files changed, 8516 insertions(+) create mode 100644 models/dave_worlds/worlds/ACT1951_1_Annual_2021.csv create mode 100644 models/dave_worlds/worlds/transientOceanCurrentDatabase.csv diff --git a/models/dave_worlds/worlds/ACT1951_1_Annual_2021.csv b/models/dave_worlds/worlds/ACT1951_1_Annual_2021.csv new file mode 100644 index 00000000..1250fb59 --- /dev/null +++ b/models/dave_worlds/worlds/ACT1951_1_Annual_2021.csv @@ -0,0 +1,8501 @@ +Date_Time (GMT), Event, Speed (cm/sec) +2020-01-01 03:29, flood,39 +2020-01-01 05:26, slack,- +2020-01-01 06:51, ebb,-24 +2020-01-01 09:20, slack,- +2020-01-01 14:17, flood,33 +2020-01-01 17:32, slack,- +2020-01-01 19:03, ebb,-26 +2020-01-01 21:50, slack,- +2020-01-02 04:11, flood,35 +2020-01-02 06:08, slack,- +2020-01-02 07:33, ebb,-23 +2020-01-02 10:14, slack,- +2020-01-02 14:53, flood,32 +2020-01-02 18:20, slack,- +2020-01-02 19:45, ebb,-24 +2020-01-02 22:38, slack,- +2020-01-03 03:23, flood,33 +2020-01-03 06:56, slack,- +2020-01-03 08:21, ebb,-22 +2020-01-03 11:08, slack,- +2020-01-03 15:35, flood,32 +2020-01-03 19:08, slack,- +2020-01-03 20:33, ebb,-23 +2020-01-03 23:20, slack,- +2020-01-04 03:59, flood,34 +2020-01-04 07:38, slack,- +2020-01-04 09:09, ebb,-22 +2020-01-04 11:56, slack,- +2020-01-04 16:23, flood,32 +2020-01-04 19:56, slack,- +2020-01-04 21:27, ebb,-22 +2020-01-05 00:02, slack,- +2020-01-05 04:41, flood,35 +2020-01-05 08:26, slack,- +2020-01-05 09:57, ebb,-22 +2020-01-05 12:44, slack,- +2020-01-05 17:11, flood,32 +2020-01-05 20:44, slack,- +2020-01-05 22:15, ebb,-23 +2020-01-06 00:44, slack,- +2020-01-06 05:23, flood,36 +2020-01-06 09:14, slack,- +2020-01-06 10:51, ebb,-24 +2020-01-06 13:32, slack,- +2020-01-06 17:59, flood,34 +2020-01-06 21:38, slack,- +2020-01-06 23:09, ebb,-24 +2020-01-07 01:32, slack,- +2020-01-07 06:05, flood,38 +2020-01-07 10:02, slack,- +2020-01-07 11:39, ebb,-27 +2020-01-07 14:14, slack,- +2020-01-07 18:53, flood,36 +2020-01-07 22:32, slack,- +2020-01-08 00:03, ebb,-26 +2020-01-08 02:14, slack,- +2020-01-08 06:53, flood,41 +2020-01-08 10:44, slack,- +2020-01-08 12:27, ebb,-30 +2020-01-08 15:02, slack,- +2020-01-08 19:41, flood,40 +2020-01-08 23:20, slack,- +2020-01-09 00:51, ebb,-28 +2020-01-09 02:56, slack,- +2020-01-09 07:41, flood,44 +2020-01-09 11:32, slack,- +2020-01-09 13:15, ebb,-33 +2020-01-09 15:44, slack,- +2020-01-09 20:41, flood,44 +2020-01-10 00:08, slack,- +2020-01-10 01:39, ebb,-31 +2020-01-10 03:44, slack,- +2020-01-10 08:35, flood,47 +2020-01-10 12:14, slack,- +2020-01-10 14:03, ebb,-36 +2020-01-10 16:32, slack,- +2020-01-10 21:35, flood,47 +2020-01-11 00:56, slack,- +2020-01-11 02:33, ebb,-33 +2020-01-11 04:32, slack,- +2020-01-11 09:29, flood,50 +2020-01-11 13:02, slack,- +2020-01-11 14:51, ebb,-38 +2020-01-11 17:14, slack,- +2020-01-11 22:23, flood,51 +2020-01-12 01:44, slack,- +2020-01-12 03:21, ebb,-35 +2020-01-12 05:20, slack,- +2020-01-12 10:17, flood,53 +2020-01-12 13:50, slack,- +2020-01-12 15:45, ebb,-40 +2020-01-12 18:02, slack,- +2020-01-12 23:11, flood,52 +2020-01-13 02:32, slack,- +2020-01-13 04:09, ebb,-36 +2020-01-13 06:08, slack,- +2020-01-13 11:11, flood,53 +2020-01-13 14:44, slack,- +2020-01-13 16:33, ebb,-41 +2020-01-13 18:50, slack,- +2020-01-14 00:05, flood,53 +2020-01-14 03:26, slack,- +2020-01-14 04:57, ebb,-37 +2020-01-14 07:02, slack,- +2020-01-14 12:05, flood,52 +2020-01-14 15:32, slack,- +2020-01-14 17:21, ebb,-40 +2020-01-14 19:38, slack,- +2020-01-15 01:05, flood,52 +2020-01-15 04:14, slack,- +2020-01-15 05:51, ebb,-37 +2020-01-15 07:56, slack,- +2020-01-15 13:11, flood,50 +2020-01-15 16:26, slack,- +2020-01-15 18:15, ebb,-39 +2020-01-15 20:32, slack,- +2020-01-16 01:59, flood,52 +2020-01-16 05:08, slack,- +2020-01-16 06:45, ebb,-36 +2020-01-16 09:02, slack,- +2020-01-16 14:11, flood,48 +2020-01-16 17:26, slack,- +2020-01-16 19:03, ebb,-37 +2020-01-16 21:32, slack,- +2020-01-17 02:53, flood,51 +2020-01-17 06:02, slack,- +2020-01-17 07:33, ebb,-34 +2020-01-17 10:08, slack,- +2020-01-17 15:17, flood,46 +2020-01-17 18:26, slack,- +2020-01-17 19:57, ebb,-33 +2020-01-17 22:32, slack,- +2020-01-18 03:53, flood,49 +2020-01-18 06:56, slack,- +2020-01-18 08:33, ebb,-32 +2020-01-18 11:08, slack,- +2020-01-18 16:23, flood,44 +2020-01-18 19:26, slack,- +2020-01-18 20:57, ebb,-30 +2020-01-18 23:32, slack,- +2020-01-19 04:53, flood,46 +2020-01-19 07:56, slack,- +2020-01-19 09:33, ebb,-30 +2020-01-19 12:14, slack,- +2020-01-19 18:17, flood,43 +2020-01-19 20:26, slack,- +2020-01-19 22:03, ebb,-28 +2020-01-20 00:26, slack,- +2020-01-20 06:35, flood,45 +2020-01-20 08:56, slack,- +2020-01-20 10:39, ebb,-29 +2020-01-20 13:14, slack,- +2020-01-20 19:29, flood,45 +2020-01-20 21:26, slack,- +2020-01-20 23:09, ebb,-26 +2020-01-21 01:26, slack,- +2020-01-21 07:53, flood,45 +2020-01-21 09:50, slack,- +2020-01-21 11:57, ebb,-30 +2020-01-21 14:14, slack,- +2020-01-21 20:35, flood,49 +2020-01-21 22:26, slack,- +2020-01-22 00:21, ebb,-27 +2020-01-22 02:20, slack,- +2020-01-22 08:53, flood,46 +2020-01-22 10:50, slack,- +2020-01-22 13:03, ebb,-30 +2020-01-22 15:08, slack,- +2020-01-22 21:29, flood,52 +2020-01-22 23:26, slack,- +2020-01-23 01:27, ebb,-27 +2020-01-23 03:14, slack,- +2020-01-23 09:47, flood,48 +2020-01-23 11:38, slack,- +2020-01-23 13:57, ebb,-31 +2020-01-23 15:56, slack,- +2020-01-23 22:23, flood,55 +2020-01-24 00:14, slack,- +2020-01-24 02:21, ebb,-28 +2020-01-24 04:02, slack,- +2020-01-24 10:35, flood,48 +2020-01-24 12:26, slack,- +2020-01-24 14:39, ebb,-31 +2020-01-24 16:44, slack,- +2020-01-24 23:11, flood,54 +2020-01-25 01:02, slack,- +2020-01-25 03:09, ebb,-28 +2020-01-25 04:50, slack,- +2020-01-25 11:23, flood,47 +2020-01-25 13:14, slack,- +2020-01-25 15:15, ebb,-31 +2020-01-25 17:32, slack,- +2020-01-25 23:59, flood,52 +2020-01-26 01:50, slack,- +2020-01-26 03:39, ebb,-28 +2020-01-26 05:38, slack,- +2020-01-26 12:05, flood,44 +2020-01-26 14:02, slack,- +2020-01-26 15:51, ebb,-31 +2020-01-26 18:14, slack,- +2020-01-27 00:41, flood,49 +2020-01-27 02:38, slack,- +2020-01-27 04:15, ebb,-28 +2020-01-27 06:26, slack,- +2020-01-27 12:47, flood,41 +2020-01-27 14:44, slack,- +2020-01-27 16:27, ebb,-30 +2020-01-27 18:56, slack,- +2020-01-28 01:29, flood,45 +2020-01-28 03:20, slack,- +2020-01-28 04:57, ebb,-27 +2020-01-28 07:08, slack,- +2020-01-28 12:11, flood,38 +2020-01-28 15:32, slack,- +2020-01-28 17:03, ebb,-30 +2020-01-28 19:38, slack,- +2020-01-29 02:11, flood,40 +2020-01-29 04:02, slack,- +2020-01-29 05:33, ebb,-26 +2020-01-29 07:56, slack,- +2020-01-29 12:47, flood,36 +2020-01-29 16:14, slack,- +2020-01-29 17:45, ebb,-29 +2020-01-29 20:20, slack,- +2020-01-30 02:41, flood,36 +2020-01-30 04:44, slack,- +2020-01-30 06:15, ebb,-26 +2020-01-30 08:44, slack,- +2020-01-30 13:29, flood,35 +2020-01-30 16:56, slack,- +2020-01-30 18:27, ebb,-28 +2020-01-30 21:02, slack,- +2020-01-31 01:59, flood,35 +2020-01-31 05:32, slack,- +2020-01-31 06:57, ebb,-25 +2020-01-31 09:32, slack,- +2020-01-31 14:17, flood,34 +2020-01-31 17:38, slack,- +2020-01-31 19:15, ebb,-26 +2020-01-31 21:50, slack,- +2020-02-01 02:35, flood,35 +2020-02-01 06:14, slack,- +2020-02-01 07:39, ebb,-24 +2020-02-01 10:26, slack,- +2020-02-01 14:59, flood,33 +2020-02-01 18:26, slack,- +2020-02-01 19:57, ebb,-25 +2020-02-01 22:32, slack,- +2020-02-02 03:17, flood,36 +2020-02-02 06:56, slack,- +2020-02-02 08:27, ebb,-24 +2020-02-02 11:20, slack,- +2020-02-02 15:47, flood,33 +2020-02-02 19:14, slack,- +2020-02-02 20:45, ebb,-24 +2020-02-02 23:20, slack,- +2020-02-03 03:59, flood,36 +2020-02-03 07:38, slack,- +2020-02-03 09:21, ebb,-24 +2020-02-03 12:08, slack,- +2020-02-03 16:41, flood,33 +2020-02-03 20:08, slack,- +2020-02-03 21:39, ebb,-23 +2020-02-04 00:08, slack,- +2020-02-04 04:47, flood,37 +2020-02-04 08:32, slack,- +2020-02-04 10:09, ebb,-25 +2020-02-04 12:56, slack,- +2020-02-04 17:29, flood,35 +2020-02-04 21:02, slack,- +2020-02-04 22:39, ebb,-24 +2020-02-05 00:56, slack,- +2020-02-05 05:35, flood,39 +2020-02-05 09:20, slack,- +2020-02-05 11:09, ebb,-27 +2020-02-05 13:44, slack,- +2020-02-05 18:23, flood,37 +2020-02-05 22:02, slack,- +2020-02-05 23:33, ebb,-26 +2020-02-06 01:44, slack,- +2020-02-06 06:23, flood,42 +2020-02-06 10:14, slack,- +2020-02-06 11:57, ebb,-30 +2020-02-06 14:32, slack,- +2020-02-06 19:17, flood,40 +2020-02-06 22:50, slack,- +2020-02-07 00:27, ebb,-29 +2020-02-07 02:32, slack,- +2020-02-07 07:17, flood,45 +2020-02-07 11:02, slack,- +2020-02-07 12:51, ebb,-33 +2020-02-07 15:20, slack,- +2020-02-07 20:17, flood,44 +2020-02-07 23:44, slack,- +2020-02-08 01:15, ebb,-31 +2020-02-08 03:20, slack,- +2020-02-08 08:17, flood,49 +2020-02-08 11:56, slack,- +2020-02-08 13:39, ebb,-36 +2020-02-08 16:08, slack,- +2020-02-08 21:17, flood,49 +2020-02-09 00:32, slack,- +2020-02-09 02:09, ebb,-34 +2020-02-09 04:08, slack,- +2020-02-09 09:11, flood,52 +2020-02-09 12:44, slack,- +2020-02-09 14:33, ebb,-39 +2020-02-09 16:56, slack,- +2020-02-09 22:05, flood,53 +2020-02-10 01:20, slack,- +2020-02-10 02:57, ebb,-36 +2020-02-10 05:02, slack,- +2020-02-10 10:05, flood,55 +2020-02-10 13:32, slack,- +2020-02-10 15:21, ebb,-41 +2020-02-10 17:44, slack,- +2020-02-10 22:53, flood,55 +2020-02-11 02:14, slack,- +2020-02-11 03:51, ebb,-38 +2020-02-11 05:50, slack,- +2020-02-11 10:59, flood,56 +2020-02-11 14:26, slack,- +2020-02-11 16:09, ebb,-41 +2020-02-11 18:32, slack,- +2020-02-11 23:47, flood,55 +2020-02-12 03:02, slack,- +2020-02-12 04:39, ebb,-39 +2020-02-12 06:44, slack,- +2020-02-12 11:53, flood,54 +2020-02-12 15:20, slack,- +2020-02-12 17:03, ebb,-41 +2020-02-12 19:20, slack,- +2020-02-13 00:41, flood,54 +2020-02-13 03:50, slack,- +2020-02-13 05:27, ebb,-38 +2020-02-13 07:38, slack,- +2020-02-13 12:53, flood,51 +2020-02-13 16:14, slack,- +2020-02-13 17:51, ebb,-39 +2020-02-13 20:08, slack,- +2020-02-14 01:41, flood,52 +2020-02-14 04:44, slack,- +2020-02-14 06:21, ebb,-37 +2020-02-14 08:38, slack,- +2020-02-14 14:05, flood,48 +2020-02-14 17:08, slack,- +2020-02-14 18:45, ebb,-36 +2020-02-14 21:08, slack,- +2020-02-15 02:35, flood,49 +2020-02-15 05:38, slack,- +2020-02-15 07:15, ebb,-34 +2020-02-15 09:44, slack,- +2020-02-15 15:11, flood,45 +2020-02-15 18:02, slack,- +2020-02-15 19:39, ebb,-32 +2020-02-15 22:08, slack,- +2020-02-16 03:35, flood,46 +2020-02-16 06:32, slack,- +2020-02-16 08:09, ebb,-31 +2020-02-16 10:50, slack,- +2020-02-16 16:47, flood,42 +2020-02-16 19:02, slack,- +2020-02-16 20:33, ebb,-28 +2020-02-16 23:08, slack,- +2020-02-17 04:59, flood,42 +2020-02-17 07:32, slack,- +2020-02-17 09:03, ebb,-28 +2020-02-17 11:56, slack,- +2020-02-17 18:11, flood,43 +2020-02-17 20:08, slack,- +2020-02-17 21:33, ebb,-25 +2020-02-18 00:08, slack,- +2020-02-18 06:29, flood,41 +2020-02-18 08:32, slack,- +2020-02-18 10:15, ebb,-27 +2020-02-18 12:56, slack,- +2020-02-18 19:17, flood,45 +2020-02-18 21:08, slack,- +2020-02-18 23:03, ebb,-24 +2020-02-19 01:08, slack,- +2020-02-19 07:35, flood,42 +2020-02-19 09:32, slack,- +2020-02-19 12:27, ebb,-27 +2020-02-19 13:56, slack,- +2020-02-19 20:17, flood,48 +2020-02-19 22:08, slack,- +2020-02-20 00:45, ebb,-25 +2020-02-20 02:02, slack,- +2020-02-20 08:35, flood,44 +2020-02-20 10:26, slack,- +2020-02-20 13:15, ebb,-28 +2020-02-20 14:50, slack,- +2020-02-20 21:11, flood,51 +2020-02-20 23:02, slack,- +2020-02-21 01:33, ebb,-26 +2020-02-21 02:56, slack,- +2020-02-21 09:29, flood,46 +2020-02-21 11:20, slack,- +2020-02-21 14:09, ebb,-29 +2020-02-21 15:38, slack,- +2020-02-21 21:59, flood,53 +2020-02-21 23:50, slack,- +2020-02-22 02:21, ebb,-27 +2020-02-22 03:44, slack,- +2020-02-22 10:17, flood,47 +2020-02-22 12:08, slack,- +2020-02-22 14:45, ebb,-30 +2020-02-22 16:20, slack,- +2020-02-22 22:47, flood,53 +2020-02-23 00:38, slack,- +2020-02-23 03:03, ebb,-28 +2020-02-23 04:26, slack,- +2020-02-23 10:59, flood,47 +2020-02-23 12:50, slack,- +2020-02-23 15:03, ebb,-30 +2020-02-23 17:08, slack,- +2020-02-23 23:29, flood,50 +2020-02-24 01:26, slack,- +2020-02-24 03:21, ebb,-28 +2020-02-24 05:14, slack,- +2020-02-24 11:35, flood,44 +2020-02-24 13:32, slack,- +2020-02-24 15:21, ebb,-30 +2020-02-24 17:44, slack,- +2020-02-25 00:11, flood,46 +2020-02-25 02:08, slack,- +2020-02-25 03:51, ebb,-28 +2020-02-25 05:56, slack,- +2020-02-25 11:11, flood,41 +2020-02-25 14:20, slack,- +2020-02-25 15:57, ebb,-30 +2020-02-25 18:20, slack,- +2020-02-26 00:53, flood,42 +2020-02-26 02:50, slack,- +2020-02-26 04:21, ebb,-28 +2020-02-26 06:38, slack,- +2020-02-26 11:29, flood,40 +2020-02-26 14:56, slack,- +2020-02-26 16:33, ebb,-30 +2020-02-26 19:02, slack,- +2020-02-26 23:59, flood,38 +2020-02-27 03:26, slack,- +2020-02-27 05:03, ebb,-28 +2020-02-27 07:20, slack,- +2020-02-27 12:05, flood,39 +2020-02-27 15:38, slack,- +2020-02-27 17:15, ebb,-30 +2020-02-27 19:38, slack,- +2020-02-28 00:29, flood,38 +2020-02-28 04:08, slack,- +2020-02-28 05:39, ebb,-28 +2020-02-28 08:08, slack,- +2020-02-28 12:47, flood,37 +2020-02-28 16:20, slack,- +2020-02-28 17:57, ebb,-29 +2020-02-28 20:14, slack,- +2020-02-29 01:11, flood,37 +2020-02-29 04:50, slack,- +2020-02-29 06:21, ebb,-27 +2020-02-29 08:56, slack,- +2020-02-29 13:41, flood,36 +2020-02-29 17:02, slack,- +2020-02-29 18:39, ebb,-28 +2020-02-29 21:02, slack,- +2020-03-01 01:53, flood,37 +2020-03-01 05:26, slack,- +2020-03-01 07:09, ebb,-27 +2020-03-01 09:44, slack,- +2020-03-01 14:29, flood,35 +2020-03-01 17:50, slack,- +2020-03-01 19:27, ebb,-27 +2020-03-01 21:50, slack,- +2020-03-02 02:41, flood,37 +2020-03-02 06:08, slack,- +2020-03-02 07:51, ebb,-26 +2020-03-02 10:38, slack,- +2020-03-02 15:17, flood,35 +2020-03-02 18:44, slack,- +2020-03-02 20:15, ebb,-26 +2020-03-02 22:38, slack,- +2020-03-03 03:23, flood,38 +2020-03-03 06:56, slack,- +2020-03-03 08:45, ebb,-26 +2020-03-03 11:32, slack,- +2020-03-03 16:11, flood,35 +2020-03-03 19:38, slack,- +2020-03-03 21:09, ebb,-25 +2020-03-03 23:32, slack,- +2020-03-04 04:17, flood,38 +2020-03-04 07:50, slack,- +2020-03-04 09:39, ebb,-27 +2020-03-04 12:20, slack,- +2020-03-04 17:05, flood,37 +2020-03-04 20:32, slack,- +2020-03-04 22:09, ebb,-25 +2020-03-05 00:26, slack,- +2020-03-05 05:11, flood,40 +2020-03-05 08:50, slack,- +2020-03-05 10:33, ebb,-28 +2020-03-05 13:14, slack,- +2020-03-05 17:59, flood,39 +2020-03-05 21:32, slack,- +2020-03-05 23:03, ebb,-27 +2020-03-06 01:20, slack,- +2020-03-06 06:05, flood,43 +2020-03-06 09:44, slack,- +2020-03-06 11:33, ebb,-31 +2020-03-06 14:08, slack,- +2020-03-06 18:59, flood,42 +2020-03-06 22:26, slack,- +2020-03-07 00:03, ebb,-30 +2020-03-07 02:08, slack,- +2020-03-07 06:59, flood,46 +2020-03-07 10:38, slack,- +2020-03-07 12:27, ebb,-34 +2020-03-07 14:56, slack,- +2020-03-07 19:59, flood,46 +2020-03-07 23:20, slack,- +2020-03-08 00:57, ebb,-33 +2020-03-08 03:02, slack,- +2020-03-08 07:59, flood,50 +2020-03-08 11:32, slack,- +2020-03-08 13:21, ebb,-37 +2020-03-08 15:44, slack,- +2020-03-08 20:59, flood,51 +2020-03-09 00:08, slack,- +2020-03-09 01:45, ebb,-36 +2020-03-09 03:56, slack,- +2020-03-09 08:59, flood,54 +2020-03-09 12:26, slack,- +2020-03-09 14:09, ebb,-39 +2020-03-09 16:32, slack,- +2020-03-09 21:53, flood,55 +2020-03-10 01:02, slack,- +2020-03-10 02:39, ebb,-38 +2020-03-10 04:44, slack,- +2020-03-10 09:53, flood,57 +2020-03-10 13:14, slack,- +2020-03-10 15:03, ebb,-41 +2020-03-10 17:20, slack,- +2020-03-10 22:41, flood,57 +2020-03-11 01:50, slack,- +2020-03-11 03:27, ebb,-39 +2020-03-11 05:38, slack,- +2020-03-11 10:47, flood,57 +2020-03-11 14:08, slack,- +2020-03-11 15:51, ebb,-41 +2020-03-11 18:08, slack,- +2020-03-11 23:23, flood,56 +2020-03-12 02:38, slack,- +2020-03-12 04:21, ebb,-40 +2020-03-12 06:32, slack,- +2020-03-12 11:41, flood,55 +2020-03-12 15:02, slack,- +2020-03-12 16:39, ebb,-40 +2020-03-12 18:56, slack,- +2020-03-13 00:17, flood,53 +2020-03-13 03:32, slack,- +2020-03-13 05:09, ebb,-39 +2020-03-13 07:26, slack,- +2020-03-13 12:47, flood,51 +2020-03-13 15:56, slack,- +2020-03-13 17:33, ebb,-37 +2020-03-13 19:44, slack,- +2020-03-14 01:17, flood,50 +2020-03-14 04:20, slack,- +2020-03-14 05:57, ebb,-37 +2020-03-14 08:20, slack,- +2020-03-14 14:11, flood,47 +2020-03-14 16:50, slack,- +2020-03-14 18:21, ebb,-34 +2020-03-14 20:44, slack,- +2020-03-15 02:23, flood,46 +2020-03-15 05:14, slack,- +2020-03-15 06:51, ebb,-34 +2020-03-15 09:26, slack,- +2020-03-15 15:35, flood,45 +2020-03-15 17:44, slack,- +2020-03-15 19:15, ebb,-30 +2020-03-15 21:44, slack,- +2020-03-16 03:35, flood,42 +2020-03-16 06:08, slack,- +2020-03-16 07:39, ebb,-30 +2020-03-16 10:32, slack,- +2020-03-16 16:47, flood,43 +2020-03-16 18:44, slack,- +2020-03-16 20:09, ebb,-26 +2020-03-16 22:44, slack,- +2020-03-17 05:05, flood,39 +2020-03-17 07:08, slack,- +2020-03-17 08:39, ebb,-27 +2020-03-17 11:38, slack,- +2020-03-17 17:53, flood,43 +2020-03-17 19:44, slack,- +2020-03-17 21:09, ebb,-23 +2020-03-17 23:50, slack,- +2020-03-18 06:11, flood,39 +2020-03-18 08:08, slack,- +2020-03-18 09:45, ebb,-24 +2020-03-18 12:38, slack,- +2020-03-18 18:53, flood,44 +2020-03-18 20:44, slack,- +2020-03-18 23:39, ebb,-23 +2020-03-19 00:44, slack,- +2020-03-19 07:11, flood,40 +2020-03-19 09:02, slack,- +2020-03-19 12:15, ebb,-25 +2020-03-19 13:32, slack,- +2020-03-19 19:53, flood,46 +2020-03-19 21:44, slack,- +2020-03-20 00:33, ebb,-24 +2020-03-20 01:38, slack,- +2020-03-20 08:11, flood,42 +2020-03-20 10:02, slack,- +2020-03-20 13:03, ebb,-27 +2020-03-20 14:20, slack,- +2020-03-20 20:47, flood,48 +2020-03-20 22:38, slack,- +2020-03-21 01:21, ebb,-26 +2020-03-21 02:32, slack,- +2020-03-21 09:05, flood,44 +2020-03-21 10:50, slack,- +2020-03-21 13:45, ebb,-28 +2020-03-21 15:08, slack,- +2020-03-21 21:35, flood,49 +2020-03-21 23:26, slack,- +2020-03-22 02:03, ebb,-27 +2020-03-22 03:20, slack,- +2020-03-22 09:47, flood,45 +2020-03-22 11:38, slack,- +2020-03-22 14:21, ebb,-28 +2020-03-22 15:50, slack,- +2020-03-22 22:17, flood,49 +2020-03-23 00:08, slack,- +2020-03-23 02:39, ebb,-27 +2020-03-23 04:02, slack,- +2020-03-23 10:29, flood,45 +2020-03-23 12:26, slack,- +2020-03-23 14:21, ebb,-28 +2020-03-23 16:32, slack,- +2020-03-23 22:59, flood,46 +2020-03-24 00:50, slack,- +2020-03-24 02:51, ebb,-28 +2020-03-24 04:44, slack,- +2020-03-24 10:59, flood,43 +2020-03-24 13:08, slack,- +2020-03-24 14:51, ebb,-29 +2020-03-24 17:08, slack,- +2020-03-24 23:35, flood,43 +2020-03-25 01:32, slack,- +2020-03-25 03:15, ebb,-29 +2020-03-25 05:26, slack,- +2020-03-25 10:23, flood,42 +2020-03-25 13:44, slack,- +2020-03-25 15:27, ebb,-30 +2020-03-25 17:44, slack,- +2020-03-25 22:41, flood,40 +2020-03-26 02:14, slack,- +2020-03-26 03:51, ebb,-29 +2020-03-26 06:08, slack,- +2020-03-26 10:53, flood,42 +2020-03-26 14:26, slack,- +2020-03-26 16:03, ebb,-30 +2020-03-26 18:20, slack,- +2020-03-26 23:05, flood,40 +2020-03-27 02:50, slack,- +2020-03-27 04:27, ebb,-30 +2020-03-27 06:50, slack,- +2020-03-27 11:35, flood,41 +2020-03-27 15:08, slack,- +2020-03-27 16:45, ebb,-30 +2020-03-27 18:56, slack,- +2020-03-27 23:41, flood,40 +2020-03-28 03:26, slack,- +2020-03-28 05:09, ebb,-30 +2020-03-28 07:32, slack,- +2020-03-28 12:17, flood,40 +2020-03-28 15:50, slack,- +2020-03-28 17:27, ebb,-30 +2020-03-28 19:32, slack,- +2020-03-29 00:23, flood,40 +2020-03-29 04:08, slack,- +2020-03-29 05:51, ebb,-30 +2020-03-29 08:14, slack,- +2020-03-29 13:11, flood,38 +2020-03-29 16:32, slack,- +2020-03-29 18:09, ebb,-29 +2020-03-29 20:20, slack,- +2020-03-30 01:11, flood,39 +2020-03-30 04:44, slack,- +2020-03-30 06:33, ebb,-30 +2020-03-30 09:02, slack,- +2020-03-30 13:59, flood,38 +2020-03-30 17:20, slack,- +2020-03-30 18:57, ebb,-28 +2020-03-30 21:08, slack,- +2020-03-31 02:05, flood,39 +2020-03-31 05:32, slack,- +2020-03-31 07:21, ebb,-29 +2020-03-31 10:02, slack,- +2020-03-31 14:53, flood,38 +2020-03-31 18:14, slack,- +2020-03-31 19:51, ebb,-27 +2020-03-31 22:02, slack,- +2020-04-01 02:59, flood,39 +2020-04-01 06:20, slack,- +2020-04-01 08:15, ebb,-29 +2020-04-01 10:56, slack,- +2020-04-01 15:47, flood,38 +2020-04-01 19:08, slack,- +2020-04-01 20:45, ebb,-27 +2020-04-01 23:02, slack,- +2020-04-02 03:47, flood,40 +2020-04-02 07:20, slack,- +2020-04-02 09:09, ebb,-29 +2020-04-02 11:50, slack,- +2020-04-02 16:41, flood,40 +2020-04-02 20:08, slack,- +2020-04-02 21:39, ebb,-27 +2020-04-03 00:02, slack,- +2020-04-03 04:47, flood,42 +2020-04-03 08:20, slack,- +2020-04-03 10:09, ebb,-29 +2020-04-03 12:44, slack,- +2020-04-03 17:41, flood,42 +2020-04-03 21:02, slack,- +2020-04-03 22:39, ebb,-28 +2020-04-04 00:56, slack,- +2020-04-04 05:47, flood,45 +2020-04-04 09:20, slack,- +2020-04-04 11:09, ebb,-32 +2020-04-04 13:38, slack,- +2020-04-04 18:35, flood,45 +2020-04-04 22:02, slack,- +2020-04-04 23:39, ebb,-31 +2020-04-05 01:50, slack,- +2020-04-05 06:41, flood,48 +2020-04-05 10:20, slack,- +2020-04-05 12:03, ebb,-34 +2020-04-05 14:32, slack,- +2020-04-05 19:35, flood,49 +2020-04-05 22:56, slack,- +2020-04-06 00:33, ebb,-34 +2020-04-06 02:44, slack,- +2020-04-06 07:47, flood,51 +2020-04-06 11:14, slack,- +2020-04-06 12:57, ebb,-37 +2020-04-06 15:20, slack,- +2020-04-06 20:41, flood,52 +2020-04-06 23:44, slack,- +2020-04-07 01:27, ebb,-37 +2020-04-07 03:38, slack,- +2020-04-07 08:47, flood,54 +2020-04-07 12:08, slack,- +2020-04-07 13:51, ebb,-38 +2020-04-07 16:08, slack,- +2020-04-07 21:35, flood,55 +2020-04-08 00:38, slack,- +2020-04-08 02:21, ebb,-39 +2020-04-08 04:32, slack,- +2020-04-08 09:53, flood,56 +2020-04-08 13:02, slack,- +2020-04-08 14:39, ebb,-39 +2020-04-08 16:56, slack,- +2020-04-08 22:23, flood,56 +2020-04-09 01:26, slack,- +2020-04-09 03:09, ebb,-40 +2020-04-09 05:20, slack,- +2020-04-09 10:47, flood,56 +2020-04-09 13:50, slack,- +2020-04-09 15:33, ebb,-39 +2020-04-09 17:44, slack,- +2020-04-09 23:05, flood,55 +2020-04-10 02:14, slack,- +2020-04-10 03:57, ebb,-40 +2020-04-10 06:14, slack,- +2020-04-10 11:47, flood,53 +2020-04-10 14:44, slack,- +2020-04-10 16:21, ebb,-37 +2020-04-10 18:32, slack,- +2020-04-10 23:59, flood,51 +2020-04-11 03:08, slack,- +2020-04-11 04:45, ebb,-38 +2020-04-11 07:08, slack,- +2020-04-11 13:17, flood,50 +2020-04-11 15:38, slack,- +2020-04-11 17:09, ebb,-35 +2020-04-11 19:26, slack,- +2020-04-12 01:05, flood,46 +2020-04-12 03:56, slack,- +2020-04-12 05:33, ebb,-36 +2020-04-12 08:02, slack,- +2020-04-12 14:29, flood,48 +2020-04-12 16:32, slack,- +2020-04-12 18:03, ebb,-31 +2020-04-12 20:20, slack,- +2020-04-13 02:35, flood,42 +2020-04-13 04:50, slack,- +2020-04-13 06:27, ebb,-32 +2020-04-13 09:08, slack,- +2020-04-13 15:29, flood,46 +2020-04-13 17:26, slack,- +2020-04-13 18:51, ebb,-28 +2020-04-13 21:20, slack,- +2020-04-14 03:41, flood,39 +2020-04-14 05:44, slack,- +2020-04-14 07:15, ebb,-29 +2020-04-14 10:08, slack,- +2020-04-14 16:29, flood,44 +2020-04-14 18:26, slack,- +2020-04-14 19:51, ebb,-25 +2020-04-14 22:26, slack,- +2020-04-15 04:47, flood,37 +2020-04-15 06:44, slack,- +2020-04-15 08:09, ebb,-25 +2020-04-15 11:14, slack,- +2020-04-15 17:29, flood,43 +2020-04-15 19:20, slack,- +2020-04-15 20:51, ebb,-22 +2020-04-15 23:26, slack,- +2020-04-16 05:47, flood,37 +2020-04-16 07:38, slack,- +2020-04-16 09:15, ebb,-23 +2020-04-16 10:57, ebb,-22 +2020-04-16 12:08, slack,- +2020-04-16 18:29, flood,43 +2020-04-16 20:20, slack,- +2020-04-16 23:21, ebb,-22 +2020-04-17 00:26, slack,- +2020-04-17 06:47, flood,37 +2020-04-17 08:38, slack,- +2020-04-17 11:57, ebb,-24 +2020-04-17 13:02, slack,- +2020-04-17 19:23, flood,44 +2020-04-17 21:14, slack,- +2020-04-18 00:15, ebb,-24 +2020-04-18 01:14, slack,- +2020-04-18 07:41, flood,39 +2020-04-18 09:32, slack,- +2020-04-18 12:39, ebb,-25 +2020-04-18 13:50, slack,- +2020-04-18 20:17, flood,44 +2020-04-18 22:08, slack,- +2020-04-19 01:03, ebb,-25 +2020-04-19 02:02, slack,- +2020-04-19 08:35, flood,40 +2020-04-19 10:26, slack,- +2020-04-19 13:21, ebb,-25 +2020-04-19 14:32, slack,- +2020-04-19 21:05, flood,44 +2020-04-19 22:56, slack,- +2020-04-20 01:45, ebb,-26 +2020-04-20 02:50, slack,- +2020-04-20 09:23, flood,41 +2020-04-20 11:08, slack,- +2020-04-20 13:39, ebb,-26 +2020-04-20 15:14, slack,- +2020-04-20 21:47, flood,43 +2020-04-20 23:38, slack,- +2020-04-21 02:09, ebb,-27 +2020-04-21 03:32, slack,- +2020-04-21 09:59, flood,41 +2020-04-21 11:56, slack,- +2020-04-21 13:39, ebb,-27 +2020-04-21 15:56, slack,- +2020-04-21 22:29, flood,41 +2020-04-22 00:20, slack,- +2020-04-22 02:09, ebb,-28 +2020-04-22 04:14, slack,- +2020-04-22 10:23, flood,40 +2020-04-22 12:32, slack,- +2020-04-22 14:15, ebb,-28 +2020-04-22 16:32, slack,- +2020-04-22 21:29, flood,39 +2020-04-23 00:56, slack,- +2020-04-23 02:39, ebb,-29 +2020-04-23 04:56, slack,- +2020-04-23 09:53, flood,42 +2020-04-23 13:14, slack,- +2020-04-23 14:51, ebb,-29 +2020-04-23 17:08, slack,- +2020-04-23 21:53, flood,41 +2020-04-24 01:38, slack,- +2020-04-24 03:15, ebb,-30 +2020-04-24 05:38, slack,- +2020-04-24 10:23, flood,43 +2020-04-24 13:56, slack,- +2020-04-24 15:33, ebb,-30 +2020-04-24 17:44, slack,- +2020-04-24 22:29, flood,42 +2020-04-25 02:14, slack,- +2020-04-25 03:57, ebb,-31 +2020-04-25 06:14, slack,- +2020-04-25 11:05, flood,43 +2020-04-25 14:38, slack,- +2020-04-25 16:15, ebb,-31 +2020-04-25 18:20, slack,- +2020-04-25 23:05, flood,43 +2020-04-26 02:50, slack,- +2020-04-26 04:39, ebb,-32 +2020-04-26 06:56, slack,- +2020-04-26 11:47, flood,42 +2020-04-26 15:20, slack,- +2020-04-26 16:57, ebb,-31 +2020-04-26 18:56, slack,- +2020-04-26 23:53, flood,42 +2020-04-27 03:32, slack,- +2020-04-27 05:21, ebb,-33 +2020-04-27 07:38, slack,- +2020-04-27 12:41, flood,41 +2020-04-27 16:08, slack,- +2020-04-27 17:45, ebb,-31 +2020-04-27 19:44, slack,- +2020-04-28 00:41, flood,41 +2020-04-28 04:14, slack,- +2020-04-28 06:09, ebb,-33 +2020-04-28 08:32, slack,- +2020-04-28 13:35, flood,41 +2020-04-28 16:56, slack,- +2020-04-28 18:33, ebb,-30 +2020-04-28 20:32, slack,- +2020-04-29 01:35, flood,41 +2020-04-29 05:02, slack,- +2020-04-29 06:57, ebb,-32 +2020-04-29 09:26, slack,- +2020-04-29 14:29, flood,41 +2020-04-29 17:50, slack,- +2020-04-29 19:27, ebb,-30 +2020-04-29 21:32, slack,- +2020-04-30 02:35, flood,41 +2020-04-30 05:56, slack,- +2020-04-30 07:45, ebb,-32 +2020-04-30 10:26, slack,- +2020-04-30 15:23, flood,42 +2020-04-30 18:44, slack,- +2020-04-30 20:15, ebb,-29 +2020-04-30 22:38, slack,- +2020-05-01 03:29, flood,42 +2020-05-01 06:56, slack,- +2020-05-01 08:39, ebb,-31 +2020-05-01 11:20, slack,- +2020-05-01 16:17, flood,43 +2020-05-01 19:38, slack,- +2020-05-01 21:15, ebb,-29 +2020-05-01 23:38, slack,- +2020-05-02 04:29, flood,44 +2020-05-02 07:56, slack,- +2020-05-02 09:39, ebb,-31 +2020-05-02 12:20, slack,- +2020-05-02 17:17, flood,45 +2020-05-02 20:38, slack,- +2020-05-02 22:15, ebb,-30 +2020-05-03 00:38, slack,- +2020-05-03 05:29, flood,46 +2020-05-03 08:56, slack,- +2020-05-03 10:39, ebb,-32 +2020-05-03 13:08, slack,- +2020-05-03 18:11, flood,48 +2020-05-03 21:38, slack,- +2020-05-03 23:15, ebb,-32 +2020-05-04 01:32, slack,- +2020-05-04 06:29, flood,48 +2020-05-04 09:56, slack,- +2020-05-04 11:39, ebb,-34 +2020-05-04 14:02, slack,- +2020-05-04 19:11, flood,50 +2020-05-04 22:32, slack,- +2020-05-05 00:15, ebb,-35 +2020-05-05 02:26, slack,- +2020-05-05 07:35, flood,50 +2020-05-05 10:56, slack,- +2020-05-05 12:33, ebb,-35 +2020-05-05 14:56, slack,- +2020-05-05 20:17, flood,52 +2020-05-05 23:26, slack,- +2020-05-06 01:03, ebb,-37 +2020-05-06 03:20, slack,- +2020-05-06 08:53, flood,53 +2020-05-06 11:50, slack,- +2020-05-06 13:27, ebb,-36 +2020-05-06 15:44, slack,- +2020-05-06 21:29, flood,54 +2020-05-07 00:14, slack,- +2020-05-07 01:57, ebb,-38 +2020-05-07 04:14, slack,- +2020-05-07 10:11, flood,55 +2020-05-07 12:44, slack,- +2020-05-07 14:21, ebb,-36 +2020-05-07 16:32, slack,- +2020-05-07 22:17, flood,54 +2020-05-08 01:02, slack,- +2020-05-08 02:51, ebb,-39 +2020-05-08 05:08, slack,- +2020-05-08 11:11, flood,55 +2020-05-08 13:38, slack,- +2020-05-08 15:15, ebb,-36 +2020-05-08 17:20, slack,- +2020-05-08 23:05, flood,52 +2020-05-09 01:56, slack,- +2020-05-09 03:39, ebb,-38 +2020-05-09 06:02, slack,- +2020-05-09 12:17, flood,53 +2020-05-09 14:26, slack,- +2020-05-09 16:03, ebb,-34 +2020-05-09 18:14, slack,- +2020-05-09 23:59, flood,48 +2020-05-10 02:44, slack,- +2020-05-10 04:27, ebb,-37 +2020-05-10 06:56, slack,- +2020-05-10 13:17, flood,51 +2020-05-10 15:20, slack,- +2020-05-10 16:51, ebb,-32 +2020-05-10 19:08, slack,- +2020-05-11 01:29, flood,44 +2020-05-11 03:38, slack,- +2020-05-11 05:15, ebb,-34 +2020-05-11 07:50, slack,- +2020-05-11 14:17, flood,49 +2020-05-11 16:14, slack,- +2020-05-11 17:45, ebb,-29 +2020-05-11 20:02, slack,- +2020-05-12 02:29, flood,41 +2020-05-12 04:26, slack,- +2020-05-12 06:03, ebb,-31 +2020-05-12 08:44, slack,- +2020-05-12 15:17, flood,47 +2020-05-12 17:08, slack,- +2020-05-12 18:33, ebb,-27 +2020-05-12 21:02, slack,- +2020-05-13 03:29, flood,38 +2020-05-13 05:20, slack,- +2020-05-13 06:51, ebb,-28 +2020-05-13 09:44, slack,- +2020-05-13 16:11, flood,45 +2020-05-13 18:02, slack,- +2020-05-13 19:27, ebb,-24 +2020-05-13 22:02, slack,- +2020-05-14 04:23, flood,36 +2020-05-14 06:14, slack,- +2020-05-14 07:45, ebb,-25 +2020-05-14 10:44, slack,- +2020-05-14 17:05, flood,43 +2020-05-14 18:56, slack,- +2020-05-14 20:21, ebb,-22 +2020-05-14 23:02, slack,- +2020-05-15 05:17, flood,35 +2020-05-15 07:08, slack,- +2020-05-15 08:39, ebb,-23 +2020-05-15 11:38, slack,- +2020-05-15 17:59, flood,41 +2020-05-15 19:50, slack,- +2020-05-15 22:57, ebb,-21 +2020-05-15 23:56, slack,- +2020-05-16 06:17, flood,35 +2020-05-16 08:02, slack,- +2020-05-16 09:39, ebb,-21 +2020-05-16 11:21, ebb,-22 +2020-05-16 12:26, slack,- +2020-05-16 18:53, flood,40 +2020-05-16 20:44, slack,- +2020-05-16 23:51, ebb,-23 +2020-05-17 00:44, slack,- +2020-05-17 07:05, flood,35 +2020-05-17 08:56, slack,- +2020-05-17 12:09, ebb,-22 +2020-05-17 13:14, slack,- +2020-05-17 19:41, flood,39 +2020-05-17 21:32, slack,- +2020-05-18 00:33, ebb,-24 +2020-05-18 01:38, slack,- +2020-05-18 07:59, flood,35 +2020-05-18 09:50, slack,- +2020-05-18 12:45, ebb,-23 +2020-05-18 13:56, slack,- +2020-05-18 20:29, flood,38 +2020-05-18 22:20, slack,- +2020-05-19 01:15, ebb,-25 +2020-05-19 02:20, slack,- +2020-05-19 08:47, flood,36 +2020-05-19 10:38, slack,- +2020-05-19 12:21, ebb,-24 +2020-05-19 14:32, slack,- +2020-05-19 21:17, flood,37 +2020-05-19 23:02, slack,- +2020-05-20 01:03, ebb,-26 +2020-05-20 03:02, slack,- +2020-05-20 09:23, flood,36 +2020-05-20 11:20, slack,- +2020-05-20 12:57, ebb,-25 +2020-05-20 15:14, slack,- +2020-05-20 19:59, flood,36 +2020-05-20 23:44, slack,- +2020-05-21 01:27, ebb,-27 +2020-05-21 03:44, slack,- +2020-05-21 08:35, flood,38 +2020-05-21 12:02, slack,- +2020-05-21 13:33, ebb,-27 +2020-05-21 15:50, slack,- +2020-05-21 20:35, flood,39 +2020-05-22 00:20, slack,- +2020-05-22 02:03, ebb,-29 +2020-05-22 04:26, slack,- +2020-05-22 09:17, flood,41 +2020-05-22 12:44, slack,- +2020-05-22 14:21, ebb,-28 +2020-05-22 16:32, slack,- +2020-05-22 21:17, flood,42 +2020-05-23 00:56, slack,- +2020-05-23 02:45, ebb,-31 +2020-05-23 05:08, slack,- +2020-05-23 09:59, flood,43 +2020-05-23 13:26, slack,- +2020-05-23 15:03, ebb,-30 +2020-05-23 17:08, slack,- +2020-05-23 21:59, flood,44 +2020-05-24 01:38, slack,- +2020-05-24 03:27, ebb,-33 +2020-05-24 05:50, slack,- +2020-05-24 10:41, flood,44 +2020-05-24 14:14, slack,- +2020-05-24 15:45, ebb,-31 +2020-05-24 17:50, slack,- +2020-05-24 22:41, flood,45 +2020-05-25 02:14, slack,- +2020-05-25 04:09, ebb,-34 +2020-05-25 06:32, slack,- +2020-05-25 11:23, flood,45 +2020-05-25 14:56, slack,- +2020-05-25 16:33, ebb,-32 +2020-05-25 18:32, slack,- +2020-05-25 23:23, flood,45 +2020-05-26 03:02, slack,- +2020-05-26 04:57, ebb,-35 +2020-05-26 07:14, slack,- +2020-05-26 12:17, flood,45 +2020-05-26 15:44, slack,- +2020-05-26 17:21, ebb,-32 +2020-05-26 19:14, slack,- +2020-05-27 00:17, flood,44 +2020-05-27 03:44, slack,- +2020-05-27 05:39, ebb,-36 +2020-05-27 08:02, slack,- +2020-05-27 13:11, flood,45 +2020-05-27 16:32, slack,- +2020-05-27 18:09, ebb,-32 +2020-05-27 20:08, slack,- +2020-05-28 01:11, flood,44 +2020-05-28 04:38, slack,- +2020-05-28 06:33, ebb,-36 +2020-05-28 08:56, slack,- +2020-05-28 14:11, flood,45 +2020-05-28 17:26, slack,- +2020-05-28 19:03, ebb,-32 +2020-05-28 21:08, slack,- +2020-05-29 02:11, flood,45 +2020-05-29 05:32, slack,- +2020-05-29 07:21, ebb,-35 +2020-05-29 09:56, slack,- +2020-05-29 15:05, flood,46 +2020-05-29 18:20, slack,- +2020-05-29 19:51, ebb,-32 +2020-05-29 22:14, slack,- +2020-05-30 03:11, flood,45 +2020-05-30 06:32, slack,- +2020-05-30 08:15, ebb,-34 +2020-05-30 10:50, slack,- +2020-05-30 15:59, flood,47 +2020-05-30 19:14, slack,- +2020-05-30 20:51, ebb,-31 +2020-05-30 23:14, slack,- +2020-05-31 04:11, flood,46 +2020-05-31 07:32, slack,- +2020-05-31 09:15, ebb,-33 +2020-05-31 11:50, slack,- +2020-05-31 16:53, flood,48 +2020-05-31 20:14, slack,- +2020-05-31 21:51, ebb,-31 +2020-06-01 00:14, slack,- +2020-06-01 05:11, flood,46 +2020-06-01 08:32, slack,- +2020-06-01 10:15, ebb,-32 +2020-06-01 12:44, slack,- +2020-06-01 17:47, flood,49 +2020-06-01 21:08, slack,- +2020-06-01 22:51, ebb,-33 +2020-06-02 01:14, slack,- +2020-06-02 06:17, flood,47 +2020-06-02 09:38, slack,- +2020-06-02 11:15, ebb,-32 +2020-06-02 13:38, slack,- +2020-06-02 18:47, flood,50 +2020-06-02 22:08, slack,- +2020-06-02 23:51, ebb,-34 +2020-06-03 02:14, slack,- +2020-06-03 07:41, flood,48 +2020-06-03 10:38, slack,- +2020-06-03 12:15, ebb,-33 +2020-06-03 14:32, slack,- +2020-06-03 20:05, flood,50 +2020-06-03 23:02, slack,- +2020-06-04 00:45, ebb,-36 +2020-06-04 03:08, slack,- +2020-06-04 09:23, flood,51 +2020-06-04 11:32, slack,- +2020-06-04 13:09, ebb,-33 +2020-06-04 15:20, slack,- +2020-06-04 21:35, flood,51 +2020-06-04 23:56, slack,- +2020-06-05 01:39, ebb,-37 +2020-06-05 04:02, slack,- +2020-06-05 10:23, flood,54 +2020-06-05 12:26, slack,- +2020-06-05 14:03, ebb,-33 +2020-06-05 16:14, slack,- +2020-06-05 22:29, flood,51 +2020-06-06 00:44, slack,- +2020-06-06 02:33, ebb,-37 +2020-06-06 04:56, slack,- +2020-06-06 11:17, flood,55 +2020-06-06 13:20, slack,- +2020-06-06 14:57, ebb,-32 +2020-06-06 17:02, slack,- +2020-06-06 23:23, flood,50 +2020-06-07 01:32, slack,- +2020-06-07 03:21, ebb,-36 +2020-06-07 05:50, slack,- +2020-06-07 12:11, flood,55 +2020-06-07 14:14, slack,- +2020-06-07 15:45, ebb,-31 +2020-06-07 17:56, slack,- +2020-06-08 00:17, flood,47 +2020-06-08 02:26, slack,- +2020-06-08 04:09, ebb,-35 +2020-06-08 06:38, slack,- +2020-06-08 13:05, flood,53 +2020-06-08 15:02, slack,- +2020-06-08 16:39, ebb,-30 +2020-06-08 18:50, slack,- +2020-06-09 01:17, flood,44 +2020-06-09 03:14, slack,- +2020-06-09 04:57, ebb,-33 +2020-06-09 07:32, slack,- +2020-06-09 13:59, flood,50 +2020-06-09 15:56, slack,- +2020-06-09 17:27, ebb,-28 +2020-06-09 19:44, slack,- +2020-06-10 02:11, flood,41 +2020-06-10 04:08, slack,- +2020-06-10 05:39, ebb,-30 +2020-06-10 08:20, slack,- +2020-06-10 14:53, flood,48 +2020-06-10 16:44, slack,- +2020-06-10 18:15, ebb,-26 +2020-06-10 20:38, slack,- +2020-06-11 03:05, flood,38 +2020-06-11 04:56, slack,- +2020-06-11 06:27, ebb,-28 +2020-06-11 09:14, slack,- +2020-06-11 15:41, flood,44 +2020-06-11 17:32, slack,- +2020-06-11 19:03, ebb,-24 +2020-06-11 21:32, slack,- +2020-06-12 03:53, flood,36 +2020-06-12 05:44, slack,- +2020-06-12 07:15, ebb,-26 +2020-06-12 10:08, slack,- +2020-06-12 16:35, flood,41 +2020-06-12 18:26, slack,- +2020-06-12 19:51, ebb,-22 +2020-06-12 22:32, slack,- +2020-06-13 04:41, flood,33 +2020-06-13 06:38, slack,- +2020-06-13 08:03, ebb,-24 +2020-06-13 11:02, slack,- +2020-06-13 17:23, flood,38 +2020-06-13 19:14, slack,- +2020-06-13 20:45, ebb,-21 +2020-06-13 23:26, slack,- +2020-06-14 05:29, flood,32 +2020-06-14 07:26, slack,- +2020-06-14 08:57, ebb,-22 +2020-06-14 11:44, slack,- +2020-06-14 18:11, flood,35 +2020-06-14 20:02, slack,- +2020-06-14 21:39, ebb,-21 +2020-06-14 23:21, ebb,-21 +2020-06-15 00:14, slack,- +2020-06-15 04:59, flood,31 +2020-06-15 08:20, slack,- +2020-06-15 09:51, ebb,-21 +2020-06-15 12:32, slack,- +2020-06-15 18:59, flood,33 +2020-06-15 20:50, slack,- +2020-06-16 00:03, ebb,-22 +2020-06-16 01:02, slack,- +2020-06-16 05:41, flood,31 +2020-06-16 09:08, slack,- +2020-06-16 10:45, ebb,-21 +2020-06-16 13:14, slack,- +2020-06-16 17:47, flood,33 +2020-06-16 21:38, slack,- +2020-06-16 23:33, ebb,-23 +2020-06-17 01:50, slack,- +2020-06-17 06:23, flood,32 +2020-06-17 10:02, slack,- +2020-06-17 11:33, ebb,-23 +2020-06-17 13:56, slack,- +2020-06-17 18:29, flood,35 +2020-06-17 22:26, slack,- +2020-06-18 00:09, ebb,-25 +2020-06-18 02:32, slack,- +2020-06-18 07:11, flood,34 +2020-06-18 10:50, slack,- +2020-06-18 12:15, ebb,-24 +2020-06-18 14:32, slack,- +2020-06-18 19:11, flood,37 +2020-06-18 23:08, slack,- +2020-06-19 00:45, ebb,-27 +2020-06-19 03:20, slack,- +2020-06-19 07:59, flood,36 +2020-06-19 11:32, slack,- +2020-06-19 13:03, ebb,-26 +2020-06-19 15:14, slack,- +2020-06-19 19:53, flood,39 +2020-06-19 23:44, slack,- +2020-06-20 01:27, ebb,-29 +2020-06-20 03:56, slack,- +2020-06-20 08:47, flood,40 +2020-06-20 12:20, slack,- +2020-06-20 13:51, ebb,-28 +2020-06-20 15:56, slack,- +2020-06-20 20:41, flood,42 +2020-06-21 00:26, slack,- +2020-06-21 02:15, ebb,-32 +2020-06-21 04:38, slack,- +2020-06-21 09:35, flood,43 +2020-06-21 13:02, slack,- +2020-06-21 14:33, ebb,-30 +2020-06-21 16:38, slack,- +2020-06-21 21:29, flood,45 +2020-06-22 01:08, slack,- +2020-06-22 02:57, ebb,-34 +2020-06-22 05:20, slack,- +2020-06-22 10:17, flood,46 +2020-06-22 13:44, slack,- +2020-06-22 15:21, ebb,-32 +2020-06-22 17:20, slack,- +2020-06-22 22:17, flood,48 +2020-06-23 01:50, slack,- +2020-06-23 03:45, ebb,-36 +2020-06-23 06:02, slack,- +2020-06-23 11:05, flood,47 +2020-06-23 14:32, slack,- +2020-06-23 16:09, ebb,-33 +2020-06-23 18:08, slack,- +2020-06-23 23:05, flood,49 +2020-06-24 02:38, slack,- +2020-06-24 04:33, ebb,-38 +2020-06-24 06:50, slack,- +2020-06-24 11:53, flood,48 +2020-06-24 15:20, slack,- +2020-06-24 16:57, ebb,-34 +2020-06-24 18:56, slack,- +2020-06-24 23:53, flood,49 +2020-06-25 03:26, slack,- +2020-06-25 05:21, ebb,-38 +2020-06-25 07:38, slack,- +2020-06-25 12:47, flood,49 +2020-06-25 16:08, slack,- +2020-06-25 17:45, ebb,-35 +2020-06-25 19:44, slack,- +2020-06-26 00:53, flood,48 +2020-06-26 04:20, slack,- +2020-06-26 06:09, ebb,-38 +2020-06-26 08:26, slack,- +2020-06-26 13:47, flood,49 +2020-06-26 17:02, slack,- +2020-06-26 18:39, ebb,-35 +2020-06-26 20:44, slack,- +2020-06-27 01:53, flood,48 +2020-06-27 05:14, slack,- +2020-06-27 07:03, ebb,-37 +2020-06-27 09:26, slack,- +2020-06-27 14:41, flood,50 +2020-06-27 17:56, slack,- +2020-06-27 19:33, ebb,-34 +2020-06-27 21:50, slack,- +2020-06-28 02:53, flood,47 +2020-06-28 06:08, slack,- +2020-06-28 07:51, ebb,-35 +2020-06-28 10:26, slack,- +2020-06-28 15:35, flood,50 +2020-06-28 18:50, slack,- +2020-06-28 20:27, ebb,-33 +2020-06-28 22:56, slack,- +2020-06-29 03:53, flood,46 +2020-06-29 07:08, slack,- +2020-06-29 08:51, ebb,-33 +2020-06-29 11:20, slack,- +2020-06-29 16:29, flood,49 +2020-06-29 19:50, slack,- +2020-06-29 21:21, ebb,-32 +2020-06-29 23:56, slack,- +2020-06-30 04:53, flood,45 +2020-06-30 08:14, slack,- +2020-06-30 09:51, ebb,-31 +2020-06-30 12:14, slack,- +2020-06-30 17:23, flood,48 +2020-06-30 20:44, slack,- +2020-06-30 22:27, ebb,-32 +2020-07-01 00:56, slack,- +2020-07-01 06:11, flood,44 +2020-07-01 09:14, slack,- +2020-07-01 10:51, ebb,-30 +2020-07-01 13:14, slack,- +2020-07-01 18:29, flood,47 +2020-07-01 21:44, slack,- +2020-07-01 23:27, ebb,-33 +2020-07-02 01:56, slack,- +2020-07-02 08:11, flood,47 +2020-07-02 10:20, slack,- +2020-07-02 11:57, ebb,-30 +2020-07-02 14:08, slack,- +2020-07-02 20:29, flood,47 +2020-07-02 22:38, slack,- +2020-07-03 00:27, ebb,-34 +2020-07-03 02:56, slack,- +2020-07-03 09:17, flood,51 +2020-07-03 11:14, slack,- +2020-07-03 12:51, ebb,-30 +2020-07-03 15:02, slack,- +2020-07-03 21:35, flood,49 +2020-07-03 23:32, slack,- +2020-07-04 01:27, ebb,-34 +2020-07-04 03:50, slack,- +2020-07-04 10:11, flood,55 +2020-07-04 12:08, slack,- +2020-07-04 13:51, ebb,-30 +2020-07-04 15:56, slack,- +2020-07-04 22:29, flood,50 +2020-07-05 00:26, slack,- +2020-07-05 02:15, ebb,-34 +2020-07-05 04:44, slack,- +2020-07-05 11:05, flood,56 +2020-07-05 13:02, slack,- +2020-07-05 14:45, ebb,-30 +2020-07-05 16:50, slack,- +2020-07-05 23:17, flood,49 +2020-07-06 01:14, slack,- +2020-07-06 03:03, ebb,-34 +2020-07-06 05:32, slack,- +2020-07-06 11:53, flood,56 +2020-07-06 13:50, slack,- +2020-07-06 15:33, ebb,-30 +2020-07-06 17:38, slack,- +2020-07-07 00:05, flood,47 +2020-07-07 02:02, slack,- +2020-07-07 03:51, ebb,-33 +2020-07-07 06:20, slack,- +2020-07-07 12:47, flood,53 +2020-07-07 14:44, slack,- +2020-07-07 16:21, ebb,-29 +2020-07-07 18:26, slack,- +2020-07-08 00:59, flood,44 +2020-07-08 02:50, slack,- +2020-07-08 04:33, ebb,-32 +2020-07-08 07:08, slack,- +2020-07-08 13:35, flood,50 +2020-07-08 15:32, slack,- +2020-07-08 17:03, ebb,-28 +2020-07-08 19:20, slack,- +2020-07-09 01:47, flood,41 +2020-07-09 03:38, slack,- +2020-07-09 05:15, ebb,-30 +2020-07-09 07:56, slack,- +2020-07-09 14:23, flood,47 +2020-07-09 16:20, slack,- +2020-07-09 17:51, ebb,-27 +2020-07-09 20:08, slack,- +2020-07-10 02:29, flood,38 +2020-07-10 04:26, slack,- +2020-07-10 06:03, ebb,-29 +2020-07-10 08:38, slack,- +2020-07-10 15:11, flood,43 +2020-07-10 17:02, slack,- +2020-07-10 18:33, ebb,-25 +2020-07-10 21:02, slack,- +2020-07-11 03:11, flood,35 +2020-07-11 05:14, slack,- +2020-07-11 06:45, ebb,-27 +2020-07-11 09:26, slack,- +2020-07-11 15:53, flood,38 +2020-07-11 17:50, slack,- +2020-07-11 19:15, ebb,-24 +2020-07-11 21:56, slack,- +2020-07-12 02:47, flood,33 +2020-07-12 06:02, slack,- +2020-07-12 07:27, ebb,-25 +2020-07-12 10:14, slack,- +2020-07-12 16:35, flood,34 +2020-07-12 18:38, slack,- +2020-07-12 20:03, ebb,-22 +2020-07-12 22:50, slack,- +2020-07-13 03:29, flood,32 +2020-07-13 06:50, slack,- +2020-07-13 08:15, ebb,-23 +2020-07-13 11:02, slack,- +2020-07-13 15:47, flood,33 +2020-07-13 19:26, slack,- +2020-07-13 20:51, ebb,-21 +2020-07-13 23:38, slack,- +2020-07-14 04:11, flood,31 +2020-07-14 07:38, slack,- +2020-07-14 09:03, ebb,-22 +2020-07-14 11:44, slack,- +2020-07-14 16:23, flood,33 +2020-07-14 20:08, slack,- +2020-07-14 21:39, ebb,-21 +2020-07-15 00:26, slack,- +2020-07-15 04:53, flood,30 +2020-07-15 08:32, slack,- +2020-07-15 09:57, ebb,-21 +2020-07-15 12:32, slack,- +2020-07-15 17:05, flood,34 +2020-07-15 20:56, slack,- +2020-07-15 22:33, ebb,-22 +2020-07-16 01:14, slack,- +2020-07-16 05:47, flood,31 +2020-07-16 09:26, slack,- +2020-07-16 10:51, ebb,-22 +2020-07-16 13:14, slack,- +2020-07-16 17:47, flood,35 +2020-07-16 21:44, slack,- +2020-07-16 23:21, ebb,-24 +2020-07-17 02:02, slack,- +2020-07-17 06:35, flood,33 +2020-07-17 10:14, slack,- +2020-07-17 11:45, ebb,-23 +2020-07-17 14:02, slack,- +2020-07-17 18:35, flood,37 +2020-07-17 22:32, slack,- +2020-07-18 00:09, ebb,-27 +2020-07-18 02:44, slack,- +2020-07-18 07:23, flood,36 +2020-07-18 11:02, slack,- +2020-07-18 12:33, ebb,-26 +2020-07-18 14:44, slack,- +2020-07-18 19:23, flood,40 +2020-07-18 23:14, slack,- +2020-07-19 00:57, ebb,-30 +2020-07-19 03:32, slack,- +2020-07-19 08:17, flood,40 +2020-07-19 11:50, slack,- +2020-07-19 13:21, ebb,-28 +2020-07-19 15:26, slack,- +2020-07-19 20:17, flood,43 +2020-07-19 23:56, slack,- +2020-07-20 01:45, ebb,-33 +2020-07-20 04:14, slack,- +2020-07-20 09:11, flood,44 +2020-07-20 12:38, slack,- +2020-07-20 14:09, ebb,-31 +2020-07-20 16:14, slack,- +2020-07-20 21:05, flood,47 +2020-07-21 00:44, slack,- +2020-07-21 02:33, ebb,-36 +2020-07-21 04:56, slack,- +2020-07-21 09:59, flood,48 +2020-07-21 13:20, slack,- +2020-07-21 14:57, ebb,-33 +2020-07-21 16:56, slack,- +2020-07-21 21:53, flood,51 +2020-07-22 01:26, slack,- +2020-07-22 03:21, ebb,-38 +2020-07-22 05:38, slack,- +2020-07-22 10:47, flood,51 +2020-07-22 14:08, slack,- +2020-07-22 15:45, ebb,-35 +2020-07-22 17:44, slack,- +2020-07-22 22:47, flood,53 +2020-07-23 02:14, slack,- +2020-07-23 04:09, ebb,-40 +2020-07-23 06:26, slack,- +2020-07-23 11:35, flood,52 +2020-07-23 14:56, slack,- +2020-07-23 16:33, ebb,-37 +2020-07-23 18:32, slack,- +2020-07-23 23:35, flood,53 +2020-07-24 03:08, slack,- +2020-07-24 04:57, ebb,-40 +2020-07-24 07:14, slack,- +2020-07-24 12:23, flood,52 +2020-07-24 15:50, slack,- +2020-07-24 17:27, ebb,-37 +2020-07-24 19:26, slack,- +2020-07-25 00:35, flood,52 +2020-07-25 04:02, slack,- +2020-07-25 05:45, ebb,-40 +2020-07-25 08:02, slack,- +2020-07-25 13:23, flood,52 +2020-07-25 16:38, slack,- +2020-07-25 18:15, ebb,-37 +2020-07-25 20:26, slack,- +2020-07-26 01:35, flood,50 +2020-07-26 04:56, slack,- +2020-07-26 06:39, ebb,-38 +2020-07-26 08:56, slack,- +2020-07-26 14:17, flood,51 +2020-07-26 17:32, slack,- +2020-07-26 19:09, ebb,-36 +2020-07-26 21:26, slack,- +2020-07-27 02:35, flood,48 +2020-07-27 05:50, slack,- +2020-07-27 07:33, ebb,-36 +2020-07-27 09:56, slack,- +2020-07-27 15:11, flood,50 +2020-07-27 18:26, slack,- +2020-07-27 20:03, ebb,-34 +2020-07-27 22:32, slack,- +2020-07-28 03:41, flood,45 +2020-07-28 06:50, slack,- +2020-07-28 08:27, ebb,-32 +2020-07-28 10:56, slack,- +2020-07-28 16:05, flood,48 +2020-07-28 19:20, slack,- +2020-07-28 20:57, ebb,-32 +2020-07-28 23:38, slack,- +2020-07-29 04:53, flood,43 +2020-07-29 07:56, slack,- +2020-07-29 09:27, ebb,-29 +2020-07-29 11:56, slack,- +2020-07-29 17:05, flood,45 +2020-07-29 20:20, slack,- +2020-07-29 22:03, ebb,-30 +2020-07-30 00:44, slack,- +2020-07-30 06:47, flood,43 +2020-07-30 08:56, slack,- +2020-07-30 10:33, ebb,-27 +2020-07-30 12:50, slack,- +2020-07-30 18:59, flood,44 +2020-07-30 21:20, slack,- +2020-07-30 23:09, ebb,-30 +2020-07-31 01:44, slack,- +2020-07-31 08:05, flood,47 +2020-07-31 10:02, slack,- +2020-07-31 11:39, ebb,-27 +2020-07-31 13:50, slack,- +2020-07-31 20:23, flood,45 +2020-07-31 22:20, slack,- +2020-08-01 00:21, ebb,-31 +2020-08-01 02:38, slack,- +2020-08-01 09:05, flood,51 +2020-08-01 10:56, slack,- +2020-08-01 12:45, ebb,-28 +2020-08-01 14:44, slack,- +2020-08-01 21:23, flood,48 +2020-08-01 23:14, slack,- +2020-08-02 01:21, ebb,-31 +2020-08-02 03:32, slack,- +2020-08-02 09:59, flood,55 +2020-08-02 11:50, slack,- +2020-08-02 13:51, ebb,-28 +2020-08-02 15:38, slack,- +2020-08-02 22:11, flood,50 +2020-08-03 00:08, slack,- +2020-08-03 02:15, ebb,-32 +2020-08-03 04:26, slack,- +2020-08-03 10:47, flood,56 +2020-08-03 12:44, slack,- +2020-08-03 14:39, ebb,-29 +2020-08-03 16:32, slack,- +2020-08-03 22:59, flood,50 +2020-08-04 00:56, slack,- +2020-08-04 02:57, ebb,-32 +2020-08-04 05:14, slack,- +2020-08-04 11:35, flood,55 +2020-08-04 13:32, slack,- +2020-08-04 15:27, ebb,-29 +2020-08-04 17:20, slack,- +2020-08-04 23:47, flood,48 +2020-08-05 01:44, slack,- +2020-08-05 03:33, ebb,-32 +2020-08-05 05:56, slack,- +2020-08-05 12:23, flood,52 +2020-08-05 14:20, slack,- +2020-08-05 16:03, ebb,-29 +2020-08-05 18:02, slack,- +2020-08-06 00:29, flood,45 +2020-08-06 02:26, slack,- +2020-08-06 04:15, ebb,-31 +2020-08-06 06:38, slack,- +2020-08-06 13:11, flood,49 +2020-08-06 15:02, slack,- +2020-08-06 16:39, ebb,-28 +2020-08-06 18:50, slack,- +2020-08-07 01:11, flood,42 +2020-08-07 03:14, slack,- +2020-08-07 04:51, ebb,-30 +2020-08-07 07:20, slack,- +2020-08-07 13:53, flood,44 +2020-08-07 15:44, slack,- +2020-08-07 17:21, ebb,-27 +2020-08-07 19:38, slack,- +2020-08-08 01:47, flood,38 +2020-08-08 03:56, slack,- +2020-08-08 05:33, ebb,-29 +2020-08-08 08:02, slack,- +2020-08-08 14:35, flood,39 +2020-08-08 16:32, slack,- +2020-08-08 17:57, ebb,-26 +2020-08-08 20:26, slack,- +2020-08-09 01:23, flood,36 +2020-08-09 04:38, slack,- +2020-08-09 06:15, ebb,-28 +2020-08-09 08:44, slack,- +2020-08-09 13:53, flood,35 +2020-08-09 17:14, slack,- +2020-08-09 18:39, ebb,-25 +2020-08-09 21:14, slack,- +2020-08-10 02:05, flood,34 +2020-08-10 05:26, slack,- +2020-08-10 06:57, ebb,-26 +2020-08-10 09:32, slack,- +2020-08-10 14:23, flood,34 +2020-08-10 17:56, slack,- +2020-08-10 19:21, ebb,-24 +2020-08-10 22:08, slack,- +2020-08-11 02:47, flood,32 +2020-08-11 06:14, slack,- +2020-08-11 07:39, ebb,-25 +2020-08-11 10:14, slack,- +2020-08-11 14:59, flood,34 +2020-08-11 18:38, slack,- +2020-08-11 20:09, ebb,-23 +2020-08-11 23:02, slack,- +2020-08-12 03:35, flood,31 +2020-08-12 07:02, slack,- +2020-08-12 08:27, ebb,-23 +2020-08-12 11:02, slack,- +2020-08-12 15:41, flood,34 +2020-08-12 19:26, slack,- +2020-08-12 20:57, ebb,-23 +2020-08-12 23:50, slack,- +2020-08-13 04:23, flood,31 +2020-08-13 07:50, slack,- +2020-08-13 09:21, ebb,-22 +2020-08-13 11:50, slack,- +2020-08-13 16:23, flood,34 +2020-08-13 20:14, slack,- +2020-08-13 21:51, ebb,-23 +2020-08-14 00:44, slack,- +2020-08-14 05:11, flood,32 +2020-08-14 08:44, slack,- +2020-08-14 10:15, ebb,-22 +2020-08-14 12:38, slack,- +2020-08-14 17:17, flood,36 +2020-08-14 21:02, slack,- +2020-08-14 22:45, ebb,-25 +2020-08-15 01:26, slack,- +2020-08-15 06:05, flood,34 +2020-08-15 09:44, slack,- +2020-08-15 11:09, ebb,-24 +2020-08-15 13:26, slack,- +2020-08-15 18:05, flood,38 +2020-08-15 21:56, slack,- +2020-08-15 23:39, ebb,-27 +2020-08-16 02:14, slack,- +2020-08-16 06:59, flood,37 +2020-08-16 10:32, slack,- +2020-08-16 12:03, ebb,-26 +2020-08-16 14:14, slack,- +2020-08-16 18:59, flood,41 +2020-08-16 22:44, slack,- +2020-08-17 00:27, ebb,-30 +2020-08-17 03:02, slack,- +2020-08-17 07:53, flood,41 +2020-08-17 11:20, slack,- +2020-08-17 12:57, ebb,-29 +2020-08-17 15:02, slack,- +2020-08-17 19:53, flood,45 +2020-08-17 23:32, slack,- +2020-08-18 01:15, ebb,-34 +2020-08-18 03:44, slack,- +2020-08-18 08:47, flood,46 +2020-08-18 12:08, slack,- +2020-08-18 13:45, ebb,-32 +2020-08-18 15:50, slack,- +2020-08-18 20:47, flood,50 +2020-08-19 00:20, slack,- +2020-08-19 02:09, ebb,-37 +2020-08-19 04:32, slack,- +2020-08-19 09:41, flood,50 +2020-08-19 12:56, slack,- +2020-08-19 14:33, ebb,-35 +2020-08-19 16:38, slack,- +2020-08-19 21:41, flood,54 +2020-08-20 01:08, slack,- +2020-08-20 02:57, ebb,-39 +2020-08-20 05:14, slack,- +2020-08-20 10:23, flood,54 +2020-08-20 13:44, slack,- +2020-08-20 15:27, ebb,-37 +2020-08-20 17:26, slack,- +2020-08-20 22:29, flood,56 +2020-08-21 01:56, slack,- +2020-08-21 03:45, ebb,-41 +2020-08-21 06:02, slack,- +2020-08-21 11:11, flood,55 +2020-08-21 14:32, slack,- +2020-08-21 16:15, ebb,-39 +2020-08-21 18:14, slack,- +2020-08-21 23:23, flood,56 +2020-08-22 02:50, slack,- +2020-08-22 04:33, ebb,-41 +2020-08-22 06:50, slack,- +2020-08-22 11:59, flood,55 +2020-08-22 15:26, slack,- +2020-08-22 17:03, ebb,-39 +2020-08-22 19:08, slack,- +2020-08-23 00:17, flood,53 +2020-08-23 03:44, slack,- +2020-08-23 05:27, ebb,-40 +2020-08-23 07:38, slack,- +2020-08-23 12:59, flood,53 +2020-08-23 16:14, slack,- +2020-08-23 17:51, ebb,-38 +2020-08-23 20:08, slack,- +2020-08-24 01:23, flood,50 +2020-08-24 04:38, slack,- +2020-08-24 06:15, ebb,-38 +2020-08-24 08:32, slack,- +2020-08-24 13:53, flood,51 +2020-08-24 17:08, slack,- +2020-08-24 18:45, ebb,-37 +2020-08-24 21:08, slack,- +2020-08-25 02:29, flood,47 +2020-08-25 05:32, slack,- +2020-08-25 07:09, ebb,-34 +2020-08-25 09:32, slack,- +2020-08-25 14:47, flood,48 +2020-08-25 18:02, slack,- +2020-08-25 19:39, ebb,-34 +2020-08-25 22:14, slack,- +2020-08-26 03:35, flood,44 +2020-08-26 06:32, slack,- +2020-08-26 08:03, ebb,-31 +2020-08-26 10:32, slack,- +2020-08-26 15:47, flood,44 +2020-08-26 19:02, slack,- +2020-08-26 20:33, ebb,-31 +2020-08-26 23:20, slack,- +2020-08-27 05:23, flood,42 +2020-08-27 07:38, slack,- +2020-08-27 09:03, ebb,-27 +2020-08-27 11:32, slack,- +2020-08-27 16:59, flood,41 +2020-08-27 19:56, slack,- +2020-08-27 21:33, ebb,-28 +2020-08-28 00:26, slack,- +2020-08-28 06:41, flood,44 +2020-08-28 08:38, slack,- +2020-08-28 10:09, ebb,-25 +2020-08-28 12:38, slack,- +2020-08-28 18:59, flood,41 +2020-08-28 21:02, slack,- +2020-08-28 22:51, ebb,-27 +2020-08-29 01:26, slack,- +2020-08-29 07:47, flood,47 +2020-08-29 09:38, slack,- +2020-08-29 11:51, ebb,-25 +2020-08-29 13:32, slack,- +2020-08-29 20:05, flood,44 +2020-08-29 21:56, slack,- +2020-08-30 00:45, ebb,-28 +2020-08-30 02:20, slack,- +2020-08-30 08:41, flood,51 +2020-08-30 10:38, slack,- +2020-08-30 13:09, ebb,-26 +2020-08-30 14:32, slack,- +2020-08-30 21:05, flood,47 +2020-08-30 22:56, slack,- +2020-08-31 01:39, ebb,-29 +2020-08-31 03:14, slack,- +2020-08-31 09:35, flood,54 +2020-08-31 11:32, slack,- +2020-08-31 13:57, ebb,-28 +2020-08-31 15:20, slack,- +2020-08-31 21:53, flood,49 +2020-08-31 23:44, slack,- +2020-09-01 02:27, ebb,-30 +2020-09-01 04:02, slack,- +2020-09-01 10:23, flood,55 +2020-09-01 12:20, slack,- +2020-09-01 14:45, ebb,-29 +2020-09-01 16:08, slack,- +2020-09-01 22:35, flood,50 +2020-09-02 00:32, slack,- +2020-09-02 02:51, ebb,-30 +2020-09-02 04:44, slack,- +2020-09-02 11:11, flood,53 +2020-09-02 13:02, slack,- +2020-09-02 15:15, ebb,-29 +2020-09-02 16:56, slack,- +2020-09-02 23:17, flood,48 +2020-09-03 01:20, slack,- +2020-09-03 03:15, ebb,-31 +2020-09-03 05:26, slack,- +2020-09-03 11:53, flood,50 +2020-09-03 13:50, slack,- +2020-09-03 15:39, ebb,-29 +2020-09-03 17:38, slack,- +2020-09-03 23:59, flood,45 +2020-09-04 02:02, slack,- +2020-09-04 03:45, ebb,-31 +2020-09-04 06:08, slack,- +2020-09-04 12:35, flood,45 +2020-09-04 14:32, slack,- +2020-09-04 16:09, ebb,-29 +2020-09-04 18:20, slack,- +2020-09-04 23:35, flood,41 +2020-09-05 02:44, slack,- +2020-09-05 04:21, ebb,-30 +2020-09-05 06:44, slack,- +2020-09-05 13:11, flood,40 +2020-09-05 15:14, slack,- +2020-09-05 16:45, ebb,-29 +2020-09-05 19:08, slack,- +2020-09-05 23:59, flood,39 +2020-09-06 03:26, slack,- +2020-09-06 04:57, ebb,-30 +2020-09-06 07:20, slack,- +2020-09-06 12:11, flood,37 +2020-09-06 15:56, slack,- +2020-09-06 17:27, ebb,-28 +2020-09-06 19:50, slack,- +2020-09-07 00:41, flood,37 +2020-09-07 04:08, slack,- +2020-09-07 05:39, ebb,-29 +2020-09-07 08:02, slack,- +2020-09-07 12:53, flood,36 +2020-09-07 16:32, slack,- +2020-09-07 18:03, ebb,-27 +2020-09-07 20:38, slack,- +2020-09-08 01:23, flood,35 +2020-09-08 04:50, slack,- +2020-09-08 06:21, ebb,-27 +2020-09-08 08:44, slack,- +2020-09-08 13:35, flood,35 +2020-09-08 17:14, slack,- +2020-09-08 18:51, ebb,-26 +2020-09-08 21:26, slack,- +2020-09-09 02:11, flood,34 +2020-09-09 05:38, slack,- +2020-09-09 07:09, ebb,-26 +2020-09-09 09:32, slack,- +2020-09-09 14:17, flood,35 +2020-09-09 17:56, slack,- +2020-09-09 19:33, ebb,-25 +2020-09-09 22:20, slack,- +2020-09-10 02:59, flood,33 +2020-09-10 06:26, slack,- +2020-09-10 07:57, ebb,-24 +2020-09-10 10:26, slack,- +2020-09-10 15:05, flood,35 +2020-09-10 18:38, slack,- +2020-09-10 20:21, ebb,-25 +2020-09-10 23:14, slack,- +2020-09-11 03:47, flood,33 +2020-09-11 07:20, slack,- +2020-09-11 08:45, ebb,-23 +2020-09-11 11:14, slack,- +2020-09-11 15:53, flood,35 +2020-09-11 19:32, slack,- +2020-09-11 21:15, ebb,-25 +2020-09-12 00:08, slack,- +2020-09-12 04:41, flood,34 +2020-09-12 08:14, slack,- +2020-09-12 09:45, ebb,-23 +2020-09-12 12:08, slack,- +2020-09-12 16:47, flood,37 +2020-09-12 20:26, slack,- +2020-09-12 22:09, ebb,-26 +2020-09-13 00:56, slack,- +2020-09-13 05:35, flood,36 +2020-09-13 09:08, slack,- +2020-09-13 10:39, ebb,-25 +2020-09-13 13:02, slack,- +2020-09-13 17:41, flood,40 +2020-09-13 21:20, slack,- +2020-09-13 23:09, ebb,-28 +2020-09-14 01:44, slack,- +2020-09-14 06:29, flood,39 +2020-09-14 10:02, slack,- +2020-09-14 11:39, ebb,-27 +2020-09-14 13:50, slack,- +2020-09-14 18:35, flood,43 +2020-09-14 22:14, slack,- +2020-09-15 00:03, ebb,-31 +2020-09-15 02:32, slack,- +2020-09-15 07:23, flood,43 +2020-09-15 10:56, slack,- +2020-09-15 12:27, ebb,-31 +2020-09-15 14:38, slack,- +2020-09-15 19:29, flood,48 +2020-09-15 23:08, slack,- +2020-09-16 00:51, ebb,-35 +2020-09-16 03:20, slack,- +2020-09-16 08:23, flood,48 +2020-09-16 11:44, slack,- +2020-09-16 13:21, ebb,-34 +2020-09-16 15:32, slack,- +2020-09-16 20:29, flood,52 +2020-09-17 00:02, slack,- +2020-09-17 01:45, ebb,-38 +2020-09-17 04:08, slack,- +2020-09-17 09:17, flood,53 +2020-09-17 12:32, slack,- +2020-09-17 14:15, ebb,-37 +2020-09-17 16:20, slack,- +2020-09-17 21:23, flood,56 +2020-09-18 00:50, slack,- +2020-09-18 02:33, ebb,-40 +2020-09-18 04:50, slack,- +2020-09-18 10:05, flood,56 +2020-09-18 13:20, slack,- +2020-09-18 15:03, ebb,-39 +2020-09-18 17:08, slack,- +2020-09-18 22:17, flood,57 +2020-09-19 01:38, slack,- +2020-09-19 03:27, ebb,-41 +2020-09-19 05:38, slack,- +2020-09-19 10:47, flood,57 +2020-09-19 14:08, slack,- +2020-09-19 15:51, ebb,-40 +2020-09-19 18:02, slack,- +2020-09-19 23:11, flood,56 +2020-09-20 02:32, slack,- +2020-09-20 04:15, ebb,-41 +2020-09-20 06:26, slack,- +2020-09-20 11:35, flood,55 +2020-09-20 15:02, slack,- +2020-09-20 16:39, ebb,-40 +2020-09-20 18:56, slack,- +2020-09-21 00:05, flood,53 +2020-09-21 03:26, slack,- +2020-09-21 05:03, ebb,-39 +2020-09-21 07:14, slack,- +2020-09-21 12:29, flood,52 +2020-09-21 15:50, slack,- +2020-09-21 17:33, ebb,-39 +2020-09-21 19:50, slack,- +2020-09-22 01:17, flood,49 +2020-09-22 04:20, slack,- +2020-09-22 05:57, ebb,-36 +2020-09-22 08:08, slack,- +2020-09-22 13:29, flood,48 +2020-09-22 16:44, slack,- +2020-09-22 18:21, ebb,-36 +2020-09-22 20:50, slack,- +2020-09-23 02:41, flood,46 +2020-09-23 05:20, slack,- +2020-09-23 06:51, ebb,-32 +2020-09-23 09:08, slack,- +2020-09-23 14:35, flood,44 +2020-09-23 17:38, slack,- +2020-09-23 19:15, ebb,-33 +2020-09-23 21:56, slack,- +2020-09-24 04:05, flood,44 +2020-09-24 06:14, slack,- +2020-09-24 07:39, ebb,-29 +2020-09-24 10:14, slack,- +2020-09-24 15:47, flood,40 +2020-09-24 18:38, slack,- +2020-09-24 20:09, ebb,-29 +2020-09-24 23:02, slack,- +2020-09-25 05:17, flood,44 +2020-09-25 07:14, slack,- +2020-09-25 08:39, ebb,-25 +2020-09-25 11:20, slack,- +2020-09-25 17:35, flood,39 +2020-09-25 19:38, slack,- +2020-09-25 21:09, ebb,-26 +2020-09-26 00:08, slack,- +2020-09-26 06:23, flood,45 +2020-09-26 08:20, slack,- +2020-09-26 09:51, ebb,-23 +2020-09-26 12:20, slack,- +2020-09-26 18:41, flood,40 +2020-09-26 20:38, slack,- +2020-09-26 23:45, ebb,-25 +2020-09-27 01:02, slack,- +2020-09-27 07:23, flood,47 +2020-09-27 09:14, slack,- +2020-09-27 12:09, ebb,-24 +2020-09-27 13:14, slack,- +2020-09-27 19:41, flood,42 +2020-09-27 21:38, slack,- +2020-09-28 00:39, ebb,-27 +2020-09-28 01:56, slack,- +2020-09-28 08:17, flood,49 +2020-09-28 10:14, slack,- +2020-09-28 12:57, ebb,-26 +2020-09-28 14:08, slack,- +2020-09-28 20:41, flood,45 +2020-09-28 22:32, slack,- +2020-09-29 01:27, ebb,-28 +2020-09-29 02:50, slack,- +2020-09-29 09:11, flood,51 +2020-09-29 11:02, slack,- +2020-09-29 13:45, ebb,-28 +2020-09-29 15:02, slack,- +2020-09-29 21:29, flood,47 +2020-09-29 23:20, slack,- +2020-09-30 02:03, ebb,-29 +2020-09-30 03:32, slack,- +2020-09-30 09:59, flood,51 +2020-09-30 11:50, slack,- +2020-09-30 14:27, ebb,-28 +2020-09-30 15:44, slack,- +2020-09-30 22:11, flood,48 +2020-10-01 00:08, slack,- +2020-10-01 02:33, ebb,-29 +2020-10-01 04:14, slack,- +2020-10-01 10:41, flood,49 +2020-10-01 12:32, slack,- +2020-10-01 14:57, ebb,-29 +2020-10-01 16:26, slack,- +2020-10-01 22:53, flood,46 +2020-10-02 00:50, slack,- +2020-10-02 02:39, ebb,-29 +2020-10-02 04:50, slack,- +2020-10-02 11:23, flood,46 +2020-10-02 13:14, slack,- +2020-10-02 15:09, ebb,-29 +2020-10-02 17:14, slack,- +2020-10-02 23:23, flood,44 +2020-10-03 01:32, slack,- +2020-10-03 03:15, ebb,-29 +2020-10-03 05:32, slack,- +2020-10-03 11:53, flood,41 +2020-10-03 13:56, slack,- +2020-10-03 15:39, ebb,-29 +2020-10-03 17:56, slack,- +2020-10-03 22:53, flood,42 +2020-10-04 02:14, slack,- +2020-10-04 03:51, ebb,-30 +2020-10-04 06:08, slack,- +2020-10-04 10:53, flood,39 +2020-10-04 14:38, slack,- +2020-10-04 16:15, ebb,-30 +2020-10-04 18:32, slack,- +2020-10-04 23:23, flood,40 +2020-10-05 02:56, slack,- +2020-10-05 04:27, ebb,-29 +2020-10-05 06:44, slack,- +2020-10-05 11:29, flood,39 +2020-10-05 15:14, slack,- +2020-10-05 16:51, ebb,-29 +2020-10-05 19:20, slack,- +2020-10-06 00:05, flood,39 +2020-10-06 03:38, slack,- +2020-10-06 05:09, ebb,-29 +2020-10-06 07:20, slack,- +2020-10-06 12:05, flood,38 +2020-10-06 15:56, slack,- +2020-10-06 17:33, ebb,-29 +2020-10-06 20:02, slack,- +2020-10-07 00:53, flood,37 +2020-10-07 04:20, slack,- +2020-10-07 05:51, ebb,-28 +2020-10-07 08:02, slack,- +2020-10-07 12:53, flood,37 +2020-10-07 16:32, slack,- +2020-10-07 18:15, ebb,-29 +2020-10-07 20:50, slack,- +2020-10-08 01:41, flood,36 +2020-10-08 05:08, slack,- +2020-10-08 06:39, ebb,-27 +2020-10-08 08:50, slack,- +2020-10-08 13:47, flood,36 +2020-10-08 17:14, slack,- +2020-10-08 19:03, ebb,-28 +2020-10-08 21:44, slack,- +2020-10-09 02:35, flood,36 +2020-10-09 05:56, slack,- +2020-10-09 07:27, ebb,-26 +2020-10-09 09:44, slack,- +2020-10-09 14:35, flood,36 +2020-10-09 18:02, slack,- +2020-10-09 19:51, ebb,-28 +2020-10-09 22:38, slack,- +2020-10-10 03:23, flood,36 +2020-10-10 06:50, slack,- +2020-10-10 08:21, ebb,-25 +2020-10-10 10:44, slack,- +2020-10-10 15:29, flood,37 +2020-10-10 18:56, slack,- +2020-10-10 20:45, ebb,-27 +2020-10-10 23:32, slack,- +2020-10-11 04:17, flood,37 +2020-10-11 07:44, slack,- +2020-10-11 09:15, ebb,-25 +2020-10-11 11:38, slack,- +2020-10-11 16:23, flood,39 +2020-10-11 19:50, slack,- +2020-10-11 21:39, ebb,-28 +2020-10-12 00:20, slack,- +2020-10-12 05:11, flood,40 +2020-10-12 08:38, slack,- +2020-10-12 10:15, ebb,-27 +2020-10-12 12:32, slack,- +2020-10-12 17:17, flood,42 +2020-10-12 20:50, slack,- +2020-10-12 22:39, ebb,-30 +2020-10-13 01:14, slack,- +2020-10-13 06:05, flood,43 +2020-10-13 09:32, slack,- +2020-10-13 11:09, ebb,-29 +2020-10-13 13:26, slack,- +2020-10-13 18:17, flood,46 +2020-10-13 21:50, slack,- +2020-10-13 23:33, ebb,-32 +2020-10-14 02:02, slack,- +2020-10-14 06:59, flood,47 +2020-10-14 10:26, slack,- +2020-10-14 12:09, ebb,-33 +2020-10-14 14:20, slack,- +2020-10-14 19:11, flood,49 +2020-10-14 22:44, slack,- +2020-10-15 00:33, ebb,-35 +2020-10-15 02:50, slack,- +2020-10-15 07:59, flood,50 +2020-10-15 11:20, slack,- +2020-10-15 12:57, ebb,-36 +2020-10-15 15:08, slack,- +2020-10-15 20:11, flood,52 +2020-10-15 23:38, slack,- +2020-10-16 01:21, ebb,-37 +2020-10-16 03:38, slack,- +2020-10-16 08:53, flood,54 +2020-10-16 12:08, slack,- +2020-10-16 13:51, ebb,-38 +2020-10-16 16:02, slack,- +2020-10-16 21:11, flood,55 +2020-10-17 00:32, slack,- +2020-10-17 02:15, ebb,-39 +2020-10-17 04:26, slack,- +2020-10-17 09:41, flood,56 +2020-10-17 12:56, slack,- +2020-10-17 14:39, ebb,-40 +2020-10-17 16:56, slack,- +2020-10-17 22:11, flood,56 +2020-10-18 01:26, slack,- +2020-10-18 03:03, ebb,-39 +2020-10-18 05:14, slack,- +2020-10-18 10:29, flood,56 +2020-10-18 13:50, slack,- +2020-10-18 15:33, ebb,-41 +2020-10-18 17:44, slack,- +2020-10-18 23:05, flood,55 +2020-10-19 02:20, slack,- +2020-10-19 03:57, ebb,-38 +2020-10-19 06:02, slack,- +2020-10-19 11:17, flood,54 +2020-10-19 14:38, slack,- +2020-10-19 16:21, ebb,-40 +2020-10-19 18:38, slack,- +2020-10-20 00:05, flood,52 +2020-10-20 03:08, slack,- +2020-10-20 04:45, ebb,-36 +2020-10-20 06:56, slack,- +2020-10-20 12:11, flood,49 +2020-10-20 15:32, slack,- +2020-10-20 17:09, ebb,-38 +2020-10-20 19:32, slack,- +2020-10-21 01:47, flood,49 +2020-10-21 04:08, slack,- +2020-10-21 05:39, ebb,-34 +2020-10-21 07:50, slack,- +2020-10-21 13:17, flood,45 +2020-10-21 16:20, slack,- +2020-10-21 18:03, ebb,-35 +2020-10-21 20:32, slack,- +2020-10-22 02:59, flood,47 +2020-10-22 05:02, slack,- +2020-10-22 06:27, ebb,-30 +2020-10-22 08:50, slack,- +2020-10-22 14:53, flood,41 +2020-10-22 17:20, slack,- +2020-10-22 18:51, ebb,-32 +2020-10-22 21:38, slack,- +2020-10-23 03:59, flood,46 +2020-10-23 05:56, slack,- +2020-10-23 07:21, ebb,-27 +2020-10-23 09:56, slack,- +2020-10-23 16:11, flood,39 +2020-10-23 18:14, slack,- +2020-10-23 19:45, ebb,-28 +2020-10-23 22:44, slack,- +2020-10-24 04:59, flood,45 +2020-10-24 06:56, slack,- +2020-10-24 08:21, ebb,-24 +2020-10-24 11:02, slack,- +2020-10-24 17:17, flood,38 +2020-10-24 19:14, slack,- +2020-10-24 20:45, ebb,-25 +2020-10-24 23:44, slack,- +2020-10-25 05:59, flood,45 +2020-10-25 07:56, slack,- +2020-10-25 09:33, ebb,-22 +2020-10-25 10:45, ebb,-22 +2020-10-25 12:02, slack,- +2020-10-25 18:17, flood,39 +2020-10-25 20:08, slack,- +2020-10-25 23:27, ebb,-24 +2020-10-26 00:38, slack,- +2020-10-26 06:59, flood,45 +2020-10-26 08:50, slack,- +2020-10-26 11:51, ebb,-24 +2020-10-26 12:56, slack,- +2020-10-26 19:17, flood,40 +2020-10-26 21:08, slack,- +2020-10-27 00:15, ebb,-25 +2020-10-27 01:26, slack,- +2020-10-27 07:53, flood,46 +2020-10-27 09:44, slack,- +2020-10-27 12:39, ebb,-26 +2020-10-27 13:44, slack,- +2020-10-27 20:11, flood,42 +2020-10-27 22:02, slack,- +2020-10-28 00:57, ebb,-26 +2020-10-28 02:14, slack,- +2020-10-28 08:41, flood,46 +2020-10-28 10:32, slack,- +2020-10-28 13:21, ebb,-27 +2020-10-28 14:32, slack,- +2020-10-28 20:59, flood,43 +2020-10-28 22:50, slack,- +2020-10-29 01:33, ebb,-26 +2020-10-29 02:56, slack,- +2020-10-29 09:29, flood,46 +2020-10-29 11:20, slack,- +2020-10-29 14:03, ebb,-28 +2020-10-29 15:20, slack,- +2020-10-29 21:47, flood,44 +2020-10-29 23:38, slack,- +2020-10-30 01:33, ebb,-27 +2020-10-30 03:38, slack,- +2020-10-30 10:11, flood,44 +2020-10-30 12:02, slack,- +2020-10-30 14:21, ebb,-28 +2020-10-30 16:02, slack,- +2020-10-30 22:23, flood,43 +2020-10-31 00:20, slack,- +2020-10-31 02:03, ebb,-27 +2020-10-31 04:14, slack,- +2020-10-31 10:47, flood,40 +2020-10-31 12:44, slack,- +2020-10-31 14:33, ebb,-29 +2020-10-31 16:44, slack,- +2020-10-31 22:53, flood,41 +2020-11-01 01:02, slack,- +2020-11-01 02:39, ebb,-28 +2020-11-01 04:50, slack,- +2020-11-01 09:41, flood,39 +2020-11-01 13:20, slack,- +2020-11-01 15:03, ebb,-30 +2020-11-01 17:26, slack,- +2020-11-01 22:17, flood,41 +2020-11-02 01:44, slack,- +2020-11-02 03:15, ebb,-29 +2020-11-02 05:32, slack,- +2020-11-02 10:11, flood,40 +2020-11-02 14:02, slack,- +2020-11-02 15:39, ebb,-30 +2020-11-02 18:08, slack,- +2020-11-02 22:53, flood,41 +2020-11-03 02:26, slack,- +2020-11-03 03:57, ebb,-29 +2020-11-03 06:08, slack,- +2020-11-03 10:53, flood,41 +2020-11-03 14:38, slack,- +2020-11-03 16:21, ebb,-31 +2020-11-03 18:44, slack,- +2020-11-03 23:35, flood,40 +2020-11-04 03:08, slack,- +2020-11-04 04:39, ebb,-29 +2020-11-04 06:44, slack,- +2020-11-04 11:35, flood,40 +2020-11-04 15:14, slack,- +2020-11-04 17:03, ebb,-31 +2020-11-04 19:26, slack,- +2020-11-05 00:23, flood,39 +2020-11-05 03:56, slack,- +2020-11-05 05:27, ebb,-29 +2020-11-05 07:32, slack,- +2020-11-05 12:23, flood,39 +2020-11-05 15:56, slack,- +2020-11-05 17:45, ebb,-31 +2020-11-05 20:14, slack,- +2020-11-06 01:11, flood,39 +2020-11-06 04:38, slack,- +2020-11-06 06:15, ebb,-29 +2020-11-06 08:14, slack,- +2020-11-06 13:17, flood,39 +2020-11-06 16:44, slack,- +2020-11-06 18:33, ebb,-31 +2020-11-06 21:02, slack,- +2020-11-07 02:05, flood,39 +2020-11-07 05:26, slack,- +2020-11-07 07:03, ebb,-28 +2020-11-07 09:14, slack,- +2020-11-07 14:11, flood,39 +2020-11-07 17:32, slack,- +2020-11-07 19:21, ebb,-31 +2020-11-07 22:02, slack,- +2020-11-08 02:59, flood,40 +2020-11-08 06:20, slack,- +2020-11-08 07:51, ebb,-28 +2020-11-08 10:14, slack,- +2020-11-08 15:05, flood,41 +2020-11-08 18:26, slack,- +2020-11-08 20:15, ebb,-31 +2020-11-08 22:56, slack,- +2020-11-09 03:53, flood,42 +2020-11-09 07:14, slack,- +2020-11-09 08:45, ebb,-28 +2020-11-09 11:08, slack,- +2020-11-09 15:59, flood,42 +2020-11-09 19:26, slack,- +2020-11-09 21:09, ebb,-31 +2020-11-09 23:50, slack,- +2020-11-10 04:41, flood,44 +2020-11-10 08:08, slack,- +2020-11-10 09:45, ebb,-29 +2020-11-10 12:08, slack,- +2020-11-10 16:59, flood,44 +2020-11-10 20:26, slack,- +2020-11-10 22:09, ebb,-31 +2020-11-11 00:44, slack,- +2020-11-11 05:35, flood,47 +2020-11-11 09:08, slack,- +2020-11-11 10:45, ebb,-31 +2020-11-11 13:02, slack,- +2020-11-11 17:53, flood,47 +2020-11-11 21:26, slack,- +2020-11-11 23:09, ebb,-33 +2020-11-12 01:32, slack,- +2020-11-12 06:35, flood,49 +2020-11-12 10:02, slack,- +2020-11-12 11:45, ebb,-34 +2020-11-12 14:02, slack,- +2020-11-12 18:53, flood,49 +2020-11-12 22:26, slack,- +2020-11-13 00:09, ebb,-35 +2020-11-13 02:26, slack,- +2020-11-13 07:29, flood,51 +2020-11-13 10:56, slack,- +2020-11-13 12:39, ebb,-36 +2020-11-13 14:56, slack,- +2020-11-13 19:59, flood,51 +2020-11-13 23:20, slack,- +2020-11-14 00:57, ebb,-36 +2020-11-14 03:14, slack,- +2020-11-14 08:29, flood,53 +2020-11-14 11:44, slack,- +2020-11-14 13:27, ebb,-38 +2020-11-14 15:44, slack,- +2020-11-14 21:17, flood,53 +2020-11-15 00:14, slack,- +2020-11-15 01:51, ebb,-36 +2020-11-15 04:02, slack,- +2020-11-15 09:23, flood,54 +2020-11-15 12:38, slack,- +2020-11-15 14:21, ebb,-40 +2020-11-15 16:38, slack,- +2020-11-15 22:23, flood,54 +2020-11-16 01:08, slack,- +2020-11-16 02:45, ebb,-36 +2020-11-16 04:56, slack,- +2020-11-16 10:17, flood,53 +2020-11-16 13:26, slack,- +2020-11-16 15:09, ebb,-40 +2020-11-16 17:32, slack,- +2020-11-16 23:35, flood,54 +2020-11-17 02:02, slack,- +2020-11-17 03:39, ebb,-35 +2020-11-17 05:44, slack,- +2020-11-17 11:05, flood,50 +2020-11-17 14:20, slack,- +2020-11-17 16:03, ebb,-39 +2020-11-17 18:26, slack,- +2020-11-18 00:47, flood,52 +2020-11-18 02:56, slack,- +2020-11-18 04:27, ebb,-34 +2020-11-18 06:38, slack,- +2020-11-18 12:05, flood,46 +2020-11-18 15:08, slack,- +2020-11-18 16:51, ebb,-36 +2020-11-18 19:20, slack,- +2020-11-19 01:47, flood,51 +2020-11-19 03:50, slack,- +2020-11-19 05:21, ebb,-31 +2020-11-19 07:32, slack,- +2020-11-19 13:53, flood,43 +2020-11-19 16:02, slack,- +2020-11-19 17:39, ebb,-34 +2020-11-19 20:14, slack,- +2020-11-20 02:47, flood,49 +2020-11-20 04:44, slack,- +2020-11-20 06:09, ebb,-29 +2020-11-20 08:32, slack,- +2020-11-20 14:59, flood,41 +2020-11-20 16:56, slack,- +2020-11-20 18:33, ebb,-31 +2020-11-20 21:14, slack,- +2020-11-21 03:41, flood,48 +2020-11-21 05:38, slack,- +2020-11-21 07:03, ebb,-26 +2020-11-21 09:38, slack,- +2020-11-21 15:53, flood,39 +2020-11-21 17:50, slack,- +2020-11-21 19:21, ebb,-28 +2020-11-21 22:14, slack,- +2020-11-22 04:35, flood,46 +2020-11-22 06:32, slack,- +2020-11-22 07:57, ebb,-24 +2020-11-22 10:38, slack,- +2020-11-22 16:53, flood,38 +2020-11-22 18:44, slack,- +2020-11-22 20:15, ebb,-25 +2020-11-22 23:14, slack,- +2020-11-23 05:35, flood,44 +2020-11-23 07:26, slack,- +2020-11-23 08:57, ebb,-22 +2020-11-23 11:32, slack,- +2020-11-23 17:47, flood,37 +2020-11-23 19:38, slack,- +2020-11-23 21:15, ebb,-23 +2020-11-24 00:02, slack,- +2020-11-24 06:29, flood,42 +2020-11-24 08:20, slack,- +2020-11-24 11:27, ebb,-23 +2020-11-24 12:26, slack,- +2020-11-24 18:41, flood,37 +2020-11-24 20:38, slack,- +2020-11-24 23:45, ebb,-23 +2020-11-25 00:50, slack,- +2020-11-25 07:17, flood,41 +2020-11-25 09:08, slack,- +2020-11-25 12:15, ebb,-25 +2020-11-25 13:14, slack,- +2020-11-25 19:35, flood,37 +2020-11-25 21:26, slack,- +2020-11-26 00:27, ebb,-23 +2020-11-26 01:38, slack,- +2020-11-26 08:11, flood,40 +2020-11-26 10:02, slack,- +2020-11-26 12:57, ebb,-26 +2020-11-26 14:02, slack,- +2020-11-26 20:29, flood,38 +2020-11-26 22:20, slack,- +2020-11-27 00:21, ebb,-23 +2020-11-27 02:20, slack,- +2020-11-27 08:59, flood,39 +2020-11-27 10:44, slack,- +2020-11-27 13:33, ebb,-26 +2020-11-27 14:50, slack,- +2020-11-27 21:17, flood,38 +2020-11-27 23:08, slack,- +2020-11-28 00:45, ebb,-24 +2020-11-28 02:56, slack,- +2020-11-28 09:41, flood,37 +2020-11-28 11:26, slack,- +2020-11-28 13:21, ebb,-27 +2020-11-28 15:32, slack,- +2020-11-28 21:53, flood,38 +2020-11-28 23:50, slack,- +2020-11-29 01:21, ebb,-25 +2020-11-29 03:38, slack,- +2020-11-29 08:23, flood,37 +2020-11-29 12:08, slack,- +2020-11-29 13:51, ebb,-28 +2020-11-29 16:14, slack,- +2020-11-29 21:23, flood,38 +2020-11-30 00:32, slack,- +2020-11-30 02:03, ebb,-27 +2020-11-30 04:14, slack,- +2020-11-30 08:59, flood,39 +2020-11-30 12:44, slack,- +2020-11-30 14:27, ebb,-30 +2020-11-30 16:56, slack,- +2020-11-30 21:47, flood,40 +2020-12-01 01:14, slack,- +2020-12-01 02:45, ebb,-28 +2020-12-01 04:56, slack,- +2020-12-01 09:41, flood,41 +2020-12-01 13:26, slack,- +2020-12-01 15:09, ebb,-31 +2020-12-01 17:38, slack,- +2020-12-01 22:29, flood,42 +2020-12-02 01:56, slack,- +2020-12-02 03:33, ebb,-29 +2020-12-02 05:38, slack,- +2020-12-02 10:23, flood,42 +2020-12-02 14:02, slack,- +2020-12-02 15:51, ebb,-32 +2020-12-02 18:14, slack,- +2020-12-02 23:11, flood,42 +2020-12-03 02:44, slack,- +2020-12-03 04:15, ebb,-30 +2020-12-03 06:14, slack,- +2020-12-03 11:05, flood,43 +2020-12-03 14:44, slack,- +2020-12-03 16:33, ebb,-34 +2020-12-03 18:56, slack,- +2020-12-03 23:53, flood,43 +2020-12-04 03:26, slack,- +2020-12-04 05:03, ebb,-31 +2020-12-04 07:02, slack,- +2020-12-04 11:53, flood,43 +2020-12-04 15:26, slack,- +2020-12-04 17:21, ebb,-34 +2020-12-04 19:44, slack,- +2020-12-05 00:47, flood,43 +2020-12-05 04:14, slack,- +2020-12-05 05:51, ebb,-31 +2020-12-05 07:50, slack,- +2020-12-05 12:47, flood,43 +2020-12-05 16:14, slack,- +2020-12-05 18:09, ebb,-35 +2020-12-05 20:32, slack,- +2020-12-06 01:41, flood,44 +2020-12-06 05:02, slack,- +2020-12-06 06:39, ebb,-31 +2020-12-06 08:44, slack,- +2020-12-06 13:47, flood,43 +2020-12-06 17:08, slack,- +2020-12-06 18:57, ebb,-34 +2020-12-06 21:26, slack,- +2020-12-07 02:35, flood,45 +2020-12-07 05:50, slack,- +2020-12-07 07:27, ebb,-31 +2020-12-07 09:44, slack,- +2020-12-07 14:41, flood,44 +2020-12-07 18:02, slack,- +2020-12-07 19:51, ebb,-34 +2020-12-07 22:20, slack,- +2020-12-08 03:23, flood,47 +2020-12-08 06:44, slack,- +2020-12-08 08:21, ebb,-31 +2020-12-08 10:44, slack,- +2020-12-08 15:41, flood,45 +2020-12-08 19:02, slack,- +2020-12-08 20:45, ebb,-33 +2020-12-08 23:20, slack,- +2020-12-09 04:17, flood,48 +2020-12-09 07:44, slack,- +2020-12-09 09:21, ebb,-31 +2020-12-09 11:44, slack,- +2020-12-09 16:35, flood,46 +2020-12-09 20:02, slack,- +2020-12-09 21:45, ebb,-32 +2020-12-10 00:08, slack,- +2020-12-10 05:11, flood,49 +2020-12-10 08:38, slack,- +2020-12-10 10:21, ebb,-32 +2020-12-10 12:44, slack,- +2020-12-10 17:35, flood,46 +2020-12-10 21:02, slack,- +2020-12-10 22:45, ebb,-32 +2020-12-11 01:02, slack,- +2020-12-11 06:05, flood,50 +2020-12-11 09:38, slack,- +2020-12-11 11:21, ebb,-34 +2020-12-11 13:44, slack,- +2020-12-11 18:41, flood,47 +2020-12-11 22:02, slack,- +2020-12-11 23:45, ebb,-33 +2020-12-12 01:56, slack,- +2020-12-12 07:05, flood,50 +2020-12-12 10:32, slack,- +2020-12-12 12:15, ebb,-36 +2020-12-12 14:38, slack,- +2020-12-12 20:11, flood,48 +2020-12-12 23:02, slack,- +2020-12-13 00:39, ebb,-33 +2020-12-13 02:50, slack,- +2020-12-13 08:11, flood,50 +2020-12-13 11:26, slack,- +2020-12-13 13:09, ebb,-37 +2020-12-13 15:32, slack,- +2020-12-13 21:47, flood,52 +2020-12-13 23:56, slack,- +2020-12-14 01:33, ebb,-33 +2020-12-14 03:44, slack,- +2020-12-14 09:35, flood,51 +2020-12-14 12:14, slack,- +2020-12-14 14:03, ebb,-38 +2020-12-14 16:26, slack,- +2020-12-14 22:47, flood,55 +2020-12-15 00:50, slack,- +2020-12-15 02:27, ebb,-33 +2020-12-15 04:38, slack,- +2020-12-15 10:41, flood,50 +2020-12-15 13:08, slack,- +2020-12-15 14:57, ebb,-37 +2020-12-15 17:20, slack,- +2020-12-15 23:41, flood,55 +2020-12-16 01:44, slack,- +2020-12-16 03:21, ebb,-33 +2020-12-16 05:32, slack,- +2020-12-16 11:41, flood,49 +2020-12-16 14:02, slack,- +2020-12-16 15:45, ebb,-36 +2020-12-16 18:14, slack,- +2020-12-17 00:35, flood,54 +2020-12-17 02:38, slack,- +2020-12-17 04:15, ebb,-32 +2020-12-17 06:20, slack,- +2020-12-17 12:41, flood,46 +2020-12-17 14:50, slack,- +2020-12-17 16:33, ebb,-35 +2020-12-17 19:02, slack,- +2020-12-18 01:35, flood,53 +2020-12-18 03:32, slack,- +2020-12-18 05:03, ebb,-30 +2020-12-18 07:14, slack,- +2020-12-18 13:41, flood,44 +2020-12-18 15:44, slack,- +2020-12-18 17:21, ebb,-33 +2020-12-18 19:56, slack,- +2020-12-19 02:29, flood,51 +2020-12-19 04:20, slack,- +2020-12-19 05:51, ebb,-28 +2020-12-19 08:14, slack,- +2020-12-19 14:35, flood,42 +2020-12-19 16:32, slack,- +2020-12-19 18:09, ebb,-30 +2020-12-19 20:50, slack,- +2020-12-20 03:17, flood,48 +2020-12-20 05:14, slack,- +2020-12-20 06:39, ebb,-26 +2020-12-20 09:08, slack,- +2020-12-20 15:29, flood,39 +2020-12-20 17:26, slack,- +2020-12-20 18:57, ebb,-28 +2020-12-20 21:44, slack,- +2020-12-21 04:11, flood,45 +2020-12-21 06:02, slack,- +2020-12-21 07:33, ebb,-24 +2020-12-21 10:08, slack,- +2020-12-21 16:17, flood,37 +2020-12-21 18:14, slack,- +2020-12-21 19:45, ebb,-25 +2020-12-21 22:38, slack,- +2020-12-22 04:59, flood,41 +2020-12-22 06:56, slack,- +2020-12-22 08:21, ebb,-23 +2020-12-22 11:02, slack,- +2020-12-22 17:11, flood,35 +2020-12-22 19:08, slack,- +2020-12-22 20:33, ebb,-23 +2020-12-22 23:26, slack,- +2020-12-23 05:53, flood,38 +2020-12-23 07:44, slack,- +2020-12-23 09:21, ebb,-21 +2020-12-23 10:51, ebb,-22 +2020-12-23 11:56, slack,- +2020-12-23 18:05, flood,33 +2020-12-23 20:02, slack,- +2020-12-23 21:27, ebb,-22 +2020-12-24 00:14, slack,- +2020-12-24 06:41, flood,35 +2020-12-24 08:32, slack,- +2020-12-24 11:45, ebb,-22 +2020-12-24 12:44, slack,- +2020-12-24 18:59, flood,32 +2020-12-24 20:50, slack,- +2020-12-24 22:27, ebb,-21 +2020-12-25 00:56, slack,- +2020-12-25 05:41, flood,32 +2020-12-25 07:29, flood,33 +2020-12-25 09:20, slack,- +2020-12-25 12:33, ebb,-23 +2020-12-25 13:32, slack,- +2020-12-25 19:53, flood,32 +2020-12-25 21:44, slack,- +2020-12-25 23:21, ebb,-21 +2020-12-26 01:38, slack,- +2020-12-26 06:11, flood,33 +2020-12-26 08:17, flood,31 +2020-12-26 10:08, slack,- +2020-12-26 12:09, ebb,-24 +2020-12-26 14:20, slack,- +2020-12-26 20:47, flood,33 +2020-12-26 22:32, slack,- +2020-12-27 00:03, ebb,-23 +2020-12-27 02:20, slack,- +2020-12-27 06:53, flood,34 +2020-12-27 10:50, slack,- +2020-12-27 12:33, ebb,-25 +2020-12-27 15:02, slack,- +2020-12-27 19:53, flood,34 +2020-12-27 21:23, flood,34 +2020-12-27 23:20, slack,- +2020-12-28 00:45, ebb,-24 +2020-12-28 03:02, slack,- +2020-12-28 07:35, flood,36 +2020-12-28 11:32, slack,- +2020-12-28 13:15, ebb,-27 +2020-12-28 15:44, slack,- +2020-12-28 20:35, flood,36 +2020-12-29 00:02, slack,- +2020-12-29 01:33, ebb,-26 +2020-12-29 03:44, slack,- +2020-12-29 08:23, flood,39 +2020-12-29 12:14, slack,- +2020-12-29 13:57, ebb,-30 +2020-12-29 16:26, slack,- +2020-12-29 21:23, flood,40 +2020-12-30 00:50, slack,- +2020-12-30 02:15, ebb,-28 +2020-12-30 04:26, slack,- +2020-12-30 09:11, flood,42 +2020-12-30 12:50, slack,- +2020-12-30 14:39, ebb,-32 +2020-12-30 17:08, slack,- +2020-12-30 22:05, flood,43 +2020-12-31 01:32, slack,- +2020-12-31 03:03, ebb,-30 +2020-12-31 05:08, slack,- +2020-12-31 09:59, flood,45 +2020-12-31 13:32, slack,- +2020-12-31 15:27, ebb,-34 +2020-12-31 17:50, slack,- +2020-12-31 22:47, flood,46 +2021-01-01 02:14, slack,- +2021-01-01 03:51, ebb,-32 +2021-01-01 05:50, slack,- +2021-01-01 10:41, flood,48 +2021-01-01 14:14, slack,- +2021-01-01 16:09, ebb,-36 +2021-01-01 18:32, slack,- +2021-01-01 23:29, flood,48 +2021-01-02 03:02, slack,- +2021-01-02 04:39, ebb,-33 +2021-01-02 06:32, slack,- +2021-01-02 11:35, flood,49 +2021-01-02 15:02, slack,- +2021-01-02 16:57, ebb,-38 +2021-01-02 19:14, slack,- +2021-01-03 00:17, flood,48 +2021-01-03 03:44, slack,- +2021-01-03 05:27, ebb,-34 +2021-01-03 07:20, slack,- +2021-01-03 12:23, flood,49 +2021-01-03 15:50, slack,- +2021-01-03 17:45, ebb,-38 +2021-01-03 20:02, slack,- +2021-01-04 01:11, flood,49 +2021-01-04 04:32, slack,- +2021-01-04 06:15, ebb,-35 +2021-01-04 08:14, slack,- +2021-01-04 13:23, flood,49 +2021-01-04 16:44, slack,- +2021-01-04 18:33, ebb,-38 +2021-01-04 20:50, slack,- +2021-01-05 02:05, flood,50 +2021-01-05 05:26, slack,- +2021-01-05 07:03, ebb,-35 +2021-01-05 09:14, slack,- +2021-01-05 14:17, flood,48 +2021-01-05 17:38, slack,- +2021-01-05 19:27, ebb,-36 +2021-01-05 21:44, slack,- +2021-01-06 02:59, flood,50 +2021-01-06 06:14, slack,- +2021-01-06 07:57, ebb,-34 +2021-01-06 10:14, slack,- +2021-01-06 15:17, flood,47 +2021-01-06 18:32, slack,- +2021-01-06 20:21, ebb,-34 +2021-01-06 22:44, slack,- +2021-01-07 03:47, flood,50 +2021-01-07 07:14, slack,- +2021-01-07 08:51, ebb,-33 +2021-01-07 11:20, slack,- +2021-01-07 16:17, flood,45 +2021-01-07 19:38, slack,- +2021-01-07 21:15, ebb,-32 +2021-01-07 23:38, slack,- +2021-01-08 04:41, flood,49 +2021-01-08 08:08, slack,- +2021-01-08 09:51, ebb,-32 +2021-01-08 12:20, slack,- +2021-01-08 17:17, flood,44 +2021-01-08 20:38, slack,- +2021-01-08 22:15, ebb,-30 +2021-01-09 00:38, slack,- +2021-01-09 05:35, flood,47 +2021-01-09 09:08, slack,- +2021-01-09 10:51, ebb,-32 +2021-01-09 13:26, slack,- +2021-01-09 18:35, flood,43 +2021-01-09 21:44, slack,- +2021-01-09 23:21, ebb,-29 +2021-01-10 01:32, slack,- +2021-01-10 06:41, flood,45 +2021-01-10 10:08, slack,- +2021-01-10 11:51, ebb,-33 +2021-01-10 14:20, slack,- +2021-01-10 20:41, flood,46 +2021-01-10 22:44, slack,- +2021-01-11 00:21, ebb,-29 +2021-01-11 02:32, slack,- +2021-01-11 08:47, flood,45 +2021-01-11 11:02, slack,- +2021-01-11 12:51, ebb,-34 +2021-01-11 15:20, slack,- +2021-01-11 21:47, flood,51 +2021-01-11 23:44, slack,- +2021-01-12 01:21, ebb,-29 +2021-01-12 03:26, slack,- +2021-01-12 09:53, flood,47 +2021-01-12 11:56, slack,- +2021-01-12 13:45, ebb,-34 +2021-01-12 16:14, slack,- +2021-01-12 22:41, flood,54 +2021-01-13 00:38, slack,- +2021-01-13 02:15, ebb,-29 +2021-01-13 04:20, slack,- +2021-01-13 10:47, flood,48 +2021-01-13 12:50, slack,- +2021-01-13 14:39, ebb,-34 +2021-01-13 17:08, slack,- +2021-01-13 23:29, flood,55 +2021-01-14 01:32, slack,- +2021-01-14 03:09, ebb,-30 +2021-01-14 05:14, slack,- +2021-01-14 11:41, flood,48 +2021-01-14 13:44, slack,- +2021-01-14 15:33, ebb,-33 +2021-01-14 18:02, slack,- +2021-01-15 00:23, flood,55 +2021-01-15 02:20, slack,- +2021-01-15 04:03, ebb,-29 +2021-01-15 06:08, slack,- +2021-01-15 12:35, flood,47 +2021-01-15 14:32, slack,- +2021-01-15 16:15, ebb,-32 +2021-01-15 18:50, slack,- +2021-01-16 01:17, flood,53 +2021-01-16 03:08, slack,- +2021-01-16 04:45, ebb,-29 +2021-01-16 06:56, slack,- +2021-01-16 13:23, flood,45 +2021-01-16 15:20, slack,- +2021-01-16 17:03, ebb,-31 +2021-01-16 19:38, slack,- +2021-01-17 02:05, flood,50 +2021-01-17 03:56, slack,- +2021-01-17 05:33, ebb,-27 +2021-01-17 07:50, slack,- +2021-01-17 14:17, flood,42 +2021-01-17 16:08, slack,- +2021-01-17 17:45, ebb,-29 +2021-01-17 20:26, slack,- +2021-01-18 02:53, flood,46 +2021-01-18 04:44, slack,- +2021-01-18 06:21, ebb,-26 +2021-01-18 08:44, slack,- +2021-01-18 14:59, flood,39 +2021-01-18 16:56, slack,- +2021-01-18 18:27, ebb,-27 +2021-01-18 21:14, slack,- +2021-01-19 03:41, flood,42 +2021-01-19 05:32, slack,- +2021-01-19 07:03, ebb,-24 +2021-01-19 09:38, slack,- +2021-01-19 15:41, flood,36 +2021-01-19 17:44, slack,- +2021-01-19 19:15, ebb,-25 +2021-01-19 22:02, slack,- +2021-01-20 04:23, flood,38 +2021-01-20 06:20, slack,- +2021-01-20 07:51, ebb,-23 +2021-01-20 10:32, slack,- +2021-01-20 16:29, flood,33 +2021-01-20 18:32, slack,- +2021-01-20 20:03, ebb,-23 +2021-01-20 22:50, slack,- +2021-01-21 05:05, flood,34 +2021-01-21 07:08, slack,- +2021-01-21 08:33, ebb,-22 +2021-01-21 11:26, slack,- +2021-01-21 16:17, flood,31 +2021-01-21 19:26, slack,- +2021-01-21 20:51, ebb,-21 +2021-01-21 23:32, slack,- +2021-01-22 04:17, flood,32 +2021-01-22 07:56, slack,- +2021-01-22 09:27, ebb,-21 +2021-01-22 11:15, ebb,-20 +2021-01-22 12:14, slack,- +2021-01-22 16:53, flood,30 +2021-01-22 20:14, slack,- +2021-01-22 21:45, ebb,-20 +2021-01-23 00:20, slack,- +2021-01-23 04:53, flood,32 +2021-01-23 08:44, slack,- +2021-01-23 10:21, ebb,-21 +2021-01-23 12:03, ebb,-21 +2021-01-23 13:02, slack,- +2021-01-23 17:41, flood,30 +2021-01-23 21:08, slack,- +2021-01-23 22:39, ebb,-20 +2021-01-24 01:02, slack,- +2021-01-24 05:35, flood,33 +2021-01-24 09:32, slack,- +2021-01-24 11:15, ebb,-23 +2021-01-24 13:50, slack,- +2021-01-24 18:29, flood,31 +2021-01-24 22:02, slack,- +2021-01-24 23:33, ebb,-21 +2021-01-25 01:50, slack,- +2021-01-25 06:23, flood,35 +2021-01-25 10:14, slack,- +2021-01-25 11:57, ebb,-25 +2021-01-25 14:38, slack,- +2021-01-25 19:17, flood,34 +2021-01-25 22:50, slack,- +2021-01-26 00:15, ebb,-23 +2021-01-26 02:32, slack,- +2021-01-26 07:05, flood,37 +2021-01-26 11:02, slack,- +2021-01-26 12:45, ebb,-28 +2021-01-26 15:20, slack,- +2021-01-26 20:05, flood,37 +2021-01-26 23:32, slack,- +2021-01-27 01:03, ebb,-26 +2021-01-27 03:14, slack,- +2021-01-27 07:59, flood,41 +2021-01-27 11:44, slack,- +2021-01-27 13:27, ebb,-30 +2021-01-27 16:02, slack,- +2021-01-27 20:59, flood,41 +2021-01-28 00:20, slack,- +2021-01-28 01:51, ebb,-29 +2021-01-28 03:56, slack,- +2021-01-28 08:47, flood,45 +2021-01-28 12:26, slack,- +2021-01-28 14:15, ebb,-33 +2021-01-28 16:38, slack,- +2021-01-28 21:41, flood,46 +2021-01-29 01:02, slack,- +2021-01-29 02:39, ebb,-31 +2021-01-29 04:38, slack,- +2021-01-29 09:35, flood,49 +2021-01-29 13:08, slack,- +2021-01-29 14:57, ebb,-36 +2021-01-29 17:20, slack,- +2021-01-29 22:23, flood,49 +2021-01-30 01:50, slack,- +2021-01-30 03:27, ebb,-34 +2021-01-30 05:26, slack,- +2021-01-30 10:23, flood,52 +2021-01-30 13:50, slack,- +2021-01-30 15:45, ebb,-39 +2021-01-30 18:02, slack,- +2021-01-30 23:05, flood,52 +2021-01-31 02:32, slack,- +2021-01-31 04:15, ebb,-36 +2021-01-31 06:08, slack,- +2021-01-31 11:11, flood,54 +2021-01-31 14:38, slack,- +2021-01-31 16:33, ebb,-40 +2021-01-31 18:44, slack,- +2021-01-31 23:53, flood,53 +2021-02-01 03:20, slack,- +2021-02-01 05:03, ebb,-37 +2021-02-01 06:56, slack,- +2021-02-01 12:05, flood,53 +2021-02-01 15:32, slack,- +2021-02-01 17:21, ebb,-40 +2021-02-01 19:32, slack,- +2021-02-02 00:41, flood,53 +2021-02-02 04:08, slack,- +2021-02-02 05:51, ebb,-37 +2021-02-02 07:50, slack,- +2021-02-02 12:59, flood,51 +2021-02-02 16:20, slack,- +2021-02-02 18:09, ebb,-39 +2021-02-02 20:20, slack,- +2021-02-03 01:35, flood,52 +2021-02-03 04:56, slack,- +2021-02-03 06:39, ebb,-37 +2021-02-03 08:50, slack,- +2021-02-03 13:59, flood,49 +2021-02-03 17:14, slack,- +2021-02-03 19:03, ebb,-37 +2021-02-03 21:14, slack,- +2021-02-04 02:29, flood,51 +2021-02-04 05:50, slack,- +2021-02-04 07:33, ebb,-35 +2021-02-04 09:56, slack,- +2021-02-04 14:59, flood,46 +2021-02-04 18:14, slack,- +2021-02-04 19:57, ebb,-34 +2021-02-04 22:14, slack,- +2021-02-05 03:23, flood,48 +2021-02-05 06:44, slack,- +2021-02-05 08:27, ebb,-33 +2021-02-05 11:02, slack,- +2021-02-05 15:59, flood,43 +2021-02-05 19:20, slack,- +2021-02-05 20:51, ebb,-30 +2021-02-05 23:14, slack,- +2021-02-06 04:17, flood,45 +2021-02-06 07:44, slack,- +2021-02-06 09:27, ebb,-31 +2021-02-06 12:08, slack,- +2021-02-06 17:17, flood,41 +2021-02-06 20:20, slack,- +2021-02-06 21:57, ebb,-27 +2021-02-07 00:14, slack,- +2021-02-07 05:17, flood,43 +2021-02-07 08:44, slack,- +2021-02-07 10:27, ebb,-30 +2021-02-07 13:08, slack,- +2021-02-07 19:23, flood,42 +2021-02-07 21:26, slack,- +2021-02-07 23:03, ebb,-26 +2021-02-08 01:20, slack,- +2021-02-08 07:29, flood,41 +2021-02-08 09:50, slack,- +2021-02-08 11:39, ebb,-30 +2021-02-08 14:08, slack,- +2021-02-08 20:35, flood,47 +2021-02-08 22:32, slack,- +2021-02-09 00:09, ebb,-27 +2021-02-09 02:20, slack,- +2021-02-09 08:47, flood,44 +2021-02-09 10:44, slack,- +2021-02-09 12:45, ebb,-30 +2021-02-09 15:08, slack,- +2021-02-09 21:29, flood,52 +2021-02-09 23:26, slack,- +2021-02-10 01:15, ebb,-27 +2021-02-10 03:14, slack,- +2021-02-10 09:47, flood,48 +2021-02-10 11:44, slack,- +2021-02-10 13:45, ebb,-31 +2021-02-10 16:02, slack,- +2021-02-10 22:23, flood,56 +2021-02-11 00:20, slack,- +2021-02-11 02:15, ebb,-28 +2021-02-11 04:08, slack,- +2021-02-11 10:35, flood,50 +2021-02-11 12:32, slack,- +2021-02-11 14:39, ebb,-32 +2021-02-11 16:50, slack,- +2021-02-11 23:11, flood,57 +2021-02-12 01:08, slack,- +2021-02-12 03:09, ebb,-29 +2021-02-12 05:02, slack,- +2021-02-12 11:23, flood,50 +2021-02-12 13:20, slack,- +2021-02-12 15:21, ebb,-32 +2021-02-12 17:38, slack,- +2021-02-12 23:59, flood,55 +2021-02-13 01:56, slack,- +2021-02-13 03:51, ebb,-29 +2021-02-13 05:50, slack,- +2021-02-13 12:11, flood,49 +2021-02-13 14:08, slack,- +2021-02-13 15:57, ebb,-32 +2021-02-13 18:26, slack,- +2021-02-14 00:47, flood,52 +2021-02-14 02:44, slack,- +2021-02-14 04:27, ebb,-29 +2021-02-14 06:38, slack,- +2021-02-14 12:59, flood,46 +2021-02-14 14:56, slack,- +2021-02-14 16:39, ebb,-31 +2021-02-14 19:08, slack,- +2021-02-15 01:35, flood,48 +2021-02-15 03:32, slack,- +2021-02-15 05:09, ebb,-28 +2021-02-15 07:20, slack,- +2021-02-15 13:41, flood,42 +2021-02-15 15:44, slack,- +2021-02-15 17:15, ebb,-29 +2021-02-15 19:50, slack,- +2021-02-16 02:23, flood,43 +2021-02-16 04:14, slack,- +2021-02-16 05:45, ebb,-27 +2021-02-16 08:08, slack,- +2021-02-16 14:23, flood,38 +2021-02-16 16:26, slack,- +2021-02-16 17:57, ebb,-28 +2021-02-16 20:32, slack,- +2021-02-17 02:59, flood,38 +2021-02-17 04:56, slack,- +2021-02-17 06:27, ebb,-26 +2021-02-17 09:02, slack,- +2021-02-17 14:41, flood,35 +2021-02-17 17:14, slack,- +2021-02-17 18:39, ebb,-26 +2021-02-17 21:14, slack,- +2021-02-18 02:23, flood,34 +2021-02-18 05:38, slack,- +2021-02-18 07:09, ebb,-24 +2021-02-18 09:56, slack,- +2021-02-18 14:53, flood,32 +2021-02-18 18:02, slack,- +2021-02-18 19:27, ebb,-24 +2021-02-18 22:02, slack,- +2021-02-19 02:53, flood,33 +2021-02-19 06:26, slack,- +2021-02-19 07:57, ebb,-23 +2021-02-19 10:50, slack,- +2021-02-19 15:29, flood,31 +2021-02-19 18:50, slack,- +2021-02-19 20:15, ebb,-22 +2021-02-19 22:50, slack,- +2021-02-20 03:29, flood,32 +2021-02-20 07:14, slack,- +2021-02-20 08:39, ebb,-22 +2021-02-20 11:38, slack,- +2021-02-20 16:11, flood,30 +2021-02-20 19:38, slack,- +2021-02-20 21:03, ebb,-21 +2021-02-20 23:38, slack,- +2021-02-21 04:11, flood,32 +2021-02-21 07:56, slack,- +2021-02-21 09:33, ebb,-22 +2021-02-21 12:26, slack,- +2021-02-21 16:59, flood,30 +2021-02-21 20:32, slack,- +2021-02-21 21:57, ebb,-20 +2021-02-22 00:26, slack,- +2021-02-22 04:59, flood,33 +2021-02-22 08:50, slack,- +2021-02-22 10:27, ebb,-23 +2021-02-22 13:14, slack,- +2021-02-22 17:53, flood,32 +2021-02-22 21:26, slack,- +2021-02-22 22:57, ebb,-22 +2021-02-23 01:14, slack,- +2021-02-23 05:47, flood,35 +2021-02-23 09:38, slack,- +2021-02-23 11:21, ebb,-25 +2021-02-23 14:02, slack,- +2021-02-23 18:41, flood,35 +2021-02-23 22:14, slack,- +2021-02-23 23:45, ebb,-24 +2021-02-24 02:02, slack,- +2021-02-24 06:41, flood,39 +2021-02-24 10:26, slack,- +2021-02-24 12:09, ebb,-28 +2021-02-24 14:44, slack,- +2021-02-24 19:35, flood,39 +2021-02-24 23:02, slack,- +2021-02-25 00:33, ebb,-27 +2021-02-25 02:44, slack,- +2021-02-25 07:29, flood,43 +2021-02-25 11:14, slack,- +2021-02-25 12:57, ebb,-31 +2021-02-25 15:26, slack,- +2021-02-25 20:23, flood,43 +2021-02-25 23:50, slack,- +2021-02-26 01:27, ebb,-30 +2021-02-26 03:32, slack,- +2021-02-26 08:23, flood,48 +2021-02-26 11:56, slack,- +2021-02-26 13:45, ebb,-35 +2021-02-26 16:08, slack,- +2021-02-26 21:11, flood,48 +2021-02-27 00:32, slack,- +2021-02-27 02:15, ebb,-34 +2021-02-27 04:14, slack,- +2021-02-27 09:17, flood,52 +2021-02-27 12:44, slack,- +2021-02-27 14:33, ebb,-38 +2021-02-27 16:50, slack,- +2021-02-27 21:59, flood,53 +2021-02-28 01:20, slack,- +2021-02-28 03:03, ebb,-36 +2021-02-28 05:02, slack,- +2021-02-28 10:05, flood,56 +2021-02-28 13:32, slack,- +2021-02-28 15:21, ebb,-40 +2021-02-28 17:32, slack,- +2021-02-28 22:41, flood,55 +2021-03-01 02:08, slack,- +2021-03-01 03:51, ebb,-39 +2021-03-01 05:50, slack,- +2021-03-01 10:53, flood,57 +2021-03-01 14:20, slack,- +2021-03-01 16:09, ebb,-41 +2021-03-01 18:20, slack,- +2021-03-01 23:23, flood,56 +2021-03-02 02:50, slack,- +2021-03-02 04:39, ebb,-40 +2021-03-02 06:38, slack,- +2021-03-02 11:41, flood,55 +2021-03-02 15:14, slack,- +2021-03-02 16:57, ebb,-41 +2021-03-02 19:02, slack,- +2021-03-03 00:11, flood,54 +2021-03-03 03:44, slack,- +2021-03-03 05:27, ebb,-39 +2021-03-03 07:32, slack,- +2021-03-03 12:41, flood,52 +2021-03-03 16:02, slack,- +2021-03-03 17:51, ebb,-39 +2021-03-03 19:50, slack,- +2021-03-04 01:11, flood,52 +2021-03-04 04:32, slack,- +2021-03-04 06:15, ebb,-38 +2021-03-04 08:32, slack,- +2021-03-04 13:47, flood,48 +2021-03-04 17:02, slack,- +2021-03-04 18:39, ebb,-36 +2021-03-04 20:50, slack,- +2021-03-05 02:05, flood,49 +2021-03-05 05:26, slack,- +2021-03-05 07:09, ebb,-36 +2021-03-05 09:38, slack,- +2021-03-05 14:47, flood,44 +2021-03-05 18:02, slack,- +2021-03-05 19:33, ebb,-32 +2021-03-05 21:50, slack,- +2021-03-06 02:59, flood,45 +2021-03-06 06:20, slack,- +2021-03-06 08:03, ebb,-33 +2021-03-06 10:44, slack,- +2021-03-06 16:05, flood,41 +2021-03-06 19:02, slack,- +2021-03-06 20:27, ebb,-28 +2021-03-06 22:56, slack,- +2021-03-07 04:05, flood,41 +2021-03-07 07:26, slack,- +2021-03-07 08:57, ebb,-29 +2021-03-07 11:50, slack,- +2021-03-07 17:59, flood,41 +2021-03-07 20:08, slack,- +2021-03-07 21:33, ebb,-25 +2021-03-08 00:02, slack,- +2021-03-08 05:35, flood,38 +2021-03-08 08:26, slack,- +2021-03-08 10:09, ebb,-27 +2021-03-08 12:56, slack,- +2021-03-08 19:11, flood,44 +2021-03-08 21:08, slack,- +2021-03-08 22:51, ebb,-24 +2021-03-09 01:08, slack,- +2021-03-09 07:29, flood,41 +2021-03-09 09:32, slack,- +2021-03-09 11:33, ebb,-27 +2021-03-09 13:56, slack,- +2021-03-09 20:17, flood,48 +2021-03-09 22:08, slack,- +2021-03-10 00:33, ebb,-25 +2021-03-10 02:08, slack,- +2021-03-10 08:35, flood,45 +2021-03-10 10:26, slack,- +2021-03-10 13:09, ebb,-28 +2021-03-10 14:50, slack,- +2021-03-10 21:11, flood,52 +2021-03-10 23:08, slack,- +2021-03-11 01:33, ebb,-27 +2021-03-11 03:02, slack,- +2021-03-11 09:29, flood,49 +2021-03-11 11:20, slack,- +2021-03-11 14:03, ebb,-30 +2021-03-11 15:44, slack,- +2021-03-11 22:05, flood,55 +2021-03-11 23:56, slack,- +2021-03-12 02:27, ebb,-29 +2021-03-12 03:50, slack,- +2021-03-12 10:17, flood,51 +2021-03-12 12:14, slack,- +2021-03-12 14:45, ebb,-30 +2021-03-12 16:32, slack,- +2021-03-12 22:47, flood,56 +2021-03-13 00:44, slack,- +2021-03-13 03:09, ebb,-29 +2021-03-13 04:38, slack,- +2021-03-13 11:05, flood,51 +2021-03-13 13:02, slack,- +2021-03-13 15:09, ebb,-31 +2021-03-13 17:14, slack,- +2021-03-13 23:35, flood,53 +2021-03-14 01:32, slack,- +2021-03-14 03:39, ebb,-30 +2021-03-14 05:26, slack,- +2021-03-14 11:47, flood,49 +2021-03-14 13:44, slack,- +2021-03-14 15:39, ebb,-31 +2021-03-14 17:50, slack,- +2021-03-15 00:17, flood,49 +2021-03-15 02:14, slack,- +2021-03-15 04:03, ebb,-29 +2021-03-15 06:08, slack,- +2021-03-15 12:29, flood,45 +2021-03-15 14:32, slack,- +2021-03-15 16:09, ebb,-30 +2021-03-15 18:32, slack,- +2021-03-16 00:59, flood,44 +2021-03-16 02:56, slack,- +2021-03-16 04:39, ebb,-29 +2021-03-16 06:56, slack,- +2021-03-16 13:05, flood,41 +2021-03-16 15:14, slack,- +2021-03-16 16:51, ebb,-29 +2021-03-16 19:08, slack,- +2021-03-17 01:35, flood,38 +2021-03-17 03:38, slack,- +2021-03-17 05:15, ebb,-28 +2021-03-17 07:38, slack,- +2021-03-17 12:47, flood,38 +2021-03-17 15:56, slack,- +2021-03-17 17:27, ebb,-28 +2021-03-17 19:50, slack,- +2021-03-18 00:47, flood,35 +2021-03-18 04:20, slack,- +2021-03-18 05:51, ebb,-27 +2021-03-18 08:26, slack,- +2021-03-18 13:23, flood,35 +2021-03-18 16:38, slack,- +2021-03-18 18:09, ebb,-26 +2021-03-18 20:32, slack,- +2021-03-19 01:23, flood,34 +2021-03-19 05:02, slack,- +2021-03-19 06:33, ebb,-26 +2021-03-19 09:14, slack,- +2021-03-19 14:11, flood,33 +2021-03-19 17:26, slack,- +2021-03-19 18:57, ebb,-25 +2021-03-19 21:20, slack,- +2021-03-20 02:05, flood,33 +2021-03-20 05:44, slack,- +2021-03-20 07:21, ebb,-25 +2021-03-20 10:08, slack,- +2021-03-20 14:53, flood,32 +2021-03-20 18:14, slack,- +2021-03-20 19:39, ebb,-23 +2021-03-20 22:14, slack,- +2021-03-21 02:53, flood,33 +2021-03-21 06:26, slack,- +2021-03-21 08:03, ebb,-24 +2021-03-21 11:02, slack,- +2021-03-21 15:41, flood,32 +2021-03-21 19:02, slack,- +2021-03-21 20:27, ebb,-22 +2021-03-21 23:02, slack,- +2021-03-22 03:41, flood,33 +2021-03-22 07:14, slack,- +2021-03-22 08:57, ebb,-23 +2021-03-22 11:50, slack,- +2021-03-22 16:29, flood,33 +2021-03-22 19:56, slack,- +2021-03-22 21:21, ebb,-22 +2021-03-22 23:56, slack,- +2021-03-23 04:29, flood,35 +2021-03-23 08:08, slack,- +2021-03-23 09:51, ebb,-24 +2021-03-23 12:38, slack,- +2021-03-23 17:17, flood,34 +2021-03-23 20:50, slack,- +2021-03-23 22:21, ebb,-23 +2021-03-24 00:44, slack,- +2021-03-24 05:17, flood,37 +2021-03-24 09:02, slack,- +2021-03-24 10:45, ebb,-26 +2021-03-24 13:26, slack,- +2021-03-24 18:11, flood,38 +2021-03-24 21:44, slack,- +2021-03-24 23:15, ebb,-26 +2021-03-25 01:32, slack,- +2021-03-25 06:11, flood,41 +2021-03-25 09:56, slack,- +2021-03-25 11:39, ebb,-29 +2021-03-25 14:14, slack,- +2021-03-25 18:59, flood,42 +2021-03-25 22:32, slack,- +2021-03-26 00:09, ebb,-29 +2021-03-26 02:20, slack,- +2021-03-26 07:05, flood,46 +2021-03-26 10:44, slack,- +2021-03-26 12:33, ebb,-33 +2021-03-26 14:56, slack,- +2021-03-26 19:53, flood,46 +2021-03-26 23:20, slack,- +2021-03-27 00:57, ebb,-33 +2021-03-27 03:08, slack,- +2021-03-27 07:59, flood,50 +2021-03-27 11:32, slack,- +2021-03-27 13:21, ebb,-36 +2021-03-27 15:38, slack,- +2021-03-27 20:41, flood,51 +2021-03-28 00:08, slack,- +2021-03-28 01:45, ebb,-36 +2021-03-28 03:50, slack,- +2021-03-28 08:53, flood,54 +2021-03-28 12:20, slack,- +2021-03-28 14:09, ebb,-39 +2021-03-28 16:20, slack,- +2021-03-28 21:29, flood,55 +2021-03-29 00:50, slack,- +2021-03-29 02:39, ebb,-39 +2021-03-29 04:38, slack,- +2021-03-29 09:47, flood,57 +2021-03-29 13:14, slack,- +2021-03-29 14:57, ebb,-40 +2021-03-29 17:08, slack,- +2021-03-29 22:17, flood,57 +2021-03-30 01:38, slack,- +2021-03-30 03:27, ebb,-40 +2021-03-30 05:32, slack,- +2021-03-30 10:35, flood,57 +2021-03-30 14:02, slack,- +2021-03-30 15:51, ebb,-40 +2021-03-30 17:50, slack,- +2021-03-30 22:59, flood,57 +2021-03-31 02:26, slack,- +2021-03-31 04:15, ebb,-41 +2021-03-31 06:20, slack,- +2021-03-31 11:29, flood,55 +2021-03-31 14:56, slack,- +2021-03-31 16:39, ebb,-39 +2021-03-31 18:38, slack,- +2021-03-31 23:47, flood,54 +2021-04-01 03:20, slack,- +2021-04-01 05:03, ebb,-40 +2021-04-01 07:14, slack,- +2021-04-01 12:29, flood,50 +2021-04-01 15:50, slack,- +2021-04-01 17:27, ebb,-37 +2021-04-01 19:32, slack,- +2021-04-02 00:41, flood,49 +2021-04-02 04:08, slack,- +2021-04-02 05:51, ebb,-38 +2021-04-02 08:14, slack,- +2021-04-02 13:41, flood,46 +2021-04-02 16:44, slack,- +2021-04-02 18:21, ebb,-34 +2021-04-02 20:32, slack,- +2021-04-03 01:47, flood,45 +2021-04-03 05:02, slack,- +2021-04-03 06:45, ebb,-35 +2021-04-03 09:20, slack,- +2021-04-03 15:05, flood,43 +2021-04-03 17:44, slack,- +2021-04-03 19:15, ebb,-30 +2021-04-03 21:38, slack,- +2021-04-04 02:53, flood,41 +2021-04-04 06:02, slack,- +2021-04-04 07:39, ebb,-31 +2021-04-04 10:26, slack,- +2021-04-04 16:41, flood,42 +2021-04-04 18:44, slack,- +2021-04-04 20:09, ebb,-26 +2021-04-04 22:44, slack,- +2021-04-05 04:17, flood,37 +2021-04-05 07:02, slack,- +2021-04-05 08:39, ebb,-27 +2021-04-05 11:38, slack,- +2021-04-05 17:53, flood,43 +2021-04-05 19:50, slack,- +2021-04-05 21:15, ebb,-23 +2021-04-05 23:50, slack,- +2021-04-06 06:11, flood,38 +2021-04-06 08:08, slack,- +2021-04-06 09:45, ebb,-25 +2021-04-06 12:38, slack,- +2021-04-06 18:53, flood,45 +2021-04-06 20:50, slack,- +2021-04-06 23:39, ebb,-23 +2021-04-07 00:50, slack,- +2021-04-07 07:17, flood,41 +2021-04-07 09:08, slack,- +2021-04-07 12:15, ebb,-26 +2021-04-07 13:38, slack,- +2021-04-07 19:53, flood,48 +2021-04-07 21:50, slack,- +2021-04-08 00:39, ebb,-26 +2021-04-08 01:50, slack,- +2021-04-08 08:11, flood,45 +2021-04-08 10:08, slack,- +2021-04-08 13:03, ebb,-27 +2021-04-08 14:26, slack,- +2021-04-08 20:47, flood,51 +2021-04-08 22:44, slack,- +2021-04-09 01:27, ebb,-28 +2021-04-09 02:44, slack,- +2021-04-09 09:05, flood,48 +2021-04-09 11:02, slack,- +2021-04-09 13:51, ebb,-28 +2021-04-09 15:14, slack,- +2021-04-09 21:35, flood,53 +2021-04-09 23:32, slack,- +2021-04-10 02:09, ebb,-29 +2021-04-10 03:32, slack,- +2021-04-10 09:53, flood,50 +2021-04-10 11:50, slack,- +2021-04-10 14:27, ebb,-29 +2021-04-10 15:56, slack,- +2021-04-10 22:23, flood,52 +2021-04-11 00:14, slack,- +2021-04-11 02:51, ebb,-30 +2021-04-11 04:14, slack,- +2021-04-11 10:41, flood,50 +2021-04-11 12:32, slack,- +2021-04-11 14:45, ebb,-29 +2021-04-11 16:38, slack,- +2021-04-11 23:05, flood,49 +2021-04-12 01:02, slack,- +2021-04-12 03:15, ebb,-30 +2021-04-12 05:02, slack,- +2021-04-12 11:17, flood,48 +2021-04-12 13:20, slack,- +2021-04-12 15:03, ebb,-29 +2021-04-12 17:14, slack,- +2021-04-12 23:47, flood,45 +2021-04-13 01:44, slack,- +2021-04-13 03:33, ebb,-30 +2021-04-13 05:44, slack,- +2021-04-13 11:53, flood,44 +2021-04-13 14:02, slack,- +2021-04-13 15:39, ebb,-29 +2021-04-13 17:56, slack,- +2021-04-14 00:17, flood,39 +2021-04-14 02:20, slack,- +2021-04-14 04:03, ebb,-29 +2021-04-14 06:26, slack,- +2021-04-14 11:35, flood,41 +2021-04-14 14:44, slack,- +2021-04-14 16:15, ebb,-28 +2021-04-14 18:32, slack,- +2021-04-14 23:23, flood,38 +2021-04-15 03:02, slack,- +2021-04-15 04:39, ebb,-29 +2021-04-15 07:08, slack,- +2021-04-15 12:05, flood,38 +2021-04-15 15:26, slack,- +2021-04-15 16:57, ebb,-28 +2021-04-15 19:14, slack,- +2021-04-15 23:53, flood,36 +2021-04-16 03:44, slack,- +2021-04-16 05:21, ebb,-29 +2021-04-16 07:50, slack,- +2021-04-16 12:47, flood,36 +2021-04-16 16:14, slack,- +2021-04-16 17:39, ebb,-27 +2021-04-16 19:56, slack,- +2021-04-17 00:41, flood,35 +2021-04-17 04:20, slack,- +2021-04-17 06:03, ebb,-28 +2021-04-17 08:38, slack,- +2021-04-17 13:35, flood,35 +2021-04-17 16:56, slack,- +2021-04-17 18:21, ebb,-26 +2021-04-17 20:38, slack,- +2021-04-18 01:29, flood,34 +2021-04-18 05:02, slack,- +2021-04-18 06:45, ebb,-27 +2021-04-18 09:26, slack,- +2021-04-18 14:23, flood,35 +2021-04-18 17:44, slack,- +2021-04-18 19:09, ebb,-25 +2021-04-18 21:32, slack,- +2021-04-19 02:17, flood,35 +2021-04-19 05:50, slack,- +2021-04-19 07:33, ebb,-27 +2021-04-19 10:20, slack,- +2021-04-19 15:05, flood,35 +2021-04-19 18:32, slack,- +2021-04-19 19:57, ebb,-24 +2021-04-19 22:26, slack,- +2021-04-20 03:11, flood,36 +2021-04-20 06:38, slack,- +2021-04-20 08:21, ebb,-26 +2021-04-20 11:14, slack,- +2021-04-20 15:53, flood,36 +2021-04-20 19:20, slack,- +2021-04-20 20:51, ebb,-24 +2021-04-20 23:20, slack,- +2021-04-21 03:59, flood,38 +2021-04-21 07:32, slack,- +2021-04-21 09:15, ebb,-27 +2021-04-21 12:02, slack,- +2021-04-21 16:47, flood,39 +2021-04-21 20:14, slack,- +2021-04-21 21:51, ebb,-25 +2021-04-22 00:14, slack,- +2021-04-22 04:53, flood,40 +2021-04-22 08:26, slack,- +2021-04-22 10:15, ebb,-28 +2021-04-22 12:50, slack,- +2021-04-22 17:35, flood,42 +2021-04-22 21:08, slack,- +2021-04-22 22:45, ebb,-28 +2021-04-23 01:02, slack,- +2021-04-23 05:47, flood,44 +2021-04-23 09:20, slack,- +2021-04-23 11:09, ebb,-31 +2021-04-23 13:38, slack,- +2021-04-23 18:29, flood,45 +2021-04-23 22:02, slack,- +2021-04-23 23:39, ebb,-31 +2021-04-24 01:56, slack,- +2021-04-24 06:41, flood,48 +2021-04-24 10:20, slack,- +2021-04-24 12:03, ebb,-34 +2021-04-24 14:20, slack,- +2021-04-24 19:17, flood,49 +2021-04-24 22:50, slack,- +2021-04-25 00:33, ebb,-34 +2021-04-25 02:44, slack,- +2021-04-25 07:35, flood,51 +2021-04-25 11:08, slack,- +2021-04-25 12:57, ebb,-36 +2021-04-25 15:08, slack,- +2021-04-25 20:11, flood,53 +2021-04-25 23:38, slack,- +2021-04-26 01:21, ebb,-37 +2021-04-26 03:32, slack,- +2021-04-26 08:35, flood,54 +2021-04-26 12:02, slack,- +2021-04-26 13:45, ebb,-38 +2021-04-26 15:56, slack,- +2021-04-26 20:59, flood,55 +2021-04-27 00:26, slack,- +2021-04-27 02:15, ebb,-40 +2021-04-27 04:26, slack,- +2021-04-27 09:29, flood,55 +2021-04-27 12:56, slack,- +2021-04-27 14:39, ebb,-39 +2021-04-27 16:38, slack,- +2021-04-27 21:53, flood,56 +2021-04-28 01:14, slack,- +2021-04-28 03:03, ebb,-41 +2021-04-28 05:14, slack,- +2021-04-28 10:23, flood,55 +2021-04-28 13:44, slack,- +2021-04-28 15:27, ebb,-38 +2021-04-28 17:32, slack,- +2021-04-28 22:35, flood,55 +2021-04-29 02:08, slack,- +2021-04-29 03:51, ebb,-41 +2021-04-29 06:08, slack,- +2021-04-29 11:23, flood,52 +2021-04-29 14:38, slack,- +2021-04-29 16:15, ebb,-37 +2021-04-29 18:20, slack,- +2021-04-29 23:29, flood,51 +2021-04-30 02:56, slack,- +2021-04-30 04:39, ebb,-39 +2021-04-30 07:02, slack,- +2021-04-30 12:29, flood,48 +2021-04-30 15:38, slack,- +2021-04-30 17:09, ebb,-34 +2021-04-30 19:14, slack,- +2021-05-01 00:23, flood,46 +2021-05-01 03:50, slack,- +2021-05-01 05:33, ebb,-37 +2021-05-01 08:02, slack,- +2021-05-01 14:17, flood,46 +2021-05-01 16:32, slack,- +2021-05-01 18:03, ebb,-31 +2021-05-01 20:14, slack,- +2021-05-02 01:41, flood,41 +2021-05-02 04:44, slack,- +2021-05-02 06:27, ebb,-33 +2021-05-02 09:02, slack,- +2021-05-02 15:23, flood,45 +2021-05-02 17:32, slack,- +2021-05-02 18:57, ebb,-28 +2021-05-02 21:20, slack,- +2021-05-03 03:17, flood,38 +2021-05-03 05:44, slack,- +2021-05-03 07:15, ebb,-30 +2021-05-03 10:14, slack,- +2021-05-03 16:29, flood,45 +2021-05-03 18:26, slack,- +2021-05-03 19:51, ebb,-25 +2021-05-03 22:32, slack,- +2021-05-04 04:47, flood,38 +2021-05-04 06:44, slack,- +2021-05-04 08:15, ebb,-26 +2021-05-04 11:14, slack,- +2021-05-04 17:35, flood,45 +2021-05-04 19:26, slack,- +2021-05-04 20:57, ebb,-23 +2021-05-04 23:32, slack,- +2021-05-05 05:53, flood,39 +2021-05-05 07:44, slack,- +2021-05-05 09:21, ebb,-24 +2021-05-05 12:14, slack,- +2021-05-05 18:29, flood,46 +2021-05-05 20:26, slack,- +2021-05-05 23:27, ebb,-23 +2021-05-06 00:32, slack,- +2021-05-06 06:47, flood,41 +2021-05-06 08:44, slack,- +2021-05-06 11:57, ebb,-25 +2021-05-06 13:08, slack,- +2021-05-06 19:29, flood,47 +2021-05-06 21:20, slack,- +2021-05-07 00:21, ebb,-26 +2021-05-07 01:26, slack,- +2021-05-07 07:47, flood,43 +2021-05-07 09:38, slack,- +2021-05-07 12:39, ebb,-26 +2021-05-07 13:56, slack,- +2021-05-07 20:23, flood,48 +2021-05-07 22:14, slack,- +2021-05-08 01:03, ebb,-27 +2021-05-08 02:20, slack,- +2021-05-08 08:41, flood,45 +2021-05-08 10:32, slack,- +2021-05-08 13:21, ebb,-26 +2021-05-08 14:38, slack,- +2021-05-08 21:11, flood,48 +2021-05-08 23:02, slack,- +2021-05-09 01:51, ebb,-28 +2021-05-09 03:02, slack,- +2021-05-09 09:29, flood,46 +2021-05-09 11:20, slack,- +2021-05-09 13:57, ebb,-27 +2021-05-09 15:20, slack,- +2021-05-09 21:53, flood,47 +2021-05-09 23:44, slack,- +2021-05-10 02:27, ebb,-29 +2021-05-10 03:50, slack,- +2021-05-10 10:11, flood,46 +2021-05-10 12:08, slack,- +2021-05-10 14:03, ebb,-27 +2021-05-10 16:02, slack,- +2021-05-10 22:35, flood,44 +2021-05-11 00:26, slack,- +2021-05-11 02:45, ebb,-29 +2021-05-11 04:32, slack,- +2021-05-11 10:53, flood,45 +2021-05-11 12:50, slack,- +2021-05-11 14:33, ebb,-27 +2021-05-11 16:38, slack,- +2021-05-11 23:05, flood,40 +2021-05-12 01:08, slack,- +2021-05-12 02:57, ebb,-29 +2021-05-12 05:14, slack,- +2021-05-12 11:23, flood,42 +2021-05-12 13:32, slack,- +2021-05-12 15:09, ebb,-27 +2021-05-12 17:20, slack,- +2021-05-12 22:11, flood,38 +2021-05-13 01:50, slack,- +2021-05-13 03:33, ebb,-30 +2021-05-13 05:56, slack,- +2021-05-13 10:59, flood,40 +2021-05-13 14:14, slack,- +2021-05-13 15:45, ebb,-27 +2021-05-13 17:56, slack,- +2021-05-13 22:41, flood,39 +2021-05-14 02:26, slack,- +2021-05-14 04:09, ebb,-30 +2021-05-14 06:38, slack,- +2021-05-14 11:29, flood,40 +2021-05-14 15:02, slack,- +2021-05-14 16:27, ebb,-28 +2021-05-14 18:38, slack,- +2021-05-14 23:23, flood,38 +2021-05-15 03:08, slack,- +2021-05-15 04:51, ebb,-30 +2021-05-15 07:20, slack,- +2021-05-15 12:11, flood,39 +2021-05-15 15:44, slack,- +2021-05-15 17:09, ebb,-27 +2021-05-15 19:20, slack,- +2021-05-16 00:05, flood,38 +2021-05-16 03:44, slack,- +2021-05-16 05:33, ebb,-30 +2021-05-16 08:02, slack,- +2021-05-16 12:59, flood,38 +2021-05-16 16:26, slack,- +2021-05-16 17:57, ebb,-27 +2021-05-16 20:02, slack,- +2021-05-17 00:59, flood,37 +2021-05-17 04:26, slack,- +2021-05-17 06:15, ebb,-30 +2021-05-17 08:50, slack,- +2021-05-17 13:47, flood,38 +2021-05-17 17:14, slack,- +2021-05-17 18:45, ebb,-27 +2021-05-17 20:56, slack,- +2021-05-18 01:47, flood,38 +2021-05-18 05:14, slack,- +2021-05-18 07:03, ebb,-30 +2021-05-18 09:38, slack,- +2021-05-18 14:35, flood,40 +2021-05-18 18:02, slack,- +2021-05-18 19:33, ebb,-27 +2021-05-18 21:50, slack,- +2021-05-19 02:41, flood,39 +2021-05-19 06:02, slack,- +2021-05-19 07:51, ebb,-30 +2021-05-19 10:32, slack,- +2021-05-19 15:29, flood,41 +2021-05-19 18:50, slack,- +2021-05-19 20:21, ebb,-28 +2021-05-19 22:44, slack,- +2021-05-20 03:35, flood,41 +2021-05-20 06:56, slack,- +2021-05-20 08:45, ebb,-30 +2021-05-20 11:20, slack,- +2021-05-20 16:17, flood,43 +2021-05-20 19:44, slack,- +2021-05-20 21:15, ebb,-28 +2021-05-20 23:44, slack,- +2021-05-21 04:29, flood,44 +2021-05-21 07:56, slack,- +2021-05-21 09:39, ebb,-31 +2021-05-21 12:14, slack,- +2021-05-21 17:05, flood,46 +2021-05-21 20:32, slack,- +2021-05-21 22:15, ebb,-30 +2021-05-22 00:38, slack,- +2021-05-22 05:23, flood,46 +2021-05-22 08:50, slack,- +2021-05-22 10:39, ebb,-32 +2021-05-22 13:02, slack,- +2021-05-22 17:59, flood,49 +2021-05-22 21:32, slack,- +2021-05-22 23:15, ebb,-33 +2021-05-23 01:32, slack,- +2021-05-23 06:17, flood,48 +2021-05-23 09:50, slack,- +2021-05-23 11:39, ebb,-33 +2021-05-23 13:50, slack,- +2021-05-23 18:47, flood,51 +2021-05-23 22:20, slack,- +2021-05-24 00:09, ebb,-35 +2021-05-24 02:20, slack,- +2021-05-24 07:17, flood,50 +2021-05-24 10:50, slack,- +2021-05-24 12:33, ebb,-35 +2021-05-24 14:38, slack,- +2021-05-24 19:41, flood,52 +2021-05-24 23:14, slack,- +2021-05-25 00:57, ebb,-38 +2021-05-25 03:14, slack,- +2021-05-25 08:23, flood,51 +2021-05-25 11:44, slack,- +2021-05-25 13:21, ebb,-36 +2021-05-25 15:32, slack,- +2021-05-25 20:35, flood,53 +2021-05-26 00:02, slack,- +2021-05-26 01:51, ebb,-39 +2021-05-26 04:08, slack,- +2021-05-26 09:29, flood,52 +2021-05-26 12:38, slack,- +2021-05-26 14:15, ebb,-36 +2021-05-26 16:20, slack,- +2021-05-26 21:29, flood,53 +2021-05-27 00:56, slack,- +2021-05-27 02:45, ebb,-40 +2021-05-27 05:02, slack,- +2021-05-27 10:35, flood,52 +2021-05-27 13:32, slack,- +2021-05-27 15:09, ebb,-35 +2021-05-27 17:14, slack,- +2021-05-27 22:23, flood,51 +2021-05-28 01:44, slack,- +2021-05-28 03:33, ebb,-39 +2021-05-28 05:56, slack,- +2021-05-28 11:53, flood,51 +2021-05-28 14:26, slack,- +2021-05-28 16:03, ebb,-34 +2021-05-28 18:08, slack,- +2021-05-28 23:17, flood,47 +2021-05-29 02:38, slack,- +2021-05-29 04:21, ebb,-38 +2021-05-29 06:50, slack,- +2021-05-29 13:11, flood,50 +2021-05-29 15:20, slack,- +2021-05-29 16:51, ebb,-32 +2021-05-29 19:02, slack,- +2021-05-30 00:23, flood,43 +2021-05-30 03:32, slack,- +2021-05-30 05:15, ebb,-35 +2021-05-30 07:50, slack,- +2021-05-30 14:17, flood,49 +2021-05-30 16:14, slack,- +2021-05-30 17:45, ebb,-30 +2021-05-30 20:02, slack,- +2021-05-31 02:23, flood,41 +2021-05-31 04:26, slack,- +2021-05-31 06:03, ebb,-32 +2021-05-31 08:50, slack,- +2021-05-31 15:11, flood,48 +2021-05-31 17:14, slack,- +2021-05-31 18:39, ebb,-27 +2021-05-31 21:08, slack,- +2021-06-01 03:23, flood,40 +2021-06-01 05:26, slack,- +2021-06-01 06:57, ebb,-29 +2021-06-01 09:50, slack,- +2021-06-01 16:11, flood,47 +2021-06-01 18:08, slack,- +2021-06-01 19:33, ebb,-25 +2021-06-01 22:08, slack,- +2021-06-02 04:23, flood,39 +2021-06-02 06:20, slack,- +2021-06-02 07:51, ebb,-26 +2021-06-02 10:50, slack,- +2021-06-02 17:05, flood,45 +2021-06-02 19:02, slack,- +2021-06-02 20:33, ebb,-23 +2021-06-02 23:14, slack,- +2021-06-03 05:23, flood,39 +2021-06-03 07:14, slack,- +2021-06-03 08:51, ebb,-24 +2021-06-03 11:44, slack,- +2021-06-03 17:59, flood,44 +2021-06-03 19:56, slack,- +2021-06-03 23:03, ebb,-23 +2021-06-04 00:08, slack,- +2021-06-04 06:23, flood,39 +2021-06-04 08:14, slack,- +2021-06-04 11:21, ebb,-23 +2021-06-04 12:32, slack,- +2021-06-04 18:53, flood,43 +2021-06-04 20:50, slack,- +2021-06-04 23:57, ebb,-25 +2021-06-05 01:02, slack,- +2021-06-05 07:17, flood,39 +2021-06-05 09:08, slack,- +2021-06-05 12:09, ebb,-23 +2021-06-05 13:20, slack,- +2021-06-05 19:47, flood,42 +2021-06-05 21:44, slack,- +2021-06-06 00:45, ebb,-26 +2021-06-06 01:50, slack,- +2021-06-06 08:11, flood,40 +2021-06-06 10:02, slack,- +2021-06-06 12:51, ebb,-24 +2021-06-06 14:02, slack,- +2021-06-06 20:41, flood,41 +2021-06-06 22:32, slack,- +2021-06-07 01:27, ebb,-27 +2021-06-07 02:38, slack,- +2021-06-07 08:59, flood,41 +2021-06-07 10:50, slack,- +2021-06-07 13:03, ebb,-24 +2021-06-07 14:44, slack,- +2021-06-07 21:23, flood,40 +2021-06-07 23:14, slack,- +2021-06-08 02:03, ebb,-27 +2021-06-08 03:20, slack,- +2021-06-08 09:47, flood,42 +2021-06-08 11:38, slack,- +2021-06-08 13:21, ebb,-24 +2021-06-08 15:26, slack,- +2021-06-08 22:05, flood,38 +2021-06-08 23:56, slack,- +2021-06-09 01:57, ebb,-28 +2021-06-09 04:08, slack,- +2021-06-09 10:29, flood,41 +2021-06-09 12:20, slack,- +2021-06-09 13:57, ebb,-25 +2021-06-09 16:08, slack,- +2021-06-09 20:53, flood,37 +2021-06-10 00:32, slack,- +2021-06-10 02:21, ebb,-29 +2021-06-10 04:50, slack,- +2021-06-10 10:53, flood,40 +2021-06-10 13:02, slack,- +2021-06-10 14:39, ebb,-26 +2021-06-10 16:44, slack,- +2021-06-10 21:29, flood,38 +2021-06-11 01:14, slack,- +2021-06-11 02:57, ebb,-30 +2021-06-11 05:26, slack,- +2021-06-11 10:29, flood,40 +2021-06-11 13:50, slack,- +2021-06-11 15:21, ebb,-27 +2021-06-11 17:26, slack,- +2021-06-11 22:11, flood,40 +2021-06-12 01:50, slack,- +2021-06-12 03:39, ebb,-31 +2021-06-12 06:08, slack,- +2021-06-12 10:59, flood,41 +2021-06-12 14:32, slack,- +2021-06-12 16:03, ebb,-28 +2021-06-12 18:08, slack,- +2021-06-12 22:53, flood,41 +2021-06-13 02:32, slack,- +2021-06-13 04:21, ebb,-32 +2021-06-13 06:50, slack,- +2021-06-13 11:41, flood,42 +2021-06-13 15:14, slack,- +2021-06-13 16:45, ebb,-29 +2021-06-13 18:50, slack,- +2021-06-13 23:35, flood,42 +2021-06-14 03:14, slack,- +2021-06-14 05:03, ebb,-33 +2021-06-14 07:26, slack,- +2021-06-14 12:29, flood,42 +2021-06-14 15:56, slack,- +2021-06-14 17:27, ebb,-30 +2021-06-14 19:32, slack,- +2021-06-15 00:29, flood,42 +2021-06-15 03:56, slack,- +2021-06-15 05:51, ebb,-34 +2021-06-15 08:14, slack,- +2021-06-15 13:17, flood,43 +2021-06-15 16:38, slack,- +2021-06-15 18:15, ebb,-30 +2021-06-15 20:20, slack,- +2021-06-16 01:23, flood,43 +2021-06-16 04:44, slack,- +2021-06-16 06:39, ebb,-34 +2021-06-16 09:02, slack,- +2021-06-16 14:11, flood,45 +2021-06-16 17:26, slack,- +2021-06-16 19:03, ebb,-31 +2021-06-16 21:14, slack,- +2021-06-17 02:17, flood,44 +2021-06-17 05:32, slack,- +2021-06-17 07:27, ebb,-34 +2021-06-17 09:50, slack,- +2021-06-17 14:59, flood,46 +2021-06-17 18:20, slack,- +2021-06-17 19:57, ebb,-31 +2021-06-17 22:14, slack,- +2021-06-18 03:11, flood,45 +2021-06-18 06:26, slack,- +2021-06-18 08:15, ebb,-33 +2021-06-18 10:44, slack,- +2021-06-18 15:47, flood,48 +2021-06-18 19:08, slack,- +2021-06-18 20:51, ebb,-31 +2021-06-18 23:14, slack,- +2021-06-19 04:05, flood,46 +2021-06-19 07:26, slack,- +2021-06-19 09:15, ebb,-32 +2021-06-19 11:38, slack,- +2021-06-19 16:35, flood,49 +2021-06-19 20:02, slack,- +2021-06-19 21:45, ebb,-32 +2021-06-20 00:08, slack,- +2021-06-20 04:59, flood,46 +2021-06-20 08:26, slack,- +2021-06-20 10:09, ebb,-32 +2021-06-20 12:32, slack,- +2021-06-20 17:29, flood,50 +2021-06-20 21:02, slack,- +2021-06-20 22:45, ebb,-33 +2021-06-21 01:08, slack,- +2021-06-21 05:59, flood,46 +2021-06-21 09:32, slack,- +2021-06-21 11:09, ebb,-32 +2021-06-21 13:20, slack,- +2021-06-21 18:23, flood,50 +2021-06-21 21:56, slack,- +2021-06-21 23:45, ebb,-35 +2021-06-22 02:08, slack,- +2021-06-22 07:05, flood,46 +2021-06-22 10:32, slack,- +2021-06-22 12:09, ebb,-32 +2021-06-22 14:14, slack,- +2021-06-22 19:17, flood,50 +2021-06-22 22:50, slack,- +2021-06-23 00:39, ebb,-36 +2021-06-23 03:02, slack,- +2021-06-23 08:35, flood,47 +2021-06-23 11:26, slack,- +2021-06-23 13:03, ebb,-33 +2021-06-23 15:08, slack,- +2021-06-23 20:17, flood,49 +2021-06-23 23:44, slack,- +2021-06-24 01:33, ebb,-37 +2021-06-24 03:56, slack,- +2021-06-24 10:11, flood,51 +2021-06-24 12:26, slack,- +2021-06-24 13:57, ebb,-33 +2021-06-24 16:02, slack,- +2021-06-24 21:29, flood,49 +2021-06-25 00:38, slack,- +2021-06-25 02:27, ebb,-37 +2021-06-25 04:50, slack,- +2021-06-25 11:11, flood,53 +2021-06-25 13:20, slack,- +2021-06-25 14:51, ebb,-32 +2021-06-25 17:02, slack,- +2021-06-25 22:35, flood,48 +2021-06-26 01:32, slack,- +2021-06-26 03:15, ebb,-37 +2021-06-26 05:44, slack,- +2021-06-26 12:05, flood,53 +2021-06-26 14:14, slack,- +2021-06-26 15:45, ebb,-32 +2021-06-26 17:56, slack,- +2021-06-26 23:53, flood,46 +2021-06-27 02:26, slack,- +2021-06-27 04:09, ebb,-36 +2021-06-27 06:38, slack,- +2021-06-27 13:05, flood,52 +2021-06-27 15:08, slack,- +2021-06-27 16:39, ebb,-31 +2021-06-27 18:50, slack,- +2021-06-28 01:11, flood,44 +2021-06-28 03:14, slack,- +2021-06-28 04:57, ebb,-34 +2021-06-28 07:32, slack,- +2021-06-28 13:59, flood,51 +2021-06-28 15:56, slack,- +2021-06-28 17:27, ebb,-29 +2021-06-28 19:44, slack,- +2021-06-29 02:11, flood,43 +2021-06-29 04:08, slack,- +2021-06-29 05:45, ebb,-31 +2021-06-29 08:26, slack,- +2021-06-29 14:53, flood,49 +2021-06-29 16:50, slack,- +2021-06-29 18:21, ebb,-27 +2021-06-29 20:44, slack,- +2021-06-30 03:05, flood,41 +2021-06-30 05:02, slack,- +2021-06-30 06:33, ebb,-29 +2021-06-30 09:20, slack,- +2021-06-30 15:47, flood,47 +2021-06-30 17:38, slack,- +2021-06-30 19:09, ebb,-25 +2021-06-30 21:44, slack,- +2021-07-01 03:59, flood,39 +2021-07-01 05:56, slack,- +2021-07-01 07:27, ebb,-26 +2021-07-01 10:14, slack,- +2021-07-01 16:35, flood,44 +2021-07-01 18:32, slack,- +2021-07-01 20:03, ebb,-23 +2021-07-01 22:44, slack,- +2021-07-02 04:53, flood,37 +2021-07-02 06:44, slack,- +2021-07-02 08:15, ebb,-24 +2021-07-02 11:08, slack,- +2021-07-02 17:29, flood,41 +2021-07-02 19:26, slack,- +2021-07-02 21:03, ebb,-22 +2021-07-02 22:27, ebb,-22 +2021-07-02 23:38, slack,- +2021-07-03 05:47, flood,36 +2021-07-03 07:38, slack,- +2021-07-03 09:09, ebb,-22 +2021-07-03 11:56, slack,- +2021-07-03 18:23, flood,38 +2021-07-03 20:14, slack,- +2021-07-03 23:27, ebb,-23 +2021-07-04 00:32, slack,- +2021-07-04 06:41, flood,35 +2021-07-04 08:32, slack,- +2021-07-04 10:15, ebb,-21 +2021-07-04 11:39, ebb,-20 +2021-07-04 12:38, slack,- +2021-07-04 19:11, flood,36 +2021-07-04 21:02, slack,- +2021-07-05 00:15, ebb,-24 +2021-07-05 01:20, slack,- +2021-07-05 07:35, flood,35 +2021-07-05 09:26, slack,- +2021-07-05 12:09, ebb,-21 +2021-07-05 13:26, slack,- +2021-07-05 20:05, flood,34 +2021-07-05 21:56, slack,- +2021-07-06 01:03, ebb,-25 +2021-07-06 02:08, slack,- +2021-07-06 08:29, flood,36 +2021-07-06 10:20, slack,- +2021-07-06 12:03, ebb,-21 +2021-07-06 14:08, slack,- +2021-07-06 18:47, flood,32 +2021-07-06 20:53, flood,33 +2021-07-06 22:38, slack,- +2021-07-07 01:33, ebb,-25 +2021-07-07 02:56, slack,- +2021-07-07 09:17, flood,37 +2021-07-07 11:08, slack,- +2021-07-07 12:39, ebb,-22 +2021-07-07 14:50, slack,- +2021-07-07 19:29, flood,34 +2021-07-07 20:47, flood,32 +2021-07-07 23:20, slack,- +2021-07-08 01:09, ebb,-26 +2021-07-08 03:38, slack,- +2021-07-08 09:59, flood,38 +2021-07-08 11:50, slack,- +2021-07-08 13:21, ebb,-24 +2021-07-08 15:32, slack,- +2021-07-08 20:17, flood,36 +2021-07-09 00:02, slack,- +2021-07-09 01:45, ebb,-28 +2021-07-09 04:20, slack,- +2021-07-09 10:23, flood,39 +2021-07-09 12:38, slack,- +2021-07-09 14:03, ebb,-26 +2021-07-09 16:14, slack,- +2021-07-09 20:59, flood,39 +2021-07-10 00:44, slack,- +2021-07-10 02:27, ebb,-30 +2021-07-10 04:56, slack,- +2021-07-10 09:59, flood,41 +2021-07-10 13:20, slack,- +2021-07-10 14:51, ebb,-28 +2021-07-10 16:56, slack,- +2021-07-10 21:41, flood,43 +2021-07-11 01:20, slack,- +2021-07-11 03:09, ebb,-32 +2021-07-11 05:38, slack,- +2021-07-11 10:35, flood,44 +2021-07-11 14:02, slack,- +2021-07-11 15:33, ebb,-30 +2021-07-11 17:38, slack,- +2021-07-11 22:29, flood,45 +2021-07-12 02:02, slack,- +2021-07-12 03:51, ebb,-34 +2021-07-12 06:14, slack,- +2021-07-12 11:11, flood,46 +2021-07-12 14:44, slack,- +2021-07-12 16:15, ebb,-32 +2021-07-12 18:20, slack,- +2021-07-12 23:11, flood,47 +2021-07-13 02:44, slack,- +2021-07-13 04:39, ebb,-36 +2021-07-13 06:56, slack,- +2021-07-13 11:59, flood,47 +2021-07-13 15:26, slack,- +2021-07-13 17:03, ebb,-33 +2021-07-13 19:02, slack,- +2021-07-13 23:59, flood,48 +2021-07-14 03:32, slack,- +2021-07-14 05:21, ebb,-37 +2021-07-14 07:38, slack,- +2021-07-14 12:47, flood,48 +2021-07-14 16:08, slack,- +2021-07-14 17:51, ebb,-34 +2021-07-14 19:50, slack,- +2021-07-15 00:53, flood,48 +2021-07-15 04:14, slack,- +2021-07-15 06:09, ebb,-37 +2021-07-15 08:26, slack,- +2021-07-15 13:35, flood,49 +2021-07-15 16:56, slack,- +2021-07-15 18:39, ebb,-34 +2021-07-15 20:44, slack,- +2021-07-16 01:53, flood,48 +2021-07-16 05:08, slack,- +2021-07-16 07:03, ebb,-37 +2021-07-16 09:14, slack,- +2021-07-16 14:29, flood,50 +2021-07-16 17:44, slack,- +2021-07-16 19:27, ebb,-34 +2021-07-16 21:44, slack,- +2021-07-17 02:47, flood,47 +2021-07-17 06:02, slack,- +2021-07-17 07:51, ebb,-35 +2021-07-17 10:08, slack,- +2021-07-17 15:17, flood,50 +2021-07-17 18:38, slack,- +2021-07-17 20:21, ebb,-34 +2021-07-17 22:44, slack,- +2021-07-18 03:41, flood,46 +2021-07-18 07:02, slack,- +2021-07-18 08:45, ebb,-33 +2021-07-18 11:08, slack,- +2021-07-18 16:05, flood,50 +2021-07-18 19:32, slack,- +2021-07-18 21:21, ebb,-33 +2021-07-18 23:50, slack,- +2021-07-19 04:41, flood,44 +2021-07-19 08:08, slack,- +2021-07-19 09:45, ebb,-31 +2021-07-19 12:02, slack,- +2021-07-19 16:59, flood,48 +2021-07-19 20:32, slack,- +2021-07-19 22:21, ebb,-33 +2021-07-20 00:50, slack,- +2021-07-20 05:47, flood,43 +2021-07-20 09:08, slack,- +2021-07-20 10:45, ebb,-30 +2021-07-20 13:02, slack,- +2021-07-20 17:59, flood,47 +2021-07-20 21:32, slack,- +2021-07-20 23:21, ebb,-33 +2021-07-21 01:50, slack,- +2021-07-21 07:17, flood,43 +2021-07-21 10:14, slack,- +2021-07-21 11:45, ebb,-30 +2021-07-21 13:56, slack,- +2021-07-21 18:59, flood,45 +2021-07-21 22:32, slack,- +2021-07-22 00:21, ebb,-34 +2021-07-22 02:50, slack,- +2021-07-22 09:11, flood,47 +2021-07-22 11:14, slack,- +2021-07-22 12:45, ebb,-30 +2021-07-22 14:56, slack,- +2021-07-22 21:05, flood,45 +2021-07-22 23:26, slack,- +2021-07-23 01:15, ebb,-34 +2021-07-23 03:44, slack,- +2021-07-23 10:11, flood,52 +2021-07-23 12:08, slack,- +2021-07-23 13:45, ebb,-30 +2021-07-23 15:56, slack,- +2021-07-23 22:17, flood,48 +2021-07-24 00:20, slack,- +2021-07-24 02:09, ebb,-35 +2021-07-24 04:38, slack,- +2021-07-24 11:05, flood,55 +2021-07-24 13:02, slack,- +2021-07-24 14:39, ebb,-31 +2021-07-24 16:50, slack,- +2021-07-24 23:11, flood,49 +2021-07-25 01:14, slack,- +2021-07-25 03:03, ebb,-35 +2021-07-25 05:32, slack,- +2021-07-25 11:53, flood,55 +2021-07-25 13:56, slack,- +2021-07-25 15:33, ebb,-31 +2021-07-25 17:44, slack,- +2021-07-26 00:05, flood,49 +2021-07-26 02:08, slack,- +2021-07-26 03:51, ebb,-34 +2021-07-26 06:20, slack,- +2021-07-26 12:47, flood,54 +2021-07-26 14:44, slack,- +2021-07-26 16:21, ebb,-30 +2021-07-26 18:32, slack,- +2021-07-27 00:59, flood,47 +2021-07-27 02:56, slack,- +2021-07-27 04:39, ebb,-33 +2021-07-27 07:08, slack,- +2021-07-27 13:41, flood,52 +2021-07-27 15:32, slack,- +2021-07-27 17:09, ebb,-29 +2021-07-27 19:26, slack,- +2021-07-28 01:47, flood,45 +2021-07-28 03:44, slack,- +2021-07-28 05:27, ebb,-31 +2021-07-28 07:56, slack,- +2021-07-28 14:29, flood,49 +2021-07-28 16:20, slack,- +2021-07-28 17:57, ebb,-28 +2021-07-28 20:20, slack,- +2021-07-29 02:41, flood,42 +2021-07-29 04:38, slack,- +2021-07-29 06:09, ebb,-29 +2021-07-29 08:50, slack,- +2021-07-29 15:17, flood,45 +2021-07-29 17:08, slack,- +2021-07-29 18:45, ebb,-26 +2021-07-29 21:14, slack,- +2021-07-30 03:29, flood,39 +2021-07-30 05:26, slack,- +2021-07-30 06:57, ebb,-27 +2021-07-30 09:38, slack,- +2021-07-30 16:05, flood,40 +2021-07-30 17:56, slack,- +2021-07-30 19:27, ebb,-24 +2021-07-30 22:08, slack,- +2021-07-31 04:17, flood,36 +2021-07-31 06:14, slack,- +2021-07-31 07:45, ebb,-24 +2021-07-31 10:26, slack,- +2021-07-31 16:53, flood,36 +2021-07-31 18:50, slack,- +2021-07-31 20:15, ebb,-22 +2021-07-31 23:08, slack,- +2021-08-01 05:11, flood,33 +2021-08-01 07:08, slack,- +2021-08-01 08:33, ebb,-22 +2021-08-01 11:14, slack,- +2021-08-01 17:41, flood,32 +2021-08-01 19:38, slack,- +2021-08-01 21:09, ebb,-21 +2021-08-01 22:57, ebb,-21 +2021-08-01 23:56, slack,- +2021-08-02 06:05, flood,31 +2021-08-02 08:02, slack,- +2021-08-02 09:21, ebb,-20 +2021-08-02 12:02, slack,- +2021-08-02 16:41, flood,30 +2021-08-02 20:26, slack,- +2021-08-02 23:51, ebb,-21 +2021-08-03 00:50, slack,- +2021-08-03 07:05, flood,31 +2021-08-03 08:56, slack,- +2021-08-03 10:21, ebb,-19 +2021-08-03 12:50, slack,- +2021-08-03 17:23, flood,30 +2021-08-03 21:14, slack,- +2021-08-04 00:33, ebb,-22 +2021-08-04 01:38, slack,- +2021-08-04 07:53, flood,31 +2021-08-04 09:44, slack,- +2021-08-04 11:21, ebb,-20 +2021-08-04 13:38, slack,- +2021-08-04 18:05, flood,32 +2021-08-04 22:02, slack,- +2021-08-04 23:51, ebb,-23 +2021-08-05 02:26, slack,- +2021-08-05 08:47, flood,33 +2021-08-05 10:38, slack,- +2021-08-05 12:09, ebb,-22 +2021-08-05 14:20, slack,- +2021-08-05 18:53, flood,34 +2021-08-05 22:50, slack,- +2021-08-06 00:33, ebb,-25 +2021-08-06 03:08, slack,- +2021-08-06 09:23, flood,35 +2021-08-06 11:20, slack,- +2021-08-06 12:51, ebb,-24 +2021-08-06 15:02, slack,- +2021-08-06 19:41, flood,37 +2021-08-06 23:32, slack,- +2021-08-07 01:15, ebb,-28 +2021-08-07 03:50, slack,- +2021-08-07 08:53, flood,38 +2021-08-07 12:02, slack,- +2021-08-07 13:33, ebb,-26 +2021-08-07 15:44, slack,- +2021-08-07 20:29, flood,41 +2021-08-08 00:14, slack,- +2021-08-08 01:57, ebb,-31 +2021-08-08 04:26, slack,- +2021-08-08 09:29, flood,42 +2021-08-08 12:50, slack,- +2021-08-08 14:21, ebb,-29 +2021-08-08 16:26, slack,- +2021-08-08 21:17, flood,46 +2021-08-09 00:50, slack,- +2021-08-09 02:39, ebb,-34 +2021-08-09 05:08, slack,- +2021-08-09 10:05, flood,47 +2021-08-09 13:32, slack,- +2021-08-09 15:09, ebb,-32 +2021-08-09 17:08, slack,- +2021-08-09 22:05, flood,50 +2021-08-10 01:32, slack,- +2021-08-10 03:27, ebb,-37 +2021-08-10 05:44, slack,- +2021-08-10 10:47, flood,50 +2021-08-10 14:14, slack,- +2021-08-10 15:51, ebb,-34 +2021-08-10 17:50, slack,- +2021-08-10 22:47, flood,52 +2021-08-11 02:20, slack,- +2021-08-11 04:09, ebb,-39 +2021-08-11 06:26, slack,- +2021-08-11 11:29, flood,52 +2021-08-11 14:56, slack,- +2021-08-11 16:39, ebb,-36 +2021-08-11 18:38, slack,- +2021-08-11 23:35, flood,53 +2021-08-12 03:08, slack,- +2021-08-12 04:57, ebb,-40 +2021-08-12 07:08, slack,- +2021-08-12 12:11, flood,53 +2021-08-12 15:44, slack,- +2021-08-12 17:27, ebb,-37 +2021-08-12 19:26, slack,- +2021-08-13 00:29, flood,52 +2021-08-13 03:56, slack,- +2021-08-13 05:45, ebb,-39 +2021-08-13 07:50, slack,- +2021-08-13 13:05, flood,52 +2021-08-13 16:26, slack,- +2021-08-13 18:15, ebb,-37 +2021-08-13 20:20, slack,- +2021-08-14 01:29, flood,50 +2021-08-14 04:44, slack,- +2021-08-14 06:39, ebb,-38 +2021-08-14 08:44, slack,- +2021-08-14 13:59, flood,52 +2021-08-14 17:20, slack,- +2021-08-14 19:03, ebb,-37 +2021-08-14 21:20, slack,- +2021-08-15 02:23, flood,48 +2021-08-15 05:44, slack,- +2021-08-15 07:27, ebb,-35 +2021-08-15 09:38, slack,- +2021-08-15 14:47, flood,50 +2021-08-15 18:14, slack,- +2021-08-15 19:57, ebb,-35 +2021-08-15 22:26, slack,- +2021-08-16 03:23, flood,45 +2021-08-16 06:44, slack,- +2021-08-16 08:21, ebb,-32 +2021-08-16 10:38, slack,- +2021-08-16 15:41, flood,47 +2021-08-16 19:08, slack,- +2021-08-16 20:51, ebb,-33 +2021-08-16 23:32, slack,- +2021-08-17 04:29, flood,42 +2021-08-17 07:50, slack,- +2021-08-17 09:21, ebb,-29 +2021-08-17 11:44, slack,- +2021-08-17 16:41, flood,45 +2021-08-17 20:14, slack,- +2021-08-17 21:51, ebb,-31 +2021-08-18 00:32, slack,- +2021-08-18 05:53, flood,41 +2021-08-18 08:56, slack,- +2021-08-18 10:27, ebb,-27 +2021-08-18 12:44, slack,- +2021-08-18 17:47, flood,42 +2021-08-18 21:14, slack,- +2021-08-18 22:57, ebb,-30 +2021-08-19 01:38, slack,- +2021-08-19 07:59, flood,44 +2021-08-19 09:56, slack,- +2021-08-19 11:33, ebb,-27 +2021-08-19 13:44, slack,- +2021-08-19 19:59, flood,42 +2021-08-19 22:14, slack,- +2021-08-20 00:03, ebb,-31 +2021-08-20 02:38, slack,- +2021-08-20 08:59, flood,49 +2021-08-20 10:56, slack,- +2021-08-20 12:39, ebb,-28 +2021-08-20 14:44, slack,- +2021-08-20 21:17, flood,47 +2021-08-20 23:14, slack,- +2021-08-21 01:03, ebb,-32 +2021-08-21 03:32, slack,- +2021-08-21 09:59, flood,54 +2021-08-21 11:50, slack,- +2021-08-21 13:39, ebb,-29 +2021-08-21 15:44, slack,- +2021-08-21 22:11, flood,50 +2021-08-22 00:08, slack,- +2021-08-22 02:03, ebb,-32 +2021-08-22 04:26, slack,- +2021-08-22 10:47, flood,57 +2021-08-22 12:44, slack,- +2021-08-22 14:39, ebb,-30 +2021-08-22 16:32, slack,- +2021-08-22 22:59, flood,52 +2021-08-23 00:56, slack,- +2021-08-23 02:51, ebb,-33 +2021-08-23 05:14, slack,- +2021-08-23 11:35, flood,56 +2021-08-23 13:32, slack,- +2021-08-23 15:27, ebb,-31 +2021-08-23 17:26, slack,- +2021-08-23 23:47, flood,51 +2021-08-24 01:50, slack,- +2021-08-24 03:39, ebb,-33 +2021-08-24 06:02, slack,- +2021-08-24 12:23, flood,54 +2021-08-24 14:20, slack,- +2021-08-24 16:09, ebb,-30 +2021-08-24 18:14, slack,- +2021-08-25 00:35, flood,49 +2021-08-25 02:32, slack,- +2021-08-25 04:21, ebb,-32 +2021-08-25 06:44, slack,- +2021-08-25 13:11, flood,50 +2021-08-25 15:08, slack,- +2021-08-25 16:51, ebb,-30 +2021-08-25 19:02, slack,- +2021-08-26 01:23, flood,45 +2021-08-26 03:20, slack,- +2021-08-26 05:03, ebb,-31 +2021-08-26 07:26, slack,- +2021-08-26 13:59, flood,46 +2021-08-26 15:50, slack,- +2021-08-26 17:27, ebb,-28 +2021-08-26 19:50, slack,- +2021-08-27 02:11, flood,42 +2021-08-27 04:08, slack,- +2021-08-27 05:39, ebb,-29 +2021-08-27 08:08, slack,- +2021-08-27 14:41, flood,41 +2021-08-27 16:38, slack,- +2021-08-27 18:09, ebb,-27 +2021-08-27 20:38, slack,- +2021-08-28 02:53, flood,38 +2021-08-28 04:56, slack,- +2021-08-28 06:27, ebb,-27 +2021-08-28 08:56, slack,- +2021-08-28 15:23, flood,35 +2021-08-28 17:20, slack,- +2021-08-28 18:51, ebb,-25 +2021-08-28 21:38, slack,- +2021-08-29 03:35, flood,34 +2021-08-29 05:44, slack,- +2021-08-29 07:09, ebb,-24 +2021-08-29 09:44, slack,- +2021-08-29 14:41, flood,32 +2021-08-29 18:08, slack,- +2021-08-29 19:39, ebb,-23 +2021-08-29 22:32, slack,- +2021-08-30 03:35, flood,31 +2021-08-30 06:32, slack,- +2021-08-30 07:57, ebb,-22 +2021-08-30 10:38, slack,- +2021-08-30 15:17, flood,30 +2021-08-30 18:56, slack,- +2021-08-30 20:21, ebb,-22 +2021-08-30 23:26, slack,- +2021-08-31 04:11, flood,29 +2021-08-31 07:26, slack,- +2021-08-31 08:45, ebb,-20 +2021-08-31 11:26, slack,- +2021-08-31 15:59, flood,30 +2021-08-31 19:44, slack,- +2021-08-31 21:15, ebb,-21 +2021-09-01 00:14, slack,- +2021-09-01 04:53, flood,29 +2021-09-01 08:20, slack,- +2021-09-01 09:39, ebb,-19 +2021-09-01 12:14, slack,- +2021-09-01 16:41, flood,30 +2021-09-01 20:32, slack,- +2021-09-01 22:09, ebb,-21 +2021-09-02 01:02, slack,- +2021-09-02 05:41, flood,30 +2021-09-02 09:08, slack,- +2021-09-02 10:39, ebb,-20 +2021-09-02 13:02, slack,- +2021-09-02 17:35, flood,32 +2021-09-02 21:26, slack,- +2021-09-02 23:03, ebb,-22 +2021-09-03 01:50, slack,- +2021-09-03 06:29, flood,32 +2021-09-03 10:02, slack,- +2021-09-03 11:33, ebb,-22 +2021-09-03 13:50, slack,- +2021-09-03 18:23, flood,35 +2021-09-03 22:14, slack,- +2021-09-03 23:57, ebb,-25 +2021-09-04 02:32, slack,- +2021-09-04 07:17, flood,35 +2021-09-04 10:50, slack,- +2021-09-04 12:21, ebb,-25 +2021-09-04 14:32, slack,- +2021-09-04 19:11, flood,39 +2021-09-04 22:56, slack,- +2021-09-05 00:39, ebb,-29 +2021-09-05 03:14, slack,- +2021-09-05 08:05, flood,40 +2021-09-05 11:32, slack,- +2021-09-05 13:09, ebb,-28 +2021-09-05 15:14, slack,- +2021-09-05 20:05, flood,44 +2021-09-05 23:44, slack,- +2021-09-06 01:27, ebb,-32 +2021-09-06 03:56, slack,- +2021-09-06 08:53, flood,45 +2021-09-06 12:14, slack,- +2021-09-06 13:51, ebb,-31 +2021-09-06 16:02, slack,- +2021-09-06 20:53, flood,49 +2021-09-07 00:26, slack,- +2021-09-07 02:15, ebb,-36 +2021-09-07 04:32, slack,- +2021-09-07 09:35, flood,50 +2021-09-07 12:56, slack,- +2021-09-07 14:39, ebb,-35 +2021-09-07 16:44, slack,- +2021-09-07 21:41, flood,53 +2021-09-08 01:08, slack,- +2021-09-08 03:03, ebb,-38 +2021-09-08 05:14, slack,- +2021-09-08 10:17, flood,54 +2021-09-08 13:44, slack,- +2021-09-08 15:27, ebb,-37 +2021-09-08 17:26, slack,- +2021-09-08 22:29, flood,56 +2021-09-09 01:56, slack,- +2021-09-09 03:45, ebb,-40 +2021-09-09 05:56, slack,- +2021-09-09 10:59, flood,56 +2021-09-09 14:26, slack,- +2021-09-09 16:15, ebb,-39 +2021-09-09 18:14, slack,- +2021-09-09 23:17, flood,56 +2021-09-10 02:44, slack,- +2021-09-10 04:33, ebb,-41 +2021-09-10 06:38, slack,- +2021-09-10 11:41, flood,55 +2021-09-10 15:14, slack,- +2021-09-10 17:03, ebb,-40 +2021-09-10 19:02, slack,- +2021-09-11 00:11, flood,53 +2021-09-11 03:38, slack,- +2021-09-11 05:21, ebb,-40 +2021-09-11 07:20, slack,- +2021-09-11 12:35, flood,53 +2021-09-11 16:02, slack,- +2021-09-11 17:51, ebb,-39 +2021-09-11 19:56, slack,- +2021-09-12 01:11, flood,50 +2021-09-12 04:32, slack,- +2021-09-12 06:15, ebb,-37 +2021-09-12 08:14, slack,- +2021-09-12 13:29, flood,51 +2021-09-12 16:56, slack,- +2021-09-12 18:39, ebb,-38 +2021-09-12 20:56, slack,- +2021-09-13 02:11, flood,46 +2021-09-13 05:26, slack,- +2021-09-13 07:03, ebb,-34 +2021-09-13 09:14, slack,- +2021-09-13 14:29, flood,47 +2021-09-13 17:50, slack,- +2021-09-13 19:33, ebb,-35 +2021-09-13 22:08, slack,- +2021-09-14 03:17, flood,43 +2021-09-14 06:26, slack,- +2021-09-14 08:03, ebb,-30 +2021-09-14 10:20, slack,- +2021-09-14 15:23, flood,44 +2021-09-14 18:50, slack,- +2021-09-14 20:27, ebb,-32 +2021-09-14 23:14, slack,- +2021-09-15 04:35, flood,40 +2021-09-15 07:32, slack,- +2021-09-15 08:57, ebb,-27 +2021-09-15 11:26, slack,- +2021-09-15 16:29, flood,41 +2021-09-15 19:50, slack,- +2021-09-15 21:33, ebb,-29 +2021-09-16 00:20, slack,- +2021-09-16 06:35, flood,42 +2021-09-16 08:38, slack,- +2021-09-16 10:09, ebb,-25 +2021-09-16 12:32, slack,- +2021-09-16 18:29, flood,39 +2021-09-16 20:56, slack,- +2021-09-16 22:39, ebb,-28 +2021-09-17 01:20, slack,- +2021-09-17 07:41, flood,46 +2021-09-17 09:38, slack,- +2021-09-17 11:27, ebb,-26 +2021-09-17 13:38, slack,- +2021-09-17 19:59, flood,43 +2021-09-17 21:56, slack,- +2021-09-18 00:03, ebb,-28 +2021-09-18 02:20, slack,- +2021-09-18 08:41, flood,50 +2021-09-18 10:38, slack,- +2021-09-18 12:51, ebb,-27 +2021-09-18 14:32, slack,- +2021-09-18 20:59, flood,48 +2021-09-18 22:56, slack,- +2021-09-19 01:15, ebb,-30 +2021-09-19 03:14, slack,- +2021-09-19 09:35, flood,54 +2021-09-19 11:32, slack,- +2021-09-19 13:51, ebb,-29 +2021-09-19 15:26, slack,- +2021-09-19 21:53, flood,52 +2021-09-19 23:50, slack,- +2021-09-20 02:03, ebb,-31 +2021-09-20 04:02, slack,- +2021-09-20 10:23, flood,56 +2021-09-20 12:20, slack,- +2021-09-20 14:39, ebb,-30 +2021-09-20 16:14, slack,- +2021-09-20 22:41, flood,53 +2021-09-21 00:38, slack,- +2021-09-21 02:45, ebb,-31 +2021-09-21 04:50, slack,- +2021-09-21 11:11, flood,55 +2021-09-21 13:08, slack,- +2021-09-21 15:15, ebb,-31 +2021-09-21 17:02, slack,- +2021-09-21 23:23, flood,52 +2021-09-22 01:26, slack,- +2021-09-22 03:21, ebb,-31 +2021-09-22 05:32, slack,- +2021-09-22 11:59, flood,52 +2021-09-22 13:56, slack,- +2021-09-22 15:51, ebb,-31 +2021-09-22 17:50, slack,- +2021-09-23 00:11, flood,49 +2021-09-23 02:08, slack,- +2021-09-23 03:57, ebb,-31 +2021-09-23 06:14, slack,- +2021-09-23 12:41, flood,47 +2021-09-23 14:38, slack,- +2021-09-23 16:21, ebb,-30 +2021-09-23 18:38, slack,- +2021-09-24 00:53, flood,45 +2021-09-24 02:56, slack,- +2021-09-24 04:33, ebb,-30 +2021-09-24 06:50, slack,- +2021-09-24 13:23, flood,41 +2021-09-24 15:20, slack,- +2021-09-24 16:57, ebb,-29 +2021-09-24 19:20, slack,- +2021-09-25 01:41, flood,41 +2021-09-25 03:44, slack,- +2021-09-25 05:15, ebb,-28 +2021-09-25 07:32, slack,- +2021-09-25 14:05, flood,36 +2021-09-25 16:02, slack,- +2021-09-25 17:39, ebb,-28 +2021-09-25 20:08, slack,- +2021-09-26 02:23, flood,37 +2021-09-26 04:26, slack,- +2021-09-26 05:57, ebb,-26 +2021-09-26 08:14, slack,- +2021-09-26 13:11, flood,33 +2021-09-26 16:44, slack,- +2021-09-26 18:21, ebb,-26 +2021-09-26 21:02, slack,- +2021-09-27 02:17, flood,33 +2021-09-27 05:14, slack,- +2021-09-27 06:39, ebb,-24 +2021-09-27 09:08, slack,- +2021-09-27 13:53, flood,31 +2021-09-27 17:32, slack,- +2021-09-27 19:03, ebb,-24 +2021-09-27 21:56, slack,- +2021-09-28 02:47, flood,32 +2021-09-28 06:02, slack,- +2021-09-28 07:21, ebb,-22 +2021-09-28 09:56, slack,- +2021-09-28 14:35, flood,31 +2021-09-28 18:14, slack,- +2021-09-28 19:45, ebb,-23 +2021-09-28 22:44, slack,- +2021-09-29 03:29, flood,31 +2021-09-29 06:50, slack,- +2021-09-29 08:15, ebb,-21 +2021-09-29 10:50, slack,- +2021-09-29 15:23, flood,31 +2021-09-29 19:02, slack,- +2021-09-29 20:33, ebb,-22 +2021-09-29 23:38, slack,- +2021-09-30 04:17, flood,31 +2021-09-30 07:44, slack,- +2021-09-30 09:03, ebb,-20 +2021-09-30 11:44, slack,- +2021-09-30 16:11, flood,32 +2021-09-30 19:50, slack,- +2021-09-30 21:27, ebb,-22 +2021-10-01 00:26, slack,- +2021-10-01 05:05, flood,32 +2021-10-01 08:32, slack,- +2021-10-01 10:03, ebb,-21 +2021-10-01 12:32, slack,- +2021-10-01 16:59, flood,34 +2021-10-01 20:44, slack,- +2021-10-01 22:27, ebb,-24 +2021-10-02 01:14, slack,- +2021-10-02 05:53, flood,35 +2021-10-02 09:26, slack,- +2021-10-02 10:57, ebb,-23 +2021-10-02 13:20, slack,- +2021-10-02 17:53, flood,38 +2021-10-02 21:38, slack,- +2021-10-02 23:21, ebb,-26 +2021-10-03 01:56, slack,- +2021-10-03 06:41, flood,39 +2021-10-03 10:14, slack,- +2021-10-03 11:51, ebb,-27 +2021-10-03 14:02, slack,- +2021-10-03 18:47, flood,42 +2021-10-03 22:26, slack,- +2021-10-04 00:09, ebb,-30 +2021-10-04 02:38, slack,- +2021-10-04 07:29, flood,43 +2021-10-04 11:02, slack,- +2021-10-04 12:39, ebb,-30 +2021-10-04 14:50, slack,- +2021-10-04 19:35, flood,47 +2021-10-04 23:14, slack,- +2021-10-05 00:57, ebb,-34 +2021-10-05 03:20, slack,- +2021-10-05 08:17, flood,48 +2021-10-05 11:44, slack,- +2021-10-05 13:27, ebb,-34 +2021-10-05 15:32, slack,- +2021-10-05 20:29, flood,51 +2021-10-06 00:02, slack,- +2021-10-06 01:45, ebb,-37 +2021-10-06 04:02, slack,- +2021-10-06 09:05, flood,53 +2021-10-06 12:26, slack,- +2021-10-06 14:15, ebb,-37 +2021-10-06 16:20, slack,- +2021-10-06 21:17, flood,55 +2021-10-07 00:44, slack,- +2021-10-07 02:33, ebb,-39 +2021-10-07 04:44, slack,- +2021-10-07 09:47, flood,56 +2021-10-07 13:14, slack,- +2021-10-07 15:03, ebb,-40 +2021-10-07 17:08, slack,- +2021-10-07 22:11, flood,57 +2021-10-08 01:38, slack,- +2021-10-08 03:21, ebb,-40 +2021-10-08 05:26, slack,- +2021-10-08 10:29, flood,57 +2021-10-08 14:02, slack,- +2021-10-08 15:51, ebb,-41 +2021-10-08 17:56, slack,- +2021-10-08 22:59, flood,56 +2021-10-09 02:26, slack,- +2021-10-09 04:15, ebb,-40 +2021-10-09 06:08, slack,- +2021-10-09 11:17, flood,56 +2021-10-09 14:50, slack,- +2021-10-09 16:39, ebb,-41 +2021-10-09 18:44, slack,- +2021-10-09 23:53, flood,53 +2021-10-10 03:20, slack,- +2021-10-10 05:03, ebb,-38 +2021-10-10 07:02, slack,- +2021-10-10 12:05, flood,52 +2021-10-10 15:38, slack,- +2021-10-10 17:27, ebb,-40 +2021-10-10 19:38, slack,- +2021-10-11 00:53, flood,49 +2021-10-11 04:14, slack,- +2021-10-11 05:51, ebb,-36 +2021-10-11 07:56, slack,- +2021-10-11 13:05, flood,48 +2021-10-11 16:32, slack,- +2021-10-11 18:15, ebb,-37 +2021-10-11 20:44, slack,- +2021-10-12 02:11, flood,45 +2021-10-12 05:14, slack,- +2021-10-12 06:45, ebb,-32 +2021-10-12 08:56, slack,- +2021-10-12 14:11, flood,43 +2021-10-12 17:32, slack,- +2021-10-12 19:09, ebb,-34 +2021-10-12 21:50, slack,- +2021-10-13 03:35, flood,42 +2021-10-13 06:14, slack,- +2021-10-13 07:39, ebb,-29 +2021-10-13 10:08, slack,- +2021-10-13 15:17, flood,40 +2021-10-13 18:32, slack,- +2021-10-13 20:09, ebb,-30 +2021-10-13 22:56, slack,- +2021-10-14 05:11, flood,42 +2021-10-14 07:14, slack,- +2021-10-14 08:39, ebb,-26 +2021-10-14 11:14, slack,- +2021-10-14 16:53, flood,38 +2021-10-14 19:32, slack,- +2021-10-14 21:09, ebb,-27 +2021-10-15 00:02, slack,- +2021-10-15 06:23, flood,44 +2021-10-15 08:20, slack,- +2021-10-15 09:51, ebb,-24 +2021-10-15 12:20, slack,- +2021-10-15 18:35, flood,40 +2021-10-15 20:38, slack,- +2021-10-15 22:21, ebb,-26 +2021-10-16 01:02, slack,- +2021-10-16 07:23, flood,47 +2021-10-16 09:20, slack,- +2021-10-16 11:57, ebb,-25 +2021-10-16 13:20, slack,- +2021-10-16 19:41, flood,44 +2021-10-16 21:38, slack,- +2021-10-17 00:27, ebb,-27 +2021-10-17 01:56, slack,- +2021-10-17 08:17, flood,50 +2021-10-17 10:14, slack,- +2021-10-17 12:57, ebb,-28 +2021-10-17 14:14, slack,- +2021-10-17 20:41, flood,48 +2021-10-17 22:32, slack,- +2021-10-18 01:15, ebb,-28 +2021-10-18 02:50, slack,- +2021-10-18 09:11, flood,53 +2021-10-18 11:08, slack,- +2021-10-18 13:45, ebb,-29 +2021-10-18 15:08, slack,- +2021-10-18 21:29, flood,51 +2021-10-18 23:26, slack,- +2021-10-19 01:57, ebb,-29 +2021-10-19 03:32, slack,- +2021-10-19 09:59, flood,53 +2021-10-19 11:56, slack,- +2021-10-19 14:27, ebb,-30 +2021-10-19 15:56, slack,- +2021-10-19 22:17, flood,52 +2021-10-20 00:14, slack,- +2021-10-20 02:27, ebb,-29 +2021-10-20 04:20, slack,- +2021-10-20 10:47, flood,51 +2021-10-20 12:38, slack,- +2021-10-20 15:03, ebb,-30 +2021-10-20 16:44, slack,- +2021-10-20 22:59, flood,50 +2021-10-21 01:02, slack,- +2021-10-21 02:51, ebb,-29 +2021-10-21 04:56, slack,- +2021-10-21 11:29, flood,47 +2021-10-21 13:26, slack,- +2021-10-21 15:27, ebb,-30 +2021-10-21 17:26, slack,- +2021-10-21 23:47, flood,47 +2021-10-22 01:44, slack,- +2021-10-22 03:27, ebb,-29 +2021-10-22 05:38, slack,- +2021-10-22 12:11, flood,42 +2021-10-22 14:08, slack,- +2021-10-22 15:51, ebb,-30 +2021-10-22 18:08, slack,- +2021-10-23 00:23, flood,43 +2021-10-23 02:32, slack,- +2021-10-23 04:03, ebb,-28 +2021-10-23 06:20, slack,- +2021-10-23 11:11, flood,37 +2021-10-23 14:44, slack,- +2021-10-23 16:27, ebb,-29 +2021-10-23 18:56, slack,- +2021-10-24 01:11, flood,39 +2021-10-24 03:14, slack,- +2021-10-24 04:45, ebb,-27 +2021-10-24 06:56, slack,- +2021-10-24 11:41, flood,35 +2021-10-24 15:26, slack,- +2021-10-24 17:03, ebb,-28 +2021-10-24 19:38, slack,- +2021-10-25 00:53, flood,36 +2021-10-25 03:56, slack,- +2021-10-25 05:27, ebb,-26 +2021-10-25 07:44, slack,- +2021-10-25 12:23, flood,33 +2021-10-25 16:08, slack,- +2021-10-25 17:45, ebb,-27 +2021-10-25 20:26, slack,- +2021-10-26 01:29, flood,34 +2021-10-26 04:44, slack,- +2021-10-26 06:09, ebb,-25 +2021-10-26 08:26, slack,- +2021-10-26 13:11, flood,32 +2021-10-26 16:50, slack,- +2021-10-26 18:27, ebb,-26 +2021-10-26 21:14, slack,- +2021-10-27 02:11, flood,34 +2021-10-27 05:32, slack,- +2021-10-27 06:57, ebb,-24 +2021-10-27 09:20, slack,- +2021-10-27 14:05, flood,32 +2021-10-27 17:38, slack,- +2021-10-27 19:15, ebb,-25 +2021-10-27 22:08, slack,- +2021-10-28 02:59, flood,34 +2021-10-28 06:14, slack,- +2021-10-28 07:45, ebb,-23 +2021-10-28 10:14, slack,- +2021-10-28 14:53, flood,33 +2021-10-28 18:20, slack,- +2021-10-28 20:03, ebb,-25 +2021-10-28 22:56, slack,- +2021-10-29 03:41, flood,35 +2021-10-29 07:08, slack,- +2021-10-29 08:33, ebb,-23 +2021-10-29 11:08, slack,- +2021-10-29 15:41, flood,35 +2021-10-29 19:14, slack,- +2021-10-29 20:51, ebb,-25 +2021-10-29 23:44, slack,- +2021-10-30 04:29, flood,36 +2021-10-30 07:56, slack,- +2021-10-30 09:27, ebb,-24 +2021-10-30 11:56, slack,- +2021-10-30 16:35, flood,37 +2021-10-30 20:02, slack,- +2021-10-30 21:51, ebb,-26 +2021-10-31 00:32, slack,- +2021-10-31 05:17, flood,39 +2021-10-31 08:44, slack,- +2021-10-31 10:21, ebb,-26 +2021-10-31 12:44, slack,- +2021-10-31 17:23, flood,41 +2021-10-31 20:56, slack,- +2021-10-31 22:45, ebb,-28 +2021-11-01 01:14, slack,- +2021-11-01 06:05, flood,43 +2021-11-01 09:38, slack,- +2021-11-01 11:15, ebb,-29 +2021-11-01 13:32, slack,- +2021-11-01 18:17, flood,45 +2021-11-01 21:50, slack,- +2021-11-01 23:39, ebb,-31 +2021-11-02 02:02, slack,- +2021-11-02 06:53, flood,47 +2021-11-02 10:26, slack,- +2021-11-02 12:09, ebb,-32 +2021-11-02 14:20, slack,- +2021-11-02 19:11, flood,48 +2021-11-02 22:44, slack,- +2021-11-03 00:27, ebb,-34 +2021-11-03 02:44, slack,- +2021-11-03 07:41, flood,51 +2021-11-03 11:14, slack,- +2021-11-03 12:57, ebb,-36 +2021-11-03 15:08, slack,- +2021-11-03 20:05, flood,52 +2021-11-03 23:38, slack,- +2021-11-04 01:21, ebb,-36 +2021-11-04 03:26, slack,- +2021-11-04 08:29, flood,54 +2021-11-04 12:02, slack,- +2021-11-04 13:45, ebb,-39 +2021-11-04 15:56, slack,- +2021-11-04 20:59, flood,54 +2021-11-05 00:26, slack,- +2021-11-05 02:09, ebb,-38 +2021-11-05 04:14, slack,- +2021-11-05 09:17, flood,56 +2021-11-05 12:50, slack,- +2021-11-05 14:39, ebb,-41 +2021-11-05 16:50, slack,- +2021-11-05 21:53, flood,55 +2021-11-06 01:20, slack,- +2021-11-06 03:03, ebb,-38 +2021-11-06 05:02, slack,- +2021-11-06 10:05, flood,56 +2021-11-06 13:38, slack,- +2021-11-06 15:27, ebb,-41 +2021-11-06 17:38, slack,- +2021-11-06 22:47, flood,54 +2021-11-07 02:14, slack,- +2021-11-07 03:51, ebb,-37 +2021-11-07 05:50, slack,- +2021-11-07 10:53, flood,54 +2021-11-07 14:26, slack,- +2021-11-07 16:15, ebb,-41 +2021-11-07 18:32, slack,- +2021-11-07 23:47, flood,51 +2021-11-08 03:08, slack,- +2021-11-08 04:45, ebb,-36 +2021-11-08 06:44, slack,- +2021-11-08 11:47, flood,49 +2021-11-08 15:20, slack,- +2021-11-08 17:03, ebb,-39 +2021-11-08 19:26, slack,- +2021-11-09 01:05, flood,47 +2021-11-09 04:02, slack,- +2021-11-09 05:33, ebb,-34 +2021-11-09 07:44, slack,- +2021-11-09 12:53, flood,45 +2021-11-09 16:14, slack,- +2021-11-09 17:57, ebb,-36 +2021-11-09 20:26, slack,- +2021-11-10 02:41, flood,46 +2021-11-10 05:02, slack,- +2021-11-10 06:27, ebb,-31 +2021-11-10 08:44, slack,- +2021-11-10 14:11, flood,41 +2021-11-10 17:14, slack,- +2021-11-10 18:51, ebb,-33 +2021-11-10 21:32, slack,- +2021-11-11 03:53, flood,45 +2021-11-11 05:56, slack,- +2021-11-11 07:21, ebb,-28 +2021-11-11 09:56, slack,- +2021-11-11 15:41, flood,39 +2021-11-11 18:14, slack,- +2021-11-11 19:45, ebb,-29 +2021-11-11 22:38, slack,- +2021-11-12 04:59, flood,45 +2021-11-12 06:56, slack,- +2021-11-12 08:21, ebb,-25 +2021-11-12 11:02, slack,- +2021-11-12 17:11, flood,39 +2021-11-12 19:14, slack,- +2021-11-12 20:45, ebb,-26 +2021-11-12 23:44, slack,- +2021-11-13 05:59, flood,46 +2021-11-13 07:56, slack,- +2021-11-13 09:33, ebb,-24 +2021-11-13 12:02, slack,- +2021-11-13 18:17, flood,41 +2021-11-13 20:14, slack,- +2021-11-13 21:57, ebb,-24 +2021-11-14 00:38, slack,- +2021-11-14 06:59, flood,47 +2021-11-14 08:50, slack,- +2021-11-14 11:51, ebb,-25 +2021-11-14 13:02, slack,- +2021-11-14 19:17, flood,43 +2021-11-14 21:14, slack,- +2021-11-15 00:09, ebb,-25 +2021-11-15 01:32, slack,- +2021-11-15 07:53, flood,48 +2021-11-15 09:50, slack,- +2021-11-15 12:39, ebb,-27 +2021-11-15 13:56, slack,- +2021-11-15 20:11, flood,45 +2021-11-15 22:08, slack,- +2021-11-16 00:57, ebb,-26 +2021-11-16 02:20, slack,- +2021-11-16 08:47, flood,48 +2021-11-16 10:38, slack,- +2021-11-16 13:27, ebb,-29 +2021-11-16 14:44, slack,- +2021-11-16 21:05, flood,47 +2021-11-16 23:02, slack,- +2021-11-17 01:33, ebb,-27 +2021-11-17 03:02, slack,- +2021-11-17 09:35, flood,48 +2021-11-17 11:26, slack,- +2021-11-17 14:09, ebb,-29 +2021-11-17 15:32, slack,- +2021-11-17 21:53, flood,48 +2021-11-17 23:50, slack,- +2021-11-18 01:57, ebb,-27 +2021-11-18 03:44, slack,- +2021-11-18 10:17, flood,46 +2021-11-18 12:08, slack,- +2021-11-18 14:45, ebb,-29 +2021-11-18 16:20, slack,- +2021-11-18 22:41, flood,47 +2021-11-19 00:32, slack,- +2021-11-19 02:21, ebb,-27 +2021-11-19 04:26, slack,- +2021-11-19 10:59, flood,42 +2021-11-19 12:50, slack,- +2021-11-19 14:57, ebb,-29 +2021-11-19 17:02, slack,- +2021-11-19 23:23, flood,45 +2021-11-20 01:20, slack,- +2021-11-20 02:57, ebb,-27 +2021-11-20 05:02, slack,- +2021-11-20 11:29, flood,38 +2021-11-20 13:32, slack,- +2021-11-20 15:21, ebb,-29 +2021-11-20 17:44, slack,- +2021-11-20 23:59, flood,42 +2021-11-21 02:02, slack,- +2021-11-21 03:33, ebb,-27 +2021-11-21 05:44, slack,- +2021-11-21 10:29, flood,37 +2021-11-21 14:14, slack,- +2021-11-21 15:57, ebb,-29 +2021-11-21 18:26, slack,- +2021-11-22 00:23, flood,39 +2021-11-22 02:44, slack,- +2021-11-22 04:15, ebb,-26 +2021-11-22 06:26, slack,- +2021-11-22 11:05, flood,37 +2021-11-22 14:56, slack,- +2021-11-22 16:33, ebb,-29 +2021-11-22 19:08, slack,- +2021-11-23 00:05, flood,38 +2021-11-23 03:32, slack,- +2021-11-23 04:57, ebb,-26 +2021-11-23 07:08, slack,- +2021-11-23 11:47, flood,36 +2021-11-23 15:32, slack,- +2021-11-23 17:15, ebb,-29 +2021-11-23 19:50, slack,- +2021-11-24 00:53, flood,37 +2021-11-24 04:14, slack,- +2021-11-24 05:39, ebb,-26 +2021-11-24 07:56, slack,- +2021-11-24 12:41, flood,35 +2021-11-24 16:14, slack,- +2021-11-24 17:57, ebb,-29 +2021-11-24 20:38, slack,- +2021-11-25 01:35, flood,37 +2021-11-25 04:56, slack,- +2021-11-25 06:27, ebb,-26 +2021-11-25 08:44, slack,- +2021-11-25 13:29, flood,36 +2021-11-25 16:56, slack,- +2021-11-25 18:45, ebb,-29 +2021-11-25 21:26, slack,- +2021-11-26 02:23, flood,38 +2021-11-26 05:44, slack,- +2021-11-26 07:15, ebb,-26 +2021-11-26 09:32, slack,- +2021-11-26 14:23, flood,37 +2021-11-26 17:44, slack,- +2021-11-26 19:33, ebb,-29 +2021-11-26 22:14, slack,- +2021-11-27 03:11, flood,40 +2021-11-27 06:32, slack,- +2021-11-27 08:03, ebb,-26 +2021-11-27 10:26, slack,- +2021-11-27 15:11, flood,39 +2021-11-27 18:32, slack,- +2021-11-27 20:21, ebb,-29 +2021-11-27 23:02, slack,- +2021-11-28 03:53, flood,42 +2021-11-28 07:20, slack,- +2021-11-28 08:51, ebb,-27 +2021-11-28 11:20, slack,- +2021-11-28 16:05, flood,41 +2021-11-28 19:26, slack,- +2021-11-28 21:15, ebb,-29 +2021-11-28 23:50, slack,- +2021-11-29 04:41, flood,44 +2021-11-29 08:08, slack,- +2021-11-29 09:51, ebb,-28 +2021-11-29 12:14, slack,- +2021-11-29 16:59, flood,43 +2021-11-29 20:26, slack,- +2021-11-29 22:09, ebb,-30 +2021-11-30 00:38, slack,- +2021-11-30 05:29, flood,47 +2021-11-30 09:02, slack,- +2021-11-30 10:45, ebb,-31 +2021-11-30 13:02, slack,- +2021-11-30 17:53, flood,46 +2021-11-30 21:20, slack,- +2021-11-30 23:09, ebb,-32 +2021-12-01 01:26, slack,- +2021-12-01 06:17, flood,50 +2021-12-01 09:56, slack,- +2021-12-01 11:39, ebb,-34 +2021-12-01 13:56, slack,- +2021-12-01 18:47, flood,48 +2021-12-01 22:20, slack,- +2021-12-02 00:03, ebb,-34 +2021-12-02 02:14, slack,- +2021-12-02 07:11, flood,52 +2021-12-02 10:44, slack,- +2021-12-02 12:33, ebb,-37 +2021-12-02 14:50, slack,- +2021-12-02 19:47, flood,50 +2021-12-02 23:14, slack,- +2021-12-03 00:57, ebb,-35 +2021-12-03 03:02, slack,- +2021-12-03 07:59, flood,53 +2021-12-03 11:38, slack,- +2021-12-03 13:21, ebb,-39 +2021-12-03 15:44, slack,- +2021-12-03 20:47, flood,51 +2021-12-04 00:08, slack,- +2021-12-04 01:51, ebb,-35 +2021-12-04 03:50, slack,- +2021-12-04 08:59, flood,53 +2021-12-04 12:26, slack,- +2021-12-04 14:15, ebb,-40 +2021-12-04 16:32, slack,- +2021-12-04 21:53, flood,52 +2021-12-05 01:02, slack,- +2021-12-05 02:39, ebb,-36 +2021-12-05 04:44, slack,- +2021-12-05 09:53, flood,53 +2021-12-05 13:20, slack,- +2021-12-05 15:09, ebb,-40 +2021-12-05 17:26, slack,- +2021-12-05 22:53, flood,52 +2021-12-06 01:56, slack,- +2021-12-06 03:33, ebb,-35 +2021-12-06 05:38, slack,- +2021-12-06 10:41, flood,51 +2021-12-06 14:08, slack,- +2021-12-06 15:57, ebb,-39 +2021-12-06 18:20, slack,- +2021-12-07 00:17, flood,50 +2021-12-07 02:56, slack,- +2021-12-07 04:27, ebb,-34 +2021-12-07 06:32, slack,- +2021-12-07 11:41, flood,47 +2021-12-07 15:08, slack,- +2021-12-07 16:45, ebb,-37 +2021-12-07 19:14, slack,- +2021-12-08 01:41, flood,49 +2021-12-08 03:50, slack,- +2021-12-08 05:21, ebb,-32 +2021-12-08 07:32, slack,- +2021-12-08 12:53, flood,43 +2021-12-08 16:02, slack,- +2021-12-08 17:39, ebb,-35 +2021-12-08 20:14, slack,- +2021-12-09 02:41, flood,49 +2021-12-09 04:44, slack,- +2021-12-09 06:09, ebb,-30 +2021-12-09 08:32, slack,- +2021-12-09 14:41, flood,42 +2021-12-09 16:56, slack,- +2021-12-09 18:33, ebb,-32 +2021-12-09 21:14, slack,- +2021-12-10 03:41, flood,48 +2021-12-10 05:38, slack,- +2021-12-10 07:03, ebb,-27 +2021-12-10 09:38, slack,- +2021-12-10 15:47, flood,41 +2021-12-10 17:50, slack,- +2021-12-10 19:27, ebb,-29 +2021-12-10 22:14, slack,- +2021-12-11 04:35, flood,47 +2021-12-11 06:32, slack,- +2021-12-11 08:03, ebb,-25 +2021-12-11 10:44, slack,- +2021-12-11 16:53, flood,40 +2021-12-11 18:50, slack,- +2021-12-11 20:21, ebb,-26 +2021-12-11 23:14, slack,- +2021-12-12 05:35, flood,45 +2021-12-12 07:26, slack,- +2021-12-12 09:09, ebb,-23 +2021-12-12 11:44, slack,- +2021-12-12 17:53, flood,40 +2021-12-12 19:44, slack,- +2021-12-12 21:21, ebb,-24 +2021-12-13 00:08, slack,- +2021-12-13 06:29, flood,44 +2021-12-13 08:26, slack,- +2021-12-13 11:27, ebb,-25 +2021-12-13 12:38, slack,- +2021-12-13 18:53, flood,40 +2021-12-13 20:44, slack,- +2021-12-13 23:45, ebb,-23 +2021-12-14 00:56, slack,- +2021-12-14 07:23, flood,43 +2021-12-14 09:20, slack,- +2021-12-14 12:21, ebb,-26 +2021-12-14 13:32, slack,- +2021-12-14 19:47, flood,41 +2021-12-14 21:38, slack,- +2021-12-15 00:27, ebb,-24 +2021-12-15 01:44, slack,- +2021-12-15 08:17, flood,42 +2021-12-15 10:08, slack,- +2021-12-15 13:03, ebb,-27 +2021-12-15 14:20, slack,- +2021-12-15 20:41, flood,42 +2021-12-15 22:32, slack,- +2021-12-16 01:03, ebb,-24 +2021-12-16 02:26, slack,- +2021-12-16 09:05, flood,41 +2021-12-16 10:56, slack,- +2021-12-16 13:51, ebb,-28 +2021-12-16 15:08, slack,- +2021-12-16 21:29, flood,43 +2021-12-16 23:20, slack,- +2021-12-17 01:21, ebb,-24 +2021-12-17 03:08, slack,- +2021-12-17 09:53, flood,40 +2021-12-17 11:38, slack,- +2021-12-17 14:21, ebb,-28 +2021-12-17 15:50, slack,- +2021-12-17 22:17, flood,44 +2021-12-18 00:08, slack,- +2021-12-18 01:51, ebb,-24 +2021-12-18 03:50, slack,- +2021-12-18 10:29, flood,37 +2021-12-18 12:20, slack,- +2021-12-18 14:21, ebb,-28 +2021-12-18 16:38, slack,- +2021-12-18 22:53, flood,43 +2021-12-19 00:50, slack,- +2021-12-19 02:27, ebb,-25 +2021-12-19 04:32, slack,- +2021-12-19 09:23, flood,36 +2021-12-19 13:02, slack,- +2021-12-19 14:51, ebb,-29 +2021-12-19 17:20, slack,- +2021-12-19 23:29, flood,41 +2021-12-20 01:38, slack,- +2021-12-20 03:09, ebb,-26 +2021-12-20 05:14, slack,- +2021-12-20 09:59, flood,38 +2021-12-20 13:44, slack,- +2021-12-20 15:27, ebb,-29 +2021-12-20 17:56, slack,- +2021-12-20 23:05, flood,40 +2021-12-21 02:20, slack,- +2021-12-21 03:45, ebb,-27 +2021-12-21 05:56, slack,- +2021-12-21 10:41, flood,39 +2021-12-21 14:20, slack,- +2021-12-21 16:03, ebb,-30 +2021-12-21 18:38, slack,- +2021-12-21 23:35, flood,40 +2021-12-22 03:02, slack,- +2021-12-22 04:27, ebb,-28 +2021-12-22 06:38, slack,- +2021-12-22 11:23, flood,40 +2021-12-22 15:02, slack,- +2021-12-22 16:45, ebb,-31 +2021-12-22 19:20, slack,- +2021-12-23 00:11, flood,40 +2021-12-23 03:44, slack,- +2021-12-23 05:15, ebb,-28 +2021-12-23 07:20, slack,- +2021-12-23 12:11, flood,40 +2021-12-23 15:44, slack,- +2021-12-23 17:33, ebb,-32 +2021-12-23 19:56, slack,- +2021-12-24 00:59, flood,41 +2021-12-24 04:26, slack,- +2021-12-24 05:57, ebb,-29 +2021-12-24 08:08, slack,- +2021-12-24 12:59, flood,41 +2021-12-24 16:26, slack,- +2021-12-24 18:15, ebb,-33 +2021-12-24 20:44, slack,- +2021-12-25 01:47, flood,43 +2021-12-25 05:08, slack,- +2021-12-25 06:45, ebb,-30 +2021-12-25 08:56, slack,- +2021-12-25 13:53, flood,42 +2021-12-25 17:14, slack,- +2021-12-25 19:03, ebb,-33 +2021-12-25 21:32, slack,- +2021-12-26 02:35, flood,45 +2021-12-26 05:56, slack,- +2021-12-26 07:33, ebb,-30 +2021-12-26 09:50, slack,- +2021-12-26 14:47, flood,43 +2021-12-26 18:02, slack,- +2021-12-26 19:51, ebb,-33 +2021-12-26 22:20, slack,- +2021-12-27 03:17, flood,47 +2021-12-27 06:44, slack,- +2021-12-27 08:21, ebb,-31 +2021-12-27 10:50, slack,- +2021-12-27 15:35, flood,44 +2021-12-27 18:56, slack,- +2021-12-27 20:45, ebb,-32 +2021-12-27 23:08, slack,- +2021-12-28 04:05, flood,48 +2021-12-28 07:32, slack,- +2021-12-28 09:15, ebb,-31 +2021-12-28 11:44, slack,- +2021-12-28 16:35, flood,45 +2021-12-28 19:56, slack,- +2021-12-28 21:45, ebb,-31 +2021-12-29 00:02, slack,- +2021-12-29 04:59, flood,49 +2021-12-29 08:26, slack,- +2021-12-29 10:15, ebb,-32 +2021-12-29 12:38, slack,- +2021-12-29 17:29, flood,45 +2021-12-29 20:56, slack,- +2021-12-29 22:39, ebb,-31 +2021-12-30 00:50, slack,- +2021-12-30 05:47, flood,50 +2021-12-30 09:26, slack,- +2021-12-30 11:15, ebb,-34 +2021-12-30 13:38, slack,- +2021-12-30 18:29, flood,46 +2021-12-30 22:02, slack,- +2021-12-30 23:39, ebb,-32 +2021-12-31 01:44, slack,- +2021-12-31 06:41, flood,50 +2021-12-31 10:20, slack,- +2021-12-31 12:09, ebb,-36 +2021-12-31 14:32, slack,- +2021-12-31 19:35, flood,46 +2021-12-31 22:56, slack,- +2022-01-01 00:33, ebb,-31 +2022-01-01 02:38, slack,- +2022-01-01 07:41, flood,47 +2022-01-01 11:14, slack,- +2022-01-01 13:03, ebb,-36 +2022-01-01 15:32, slack,- +2022-01-01 21:29, flood,46 +2022-01-01 23:56, slack,- +2022-01-02 01:27, ebb,-31 +2022-01-02 03:38, slack,- +2022-01-02 08:47, flood,47 +2022-01-02 12:08, slack,- +2022-01-02 13:57, ebb,-37 +2022-01-02 16:26, slack,- +2022-01-02 22:41, flood,50 +2022-01-03 00:50, slack,- +2022-01-03 02:27, ebb,-32 +2022-01-03 04:32, slack,- +2022-01-03 09:53, flood,47 +2022-01-03 13:02, slack,- +2022-01-03 14:51, ebb,-37 +2022-01-03 17:20, slack,- +2022-01-03 23:35, flood,52 +2022-01-04 01:44, slack,- +2022-01-04 03:21, ebb,-32 +2022-01-04 05:26, slack,- +2022-01-04 11:05, flood,47 +2022-01-04 13:56, slack,- +2022-01-04 15:39, ebb,-36 +2022-01-04 18:14, slack,- +2022-01-05 00:35, flood,53 +2022-01-05 02:38, slack,- +2022-01-05 04:15, ebb,-31 +2022-01-05 06:26, slack,- +2022-01-05 12:29, flood,46 +2022-01-05 14:50, slack,- +2022-01-05 16:33, ebb,-35 +2022-01-05 19:08, slack,- +2022-01-06 01:35, flood,53 +2022-01-06 03:32, slack,- +2022-01-06 05:03, ebb,-30 +2022-01-06 07:20, slack,- +2022-01-06 13:41, flood,46 +2022-01-06 15:44, slack,- +2022-01-06 17:21, ebb,-33 +2022-01-06 20:02, slack,- +2022-01-07 02:23, flood,52 +2022-01-07 04:26, slack,- +2022-01-07 05:57, ebb,-29 +2022-01-07 08:20, slack,- +2022-01-07 14:35, flood,45 +2022-01-07 16:38, slack,- +2022-01-07 18:15, ebb,-31 +2022-01-07 20:56, slack,- +2022-01-08 03:17, flood,50 +2022-01-08 05:14, slack,- +2022-01-08 06:51, ebb,-27 +2022-01-08 09:20, slack,- +2022-01-08 15:29, flood,43 +2022-01-08 17:32, slack,- +2022-01-08 19:03, ebb,-28 +2022-01-08 21:50, slack,- +2022-01-09 04:11, flood,47 +2022-01-09 06:08, slack,- +2022-01-09 07:45, ebb,-25 +2022-01-09 10:20, slack,- +2022-01-09 16:23, flood,41 +2022-01-09 18:26, slack,- +2022-01-09 19:57, ebb,-26 +2022-01-09 22:44, slack,- +2022-01-10 04:59, flood,44 +2022-01-10 07:02, slack,- +2022-01-10 08:39, ebb,-24 +2022-01-10 11:14, slack,- +2022-01-10 17:23, flood,39 +2022-01-10 19:20, slack,- +2022-01-10 20:51, ebb,-23 +2022-01-10 23:32, slack,- +2022-01-11 05:53, flood,41 +2022-01-11 07:50, slack,- +2022-01-11 11:03, ebb,-24 +2022-01-11 12:08, slack,- +2022-01-11 18:17, flood,38 +2022-01-11 20:14, slack,- +2022-01-11 21:51, ebb,-21 +2022-01-12 00:20, slack,- +2022-01-12 06:47, flood,38 +2022-01-12 08:44, slack,- +2022-01-12 11:57, ebb,-25 +2022-01-12 13:02, slack,- +2022-01-12 19:11, flood,38 +2022-01-12 21:08, slack,- +2022-01-12 23:57, ebb,-21 +2022-01-13 01:08, slack,- +2022-01-13 07:41, flood,36 +2022-01-13 09:32, slack,- +2022-01-13 12:39, ebb,-26 +2022-01-13 13:50, slack,- +2022-01-13 20:11, flood,38 +2022-01-13 22:02, slack,- +2022-01-14 00:27, ebb,-22 +2022-01-14 01:50, slack,- +2022-01-14 08:29, flood,35 +2022-01-14 10:20, slack,- +2022-01-14 13:21, ebb,-26 +2022-01-14 14:38, slack,- +2022-01-14 20:59, flood,39 +2022-01-14 22:50, slack,- +2022-01-15 00:45, ebb,-22 +2022-01-15 02:38, slack,- +2022-01-15 07:23, flood,33 +2022-01-15 09:17, flood,34 +2022-01-15 11:08, slack,- +2022-01-15 13:51, ebb,-26 +2022-01-15 15:20, slack,- +2022-01-15 21:47, flood,40 +2022-01-15 23:38, slack,- +2022-01-16 01:15, ebb,-23 +2022-01-16 03:20, slack,- +2022-01-16 07:59, flood,35 +2022-01-16 11:50, slack,- +2022-01-16 13:39, ebb,-27 +2022-01-16 16:02, slack,- +2022-01-16 22:23, flood,40 +2022-01-17 00:20, slack,- +2022-01-17 01:57, ebb,-25 +2022-01-17 04:02, slack,- +2022-01-17 08:47, flood,37 +2022-01-17 12:26, slack,- +2022-01-17 14:15, ebb,-29 +2022-01-17 16:44, slack,- +2022-01-17 22:41, flood,40 +2022-01-18 01:02, slack,- +2022-01-18 02:33, ebb,-26 +2022-01-18 04:44, slack,- +2022-01-18 09:29, flood,41 +2022-01-18 13:08, slack,- +2022-01-18 14:57, ebb,-31 +2022-01-18 17:26, slack,- +2022-01-18 22:23, flood,42 +2022-01-19 01:44, slack,- +2022-01-19 03:15, ebb,-28 +2022-01-19 05:20, slack,- +2022-01-19 10:11, flood,44 +2022-01-19 13:44, slack,- +2022-01-19 15:33, ebb,-33 +2022-01-19 18:02, slack,- +2022-01-19 22:59, flood,44 +2022-01-20 02:26, slack,- +2022-01-20 03:57, ebb,-30 +2022-01-20 06:02, slack,- +2022-01-20 10:53, flood,46 +2022-01-20 14:26, slack,- +2022-01-20 16:15, ebb,-34 +2022-01-20 18:38, slack,- +2022-01-20 23:35, flood,45 +2022-01-21 03:08, slack,- +2022-01-21 04:45, ebb,-32 +2022-01-21 06:44, slack,- +2022-01-21 11:41, flood,47 +2022-01-21 15:08, slack,- +2022-01-21 17:03, ebb,-36 +2022-01-21 19:14, slack,- +2022-01-22 00:17, flood,47 +2022-01-22 03:50, slack,- +2022-01-22 05:27, ebb,-33 +2022-01-22 07:26, slack,- +2022-01-22 12:29, flood,47 +2022-01-22 15:50, slack,- +2022-01-22 17:45, ebb,-36 +2022-01-22 19:56, slack,- +2022-01-23 01:05, flood,47 +2022-01-23 04:32, slack,- +2022-01-23 06:15, ebb,-33 +2022-01-23 08:14, slack,- +2022-01-23 13:23, flood,46 +2022-01-23 16:38, slack,- +2022-01-23 18:33, ebb,-36 +2022-01-23 20:44, slack,- +2022-01-24 01:53, flood,48 +2022-01-24 05:14, slack,- +2022-01-24 07:03, ebb,-34 +2022-01-24 09:08, slack,- +2022-01-24 14:17, flood,45 +2022-01-24 17:32, slack,- +2022-01-24 19:27, ebb,-35 +2022-01-24 21:32, slack,- +2022-01-25 02:41, flood,48 +2022-01-25 06:08, slack,- +2022-01-25 07:51, ebb,-33 +2022-01-25 10:14, slack,- +2022-01-25 15:11, flood,44 +2022-01-25 18:26, slack,- +2022-01-25 20:15, ebb,-32 +2022-01-25 22:26, slack,- +2022-01-26 03:35, flood,48 +2022-01-26 06:56, slack,- +2022-01-26 08:45, ebb,-32 +2022-01-26 11:14, slack,- +2022-01-26 16:05, flood,42 +2022-01-26 19:32, slack,- +2022-01-26 21:15, ebb,-30 +2022-01-26 23:26, slack,- +2022-01-27 04:23, flood,46 +2022-01-27 07:56, slack,- +2022-01-27 09:45, ebb,-32 +2022-01-27 12:20, slack,- +2022-01-27 17:11, flood,41 +2022-01-27 20:38, slack,- +2022-01-27 22:15, ebb,-28 +2022-01-28 00:26, slack,- +2022-01-28 05:23, flood,45 +2022-01-28 08:56, slack,- +2022-01-28 10:45, ebb,-32 +2022-01-28 13:20, slack,- +2022-01-28 18:23, flood,40 +2022-01-28 21:44, slack,- +2022-01-28 23:15, ebb,-28 +2022-01-29 01:26, slack,- +2022-01-29 06:23, flood,44 +2022-01-29 10:02, slack,- +2022-01-29 11:45, ebb,-32 +2022-01-29 14:20, slack,- +2022-01-29 20:35, flood,43 +2022-01-29 22:44, slack,- +2022-01-30 00:15, ebb,-29 +2022-01-30 02:26, slack,- +2022-01-30 07:35, flood,44 +2022-01-30 11:02, slack,- +2022-01-30 12:45, ebb,-33 +2022-01-30 15:20, slack,- +2022-01-30 21:41, flood,49 +2022-01-30 23:44, slack,- +2022-01-31 01:15, ebb,-29 +2022-01-31 03:26, slack,- +2022-01-31 09:35, flood,46 +2022-01-31 11:56, slack,- +2022-01-31 13:45, ebb,-34 +2022-01-31 16:14, slack,- +2022-01-31 22:35, flood,53 +2022-02-01 00:38, slack,- +2022-02-01 02:15, ebb,-30 +2022-02-01 04:20, slack,- +2022-02-01 10:41, flood,49 +2022-02-01 12:50, slack,- +2022-02-01 14:39, ebb,-35 +2022-02-01 17:08, slack,- +2022-02-01 23:29, flood,56 +2022-02-02 01:32, slack,- +2022-02-02 03:09, ebb,-31 +2022-02-02 05:20, slack,- +2022-02-02 11:29, flood,51 +2022-02-02 13:44, slack,- +2022-02-02 15:33, ebb,-35 +2022-02-02 18:02, slack,- +2022-02-03 00:17, flood,56 +2022-02-03 02:20, slack,- +2022-02-03 04:03, ebb,-31 +2022-02-03 06:08, slack,- +2022-02-03 12:29, flood,50 +2022-02-03 14:32, slack,- +2022-02-03 16:21, ebb,-34 +2022-02-03 18:50, slack,- +2022-02-04 01:11, flood,54 +2022-02-04 03:08, slack,- +2022-02-04 04:51, ebb,-31 +2022-02-04 07:02, slack,- +2022-02-04 13:23, flood,49 +2022-02-04 15:26, slack,- +2022-02-04 17:03, ebb,-33 +2022-02-04 19:38, slack,- +2022-02-05 01:59, flood,52 +2022-02-05 04:02, slack,- +2022-02-05 05:39, ebb,-30 +2022-02-05 07:56, slack,- +2022-02-05 14:11, flood,46 +2022-02-05 16:14, slack,- +2022-02-05 17:51, ebb,-31 +2022-02-05 20:26, slack,- +2022-02-06 02:47, flood,48 +2022-02-06 04:50, slack,- +2022-02-06 06:27, ebb,-28 +2022-02-06 08:50, slack,- +2022-02-06 15:05, flood,43 +2022-02-06 17:02, slack,- +2022-02-06 18:39, ebb,-28 +2022-02-06 21:14, slack,- +2022-02-07 03:35, flood,44 +2022-02-07 05:38, slack,- +2022-02-07 07:09, ebb,-26 +2022-02-07 09:50, slack,- +2022-02-07 15:53, flood,40 +2022-02-07 17:56, slack,- +2022-02-07 19:27, ebb,-26 +2022-02-07 22:02, slack,- +2022-02-08 04:29, flood,39 +2022-02-08 06:26, slack,- +2022-02-08 07:57, ebb,-24 +2022-02-08 10:44, slack,- +2022-02-08 16:47, flood,37 +2022-02-08 18:44, slack,- +2022-02-08 20:15, ebb,-23 +2022-02-08 22:56, slack,- +2022-02-09 05:23, flood,35 +2022-02-09 07:14, slack,- +2022-02-09 08:51, ebb,-22 +2022-02-09 11:38, slack,- +2022-02-09 17:47, flood,35 +2022-02-09 19:38, slack,- +2022-02-09 21:03, ebb,-20 +2022-02-09 23:44, slack,- +2022-02-10 06:11, flood,32 +2022-02-10 08:02, slack,- +2022-02-10 11:27, ebb,-22 +2022-02-10 12:32, slack,- +2022-02-10 18:41, flood,34 +2022-02-10 20:32, slack,- +2022-02-10 22:09, ebb,-19 +2022-02-11 00:32, slack,- +2022-02-11 05:11, flood,30 +2022-02-11 06:59, flood,30 +2022-02-11 08:56, slack,- +2022-02-11 12:15, ebb,-23 +2022-02-11 13:20, slack,- +2022-02-11 19:35, flood,34 +2022-02-11 21:26, slack,- +2022-02-11 23:09, ebb,-20 +2022-02-12 01:20, slack,- +2022-02-12 05:53, flood,30 +2022-02-12 07:11, flood,29 +2022-02-12 09:44, slack,- +2022-02-12 12:57, ebb,-23 +2022-02-12 14:08, slack,- +2022-02-12 20:29, flood,35 +2022-02-12 22:20, slack,- +2022-02-12 23:57, ebb,-21 +2022-02-13 02:08, slack,- +2022-02-13 06:41, flood,32 +2022-02-13 10:32, slack,- +2022-02-13 12:27, ebb,-24 +2022-02-13 14:50, slack,- +2022-02-13 21:11, flood,36 +2022-02-13 23:08, slack,- +2022-02-14 00:39, ebb,-23 +2022-02-14 02:50, slack,- +2022-02-14 07:23, flood,35 +2022-02-14 11:14, slack,- +2022-02-14 13:03, ebb,-26 +2022-02-14 15:32, slack,- +2022-02-14 21:47, flood,38 +2022-02-14 23:50, slack,- +2022-02-15 01:21, ebb,-25 +2022-02-15 03:32, slack,- +2022-02-15 08:17, flood,39 +2022-02-15 11:56, slack,- +2022-02-15 13:39, ebb,-29 +2022-02-15 16:14, slack,- +2022-02-15 21:23, flood,40 +2022-02-16 00:32, slack,- +2022-02-16 02:03, ebb,-28 +2022-02-16 04:14, slack,- +2022-02-16 08:59, flood,43 +2022-02-16 12:38, slack,- +2022-02-16 14:21, ebb,-32 +2022-02-16 16:50, slack,- +2022-02-16 21:47, flood,44 +2022-02-17 01:08, slack,- +2022-02-17 02:45, ebb,-30 +2022-02-17 04:50, slack,- +2022-02-17 09:47, flood,47 +2022-02-17 13:14, slack,- +2022-02-17 15:09, ebb,-34 +2022-02-17 17:26, slack,- +2022-02-17 22:23, flood,48 +2022-02-18 01:50, slack,- +2022-02-18 03:33, ebb,-33 +2022-02-18 05:32, slack,- +2022-02-18 10:29, flood,50 +2022-02-18 13:56, slack,- +2022-02-18 15:51, ebb,-37 +2022-02-18 18:02, slack,- +2022-02-18 23:05, flood,50 +2022-02-19 02:32, slack,- +2022-02-19 04:15, ebb,-35 +2022-02-19 06:14, slack,- +2022-02-19 11:11, flood,51 +2022-02-19 14:38, slack,- +2022-02-19 16:33, ebb,-38 +2022-02-19 18:38, slack,- +2022-02-19 23:47, flood,51 +2022-02-20 03:14, slack,- +2022-02-20 05:03, ebb,-36 +2022-02-20 06:56, slack,- +2022-02-20 11:59, flood,51 +2022-02-20 15:26, slack,- +2022-02-20 17:21, ebb,-38 +2022-02-20 19:20, slack,- +2022-02-21 00:29, flood,51 +2022-02-21 03:56, slack,- +2022-02-21 05:45, ebb,-37 +2022-02-21 07:44, slack,- +2022-02-21 12:53, flood,49 +2022-02-21 16:14, slack,- +2022-02-21 18:09, ebb,-37 +2022-02-21 20:08, slack,- +2022-02-22 01:23, flood,50 +2022-02-22 04:44, slack,- +2022-02-22 06:39, ebb,-36 +2022-02-22 08:44, slack,- +2022-02-22 13:53, flood,46 +2022-02-22 17:08, slack,- +2022-02-22 19:03, ebb,-35 +2022-02-22 21:02, slack,- +2022-02-23 02:17, flood,48 +2022-02-23 05:38, slack,- +2022-02-23 07:27, ebb,-35 +2022-02-23 09:50, slack,- +2022-02-23 14:53, flood,44 +2022-02-23 18:08, slack,- +2022-02-23 19:51, ebb,-32 +2022-02-23 22:02, slack,- +2022-02-24 03:11, flood,46 +2022-02-24 06:32, slack,- +2022-02-24 08:21, ebb,-33 +2022-02-24 10:56, slack,- +2022-02-24 15:53, flood,41 +2022-02-24 19:14, slack,- +2022-02-24 20:51, ebb,-29 +2022-02-24 23:08, slack,- +2022-02-25 04:05, flood,43 +2022-02-25 07:32, slack,- +2022-02-25 09:21, ebb,-31 +2022-02-25 12:02, slack,- +2022-02-25 17:05, flood,39 +2022-02-25 20:20, slack,- +2022-02-25 21:51, ebb,-27 +2022-02-26 00:14, slack,- +2022-02-26 05:05, flood,41 +2022-02-26 08:38, slack,- +2022-02-26 10:21, ebb,-30 +2022-02-26 13:08, slack,- +2022-02-26 19:11, flood,40 +2022-02-26 21:26, slack,- +2022-02-26 22:57, ebb,-26 +2022-02-27 01:14, slack,- +2022-02-27 06:23, flood,41 +2022-02-27 09:44, slack,- +2022-02-27 11:33, ebb,-30 +2022-02-27 14:08, slack,- +2022-02-27 20:29, flood,45 +2022-02-27 22:26, slack,- +2022-02-28 00:09, ebb,-27 +2022-02-28 02:20, slack,- +2022-02-28 08:41, flood,44 +2022-02-28 10:44, slack,- +2022-02-28 12:33, ebb,-31 +2022-02-28 15:08, slack,- +2022-02-28 21:29, flood,52 +2022-02-28 23:26, slack,- +2022-03-01 01:09, ebb,-29 +2022-03-01 03:14, slack,- +2022-03-01 09:41, flood,49 +2022-03-01 11:44, slack,- +2022-03-01 13:33, ebb,-32 +2022-03-01 16:02, slack,- +2022-03-01 22:23, flood,56 +2022-03-02 00:20, slack,- +2022-03-02 02:09, ebb,-30 +2022-03-02 04:08, slack,- +2022-03-02 10:29, flood,53 +2022-03-02 12:32, slack,- +2022-03-02 14:33, ebb,-33 +2022-03-02 16:50, slack,- +2022-03-02 23:11, flood,58 +2022-03-03 01:08, slack,- +2022-03-03 03:03, ebb,-31 +2022-03-03 05:02, slack,- +2022-03-03 11:17, flood,54 +2022-03-03 13:26, slack,- +2022-03-03 15:21, ebb,-34 +2022-03-03 17:38, slack,- +2022-03-03 23:59, flood,57 +2022-03-04 01:56, slack,- +2022-03-04 03:51, ebb,-32 +2022-03-04 05:50, slack,- +2022-03-04 12:05, flood,53 +2022-03-04 14:14, slack,- +2022-03-04 16:03, ebb,-33 +2022-03-04 18:20, slack,- +2022-03-05 00:47, flood,54 +2022-03-05 02:44, slack,- +2022-03-05 04:33, ebb,-31 +2022-03-05 06:38, slack,- +2022-03-05 12:59, flood,50 +2022-03-05 15:02, slack,- +2022-03-05 16:45, ebb,-32 +2022-03-05 19:08, slack,- +2022-03-06 01:35, flood,49 +2022-03-06 03:32, slack,- +2022-03-06 05:15, ebb,-30 +2022-03-06 07:32, slack,- +2022-03-06 13:47, flood,46 +2022-03-06 15:50, slack,- +2022-03-06 17:27, ebb,-30 +2022-03-06 19:50, slack,- +2022-03-07 02:17, flood,44 +2022-03-07 04:14, slack,- +2022-03-07 05:57, ebb,-29 +2022-03-07 08:20, slack,- +2022-03-07 14:35, flood,42 +2022-03-07 16:38, slack,- +2022-03-07 18:09, ebb,-28 +2022-03-07 20:32, slack,- +2022-03-08 03:05, flood,39 +2022-03-08 05:02, slack,- +2022-03-08 06:39, ebb,-27 +2022-03-08 09:14, slack,- +2022-03-08 15:23, flood,39 +2022-03-08 17:26, slack,- +2022-03-08 18:51, ebb,-25 +2022-03-08 21:26, slack,- +2022-03-09 03:41, flood,34 +2022-03-09 05:50, slack,- +2022-03-09 07:21, ebb,-25 +2022-03-09 10:08, slack,- +2022-03-09 16:17, flood,35 +2022-03-09 18:14, slack,- +2022-03-09 19:39, ebb,-23 +2022-03-09 22:14, slack,- +2022-03-10 03:11, flood,31 +2022-03-10 06:38, slack,- +2022-03-10 08:09, ebb,-23 +2022-03-10 11:02, slack,- +2022-03-10 17:05, flood,32 +2022-03-10 19:08, slack,- +2022-03-10 20:27, ebb,-20 +2022-03-10 23:08, slack,- +2022-03-11 03:47, flood,30 +2022-03-11 07:26, slack,- +2022-03-11 08:57, ebb,-21 +2022-03-11 11:56, slack,- +2022-03-11 17:59, flood,31 +2022-03-11 20:02, slack,- +2022-03-11 21:21, ebb,-19 +2022-03-11 23:56, slack,- +2022-03-12 04:29, flood,29 +2022-03-12 08:14, slack,- +2022-03-12 09:51, ebb,-20 +2022-03-12 11:51, ebb,-20 +2022-03-12 12:44, slack,- +2022-03-12 18:53, flood,31 +2022-03-12 20:50, slack,- +2022-03-12 22:21, ebb,-19 +2022-03-13 00:50, slack,- +2022-03-13 05:17, flood,31 +2022-03-13 09:08, slack,- +2022-03-13 10:51, ebb,-21 +2022-03-13 13:32, slack,- +2022-03-13 19:41, flood,32 +2022-03-13 21:44, slack,- +2022-03-13 23:15, ebb,-21 +2022-03-14 01:32, slack,- +2022-03-14 06:05, flood,33 +2022-03-14 09:56, slack,- +2022-03-14 11:39, ebb,-24 +2022-03-14 14:14, slack,- +2022-03-14 19:05, flood,34 +2022-03-14 22:32, slack,- +2022-03-15 00:03, ebb,-23 +2022-03-15 02:20, slack,- +2022-03-15 06:53, flood,37 +2022-03-15 10:44, slack,- +2022-03-15 12:27, ebb,-26 +2022-03-15 14:56, slack,- +2022-03-15 19:47, flood,37 +2022-03-15 23:14, slack,- +2022-03-16 00:51, ebb,-26 +2022-03-16 03:02, slack,- +2022-03-16 07:41, flood,41 +2022-03-16 11:26, slack,- +2022-03-16 13:09, ebb,-30 +2022-03-16 15:38, slack,- +2022-03-16 20:29, flood,42 +2022-03-16 23:56, slack,- +2022-03-17 01:33, ebb,-29 +2022-03-17 03:44, slack,- +2022-03-17 08:29, flood,46 +2022-03-17 12:08, slack,- +2022-03-17 13:51, ebb,-33 +2022-03-17 16:14, slack,- +2022-03-17 21:11, flood,47 +2022-03-18 00:38, slack,- +2022-03-18 02:15, ebb,-33 +2022-03-18 04:20, slack,- +2022-03-18 09:17, flood,50 +2022-03-18 12:50, slack,- +2022-03-18 14:39, ebb,-36 +2022-03-18 16:50, slack,- +2022-03-18 21:53, flood,51 +2022-03-19 01:20, slack,- +2022-03-19 03:03, ebb,-35 +2022-03-19 05:02, slack,- +2022-03-19 10:05, flood,53 +2022-03-19 13:32, slack,- +2022-03-19 15:21, ebb,-38 +2022-03-19 17:26, slack,- +2022-03-19 22:29, flood,53 +2022-03-20 02:02, slack,- +2022-03-20 03:51, ebb,-38 +2022-03-20 05:50, slack,- +2022-03-20 10:47, flood,54 +2022-03-20 14:20, slack,- +2022-03-20 16:09, ebb,-39 +2022-03-20 18:08, slack,- +2022-03-20 23:11, flood,54 +2022-03-21 02:44, slack,- +2022-03-21 04:33, ebb,-39 +2022-03-21 06:32, slack,- +2022-03-21 11:35, flood,52 +2022-03-21 15:08, slack,- +2022-03-21 16:57, ebb,-38 +2022-03-21 18:50, slack,- +2022-03-21 23:59, flood,53 +2022-03-22 03:32, slack,- +2022-03-22 05:21, ebb,-39 +2022-03-22 07:26, slack,- +2022-03-22 12:35, flood,49 +2022-03-22 16:02, slack,- +2022-03-22 17:45, ebb,-37 +2022-03-22 19:38, slack,- +2022-03-23 00:53, flood,50 +2022-03-23 04:20, slack,- +2022-03-23 06:09, ebb,-38 +2022-03-23 08:20, slack,- +2022-03-23 13:35, flood,45 +2022-03-23 16:56, slack,- +2022-03-23 18:39, ebb,-34 +2022-03-23 20:38, slack,- +2022-03-24 01:47, flood,46 +2022-03-24 05:14, slack,- +2022-03-24 07:03, ebb,-36 +2022-03-24 09:26, slack,- +2022-03-24 14:41, flood,42 +2022-03-24 17:56, slack,- +2022-03-24 19:33, ebb,-31 +2022-03-24 21:44, slack,- +2022-03-25 02:47, flood,43 +2022-03-25 06:14, slack,- +2022-03-25 07:57, ebb,-33 +2022-03-25 10:38, slack,- +2022-03-25 15:47, flood,40 +2022-03-25 19:02, slack,- +2022-03-25 20:27, ebb,-27 +2022-03-25 22:56, slack,- +2022-03-26 03:53, flood,40 +2022-03-26 07:20, slack,- +2022-03-26 08:57, ebb,-29 +2022-03-26 11:50, slack,- +2022-03-26 17:47, flood,39 +2022-03-26 20:02, slack,- +2022-03-26 21:33, ebb,-25 +2022-03-27 00:02, slack,- +2022-03-27 05:05, flood,39 +2022-03-27 08:26, slack,- +2022-03-27 10:03, ebb,-28 +2022-03-27 12:50, slack,- +2022-03-27 19:11, flood,43 +2022-03-27 21:08, slack,- +2022-03-27 22:51, ebb,-25 +2022-03-28 01:08, slack,- +2022-03-28 07:17, flood,41 +2022-03-28 09:26, slack,- +2022-03-28 11:21, ebb,-28 +2022-03-28 13:56, slack,- +2022-03-28 20:11, flood,48 +2022-03-28 22:08, slack,- +2022-03-29 00:15, ebb,-27 +2022-03-29 02:08, slack,- +2022-03-29 08:29, flood,46 +2022-03-29 10:26, slack,- +2022-03-29 12:33, ebb,-29 +2022-03-29 14:50, slack,- +2022-03-29 21:11, flood,53 +2022-03-29 23:08, slack,- +2022-03-30 01:21, ebb,-29 +2022-03-30 03:02, slack,- +2022-03-30 09:23, flood,52 +2022-03-30 11:26, slack,- +2022-03-30 13:33, ebb,-31 +2022-03-30 15:38, slack,- +2022-03-30 21:59, flood,57 +2022-03-30 23:56, slack,- +2022-03-31 02:15, ebb,-31 +2022-03-31 03:56, slack,- +2022-03-31 10:17, flood,55 +2022-03-31 12:14, slack,- +2022-03-31 14:21, ebb,-32 +2022-03-31 16:26, slack,- +2022-03-31 22:47, flood,57 +2022-04-01 00:44, slack,- +2022-04-01 02:57, ebb,-32 +2022-04-01 04:44, slack,- +2022-04-01 10:59, flood,55 +2022-04-01 13:02, slack,- +2022-04-01 15:03, ebb,-32 +2022-04-01 17:08, slack,- +2022-04-01 23:29, flood,55 +2022-04-02 01:32, slack,- +2022-04-02 03:39, ebb,-32 +2022-04-02 05:32, slack,- +2022-04-02 11:47, flood,53 +2022-04-02 13:50, slack,- +2022-04-02 15:39, ebb,-32 +2022-04-02 17:50, slack,- +2022-04-03 00:17, flood,51 +2022-04-03 02:14, slack,- +2022-04-03 04:15, ebb,-32 +2022-04-03 06:20, slack,- +2022-04-03 12:35, flood,50 +2022-04-03 14:38, slack,- +2022-04-03 16:15, ebb,-30 +2022-04-03 18:32, slack,- +2022-04-04 01:05, flood,45 +2022-04-04 03:02, slack,- +2022-04-04 04:45, ebb,-31 +2022-04-04 07:02, slack,- +2022-04-04 13:23, flood,45 +2022-04-04 15:26, slack,- +2022-04-04 16:57, ebb,-29 +2022-04-04 19:14, slack,- +2022-04-05 01:47, flood,39 +2022-04-05 03:44, slack,- +2022-04-05 05:27, ebb,-29 +2022-04-05 07:50, slack,- +2022-04-05 14:11, flood,41 +2022-04-05 16:08, slack,- +2022-04-05 17:39, ebb,-27 +2022-04-05 19:56, slack,- +2022-04-06 02:23, flood,34 +2022-04-06 04:26, slack,- +2022-04-06 06:03, ebb,-27 +2022-04-06 08:44, slack,- +2022-04-06 14:53, flood,38 +2022-04-06 16:56, slack,- +2022-04-06 18:21, ebb,-25 +2022-04-06 20:44, slack,- +2022-04-07 01:41, flood,32 +2022-04-07 05:14, slack,- +2022-04-07 06:45, ebb,-25 +2022-04-07 09:32, slack,- +2022-04-07 15:35, flood,34 +2022-04-07 17:44, slack,- +2022-04-07 19:09, ebb,-23 +2022-04-07 21:38, slack,- +2022-04-08 02:23, flood,30 +2022-04-08 05:56, slack,- +2022-04-08 07:33, ebb,-23 +2022-04-08 10:26, slack,- +2022-04-08 15:35, flood,32 +2022-04-08 18:32, slack,- +2022-04-08 19:57, ebb,-21 +2022-04-08 22:32, slack,- +2022-04-09 03:05, flood,30 +2022-04-09 06:44, slack,- +2022-04-09 08:21, ebb,-22 +2022-04-09 11:20, slack,- +2022-04-09 16:11, flood,31 +2022-04-09 19:26, slack,- +2022-04-09 20:45, ebb,-20 +2022-04-09 23:26, slack,- +2022-04-10 03:53, flood,31 +2022-04-10 07:38, slack,- +2022-04-10 09:09, ebb,-21 +2022-04-10 12:08, slack,- +2022-04-10 16:53, flood,31 +2022-04-10 20:14, slack,- +2022-04-10 21:39, ebb,-20 +2022-04-11 00:14, slack,- +2022-04-11 04:47, flood,32 +2022-04-11 08:26, slack,- +2022-04-11 10:03, ebb,-22 +2022-04-11 12:50, slack,- +2022-04-11 17:35, flood,33 +2022-04-11 21:02, slack,- +2022-04-11 22:39, ebb,-22 +2022-04-12 01:02, slack,- +2022-04-12 05:35, flood,35 +2022-04-12 09:14, slack,- +2022-04-12 10:57, ebb,-24 +2022-04-12 13:38, slack,- +2022-04-12 18:17, flood,37 +2022-04-12 21:50, slack,- +2022-04-12 23:27, ebb,-25 +2022-04-13 01:44, slack,- +2022-04-13 06:23, flood,39 +2022-04-13 10:02, slack,- +2022-04-13 11:51, ebb,-27 +2022-04-13 14:14, slack,- +2022-04-13 19:05, flood,40 +2022-04-13 22:38, slack,- +2022-04-14 00:15, ebb,-28 +2022-04-14 02:32, slack,- +2022-04-14 07:11, flood,43 +2022-04-14 10:50, slack,- +2022-04-14 12:33, ebb,-31 +2022-04-14 14:56, slack,- +2022-04-14 19:47, flood,45 +2022-04-14 23:20, slack,- +2022-04-15 01:03, ebb,-32 +2022-04-15 03:14, slack,- +2022-04-15 07:59, flood,47 +2022-04-15 11:38, slack,- +2022-04-15 13:21, ebb,-34 +2022-04-15 15:38, slack,- +2022-04-15 20:35, flood,49 +2022-04-16 00:02, slack,- +2022-04-16 01:51, ebb,-35 +2022-04-16 03:56, slack,- +2022-04-16 08:53, flood,51 +2022-04-16 12:20, slack,- +2022-04-16 14:09, ebb,-36 +2022-04-16 16:14, slack,- +2022-04-16 21:17, flood,53 +2022-04-17 00:44, slack,- +2022-04-17 02:33, ebb,-38 +2022-04-17 04:38, slack,- +2022-04-17 09:41, flood,53 +2022-04-17 13:08, slack,- +2022-04-17 14:57, ebb,-37 +2022-04-17 16:56, slack,- +2022-04-17 21:59, flood,55 +2022-04-18 01:32, slack,- +2022-04-18 03:21, ebb,-40 +2022-04-18 05:26, slack,- +2022-04-18 10:29, flood,54 +2022-04-18 14:02, slack,- +2022-04-18 15:45, ebb,-38 +2022-04-18 17:38, slack,- +2022-04-18 22:47, flood,54 +2022-04-19 02:14, slack,- +2022-04-19 04:09, ebb,-40 +2022-04-19 06:14, slack,- +2022-04-19 11:23, flood,52 +2022-04-19 14:50, slack,- +2022-04-19 16:33, ebb,-37 +2022-04-19 18:26, slack,- +2022-04-19 23:35, flood,52 +2022-04-20 03:08, slack,- +2022-04-20 04:57, ebb,-40 +2022-04-20 07:08, slack,- +2022-04-20 12:17, flood,48 +2022-04-20 15:44, slack,- +2022-04-20 17:27, ebb,-35 +2022-04-20 19:20, slack,- +2022-04-21 00:29, flood,47 +2022-04-21 04:02, slack,- +2022-04-21 05:51, ebb,-38 +2022-04-21 08:08, slack,- +2022-04-21 13:29, flood,44 +2022-04-21 16:44, slack,- +2022-04-21 18:21, ebb,-32 +2022-04-21 20:20, slack,- +2022-04-22 01:35, flood,43 +2022-04-22 04:56, slack,- +2022-04-22 06:45, ebb,-35 +2022-04-22 09:14, slack,- +2022-04-22 14:47, flood,42 +2022-04-22 17:44, slack,- +2022-04-22 19:15, ebb,-30 +2022-04-22 21:32, slack,- +2022-04-23 02:41, flood,40 +2022-04-23 05:56, slack,- +2022-04-23 07:39, ebb,-32 +2022-04-23 10:26, slack,- +2022-04-23 16:23, flood,41 +2022-04-23 18:44, slack,- +2022-04-23 20:09, ebb,-27 +2022-04-23 22:44, slack,- +2022-04-24 03:53, flood,39 +2022-04-24 07:02, slack,- +2022-04-24 08:33, ebb,-28 +2022-04-24 11:32, slack,- +2022-04-24 17:47, flood,43 +2022-04-24 19:50, slack,- +2022-04-24 21:15, ebb,-25 +2022-04-24 23:50, slack,- +2022-04-25 05:53, flood,39 +2022-04-25 08:08, slack,- +2022-04-25 09:45, ebb,-26 +2022-04-25 12:32, slack,- +2022-04-25 18:47, flood,46 +2022-04-25 20:50, slack,- +2022-04-25 22:45, ebb,-25 +2022-04-26 00:56, slack,- +2022-04-26 07:05, flood,43 +2022-04-26 09:08, slack,- +2022-04-26 11:21, ebb,-26 +2022-04-26 13:32, slack,- +2022-04-26 19:47, flood,50 +2022-04-26 21:50, slack,- +2022-04-27 00:33, ebb,-27 +2022-04-27 01:50, slack,- +2022-04-27 08:11, flood,47 +2022-04-27 10:08, slack,- +2022-04-27 12:45, ebb,-28 +2022-04-27 14:26, slack,- +2022-04-27 20:47, flood,53 +2022-04-27 22:44, slack,- +2022-04-28 01:21, ebb,-30 +2022-04-28 02:44, slack,- +2022-04-28 09:05, flood,51 +2022-04-28 11:02, slack,- +2022-04-28 13:33, ebb,-29 +2022-04-28 15:14, slack,- +2022-04-28 21:35, flood,54 +2022-04-28 23:32, slack,- +2022-04-29 02:03, ebb,-31 +2022-04-29 03:38, slack,- +2022-04-29 09:53, flood,54 +2022-04-29 11:50, slack,- +2022-04-29 14:09, ebb,-30 +2022-04-29 15:56, slack,- +2022-04-29 22:23, flood,54 +2022-04-30 00:20, slack,- +2022-04-30 02:45, ebb,-32 +2022-04-30 04:26, slack,- +2022-04-30 10:41, flood,54 +2022-04-30 12:38, slack,- +2022-04-30 14:39, ebb,-30 +2022-04-30 16:38, slack,- +2022-04-30 23:05, flood,51 +2022-05-01 01:02, slack,- +2022-05-01 03:21, ebb,-32 +2022-05-01 05:08, slack,- +2022-05-01 11:23, flood,52 +2022-05-01 13:26, slack,- +2022-05-01 15:15, ebb,-29 +2022-05-01 17:20, slack,- +2022-05-01 23:47, flood,46 +2022-05-02 01:44, slack,- +2022-05-02 03:45, ebb,-31 +2022-05-02 05:56, slack,- +2022-05-02 12:11, flood,48 +2022-05-02 14:14, slack,- +2022-05-02 15:51, ebb,-28 +2022-05-02 18:02, slack,- +2022-05-03 00:29, flood,40 +2022-05-03 02:26, slack,- +2022-05-03 04:15, ebb,-30 +2022-05-03 06:38, slack,- +2022-05-03 12:59, flood,44 +2022-05-03 14:56, slack,- +2022-05-03 16:33, ebb,-27 +2022-05-03 18:44, slack,- +2022-05-03 23:35, flood,36 +2022-05-04 03:14, slack,- +2022-05-04 04:51, ebb,-29 +2022-05-04 07:20, slack,- +2022-05-04 13:41, flood,40 +2022-05-04 15:44, slack,- +2022-05-04 17:09, ebb,-26 +2022-05-04 19:26, slack,- +2022-05-05 00:11, flood,34 +2022-05-05 03:56, slack,- +2022-05-05 05:33, ebb,-28 +2022-05-05 08:08, slack,- +2022-05-05 14:23, flood,37 +2022-05-05 16:26, slack,- +2022-05-05 17:57, ebb,-25 +2022-05-05 20:14, slack,- +2022-05-06 00:59, flood,32 +2022-05-06 04:38, slack,- +2022-05-06 06:15, ebb,-27 +2022-05-06 08:56, slack,- +2022-05-06 14:23, flood,35 +2022-05-06 17:14, slack,- +2022-05-06 18:39, ebb,-24 +2022-05-06 21:02, slack,- +2022-05-07 01:47, flood,32 +2022-05-07 05:20, slack,- +2022-05-07 06:57, ebb,-25 +2022-05-07 09:50, slack,- +2022-05-07 14:47, flood,34 +2022-05-07 18:02, slack,- +2022-05-07 19:27, ebb,-23 +2022-05-07 21:56, slack,- +2022-05-08 02:35, flood,32 +2022-05-08 06:08, slack,- +2022-05-08 07:45, ebb,-25 +2022-05-08 10:38, slack,- +2022-05-08 15:23, flood,34 +2022-05-08 18:50, slack,- +2022-05-08 20:15, ebb,-23 +2022-05-08 22:50, slack,- +2022-05-09 03:23, flood,34 +2022-05-09 06:56, slack,- +2022-05-09 08:33, ebb,-24 +2022-05-09 11:26, slack,- +2022-05-09 16:11, flood,35 +2022-05-09 19:32, slack,- +2022-05-09 21:03, ebb,-23 +2022-05-09 23:38, slack,- +2022-05-10 04:11, flood,36 +2022-05-10 07:44, slack,- +2022-05-10 09:27, ebb,-25 +2022-05-10 12:08, slack,- +2022-05-10 16:53, flood,38 +2022-05-10 20:26, slack,- +2022-05-10 21:57, ebb,-24 +2022-05-11 00:26, slack,- +2022-05-11 04:59, flood,38 +2022-05-11 08:32, slack,- +2022-05-11 10:21, ebb,-26 +2022-05-11 12:50, slack,- +2022-05-11 17:35, flood,41 +2022-05-11 21:14, slack,- +2022-05-11 22:51, ebb,-26 +2022-05-12 01:14, slack,- +2022-05-12 05:53, flood,41 +2022-05-12 09:26, slack,- +2022-05-12 11:15, ebb,-29 +2022-05-12 13:32, slack,- +2022-05-12 18:23, flood,44 +2022-05-12 22:02, slack,- +2022-05-12 23:45, ebb,-30 +2022-05-13 01:56, slack,- +2022-05-13 06:41, flood,44 +2022-05-13 10:20, slack,- +2022-05-13 12:03, ebb,-31 +2022-05-13 14:20, slack,- +2022-05-13 19:11, flood,48 +2022-05-13 22:44, slack,- +2022-05-14 00:33, ebb,-33 +2022-05-14 02:44, slack,- +2022-05-14 07:35, flood,47 +2022-05-14 11:08, slack,- +2022-05-14 12:51, ebb,-34 +2022-05-14 15:02, slack,- +2022-05-14 19:59, flood,51 +2022-05-14 23:32, slack,- +2022-05-15 01:21, ebb,-36 +2022-05-15 03:32, slack,- +2022-05-15 08:29, flood,50 +2022-05-15 12:02, slack,- +2022-05-15 13:45, ebb,-35 +2022-05-15 15:44, slack,- +2022-05-15 20:47, flood,53 +2022-05-16 00:20, slack,- +2022-05-16 02:09, ebb,-39 +2022-05-16 04:20, slack,- +2022-05-16 09:23, flood,52 +2022-05-16 12:50, slack,- +2022-05-16 14:33, ebb,-36 +2022-05-16 16:32, slack,- +2022-05-16 21:35, flood,54 +2022-05-17 01:08, slack,- +2022-05-17 02:57, ebb,-40 +2022-05-17 05:14, slack,- +2022-05-17 10:17, flood,52 +2022-05-17 13:44, slack,- +2022-05-17 15:27, ebb,-36 +2022-05-17 17:20, slack,- +2022-05-17 22:23, flood,53 +2022-05-18 01:56, slack,- +2022-05-18 03:51, ebb,-40 +2022-05-18 06:02, slack,- +2022-05-18 11:11, flood,50 +2022-05-18 14:38, slack,- +2022-05-18 16:15, ebb,-35 +2022-05-18 18:14, slack,- +2022-05-18 23:17, flood,49 +2022-05-19 02:50, slack,- +2022-05-19 04:39, ebb,-39 +2022-05-19 06:56, slack,- +2022-05-19 12:17, flood,47 +2022-05-19 15:32, slack,- +2022-05-19 17:09, ebb,-33 +2022-05-19 19:08, slack,- +2022-05-20 00:17, flood,45 +2022-05-20 03:44, slack,- +2022-05-20 05:33, ebb,-37 +2022-05-20 07:56, slack,- +2022-05-20 13:53, flood,45 +2022-05-20 16:32, slack,- +2022-05-20 18:03, ebb,-31 +2022-05-20 20:14, slack,- +2022-05-21 01:29, flood,42 +2022-05-21 04:44, slack,- +2022-05-21 06:21, ebb,-34 +2022-05-21 09:02, slack,- +2022-05-21 15:17, flood,45 +2022-05-21 17:26, slack,- +2022-05-21 18:57, ebb,-29 +2022-05-21 21:20, slack,- +2022-05-22 02:47, flood,40 +2022-05-22 05:44, slack,- +2022-05-22 07:21, ebb,-31 +2022-05-22 10:08, slack,- +2022-05-22 16:23, flood,45 +2022-05-22 18:26, slack,- +2022-05-22 19:57, ebb,-27 +2022-05-22 22:32, slack,- +2022-05-23 04:23, flood,40 +2022-05-23 06:44, slack,- +2022-05-23 08:15, ebb,-28 +2022-05-23 11:14, slack,- +2022-05-23 17:29, flood,46 +2022-05-23 19:26, slack,- +2022-05-23 20:57, ebb,-25 +2022-05-23 23:38, slack,- +2022-05-24 05:41, flood,41 +2022-05-24 07:44, slack,- +2022-05-24 09:21, ebb,-26 +2022-05-24 12:08, slack,- +2022-05-24 18:23, flood,47 +2022-05-24 20:26, slack,- +2022-05-24 23:15, ebb,-25 +2022-05-25 00:38, slack,- +2022-05-25 06:47, flood,44 +2022-05-25 08:44, slack,- +2022-05-25 11:33, ebb,-25 +2022-05-25 13:02, slack,- +2022-05-25 19:23, flood,49 +2022-05-25 21:20, slack,- +2022-05-26 00:15, ebb,-27 +2022-05-26 01:32, slack,- +2022-05-26 07:47, flood,46 +2022-05-26 09:44, slack,- +2022-05-26 12:27, ebb,-26 +2022-05-26 13:50, slack,- +2022-05-26 20:17, flood,49 +2022-05-26 22:14, slack,- +2022-05-27 01:03, ebb,-29 +2022-05-27 02:26, slack,- +2022-05-27 08:41, flood,48 +2022-05-27 10:38, slack,- +2022-05-27 13:09, ebb,-27 +2022-05-27 14:38, slack,- +2022-05-27 21:11, flood,50 +2022-05-27 23:02, slack,- +2022-05-28 01:51, ebb,-30 +2022-05-28 03:14, slack,- +2022-05-28 09:35, flood,50 +2022-05-28 11:26, slack,- +2022-05-28 13:45, ebb,-27 +2022-05-28 15:26, slack,- +2022-05-28 21:59, flood,48 +2022-05-28 23:50, slack,- +2022-05-29 02:27, ebb,-31 +2022-05-29 04:02, slack,- +2022-05-29 10:17, flood,51 +2022-05-29 12:14, slack,- +2022-05-29 14:15, ebb,-27 +2022-05-29 16:08, slack,- +2022-05-29 22:41, flood,46 +2022-05-30 00:32, slack,- +2022-05-30 03:03, ebb,-31 +2022-05-30 04:44, slack,- +2022-05-30 11:05, flood,49 +2022-05-30 13:02, slack,- +2022-05-30 14:51, ebb,-27 +2022-05-30 16:50, slack,- +2022-05-30 23:17, flood,42 +2022-05-31 01:14, slack,- +2022-05-31 03:21, ebb,-30 +2022-05-31 05:32, slack,- +2022-05-31 11:47, flood,47 +2022-05-31 13:50, slack,- +2022-05-31 15:27, ebb,-27 +2022-05-31 17:32, slack,- +2022-05-31 22:35, flood,37 +2022-06-01 01:56, slack,- +2022-06-01 03:45, ebb,-30 +2022-06-01 06:14, slack,- +2022-06-01 12:29, flood,43 +2022-06-01 14:32, slack,- +2022-06-01 16:03, ebb,-26 +2022-06-01 18:14, slack,- +2022-06-01 22:59, flood,37 +2022-06-02 02:38, slack,- +2022-06-02 04:21, ebb,-29 +2022-06-02 06:56, slack,- +2022-06-02 13:05, flood,40 +2022-06-02 15:14, slack,- +2022-06-02 16:45, ebb,-26 +2022-06-02 18:56, slack,- +2022-06-02 23:35, flood,36 +2022-06-03 03:20, slack,- +2022-06-03 05:03, ebb,-29 +2022-06-03 07:38, slack,- +2022-06-03 12:53, flood,38 +2022-06-03 15:56, slack,- +2022-06-03 17:27, ebb,-26 +2022-06-03 19:38, slack,- +2022-06-04 00:23, flood,35 +2022-06-04 04:02, slack,- +2022-06-04 05:45, ebb,-29 +2022-06-04 08:20, slack,- +2022-06-04 13:29, flood,37 +2022-06-04 16:44, slack,- +2022-06-04 18:09, ebb,-26 +2022-06-04 20:26, slack,- +2022-06-05 01:11, flood,35 +2022-06-05 04:44, slack,- +2022-06-05 06:27, ebb,-28 +2022-06-05 09:08, slack,- +2022-06-05 14:05, flood,37 +2022-06-05 17:26, slack,- +2022-06-05 18:57, ebb,-26 +2022-06-05 21:14, slack,- +2022-06-06 02:05, flood,36 +2022-06-06 05:26, slack,- +2022-06-06 07:15, ebb,-28 +2022-06-06 09:50, slack,- +2022-06-06 14:47, flood,38 +2022-06-06 18:08, slack,- +2022-06-06 19:39, ebb,-26 +2022-06-06 22:08, slack,- +2022-06-07 02:53, flood,38 +2022-06-07 06:14, slack,- +2022-06-07 07:57, ebb,-28 +2022-06-07 10:38, slack,- +2022-06-07 15:29, flood,40 +2022-06-07 18:56, slack,- +2022-06-07 20:27, ebb,-26 +2022-06-07 22:56, slack,- +2022-06-08 03:41, flood,39 +2022-06-08 07:02, slack,- +2022-06-08 08:51, ebb,-28 +2022-06-08 11:26, slack,- +2022-06-08 16:11, flood,42 +2022-06-08 19:44, slack,- +2022-06-08 21:21, ebb,-27 +2022-06-08 23:50, slack,- +2022-06-09 04:29, flood,41 +2022-06-09 07:56, slack,- +2022-06-09 09:45, ebb,-28 +2022-06-09 12:08, slack,- +2022-06-09 16:59, flood,44 +2022-06-09 20:32, slack,- +2022-06-09 22:15, ebb,-29 +2022-06-10 00:38, slack,- +2022-06-10 05:23, flood,42 +2022-06-10 08:50, slack,- +2022-06-10 10:39, ebb,-29 +2022-06-10 12:56, slack,- +2022-06-10 17:47, flood,47 +2022-06-10 21:20, slack,- +2022-06-10 23:09, ebb,-31 +2022-06-11 01:32, slack,- +2022-06-11 06:17, flood,44 +2022-06-11 09:50, slack,- +2022-06-11 11:33, ebb,-31 +2022-06-11 13:44, slack,- +2022-06-11 18:35, flood,49 +2022-06-11 22:14, slack,- +2022-06-12 00:03, ebb,-34 +2022-06-12 02:20, slack,- +2022-06-12 07:11, flood,46 +2022-06-12 10:44, slack,- +2022-06-12 12:27, ebb,-32 +2022-06-12 14:32, slack,- +2022-06-12 19:29, flood,50 +2022-06-12 23:02, slack,- +2022-06-13 00:57, ebb,-37 +2022-06-13 03:14, slack,- +2022-06-13 08:11, flood,47 +2022-06-13 11:44, slack,- +2022-06-13 13:21, ebb,-33 +2022-06-13 15:20, slack,- +2022-06-13 20:23, flood,51 +2022-06-13 23:56, slack,- +2022-06-14 01:45, ebb,-38 +2022-06-14 04:08, slack,- +2022-06-14 09:17, flood,49 +2022-06-14 12:38, slack,- +2022-06-14 14:15, ebb,-34 +2022-06-14 16:14, slack,- +2022-06-14 21:17, flood,51 +2022-06-15 00:50, slack,- +2022-06-15 02:39, ebb,-39 +2022-06-15 05:02, slack,- +2022-06-15 10:17, flood,49 +2022-06-15 13:32, slack,- +2022-06-15 15:09, ebb,-34 +2022-06-15 17:08, slack,- +2022-06-15 22:11, flood,50 +2022-06-16 01:44, slack,- +2022-06-16 03:33, ebb,-39 +2022-06-16 05:56, slack,- +2022-06-16 11:23, flood,49 +2022-06-16 14:26, slack,- +2022-06-16 15:57, ebb,-33 +2022-06-16 18:02, slack,- +2022-06-16 23:11, flood,48 +2022-06-17 02:38, slack,- +2022-06-17 04:21, ebb,-38 +2022-06-17 06:50, slack,- +2022-06-17 12:59, flood,48 +2022-06-17 15:20, slack,- +2022-06-17 16:51, ebb,-32 +2022-06-17 19:02, slack,- +2022-06-18 00:17, flood,45 +2022-06-18 03:32, slack,- +2022-06-18 05:15, ebb,-36 +2022-06-18 07:44, slack,- +2022-06-18 14:11, flood,48 +2022-06-18 16:14, slack,- +2022-06-18 17:45, ebb,-31 +2022-06-18 20:02, slack,- +2022-06-19 01:47, flood,43 +2022-06-19 04:26, slack,- +2022-06-19 06:03, ebb,-34 +2022-06-19 08:44, slack,- +2022-06-19 15:05, flood,48 +2022-06-19 17:08, slack,- +2022-06-19 18:39, ebb,-29 +2022-06-19 21:08, slack,- +2022-06-20 03:05, flood,42 +2022-06-20 05:26, slack,- +2022-06-20 06:57, ebb,-31 +2022-06-20 09:44, slack,- +2022-06-20 16:05, flood,48 +2022-06-20 18:08, slack,- +2022-06-20 19:33, ebb,-27 +2022-06-20 22:14, slack,- +2022-06-21 04:11, flood,42 +2022-06-21 06:20, slack,- +2022-06-21 07:51, ebb,-28 +2022-06-21 10:44, slack,- +2022-06-21 16:59, flood,47 +2022-06-21 19:02, slack,- +2022-06-21 20:39, ebb,-25 +2022-06-21 23:14, slack,- +2022-06-22 05:17, flood,42 +2022-06-22 07:20, slack,- +2022-06-22 08:51, ebb,-25 +2022-06-22 11:38, slack,- +2022-06-22 17:59, flood,46 +2022-06-22 19:56, slack,- +2022-06-22 22:57, ebb,-25 +2022-06-23 00:14, slack,- +2022-06-23 06:17, flood,42 +2022-06-23 08:20, slack,- +2022-06-23 10:09, ebb,-24 +2022-06-23 12:32, slack,- +2022-06-23 18:53, flood,45 +2022-06-23 20:50, slack,- +2022-06-23 23:57, ebb,-27 +2022-06-24 01:08, slack,- +2022-06-24 07:17, flood,43 +2022-06-24 09:14, slack,- +2022-06-24 12:03, ebb,-24 +2022-06-24 13:20, slack,- +2022-06-24 19:47, flood,44 +2022-06-24 21:44, slack,- +2022-06-25 00:45, ebb,-28 +2022-06-25 02:02, slack,- +2022-06-25 08:17, flood,44 +2022-06-25 10:08, slack,- +2022-06-25 12:45, ebb,-24 +2022-06-25 14:08, slack,- +2022-06-25 20:41, flood,43 +2022-06-25 22:32, slack,- +2022-06-26 01:27, ebb,-29 +2022-06-26 02:50, slack,- +2022-06-26 09:11, flood,46 +2022-06-26 11:02, slack,- +2022-06-26 13:27, ebb,-24 +2022-06-26 14:50, slack,- +2022-06-26 21:29, flood,42 +2022-06-26 23:20, slack,- +2022-06-27 02:09, ebb,-29 +2022-06-27 03:38, slack,- +2022-06-27 09:59, flood,47 +2022-06-27 11:50, slack,- +2022-06-27 13:51, ebb,-25 +2022-06-27 15:38, slack,- +2022-06-27 22:11, flood,40 +2022-06-28 00:02, slack,- +2022-06-28 02:45, ebb,-29 +2022-06-28 04:20, slack,- +2022-06-28 10:41, flood,47 +2022-06-28 12:38, slack,- +2022-06-28 14:21, ebb,-25 +2022-06-28 16:20, slack,- +2022-06-28 22:47, flood,38 +2022-06-29 00:44, slack,- +2022-06-29 02:51, ebb,-29 +2022-06-29 05:02, slack,- +2022-06-29 11:17, flood,45 +2022-06-29 13:20, slack,- +2022-06-29 14:57, ebb,-26 +2022-06-29 17:02, slack,- +2022-06-29 21:53, flood,37 +2022-06-30 01:26, slack,- +2022-06-30 03:15, ebb,-29 +2022-06-30 05:44, slack,- +2022-06-30 11:53, flood,42 +2022-06-30 14:02, slack,- +2022-06-30 15:33, ebb,-26 +2022-06-30 17:44, slack,- +2022-06-30 22:29, flood,39 +2022-07-01 02:08, slack,- +2022-07-01 03:51, ebb,-30 +2022-07-01 06:26, slack,- +2022-07-01 11:35, flood,40 +2022-07-01 14:44, slack,- +2022-07-01 16:15, ebb,-27 +2022-07-01 18:26, slack,- +2022-07-01 23:05, flood,40 +2022-07-02 02:50, slack,- +2022-07-02 04:33, ebb,-31 +2022-07-02 07:02, slack,- +2022-07-02 11:59, flood,40 +2022-07-02 15:26, slack,- +2022-07-02 16:57, ebb,-28 +2022-07-02 19:08, slack,- +2022-07-02 23:53, flood,40 +2022-07-03 03:26, slack,- +2022-07-03 05:15, ebb,-31 +2022-07-03 07:44, slack,- +2022-07-03 12:41, flood,41 +2022-07-03 16:08, slack,- +2022-07-03 17:39, ebb,-28 +2022-07-03 19:50, slack,- +2022-07-04 00:41, flood,40 +2022-07-04 04:08, slack,- +2022-07-04 05:57, ebb,-32 +2022-07-04 08:20, slack,- +2022-07-04 13:23, flood,42 +2022-07-04 16:50, slack,- +2022-07-04 18:21, ebb,-29 +2022-07-04 20:32, slack,- +2022-07-05 01:29, flood,41 +2022-07-05 04:50, slack,- +2022-07-05 06:45, ebb,-32 +2022-07-05 09:08, slack,- +2022-07-05 14:11, flood,43 +2022-07-05 17:32, slack,- +2022-07-05 19:09, ebb,-29 +2022-07-05 21:26, slack,- +2022-07-06 02:23, flood,41 +2022-07-06 05:38, slack,- +2022-07-06 07:27, ebb,-32 +2022-07-06 09:50, slack,- +2022-07-06 14:53, flood,45 +2022-07-06 18:14, slack,- +2022-07-06 19:57, ebb,-30 +2022-07-06 22:20, slack,- +2022-07-07 03:11, flood,42 +2022-07-07 06:26, slack,- +2022-07-07 08:21, ebb,-31 +2022-07-07 10:38, slack,- +2022-07-07 15:41, flood,46 +2022-07-07 19:02, slack,- +2022-07-07 20:51, ebb,-30 +2022-07-07 23:14, slack,- +2022-07-08 04:05, flood,42 +2022-07-08 07:20, slack,- +2022-07-08 09:15, ebb,-30 +2022-07-08 11:26, slack,- +2022-07-08 16:23, flood,47 +2022-07-08 19:56, slack,- +2022-07-08 21:45, ebb,-31 +2022-07-09 00:08, slack,- +2022-07-09 04:59, flood,42 +2022-07-09 08:20, slack,- +2022-07-09 10:09, ebb,-29 +2022-07-09 12:20, slack,- +2022-07-09 17:17, flood,47 +2022-07-09 20:50, slack,- +2022-07-09 22:39, ebb,-32 +2022-07-10 01:08, slack,- +2022-07-10 05:53, flood,42 +2022-07-10 09:26, slack,- +2022-07-10 11:09, ebb,-29 +2022-07-10 13:14, slack,- +2022-07-10 18:05, flood,47 +2022-07-10 21:44, slack,- +2022-07-10 23:39, ebb,-34 +2022-07-11 02:02, slack,- +2022-07-11 06:53, flood,43 +2022-07-11 10:26, slack,- +2022-07-11 12:03, ebb,-30 +2022-07-11 14:08, slack,- +2022-07-11 19:05, flood,47 +2022-07-11 22:44, slack,- +2022-07-12 00:33, ebb,-35 +2022-07-12 03:02, slack,- +2022-07-12 08:05, flood,44 +2022-07-12 11:26, slack,- +2022-07-12 13:03, ebb,-31 +2022-07-12 15:02, slack,- +2022-07-12 20:05, flood,48 +2022-07-12 23:38, slack,- +2022-07-13 01:27, ebb,-37 +2022-07-13 03:56, slack,- +2022-07-13 09:47, flood,47 +2022-07-13 12:20, slack,- +2022-07-13 13:57, ebb,-32 +2022-07-13 16:02, slack,- +2022-07-13 21:11, flood,48 +2022-07-14 00:32, slack,- +2022-07-14 02:21, ebb,-37 +2022-07-14 04:50, slack,- +2022-07-14 10:59, flood,50 +2022-07-14 13:14, slack,- +2022-07-14 14:51, ebb,-32 +2022-07-14 16:56, slack,- +2022-07-14 22:17, flood,49 +2022-07-15 01:26, slack,- +2022-07-15 03:15, ebb,-37 +2022-07-15 05:44, slack,- +2022-07-15 11:59, flood,52 +2022-07-15 14:08, slack,- +2022-07-15 15:45, ebb,-33 +2022-07-15 17:56, slack,- +2022-07-15 23:17, flood,48 +2022-07-16 02:20, slack,- +2022-07-16 04:09, ebb,-37 +2022-07-16 06:38, slack,- +2022-07-16 12:59, flood,52 +2022-07-16 15:02, slack,- +2022-07-16 16:39, ebb,-32 +2022-07-16 18:50, slack,- +2022-07-17 00:41, flood,47 +2022-07-17 03:14, slack,- +2022-07-17 04:57, ebb,-35 +2022-07-17 07:32, slack,- +2022-07-17 13:53, flood,52 +2022-07-17 15:56, slack,- +2022-07-17 17:27, ebb,-31 +2022-07-17 19:44, slack,- +2022-07-18 01:59, flood,46 +2022-07-18 04:08, slack,- +2022-07-18 05:45, ebb,-33 +2022-07-18 08:20, slack,- +2022-07-18 14:47, flood,50 +2022-07-18 16:50, slack,- +2022-07-18 18:21, ebb,-29 +2022-07-18 20:44, slack,- +2022-07-19 02:59, flood,45 +2022-07-19 05:02, slack,- +2022-07-19 06:39, ebb,-31 +2022-07-19 09:20, slack,- +2022-07-19 15:41, flood,48 +2022-07-19 17:38, slack,- +2022-07-19 19:15, ebb,-27 +2022-07-19 21:50, slack,- +2022-07-20 03:53, flood,43 +2022-07-20 05:56, slack,- +2022-07-20 07:27, ebb,-28 +2022-07-20 10:14, slack,- +2022-07-20 16:35, flood,45 +2022-07-20 18:32, slack,- +2022-07-20 20:09, ebb,-25 +2022-07-20 22:50, slack,- +2022-07-21 04:53, flood,41 +2022-07-21 06:50, slack,- +2022-07-21 08:21, ebb,-25 +2022-07-21 11:08, slack,- +2022-07-21 17:29, flood,42 +2022-07-21 19:26, slack,- +2022-07-21 21:15, ebb,-24 +2022-07-21 22:27, ebb,-24 +2022-07-21 23:44, slack,- +2022-07-22 05:53, flood,40 +2022-07-22 07:50, slack,- +2022-07-22 09:21, ebb,-22 +2022-07-22 11:56, slack,- +2022-07-22 18:23, flood,40 +2022-07-22 20:20, slack,- +2022-07-22 23:33, ebb,-25 +2022-07-23 00:44, slack,- +2022-07-23 06:53, flood,39 +2022-07-23 08:44, slack,- +2022-07-23 11:39, ebb,-21 +2022-07-23 12:44, slack,- +2022-07-23 19:17, flood,38 +2022-07-23 21:14, slack,- +2022-07-24 00:21, ebb,-26 +2022-07-24 01:32, slack,- +2022-07-24 07:47, flood,40 +2022-07-24 09:44, slack,- +2022-07-24 12:27, ebb,-22 +2022-07-24 13:32, slack,- +2022-07-24 20:11, flood,37 +2022-07-24 22:02, slack,- +2022-07-25 01:09, ebb,-26 +2022-07-25 02:26, slack,- +2022-07-25 08:41, flood,42 +2022-07-25 10:32, slack,- +2022-07-25 13:03, ebb,-22 +2022-07-25 14:20, slack,- +2022-07-25 21:05, flood,36 +2022-07-25 22:50, slack,- +2022-07-26 01:51, ebb,-27 +2022-07-26 03:08, slack,- +2022-07-26 09:29, flood,43 +2022-07-26 11:26, slack,- +2022-07-26 13:27, ebb,-23 +2022-07-26 15:08, slack,- +2022-07-26 21:47, flood,36 +2022-07-26 23:38, slack,- +2022-07-27 02:27, ebb,-27 +2022-07-27 03:56, slack,- +2022-07-27 10:17, flood,43 +2022-07-27 12:08, slack,- +2022-07-27 13:51, ebb,-24 +2022-07-27 15:50, slack,- +2022-07-27 20:47, flood,36 +2022-07-27 22:17, flood,36 +2022-07-28 00:14, slack,- +2022-07-28 02:15, ebb,-28 +2022-07-28 04:32, slack,- +2022-07-28 10:53, flood,43 +2022-07-28 12:50, slack,- +2022-07-28 14:27, ebb,-26 +2022-07-28 16:32, slack,- +2022-07-28 21:23, flood,39 +2022-07-29 00:56, slack,- +2022-07-29 02:45, ebb,-29 +2022-07-29 05:14, slack,- +2022-07-29 11:05, flood,42 +2022-07-29 13:32, slack,- +2022-07-29 15:03, ebb,-27 +2022-07-29 17:14, slack,- +2022-07-29 21:59, flood,42 +2022-07-30 01:38, slack,- +2022-07-30 03:21, ebb,-31 +2022-07-30 05:50, slack,- +2022-07-30 10:47, flood,43 +2022-07-30 14:14, slack,- +2022-07-30 15:45, ebb,-29 +2022-07-30 17:50, slack,- +2022-07-30 22:41, flood,44 +2022-07-31 02:14, slack,- +2022-07-31 04:03, ebb,-33 +2022-07-31 06:26, slack,- +2022-07-31 11:23, flood,44 +2022-07-31 14:50, slack,- +2022-07-31 16:27, ebb,-30 +2022-07-31 18:32, slack,- +2022-07-31 23:23, flood,45 +2022-08-01 02:56, slack,- +2022-08-01 04:45, ebb,-34 +2022-08-01 07:02, slack,- +2022-08-01 11:59, flood,45 +2022-08-01 15:32, slack,- +2022-08-01 17:09, ebb,-32 +2022-08-01 19:14, slack,- +2022-08-02 00:05, flood,46 +2022-08-02 03:32, slack,- +2022-08-02 05:27, ebb,-35 +2022-08-02 07:38, slack,- +2022-08-02 12:41, flood,46 +2022-08-02 16:14, slack,- +2022-08-02 17:51, ebb,-33 +2022-08-02 19:56, slack,- +2022-08-03 00:59, flood,45 +2022-08-03 04:20, slack,- +2022-08-03 06:15, ebb,-35 +2022-08-03 08:20, slack,- +2022-08-03 13:29, flood,47 +2022-08-03 16:50, slack,- +2022-08-03 18:39, ebb,-33 +2022-08-03 20:44, slack,- +2022-08-04 01:53, flood,45 +2022-08-04 05:08, slack,- +2022-08-04 07:03, ebb,-34 +2022-08-04 09:08, slack,- +2022-08-04 14:17, flood,48 +2022-08-04 17:38, slack,- +2022-08-04 19:27, ebb,-33 +2022-08-04 21:44, slack,- +2022-08-05 02:41, flood,44 +2022-08-05 05:56, slack,- +2022-08-05 07:51, ebb,-32 +2022-08-05 09:56, slack,- +2022-08-05 15:05, flood,48 +2022-08-05 18:26, slack,- +2022-08-05 20:21, ebb,-32 +2022-08-05 22:44, slack,- +2022-08-06 03:35, flood,42 +2022-08-06 06:56, slack,- +2022-08-06 08:45, ebb,-30 +2022-08-06 10:56, slack,- +2022-08-06 15:53, flood,47 +2022-08-06 19:20, slack,- +2022-08-06 21:15, ebb,-32 +2022-08-06 23:44, slack,- +2022-08-07 04:35, flood,41 +2022-08-07 08:02, slack,- +2022-08-07 09:45, ebb,-28 +2022-08-07 11:56, slack,- +2022-08-07 16:47, flood,45 +2022-08-07 20:20, slack,- +2022-08-07 22:15, ebb,-32 +2022-08-08 00:44, slack,- +2022-08-08 05:35, flood,40 +2022-08-08 09:08, slack,- +2022-08-08 10:45, ebb,-28 +2022-08-08 12:56, slack,- +2022-08-08 17:47, flood,44 +2022-08-08 21:26, slack,- +2022-08-08 23:15, ebb,-32 +2022-08-09 01:50, slack,- +2022-08-09 06:47, flood,40 +2022-08-09 10:14, slack,- +2022-08-09 11:45, ebb,-28 +2022-08-09 13:56, slack,- +2022-08-09 18:53, flood,44 +2022-08-09 22:26, slack,- +2022-08-10 00:15, ebb,-33 +2022-08-10 02:44, slack,- +2022-08-10 08:59, flood,44 +2022-08-10 11:14, slack,- +2022-08-10 12:45, ebb,-30 +2022-08-10 14:56, slack,- +2022-08-10 20:05, flood,45 +2022-08-10 23:26, slack,- +2022-08-11 01:09, ebb,-34 +2022-08-11 03:44, slack,- +2022-08-11 10:05, flood,50 +2022-08-11 12:08, slack,- +2022-08-11 13:45, ebb,-31 +2022-08-11 15:50, slack,- +2022-08-11 21:47, flood,48 +2022-08-12 00:20, slack,- +2022-08-12 02:09, ebb,-35 +2022-08-12 04:38, slack,- +2022-08-12 10:59, flood,54 +2022-08-12 13:02, slack,- +2022-08-12 14:39, ebb,-32 +2022-08-12 16:50, slack,- +2022-08-12 22:47, flood,51 +2022-08-13 01:14, slack,- +2022-08-13 03:03, ebb,-36 +2022-08-13 05:32, slack,- +2022-08-13 11:47, flood,55 +2022-08-13 13:50, slack,- +2022-08-13 15:33, ebb,-33 +2022-08-13 17:44, slack,- +2022-08-13 23:47, flood,51 +2022-08-14 02:08, slack,- +2022-08-14 03:51, ebb,-36 +2022-08-14 06:20, slack,- +2022-08-14 12:41, flood,54 +2022-08-14 14:44, slack,- +2022-08-14 16:21, ebb,-33 +2022-08-14 18:32, slack,- +2022-08-15 00:47, flood,50 +2022-08-15 02:56, slack,- +2022-08-15 04:39, ebb,-35 +2022-08-15 07:08, slack,- +2022-08-15 13:35, flood,53 +2022-08-15 15:32, slack,- +2022-08-15 17:09, ebb,-32 +2022-08-15 19:26, slack,- +2022-08-16 01:41, flood,48 +2022-08-16 03:50, slack,- +2022-08-16 05:27, ebb,-33 +2022-08-16 07:56, slack,- +2022-08-16 14:23, flood,50 +2022-08-16 16:20, slack,- +2022-08-16 17:57, ebb,-30 +2022-08-16 20:20, slack,- +2022-08-17 02:41, flood,46 +2022-08-17 04:38, slack,- +2022-08-17 06:15, ebb,-30 +2022-08-17 08:44, slack,- +2022-08-17 15:11, flood,46 +2022-08-17 17:14, slack,- +2022-08-17 18:45, ebb,-28 +2022-08-17 21:20, slack,- +2022-08-18 03:29, flood,42 +2022-08-18 05:32, slack,- +2022-08-18 07:03, ebb,-27 +2022-08-18 09:38, slack,- +2022-08-18 16:05, flood,41 +2022-08-18 18:02, slack,- +2022-08-18 19:33, ebb,-26 +2022-08-18 22:20, slack,- +2022-08-19 04:29, flood,39 +2022-08-19 06:26, slack,- +2022-08-19 07:51, ebb,-24 +2022-08-19 10:32, slack,- +2022-08-19 16:59, flood,37 +2022-08-19 18:50, slack,- +2022-08-19 20:27, ebb,-23 +2022-08-19 23:20, slack,- +2022-08-20 05:23, flood,37 +2022-08-20 07:20, slack,- +2022-08-20 08:45, ebb,-21 +2022-08-20 11:26, slack,- +2022-08-20 17:53, flood,34 +2022-08-20 19:44, slack,- +2022-08-20 23:09, ebb,-22 +2022-08-21 00:14, slack,- +2022-08-21 06:23, flood,36 +2022-08-21 08:14, slack,- +2022-08-21 09:45, ebb,-19 +2022-08-21 12:14, slack,- +2022-08-21 18:47, flood,32 +2022-08-21 20:38, slack,- +2022-08-21 23:57, ebb,-23 +2022-08-22 01:02, slack,- +2022-08-22 07:17, flood,37 +2022-08-22 09:14, slack,- +2022-08-22 12:03, ebb,-20 +2022-08-22 13:08, slack,- +2022-08-22 19:41, flood,31 +2022-08-22 21:32, slack,- +2022-08-23 00:45, ebb,-24 +2022-08-23 01:56, slack,- +2022-08-23 08:11, flood,38 +2022-08-23 10:02, slack,- +2022-08-23 12:45, ebb,-21 +2022-08-23 13:50, slack,- +2022-08-23 18:35, flood,30 +2022-08-23 20:29, flood,32 +2022-08-23 22:20, slack,- +2022-08-24 01:27, ebb,-25 +2022-08-24 02:38, slack,- +2022-08-24 08:59, flood,39 +2022-08-24 10:50, slack,- +2022-08-24 12:45, ebb,-22 +2022-08-24 14:38, slack,- +2022-08-24 19:17, flood,33 +2022-08-24 21:11, flood,33 +2022-08-24 23:02, slack,- +2022-08-25 01:15, ebb,-25 +2022-08-25 03:20, slack,- +2022-08-25 09:41, flood,40 +2022-08-25 11:38, slack,- +2022-08-25 13:15, ebb,-24 +2022-08-25 15:20, slack,- +2022-08-25 20:05, flood,37 +2022-08-25 23:44, slack,- +2022-08-26 01:33, ebb,-27 +2022-08-26 04:02, slack,- +2022-08-26 10:11, flood,40 +2022-08-26 12:20, slack,- +2022-08-26 13:51, ebb,-26 +2022-08-26 16:02, slack,- +2022-08-26 20:47, flood,41 +2022-08-27 00:26, slack,- +2022-08-27 02:09, ebb,-30 +2022-08-27 04:38, slack,- +2022-08-27 09:47, flood,42 +2022-08-27 12:56, slack,- +2022-08-27 14:33, ebb,-29 +2022-08-27 16:44, slack,- +2022-08-27 21:29, flood,45 +2022-08-28 01:02, slack,- +2022-08-28 02:51, ebb,-32 +2022-08-28 05:14, slack,- +2022-08-28 10:11, flood,46 +2022-08-28 13:38, slack,- +2022-08-28 15:15, ebb,-31 +2022-08-28 17:20, slack,- +2022-08-28 22:11, flood,48 +2022-08-29 01:44, slack,- +2022-08-29 03:33, ebb,-35 +2022-08-29 05:50, slack,- +2022-08-29 10:41, flood,48 +2022-08-29 14:14, slack,- +2022-08-29 15:57, ebb,-33 +2022-08-29 18:02, slack,- +2022-08-29 22:53, flood,50 +2022-08-30 02:26, slack,- +2022-08-30 04:15, ebb,-36 +2022-08-30 06:26, slack,- +2022-08-30 11:23, flood,50 +2022-08-30 14:56, slack,- +2022-08-30 16:39, ebb,-35 +2022-08-30 18:38, slack,- +2022-08-30 23:41, flood,50 +2022-08-31 03:08, slack,- +2022-08-31 05:03, ebb,-37 +2022-08-31 07:02, slack,- +2022-08-31 12:05, flood,51 +2022-08-31 15:38, slack,- +2022-08-31 17:27, ebb,-36 +2022-08-31 19:26, slack,- +2022-09-01 00:29, flood,48 +2022-09-01 03:50, slack,- +2022-09-01 05:45, ebb,-37 +2022-09-01 07:44, slack,- +2022-09-01 12:53, flood,50 +2022-09-01 16:20, slack,- +2022-09-01 18:15, ebb,-36 +2022-09-01 20:14, slack,- +2022-09-02 01:23, flood,46 +2022-09-02 04:44, slack,- +2022-09-02 06:33, ebb,-35 +2022-09-02 08:26, slack,- +2022-09-02 13:47, flood,49 +2022-09-02 17:08, slack,- +2022-09-02 19:03, ebb,-36 +2022-09-02 21:14, slack,- +2022-09-03 02:23, flood,44 +2022-09-03 05:38, slack,- +2022-09-03 07:27, ebb,-33 +2022-09-03 09:26, slack,- +2022-09-03 14:35, flood,47 +2022-09-03 18:02, slack,- +2022-09-03 19:51, ebb,-34 +2022-09-03 22:20, slack,- +2022-09-04 03:17, flood,42 +2022-09-04 06:38, slack,- +2022-09-04 08:21, ebb,-30 +2022-09-04 10:32, slack,- +2022-09-04 15:35, flood,45 +2022-09-04 18:56, slack,- +2022-09-04 20:45, ebb,-32 +2022-09-04 23:26, slack,- +2022-09-05 04:23, flood,40 +2022-09-05 07:44, slack,- +2022-09-05 09:21, ebb,-27 +2022-09-05 11:38, slack,- +2022-09-05 16:29, flood,43 +2022-09-05 20:02, slack,- +2022-09-05 21:51, ebb,-30 +2022-09-06 00:32, slack,- +2022-09-06 05:35, flood,39 +2022-09-06 08:50, slack,- +2022-09-06 10:27, ebb,-26 +2022-09-06 12:44, slack,- +2022-09-06 17:35, flood,42 +2022-09-06 21:08, slack,- +2022-09-06 22:57, ebb,-30 +2022-09-07 01:32, slack,- +2022-09-07 07:41, flood,41 +2022-09-07 09:56, slack,- +2022-09-07 11:33, ebb,-27 +2022-09-07 13:44, slack,- +2022-09-07 18:53, flood,42 +2022-09-07 22:14, slack,- +2022-09-07 23:57, ebb,-31 +2022-09-08 02:32, slack,- +2022-09-08 08:53, flood,47 +2022-09-08 10:56, slack,- +2022-09-08 12:33, ebb,-29 +2022-09-08 14:44, slack,- +2022-09-08 20:59, flood,46 +2022-09-08 23:14, slack,- +2022-09-09 00:57, ebb,-32 +2022-09-09 03:26, slack,- +2022-09-09 09:53, flood,53 +2022-09-09 11:50, slack,- +2022-09-09 13:33, ebb,-31 +2022-09-09 15:38, slack,- +2022-09-09 21:59, flood,51 +2022-09-10 00:08, slack,- +2022-09-10 01:57, ebb,-34 +2022-09-10 04:20, slack,- +2022-09-10 10:41, flood,56 +2022-09-10 12:38, slack,- +2022-09-10 14:33, ebb,-32 +2022-09-10 16:32, slack,- +2022-09-10 22:47, flood,54 +2022-09-11 00:56, slack,- +2022-09-11 02:45, ebb,-34 +2022-09-11 05:08, slack,- +2022-09-11 11:29, flood,57 +2022-09-11 13:32, slack,- +2022-09-11 15:21, ebb,-33 +2022-09-11 17:26, slack,- +2022-09-11 23:41, flood,54 +2022-09-12 01:50, slack,- +2022-09-12 03:33, ebb,-34 +2022-09-12 05:56, slack,- +2022-09-12 12:17, flood,55 +2022-09-12 14:20, slack,- +2022-09-12 16:09, ebb,-33 +2022-09-12 18:14, slack,- +2022-09-13 00:29, flood,52 +2022-09-13 02:38, slack,- +2022-09-13 04:21, ebb,-33 +2022-09-13 06:38, slack,- +2022-09-13 13:05, flood,51 +2022-09-13 15:08, slack,- +2022-09-13 16:51, ebb,-32 +2022-09-13 19:08, slack,- +2022-09-14 01:23, flood,49 +2022-09-14 03:26, slack,- +2022-09-14 05:03, ebb,-31 +2022-09-14 07:26, slack,- +2022-09-14 13:59, flood,46 +2022-09-14 15:56, slack,- +2022-09-14 17:33, ebb,-30 +2022-09-14 19:56, slack,- +2022-09-15 02:17, flood,45 +2022-09-15 04:14, slack,- +2022-09-15 05:51, ebb,-29 +2022-09-15 08:14, slack,- +2022-09-15 14:47, flood,41 +2022-09-15 16:38, slack,- +2022-09-15 18:21, ebb,-28 +2022-09-15 20:50, slack,- +2022-09-16 03:11, flood,42 +2022-09-16 05:08, slack,- +2022-09-16 06:33, ebb,-26 +2022-09-16 09:02, slack,- +2022-09-16 15:35, flood,36 +2022-09-16 17:26, slack,- +2022-09-16 19:03, ebb,-26 +2022-09-16 21:50, slack,- +2022-09-17 03:59, flood,38 +2022-09-17 05:56, slack,- +2022-09-17 07:21, ebb,-23 +2022-09-17 09:56, slack,- +2022-09-17 16:23, flood,32 +2022-09-17 18:20, slack,- +2022-09-17 19:51, ebb,-23 +2022-09-17 22:44, slack,- +2022-09-18 04:53, flood,36 +2022-09-18 06:50, slack,- +2022-09-18 08:09, ebb,-21 +2022-09-18 10:50, slack,- +2022-09-18 15:41, flood,29 +2022-09-18 17:11, flood,29 +2022-09-18 19:08, slack,- +2022-09-18 20:39, ebb,-21 +2022-09-18 23:38, slack,- +2022-09-19 05:53, flood,34 +2022-09-19 07:44, slack,- +2022-09-19 09:09, ebb,-19 +2022-09-19 11:44, slack,- +2022-09-19 16:23, flood,28 +2022-09-19 17:29, flood,27 +2022-09-19 20:02, slack,- +2022-09-19 21:39, ebb,-20 +2022-09-19 23:33, ebb,-21 +2022-09-20 00:32, slack,- +2022-09-20 06:47, flood,34 +2022-09-20 08:38, slack,- +2022-09-20 10:09, ebb,-19 +2022-09-20 12:32, slack,- +2022-09-20 17:11, flood,29 +2022-09-20 20:50, slack,- +2022-09-21 00:21, ebb,-22 +2022-09-21 01:20, slack,- +2022-09-21 07:35, flood,34 +2022-09-21 09:26, slack,- +2022-09-21 11:15, ebb,-20 +2022-09-21 13:20, slack,- +2022-09-21 17:53, flood,31 +2022-09-21 21:44, slack,- +2022-09-22 00:51, ebb,-22 +2022-09-22 02:02, slack,- +2022-09-22 08:23, flood,35 +2022-09-22 10:14, slack,- +2022-09-22 11:57, ebb,-22 +2022-09-22 14:08, slack,- +2022-09-22 18:41, flood,34 +2022-09-22 22:26, slack,- +2022-09-23 00:15, ebb,-25 +2022-09-23 02:44, slack,- +2022-09-23 08:59, flood,36 +2022-09-23 11:02, slack,- +2022-09-23 12:39, ebb,-25 +2022-09-23 14:50, slack,- +2022-09-23 19:29, flood,39 +2022-09-23 23:14, slack,- +2022-09-24 00:57, ebb,-28 +2022-09-24 03:20, slack,- +2022-09-24 08:23, flood,40 +2022-09-24 11:44, slack,- +2022-09-24 13:21, ebb,-28 +2022-09-24 15:32, slack,- +2022-09-24 20:17, flood,43 +2022-09-24 23:50, slack,- +2022-09-25 01:39, ebb,-31 +2022-09-25 03:56, slack,- +2022-09-25 08:53, flood,44 +2022-09-25 12:20, slack,- +2022-09-25 14:03, ebb,-31 +2022-09-25 16:08, slack,- +2022-09-25 20:59, flood,47 +2022-09-26 00:32, slack,- +2022-09-26 02:21, ebb,-33 +2022-09-26 04:32, slack,- +2022-09-26 09:29, flood,48 +2022-09-26 13:02, slack,- +2022-09-26 14:45, ebb,-34 +2022-09-26 16:50, slack,- +2022-09-26 21:47, flood,51 +2022-09-27 01:14, slack,- +2022-09-27 03:03, ebb,-36 +2022-09-27 05:08, slack,- +2022-09-27 10:11, flood,52 +2022-09-27 13:38, slack,- +2022-09-27 15:27, ebb,-36 +2022-09-27 17:32, slack,- +2022-09-27 22:29, flood,52 +2022-09-28 01:56, slack,- +2022-09-28 03:51, ebb,-37 +2022-09-28 05:50, slack,- +2022-09-28 10:47, flood,53 +2022-09-28 14:20, slack,- +2022-09-28 16:15, ebb,-38 +2022-09-28 18:14, slack,- +2022-09-28 23:17, flood,52 +2022-09-29 02:44, slack,- +2022-09-29 04:33, ebb,-37 +2022-09-29 06:26, slack,- +2022-09-29 11:29, flood,53 +2022-09-29 15:02, slack,- +2022-09-29 16:57, ebb,-39 +2022-09-29 19:02, slack,- +2022-09-30 00:05, flood,50 +2022-09-30 03:32, slack,- +2022-09-30 05:21, ebb,-37 +2022-09-30 07:14, slack,- +2022-09-30 12:23, flood,50 +2022-09-30 15:50, slack,- +2022-09-30 17:45, ebb,-38 +2022-09-30 19:50, slack,- +2022-10-01 00:59, flood,47 +2022-10-01 04:26, slack,- +2022-10-01 06:15, ebb,-35 +2022-10-01 08:02, slack,- +2022-10-01 13:17, flood,47 +2022-10-01 16:44, slack,- +2022-10-01 18:39, ebb,-37 +2022-10-01 20:50, slack,- +2022-10-02 02:05, flood,44 +2022-10-02 05:26, slack,- +2022-10-02 07:03, ebb,-32 +2022-10-02 09:02, slack,- +2022-10-02 14:17, flood,44 +2022-10-02 17:38, slack,- +2022-10-02 19:27, ebb,-34 +2022-10-02 22:02, slack,- +2022-10-03 03:05, flood,41 +2022-10-03 06:26, slack,- +2022-10-03 07:57, ebb,-29 +2022-10-03 10:14, slack,- +2022-10-03 15:17, flood,42 +2022-10-03 18:44, slack,- +2022-10-03 20:27, ebb,-31 +2022-10-03 23:08, slack,- +2022-10-04 04:17, flood,39 +2022-10-04 07:32, slack,- +2022-10-04 08:57, ebb,-27 +2022-10-04 11:26, slack,- +2022-10-04 16:23, flood,40 +2022-10-04 19:50, slack,- +2022-10-04 21:27, ebb,-29 +2022-10-05 00:14, slack,- +2022-10-05 06:17, flood,40 +2022-10-05 08:32, slack,- +2022-10-05 10:09, ebb,-26 +2022-10-05 12:32, slack,- +2022-10-05 17:35, flood,40 +2022-10-05 20:56, slack,- +2022-10-05 22:33, ebb,-28 +2022-10-06 01:20, slack,- +2022-10-06 07:35, flood,44 +2022-10-06 09:38, slack,- +2022-10-06 11:21, ebb,-27 +2022-10-06 13:32, slack,- +2022-10-06 19:41, flood,43 +2022-10-06 21:56, slack,- +2022-10-06 23:45, ebb,-29 +2022-10-07 02:14, slack,- +2022-10-07 08:35, flood,50 +2022-10-07 10:38, slack,- +2022-10-07 12:33, ebb,-29 +2022-10-07 14:32, slack,- +2022-10-07 20:53, flood,49 +2022-10-07 22:56, slack,- +2022-10-08 00:51, ebb,-31 +2022-10-08 03:08, slack,- +2022-10-08 09:29, flood,54 +2022-10-08 11:26, slack,- +2022-10-08 13:33, ebb,-31 +2022-10-08 15:26, slack,- +2022-10-08 21:47, flood,53 +2022-10-08 23:50, slack,- +2022-10-09 01:45, ebb,-32 +2022-10-09 03:56, slack,- +2022-10-09 10:23, flood,56 +2022-10-09 12:20, slack,- +2022-10-09 14:21, ebb,-32 +2022-10-09 16:20, slack,- +2022-10-09 22:35, flood,55 +2022-10-10 00:38, slack,- +2022-10-10 02:33, ebb,-32 +2022-10-10 04:44, slack,- +2022-10-10 11:05, flood,56 +2022-10-10 13:08, slack,- +2022-10-10 15:09, ebb,-33 +2022-10-10 17:08, slack,- +2022-10-10 23:23, flood,55 +2022-10-11 01:26, slack,- +2022-10-11 03:15, ebb,-32 +2022-10-11 05:26, slack,- +2022-10-11 11:53, flood,52 +2022-10-11 13:56, slack,- +2022-10-11 15:51, ebb,-33 +2022-10-11 17:56, slack,- +2022-10-12 00:11, flood,52 +2022-10-12 02:14, slack,- +2022-10-12 03:57, ebb,-31 +2022-10-12 06:14, slack,- +2022-10-12 12:41, flood,47 +2022-10-12 14:38, slack,- +2022-10-12 16:27, ebb,-32 +2022-10-12 18:44, slack,- +2022-10-13 01:05, flood,49 +2022-10-13 03:02, slack,- +2022-10-13 04:39, ebb,-29 +2022-10-13 06:56, slack,- +2022-10-13 13:29, flood,42 +2022-10-13 15:26, slack,- +2022-10-13 17:09, ebb,-30 +2022-10-13 19:32, slack,- +2022-10-14 01:53, flood,45 +2022-10-14 03:50, slack,- +2022-10-14 05:21, ebb,-27 +2022-10-14 07:38, slack,- +2022-10-14 14:17, flood,37 +2022-10-14 16:08, slack,- +2022-10-14 17:51, ebb,-28 +2022-10-14 20:26, slack,- +2022-10-15 02:47, flood,41 +2022-10-15 04:44, slack,- +2022-10-15 06:09, ebb,-25 +2022-10-15 08:32, slack,- +2022-10-15 14:59, flood,32 +2022-10-15 16:56, slack,- +2022-10-15 18:33, ebb,-26 +2022-10-15 21:14, slack,- +2022-10-16 03:35, flood,38 +2022-10-16 05:32, slack,- +2022-10-16 06:51, ebb,-23 +2022-10-16 09:26, slack,- +2022-10-16 14:23, flood,30 +2022-10-16 17:44, slack,- +2022-10-16 19:15, ebb,-24 +2022-10-16 22:14, slack,- +2022-10-17 04:23, flood,35 +2022-10-17 06:20, slack,- +2022-10-17 07:39, ebb,-21 +2022-10-17 10:20, slack,- +2022-10-17 14:59, flood,29 +2022-10-17 18:32, slack,- +2022-10-17 20:03, ebb,-22 +2022-10-17 23:02, slack,- +2022-10-18 05:11, flood,33 +2022-10-18 07:08, slack,- +2022-10-18 08:33, ebb,-20 +2022-10-18 11:14, slack,- +2022-10-18 15:47, flood,29 +2022-10-18 19:20, slack,- +2022-10-18 20:51, ebb,-21 +2022-10-18 23:56, slack,- +2022-10-19 05:53, flood,32 +2022-10-19 08:02, slack,- +2022-10-19 09:27, ebb,-20 +2022-10-19 12:02, slack,- +2022-10-19 16:29, flood,31 +2022-10-19 20:14, slack,- +2022-10-19 21:51, ebb,-21 +2022-10-20 00:38, slack,- +2022-10-20 05:29, flood,32 +2022-10-20 08:50, slack,- +2022-10-20 10:21, ebb,-21 +2022-10-20 12:50, slack,- +2022-10-20 17:17, flood,33 +2022-10-20 21:02, slack,- +2022-10-20 22:45, ebb,-23 +2022-10-21 01:20, slack,- +2022-10-21 06:05, flood,35 +2022-10-21 09:38, slack,- +2022-10-21 11:15, ebb,-23 +2022-10-21 13:32, slack,- +2022-10-21 18:05, flood,37 +2022-10-21 21:50, slack,- +2022-10-21 23:33, ebb,-25 +2022-10-22 02:02, slack,- +2022-10-22 06:47, flood,38 +2022-10-22 10:20, slack,- +2022-10-22 11:57, ebb,-26 +2022-10-22 14:14, slack,- +2022-10-22 18:53, flood,40 +2022-10-22 22:32, slack,- +2022-10-23 00:15, ebb,-28 +2022-10-23 02:38, slack,- +2022-10-23 07:29, flood,42 +2022-10-23 11:02, slack,- +2022-10-23 12:45, ebb,-30 +2022-10-23 14:56, slack,- +2022-10-23 19:41, flood,44 +2022-10-23 23:20, slack,- +2022-10-24 01:03, ebb,-31 +2022-10-24 03:20, slack,- +2022-10-24 08:11, flood,47 +2022-10-24 11:44, slack,- +2022-10-24 13:27, ebb,-33 +2022-10-24 15:38, slack,- +2022-10-24 20:29, flood,48 +2022-10-25 00:02, slack,- +2022-10-25 01:51, ebb,-34 +2022-10-25 03:56, slack,- +2022-10-25 08:53, flood,51 +2022-10-25 12:26, slack,- +2022-10-25 14:15, ebb,-36 +2022-10-25 16:20, slack,- +2022-10-25 21:17, flood,51 +2022-10-26 00:50, slack,- +2022-10-26 02:33, ebb,-36 +2022-10-26 04:32, slack,- +2022-10-26 09:35, flood,53 +2022-10-26 13:08, slack,- +2022-10-26 14:57, ebb,-38 +2022-10-26 17:08, slack,- +2022-10-26 22:05, flood,52 +2022-10-27 01:38, slack,- +2022-10-27 03:21, ebb,-37 +2022-10-27 05:14, slack,- +2022-10-27 10:17, flood,54 +2022-10-27 13:50, slack,- +2022-10-27 15:45, ebb,-40 +2022-10-27 17:50, slack,- +2022-10-27 22:53, flood,52 +2022-10-28 02:26, slack,- +2022-10-28 04:09, ebb,-37 +2022-10-28 06:02, slack,- +2022-10-28 11:05, flood,53 +2022-10-28 14:38, slack,- +2022-10-28 16:33, ebb,-40 +2022-10-28 18:44, slack,- +2022-10-28 23:47, flood,49 +2022-10-29 03:20, slack,- +2022-10-29 05:03, ebb,-36 +2022-10-29 06:50, slack,- +2022-10-29 11:59, flood,50 +2022-10-29 15:26, slack,- +2022-10-29 17:21, ebb,-39 +2022-10-29 19:38, slack,- +2022-10-30 00:47, flood,46 +2022-10-30 04:14, slack,- +2022-10-30 05:51, ebb,-34 +2022-10-30 07:44, slack,- +2022-10-30 12:59, flood,46 +2022-10-30 16:26, slack,- +2022-10-30 18:15, ebb,-37 +2022-10-30 20:38, slack,- +2022-10-31 01:59, flood,43 +2022-10-31 05:14, slack,- +2022-10-31 06:45, ebb,-31 +2022-10-31 08:50, slack,- +2022-10-31 14:05, flood,43 +2022-10-31 17:26, slack,- +2022-10-31 19:09, ebb,-34 +2022-10-31 21:44, slack,- +2022-11-01 03:11, flood,42 +2022-11-01 06:14, slack,- +2022-11-01 07:39, ebb,-29 +2022-11-01 10:02, slack,- +2022-11-01 15:11, flood,41 +2022-11-01 18:26, slack,- +2022-11-01 20:03, ebb,-31 +2022-11-01 22:56, slack,- +2022-11-02 04:47, flood,41 +2022-11-02 07:14, slack,- +2022-11-02 08:39, ebb,-27 +2022-11-02 11:14, slack,- +2022-11-02 16:23, flood,40 +2022-11-02 19:32, slack,- +2022-11-02 21:09, ebb,-28 +2022-11-02 23:56, slack,- +2022-11-03 06:11, flood,44 +2022-11-03 08:14, slack,- +2022-11-03 09:51, ebb,-26 +2022-11-03 12:20, slack,- +2022-11-03 18:11, flood,41 +2022-11-03 20:32, slack,- +2022-11-03 22:15, ebb,-27 +2022-11-04 00:56, slack,- +2022-11-04 07:17, flood,47 +2022-11-04 09:14, slack,- +2022-11-04 11:21, ebb,-27 +2022-11-04 13:20, slack,- +2022-11-04 19:29, flood,45 +2022-11-04 21:38, slack,- +2022-11-04 23:33, ebb,-28 +2022-11-05 01:50, slack,- +2022-11-05 08:11, flood,50 +2022-11-05 10:14, slack,- +2022-11-05 12:39, ebb,-29 +2022-11-05 14:20, slack,- +2022-11-05 20:35, flood,49 +2022-11-05 22:32, slack,- +2022-11-06 00:39, ebb,-29 +2022-11-06 02:44, slack,- +2022-11-06 09:11, flood,53 +2022-11-06 11:02, slack,- +2022-11-06 13:33, ebb,-31 +2022-11-06 15:08, slack,- +2022-11-06 21:29, flood,53 +2022-11-06 23:26, slack,- +2022-11-07 01:33, ebb,-30 +2022-11-07 03:32, slack,- +2022-11-07 09:59, flood,54 +2022-11-07 11:56, slack,- +2022-11-07 14:21, ebb,-32 +2022-11-07 16:02, slack,- +2022-11-07 22:17, flood,54 +2022-11-08 00:20, slack,- +2022-11-08 02:15, ebb,-30 +2022-11-08 04:14, slack,- +2022-11-08 10:47, flood,52 +2022-11-08 12:38, slack,- +2022-11-08 14:57, ebb,-32 +2022-11-08 16:50, slack,- +2022-11-08 23:05, flood,54 +2022-11-09 01:08, slack,- +2022-11-09 02:57, ebb,-29 +2022-11-09 05:02, slack,- +2022-11-09 11:29, flood,48 +2022-11-09 13:26, slack,- +2022-11-09 15:33, ebb,-32 +2022-11-09 17:38, slack,- +2022-11-09 23:53, flood,52 +2022-11-10 01:56, slack,- +2022-11-10 03:39, ebb,-29 +2022-11-10 05:44, slack,- +2022-11-10 12:11, flood,43 +2022-11-10 14:08, slack,- +2022-11-10 16:03, ebb,-31 +2022-11-10 18:20, slack,- +2022-11-11 00:41, flood,48 +2022-11-11 02:44, slack,- +2022-11-11 04:15, ebb,-28 +2022-11-11 06:26, slack,- +2022-11-11 12:59, flood,38 +2022-11-11 14:56, slack,- +2022-11-11 16:39, ebb,-30 +2022-11-11 19:08, slack,- +2022-11-12 01:29, flood,45 +2022-11-12 03:26, slack,- +2022-11-12 04:57, ebb,-26 +2022-11-12 07:14, slack,- +2022-11-12 12:05, flood,34 +2022-11-12 13:41, flood,34 +2022-11-12 15:38, slack,- +2022-11-12 17:21, ebb,-28 +2022-11-12 19:56, slack,- +2022-11-13 02:17, flood,41 +2022-11-13 04:14, slack,- +2022-11-13 05:39, ebb,-25 +2022-11-13 07:56, slack,- +2022-11-13 12:53, flood,32 +2022-11-13 16:26, slack,- +2022-11-13 18:03, ebb,-27 +2022-11-13 20:44, slack,- +2022-11-14 02:59, flood,38 +2022-11-14 05:02, slack,- +2022-11-14 06:27, ebb,-24 +2022-11-14 08:50, slack,- +2022-11-14 13:35, flood,31 +2022-11-14 17:08, slack,- +2022-11-14 18:45, ebb,-25 +2022-11-14 21:32, slack,- +2022-11-15 03:29, flood,35 +2022-11-15 05:44, slack,- +2022-11-15 07:09, ebb,-23 +2022-11-15 09:44, slack,- +2022-11-15 14:23, flood,31 +2022-11-15 17:56, slack,- +2022-11-15 19:27, ebb,-24 +2022-11-15 22:26, slack,- +2022-11-16 03:23, flood,34 +2022-11-16 06:32, slack,- +2022-11-16 07:57, ebb,-22 +2022-11-16 10:32, slack,- +2022-11-16 15:11, flood,32 +2022-11-16 18:38, slack,- +2022-11-16 20:15, ebb,-24 +2022-11-16 23:08, slack,- +2022-11-17 03:59, flood,34 +2022-11-17 07:20, slack,- +2022-11-17 08:45, ebb,-22 +2022-11-17 11:26, slack,- +2022-11-17 15:59, flood,34 +2022-11-17 19:26, slack,- +2022-11-17 21:03, ebb,-24 +2022-11-17 23:56, slack,- +2022-11-18 04:35, flood,36 +2022-11-18 08:08, slack,- +2022-11-18 09:39, ebb,-23 +2022-11-18 12:14, slack,- +2022-11-18 16:41, flood,36 +2022-11-18 20:14, slack,- +2022-11-18 21:57, ebb,-25 +2022-11-19 00:38, slack,- +2022-11-19 05:17, flood,39 +2022-11-19 08:56, slack,- +2022-11-19 10:33, ebb,-25 +2022-11-19 12:56, slack,- +2022-11-19 17:35, flood,38 +2022-11-19 21:08, slack,- +2022-11-19 22:51, ebb,-26 +2022-11-20 01:14, slack,- +2022-11-20 06:05, flood,42 +2022-11-20 09:38, slack,- +2022-11-20 11:21, ebb,-28 +2022-11-20 13:44, slack,- +2022-11-20 18:23, flood,41 +2022-11-20 21:56, slack,- +2022-11-20 23:45, ebb,-29 +2022-11-21 01:56, slack,- +2022-11-21 06:47, flood,45 +2022-11-21 10:26, slack,- +2022-11-21 12:09, ebb,-31 +2022-11-21 14:26, slack,- +2022-11-21 19:11, flood,44 +2022-11-21 22:50, slack,- +2022-11-22 00:33, ebb,-31 +2022-11-22 02:38, slack,- +2022-11-22 07:29, flood,48 +2022-11-22 11:08, slack,- +2022-11-22 12:57, ebb,-34 +2022-11-22 15:14, slack,- +2022-11-22 20:05, flood,47 +2022-11-22 23:38, slack,- +2022-11-23 01:21, ebb,-33 +2022-11-23 03:20, slack,- +2022-11-23 08:17, flood,51 +2022-11-23 11:56, slack,- +2022-11-23 13:45, ebb,-37 +2022-11-23 15:56, slack,- +2022-11-23 20:59, flood,49 +2022-11-24 00:26, slack,- +2022-11-24 02:09, ebb,-34 +2022-11-24 04:08, slack,- +2022-11-24 09:05, flood,53 +2022-11-24 12:38, slack,- +2022-11-24 14:33, ebb,-39 +2022-11-24 16:44, slack,- +2022-11-24 21:47, flood,51 +2022-11-25 01:20, slack,- +2022-11-25 03:03, ebb,-35 +2022-11-25 04:50, slack,- +2022-11-25 09:59, flood,53 +2022-11-25 13:26, slack,- +2022-11-25 15:21, ebb,-40 +2022-11-25 17:38, slack,- +2022-11-25 22:41, flood,51 +2022-11-26 02:14, slack,- +2022-11-26 03:51, ebb,-35 +2022-11-26 05:44, slack,- +2022-11-26 10:47, flood,51 +2022-11-26 14:20, slack,- +2022-11-26 16:15, ebb,-40 +2022-11-26 18:26, slack,- +2022-11-26 23:35, flood,49 +2022-11-27 03:08, slack,- +2022-11-27 04:39, ebb,-34 +2022-11-27 06:38, slack,- +2022-11-27 11:41, flood,48 +2022-11-27 15:14, slack,- +2022-11-27 17:03, ebb,-39 +2022-11-27 19:26, slack,- +2022-11-28 00:47, flood,46 +2022-11-28 04:02, slack,- +2022-11-28 05:33, ebb,-33 +2022-11-28 07:38, slack,- +2022-11-28 12:47, flood,45 +2022-11-28 16:08, slack,- +2022-11-28 17:57, ebb,-37 +2022-11-28 20:26, slack,- +2022-11-29 02:11, flood,44 +2022-11-29 04:56, slack,- +2022-11-29 06:27, ebb,-31 +2022-11-29 08:44, slack,- +2022-11-29 13:59, flood,42 +2022-11-29 17:08, slack,- +2022-11-29 18:51, ebb,-34 +2022-11-29 21:26, slack,- +2022-11-30 03:35, flood,44 +2022-11-30 05:56, slack,- +2022-11-30 07:21, ebb,-29 +2022-11-30 09:50, slack,- +2022-11-30 15:11, flood,41 +2022-11-30 18:08, slack,- +2022-11-30 19:45, ebb,-31 +2022-11-30 22:32, slack,- +2022-12-01 04:47, flood,45 +2022-12-01 06:56, slack,- +2022-12-01 08:21, ebb,-27 +2022-12-01 11:02, slack,- +2022-12-01 16:41, flood,41 +2022-12-01 19:08, slack,- +2022-12-01 20:45, ebb,-28 +2022-12-01 23:32, slack,- +2022-12-02 05:47, flood,46 +2022-12-02 07:50, slack,- +2022-12-02 09:27, ebb,-26 +2022-12-02 12:02, slack,- +2022-12-02 18:05, flood,42 +2022-12-02 20:14, slack,- +2022-12-02 21:51, ebb,-26 +2022-12-03 00:32, slack,- +2022-12-03 06:47, flood,47 +2022-12-03 08:50, slack,- +2022-12-03 11:33, ebb,-27 +2022-12-03 13:02, slack,- +2022-12-03 19:11, flood,45 +2022-12-03 21:14, slack,- +2022-12-03 23:21, ebb,-26 +2022-12-04 01:26, slack,- +2022-12-04 07:47, flood,48 +2022-12-04 09:44, slack,- +2022-12-04 12:33, ebb,-29 +2022-12-04 13:56, slack,- +2022-12-04 20:11, flood,47 +2022-12-04 22:08, slack,- +2022-12-05 00:33, ebb,-27 +2022-12-05 02:14, slack,- +2022-12-05 08:41, flood,49 +2022-12-05 10:38, slack,- +2022-12-05 13:21, ebb,-30 +2022-12-05 14:50, slack,- +2022-12-05 21:11, flood,50 +2022-12-05 23:02, slack,- +2022-12-06 01:21, ebb,-27 +2022-12-06 03:02, slack,- +2022-12-06 09:35, flood,49 +2022-12-06 11:26, slack,- +2022-12-06 14:09, ebb,-31 +2022-12-06 15:44, slack,- +2022-12-06 21:59, flood,52 +2022-12-06 23:56, slack,- +2022-12-07 02:03, ebb,-27 +2022-12-07 03:44, slack,- +2022-12-07 10:23, flood,47 +2022-12-07 12:14, slack,- +2022-12-07 14:51, ebb,-31 +2022-12-07 16:26, slack,- +2022-12-07 22:47, flood,52 +2022-12-08 00:44, slack,- +2022-12-08 02:39, ebb,-27 +2022-12-08 04:32, slack,- +2022-12-08 11:05, flood,44 +2022-12-08 12:56, slack,- +2022-12-08 15:21, ebb,-31 +2022-12-08 17:14, slack,- +2022-12-08 23:29, flood,50 +2022-12-09 01:32, slack,- +2022-12-09 03:15, ebb,-27 +2022-12-09 05:14, slack,- +2022-12-09 11:47, flood,41 +2022-12-09 13:44, slack,- +2022-12-09 15:39, ebb,-30 +2022-12-09 17:56, slack,- +2022-12-10 00:17, flood,47 +2022-12-10 02:14, slack,- +2022-12-10 03:51, ebb,-26 +2022-12-10 06:02, slack,- +2022-12-10 11:05, flood,36 +2022-12-10 12:23, flood,36 +2022-12-10 14:26, slack,- +2022-12-10 16:15, ebb,-29 +2022-12-10 18:44, slack,- +2022-12-11 00:59, flood,44 +2022-12-11 03:02, slack,- +2022-12-11 04:33, ebb,-26 +2022-12-11 06:44, slack,- +2022-12-11 11:29, flood,35 +2022-12-11 15:08, slack,- +2022-12-11 16:51, ebb,-29 +2022-12-11 19:26, slack,- +2022-12-12 01:47, flood,41 +2022-12-12 03:44, slack,- +2022-12-12 05:15, ebb,-26 +2022-12-12 07:26, slack,- +2022-12-12 12:11, flood,35 +2022-12-12 15:50, slack,- +2022-12-12 17:33, ebb,-28 +2022-12-12 20:08, slack,- +2022-12-13 02:11, flood,38 +2022-12-13 04:26, slack,- +2022-12-13 05:57, ebb,-25 +2022-12-13 08:14, slack,- +2022-12-13 12:59, flood,35 +2022-12-13 16:32, slack,- +2022-12-13 18:15, ebb,-28 +2022-12-13 20:50, slack,- +2022-12-14 02:05, flood,37 +2022-12-14 05:14, slack,- +2022-12-14 06:39, ebb,-25 +2022-12-14 09:02, slack,- +2022-12-14 13:47, flood,35 +2022-12-14 17:14, slack,- +2022-12-14 18:57, ebb,-27 +2022-12-14 21:38, slack,- +2022-12-15 02:35, flood,38 +2022-12-15 05:56, slack,- +2022-12-15 07:21, ebb,-25 +2022-12-15 09:50, slack,- +2022-12-15 14:35, flood,36 +2022-12-15 17:56, slack,- +2022-12-15 19:39, ebb,-27 +2022-12-15 22:20, slack,- +2022-12-16 03:11, flood,39 +2022-12-16 06:38, slack,- +2022-12-16 08:09, ebb,-25 +2022-12-16 10:44, slack,- +2022-12-16 15:23, flood,37 +2022-12-16 18:44, slack,- +2022-12-16 20:27, ebb,-27 +2022-12-16 23:08, slack,- +2022-12-17 03:53, flood,41 +2022-12-17 07:26, slack,- +2022-12-17 08:57, ebb,-26 +2022-12-17 11:32, slack,- +2022-12-17 16:11, flood,38 +2022-12-17 19:32, slack,- +2022-12-17 21:21, ebb,-27 +2022-12-17 23:50, slack,- +2022-12-18 04:35, flood,43 +2022-12-18 08:08, slack,- +2022-12-18 09:51, ebb,-27 +2022-12-18 12:20, slack,- +2022-12-18 16:59, flood,40 +2022-12-18 20:26, slack,- +2022-12-18 22:15, ebb,-28 +2022-12-19 00:32, slack,- +2022-12-19 05:23, flood,45 +2022-12-19 08:56, slack,- +2022-12-19 10:45, ebb,-29 +2022-12-19 13:08, slack,- +2022-12-19 17:53, flood,41 +2022-12-19 21:26, slack,- +2022-12-19 23:09, ebb,-29 +2022-12-20 01:20, slack,- +2022-12-20 06:11, flood,47 +2022-12-20 09:50, slack,- +2022-12-20 11:39, ebb,-32 +2022-12-20 14:02, slack,- +2022-12-20 18:47, flood,43 +2022-12-20 22:20, slack,- +2022-12-21 00:03, ebb,-30 +2022-12-21 02:08, slack,- +2022-12-21 06:59, flood,48 +2022-12-21 10:38, slack,- +2022-12-21 12:27, ebb,-35 +2022-12-21 14:50, slack,- +2022-12-21 19:41, flood,45 +2022-12-21 23:14, slack,- +2022-12-22 00:57, ebb,-32 +2022-12-22 02:56, slack,- +2022-12-22 07:53, flood,50 +2022-12-22 11:26, slack,- +2022-12-22 13:21, ebb,-37 +2022-12-22 15:38, slack,- +2022-12-22 20:41, flood,47 +2022-12-23 00:08, slack,- +2022-12-23 01:45, ebb,-33 +2022-12-23 03:44, slack,- +2022-12-23 08:47, flood,51 +2022-12-23 12:20, slack,- +2022-12-23 14:09, ebb,-39 +2022-12-23 16:32, slack,- +2022-12-23 21:41, flood,49 +2022-12-24 01:02, slack,- +2022-12-24 02:39, ebb,-34 +2022-12-24 04:38, slack,- +2022-12-24 09:41, flood,51 +2022-12-24 13:14, slack,- +2022-12-24 15:03, ebb,-39 +2022-12-24 17:26, slack,- +2022-12-24 22:41, flood,49 +2022-12-25 01:56, slack,- +2022-12-25 03:33, ebb,-34 +2022-12-25 05:32, slack,- +2022-12-25 10:35, flood,50 +2022-12-25 14:08, slack,- +2022-12-25 15:57, ebb,-39 +2022-12-25 18:20, slack,- +2022-12-25 23:47, flood,48 +2022-12-26 02:50, slack,- +2022-12-26 04:27, ebb,-34 +2022-12-26 06:26, slack,- +2022-12-26 11:35, flood,48 +2022-12-26 15:02, slack,- +2022-12-26 16:45, ebb,-38 +2022-12-26 19:14, slack,- +2022-12-27 01:17, flood,48 +2022-12-27 03:44, slack,- +2022-12-27 05:15, ebb,-33 +2022-12-27 07:26, slack,- +2022-12-27 12:41, flood,46 +2022-12-27 15:56, slack,- +2022-12-27 17:39, ebb,-36 +2022-12-27 20:08, slack,- +2022-12-28 02:29, flood,48 +2022-12-28 04:38, slack,- +2022-12-28 06:09, ebb,-31 +2022-12-28 08:26, slack,- +2022-12-28 14:05, flood,44 +2022-12-28 16:56, slack,- +2022-12-28 18:33, ebb,-34 +2022-12-28 21:08, slack,- +2022-12-29 03:23, flood,47 +2022-12-29 05:32, slack,- +2022-12-29 07:03, ebb,-29 +2022-12-29 09:38, slack,- +2022-12-29 15:23, flood,43 +2022-12-29 17:50, slack,- +2022-12-29 19:27, ebb,-31 +2022-12-29 22:08, slack,- +2022-12-30 04:23, flood,47 +2022-12-30 06:32, slack,- +2022-12-30 08:03, ebb,-27 +2022-12-30 10:44, slack,- +2022-12-30 16:35, flood,42 +2022-12-30 18:50, slack,- +2022-12-30 20:21, ebb,-28 +2022-12-30 23:08, slack,- +2022-12-31 05:23, flood,46 +2022-12-31 07:26, slack,- +2022-12-31 09:03, ebb,-26 +2022-12-31 11:44, slack,- +2022-12-31 17:47, flood,42 +2022-12-31 19:50, slack,- +2022-12-31 21:21, ebb,-26 +2022-12-31 23:56, slack,- diff --git a/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv b/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv new file mode 100644 index 00000000..34795da8 --- /dev/null +++ b/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv @@ -0,0 +1,15 @@ +# Information,, +Information about this data can be written here (in one line without comma),, +"# Data (North direction velocity [m/s], East direction velocity [m/s], Depth [m])",, +0.0,0.05,0 +0.01,0.03,10 +0.01,0.02,20 +0.01,0.01,30 +0.0,0.01,40 +0.0,0.02,50 +0.0,0.02,60 +0.01,0.02,70 +0.01,0.02,80 +0.01,0.02,90 +0.01,0.02,100 +0.01,0.0,110 From 11eb6b680b6bbdfca0f82dfa32df8cdb6393b653 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 10 Aug 2024 21:03:09 +0530 Subject: [PATCH 17/79] fixes --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 9 +- .../ocean_current_model_plugin.hh | 6 +- gazebo/dave_gz_model_plugins/package.xml | 29 +- .../src/ocean_current_model_plugin.cc | 167 ++--- gazebo/dave_gz_world_plugins/package.xml | 5 +- .../src/ocean_current_world_plugin.cc | 6 +- gazebo/dave_ros_gz_plugins/CMakeLists.txt | 4 +- .../ocean_current_plugin.hh | 2 + gazebo/dave_ros_gz_plugins/package.xml | 1 + .../src/ocean_current_plugin.cc | 658 +++++++++--------- 10 files changed, 409 insertions(+), 478 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index 17727dae..dc636cd7 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(dave_gz_world_plugins REQUIRED) 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") @@ -50,14 +51,16 @@ install(DIRECTORY include/ DESTINATION include/ ) +# Environment hooks +ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" +) + # Following directives are used when testing. if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -# Environment Hooks -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") - ament_package() diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index a309af2c..e2914964 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -17,6 +17,8 @@ #include #include #include +#include "dave_interfaces/msg/Stratified_Current_Database.hpp" +#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" namespace dave_gz_model_plugins { @@ -27,8 +29,8 @@ class TransientCurrentPlugin : public gz::sim::System, public gz::sim::ISystemPostUpdate { public: - TransientCurrentPlugin(); // constructor - ~TransientCurrentPlugin() override = default; // Destructor + TransientCurrentPlugin(); // constructor + ~TransientCurrentPlugin(); // Destructor // Configure the plugin with necessary entities and component managers void Configure( diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index 7ffbefbf..8e6fde01 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -2,31 +2,18 @@ dave_gz_model_plugins 0.0.0 - The dave_gz_model_plugins package (gz model plugin definitions) - Gaurav Kumar and woensug - Gaurav Kumar and woensug + The dave_gz_model_plugins + Gaurav Kumar Apache 2.0 ament_cmake - - gz_ros - rclcpp - protobuf - dave_interfaces - - message_generation - gz_ros - - message_generation - gz_ros - rclcpp - protobuf - message_runtime - dave_interfaces - - gz_dev - gz_msgs dave_gz_world_plugins dave_gz_ros_plugins + dave_interfaces + rclcpp + protobuf + std_msgs + ament_lint_auto + ament_lint_common ament_cmake diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index 9c1f2f6d..7d84b772 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -1,21 +1,5 @@ -// Copyright (c) 2016 The UUV 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. - -/// \file ocean_current_model_plugin.cc #include "dave_gz_model_plugins/ocean_current_model_plugin.hh" -// #include +// #include "dave_gz_world_plugins/gauss_markov_process.hh" // #include #include #include @@ -27,11 +11,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include #include @@ -66,43 +50,26 @@ struct TransientCurrentPlugin::PrivateData // Initialize any necessary states before the plugin starts virtual void Init(); - - std::shared_ptr rosNode; // This is a smart pointer to a ROS 2 node, facilitating - // communication with other ROS nodes. std::string transientCurrentVelocityTopic; // Declare the variable (updated) - -protected: - /// \brief Pointer to world gz::sim::World world{gz::sim::kNullEntity}; - - /// \brief Pointer to model gz::sim::Entity entity{gz::sim::kNullEntity}; gz::sim::Model model{gz::sim::kNullEntity}; gz::sim::Entity modelLink{gz::sim::kNullEntity}; - - /// \brief Namespace for topics and services std::string ns; - - /// \brief Connects the update event callback virtual void Connect(); - - /// \brief Pointer to a node for communication std::shared_ptr gz_node; - - /// \brief Map of publishers - std::map publishers; - - /// \brief Current velocity topic and transient current velocity topic + // std::map publishers; + gz::transport::Node::Publisher gz_current_vel_pub; std::string currentVelocityTopic; - - /// \brief Last update time stamp std::chrono::steady_clock::duration lastUpdate{0}; - - /// \brief Last depth index - int lastDepthIndex; - - /// \brief Current linear velocity vector + int lastDepthIndex = 0; gz::math::Vector3d currentVelocity; + std::mutex lock_; + std::chrono::steady_clock::duration rosPublishPeriod{0}; + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr flowVelocityPub; + rclcpp::Subscription::SharedPtr databaseSub; + std::string modelName; /// \brief Gauss-Markov process instance for the velocity components // TODO // gz::GaussMarkovProcess currentVelNorthModel; // TODO @@ -117,7 +84,6 @@ struct TransientCurrentPlugin::PrivateData double noiseFreq_East; double noiseFreq_Down; - /// \brief Vector to read database std::vector database; /// \brief Tidal Oscillation interpolation model @@ -154,57 +120,33 @@ struct TransientCurrentPlugin::PrivateData std::array world_start_time; /// \brief Private data pointer -private: - std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr flowVelocityPub; - - /// \brief Pointer to this ROS node's handle. - std::shared_ptr rosNode; - - /// \brief Publisher for the flow velocity in the world frame - - /// \brief Subscriber for the transient ocean current database - rclcpp::Subscriber databaseSub; - - /// \brief A ROS callbackqueue that helps process messages - rclcpp::CallbackQueue databaseSubQueue; - - /// \brief A thread the keeps running the rosQueue - std::thread databaseSubQueueThread; - - /// \brief A thread mutex to lock - std::mutex lock_; - - /// \brief Period after which we should publish a message via ROS. - std::chrono::steady_clock::duration rosPublishPeriod{0}; - - /// \brief Last time we published a message via ROS. - // std::chrono::steady_clock::duration lastRosPublishTime{0}; - - std::shared_ptr ros_node_; - // std::chrono::steady_clock::duration currentTime{0}; - this->dataPtr->rosPublishPeriod = 0.05; - // ends here }; // constructor TransientCurrentPlugin::TransientCurrentPlugin() : dataPtr(std::make_unique()) {} -TransientCurrentPlugin::~TransientCurrentPlugin() -{ - // Shutdown the ROS 2 node - this->rosNode.reset(); -} +TransientCurrentPlugin::~TransientCurrentPlugin() = default; void TransientCurrentPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { - this->dataPtr->world = _entity; - this->dataPtr->entity = _entity; - this->dataPtr->model = gz::sim::Model(_entity); - this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); - this->dataPtr->sdf = _sdf; + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + gzdbg << "dave_gz_model_plugins::TransientCureentPlugin::Configure on entity: " << _entity + << std::endl; + + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); + this->dataPtr->world = gz::sim::World(worldEntity); + + auto model = gz::sim::Model(_entity); + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + // this->dataPtr->sdf = _sdf; // not needed check gzmsg << "Loading transient ocean current model plugin..." << std::endl; @@ -215,12 +157,12 @@ void TransientCurrentPlugin::Configure( else { this->dataPtr->currentVelocityTopic = - "hydrodynamics/current_velocity/" + this->dataPtr->model.Name(); - gz::sim::log::error() << "Empty flow_velocity_topic for transient_current model plugin. " - << "Default topicName definition is used" << std::endl; + "hydrodynamics/current_velocity/" + this->dataPtr->modelName; + gzerr << "Empty flow_velocity_topic for transient_current model plugin." + << "Default topicName definition is used" << std::endl; } - gz::sim::log::info() << "Transient velocity topic name for " << this->dataPtr->model.Name() - << " : " << this->dataPtr->currentVelocityTopic << std::endl; + gzmsg << "Transient velocity topic name for " << this->dataPtr->modelName << " : " + << this->dataPtr->currentVelocityTopic << std::endl; // Read the namespace for topics and services this->dataPtr->ns = _sdf->Get("namespace"); @@ -230,20 +172,20 @@ void TransientCurrentPlugin::Configure( // Advertise the ROS flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->dataPtr->ros_node_->create_publisher( - this->dataPtr->currentVelocityTopic, rclcpp::QOS(10)); + this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); // Initializing the Gazebo transport node - this->dataPtr->gz_node = transport::NodePtr(new transport::Node()); // check + this->dataPtr->gz_node = std::make_shared(); // Advertise the current velocity topic in ROS 2 - this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = + this->dataPtr->gz_current_vel_pub[this->dataPtr->currentVelocityTopic] = this->dataPtr->gz_node->create_publisher( this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); // Read topic name of stratified ocean current from SDF if (_sdf->HasElement("transient_current")) { - sdf::ElementPtr currentVelocityParams = _sdf->GetElement("transient_current"); + sdf::Element currentVelocityParams = _sdf->GetElement("transient_current"); if (currentVelocityParams->HasElement("topic_stratified")) { this->dataPtr->transientCurrentVelocityTopic = @@ -257,18 +199,14 @@ void TransientCurrentPlugin::Configure( } // Tidal Oscillation - if ( - this->dataPtr->sdf->HasElement("tide_oscillation") && - this->dataPtr->sdf->Get("tide_oscillation") == true) + if (_sdf->HasElement("tide_oscillation")) // TODO { - this->dataPtr->tideFlag = true; + this->dataPtr->tideFlag = _sdf->Get("tide_oscillation"); } else { this->dataPtr->tideFlag = false; } - - this->dataPtr->lastDepthIndex = 0; } } ///////////////////////////////////////////////// @@ -436,7 +374,7 @@ void TransientCurrentPlugin::PublishCurrentVelocity() ¤tVel, gz::math::Vector3d( this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), this->dataPtr->currentVelocity.Z())); - this->dataPtr->publishers[this->dataPtr->currentVelocityTopic]->Publish(currentVel); + this->dataPtr->gz_current_vel_pub[this->dataPtr->currentVelocityTopic]->Publish(currentVel); } ///////////////////////////////////////////////// @@ -503,7 +441,7 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() // initialize velocity_north_model parameters if (currentVelocityParams->HasElement("velocity_north")) { - sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); + sdf::Element elem = currentVelocityParams->GetElement("velocity_north"); if (elem->HasElement("mean")) { this->currentVelNorthModel.mean = 0.0; @@ -534,7 +472,7 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() // initialize velocity_east_model parameters if (currentVelocityParams->HasElement("velocity_east")) { - sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); + sdf::Element elem = currentVelocityParams->GetElement("velocity_east"); if (elem->HasElement("mean")) { this->currentVelEastModel.mean = 0.0; @@ -608,7 +546,7 @@ void TransientCurrentPlugin::PreUpdate( // Subscribe stratified ocean current database this->dataPtr->databaseSub = - this->dataPtr->rosNode + this->dataPtr->ros_node_ ->create_subscription( this->dataPtr->transientCurrentVelocityTopic, 10, std::bind(&TransientCurrentPlugin::UpdateDatabase, this, _1)); @@ -629,11 +567,22 @@ void TransientCurrentPlugin::Update( void TransientCurrentPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - // Publish the Current Velocity. - if (!_info.paused && _info.simTime > this->dataPtr->lastUpdate) + // Publish the Current Velocity. TODO find if we really need this + // if (!_info.paused && _info.simTime > this->dataPtr->lastUpdate) + // { + // this->dataPtr->lastUpdate = _info.simTime; + // PostPublishCurrentVelocity(); + // } + this->dataPtr->lastUpdate = _info.simTime; + PostPublishCurrentVelocity(); + if (!_info.paused) { - this->dataPtr->lastUpdate = _info.simTime; - PostPublishCurrentVelocity(); + rclcpp::spin_some(this->rosNode); + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_gz_model_plugins::TransientCurrentPlugin::PostUpdate" << std::endl; + } } } } // namespace dave_gz_model_plugins \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/package.xml b/gazebo/dave_gz_world_plugins/package.xml index b0d5d322..7a66c865 100644 --- a/gazebo/dave_gz_world_plugins/package.xml +++ b/gazebo/dave_gz_world_plugins/package.xml @@ -2,9 +2,8 @@ dave_gz_world_plugins 0.0.0 - The dave_gz_world_plugins package (gz world plugin definitions) - Gaurav Kumar and woensug - Gaurav Kumar and woensug + The dave_gz_world_plugins + Gaurav Kumar TODO: License declaration ament_cmake diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 24a886af..3b5870dd 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -216,7 +216,8 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() this->dataPtr->tideFlag = true; this->dataPtr->tidalHarmonicFlag = false; - sdf::ElementPtr tidalOscillationParams = this->dataPtr->sdf->GetElement("tidal_oscillation"); + sdf::ElementPtr tidalOscillationParams = + this->dataPtr->sdf->GetElement("tidal_oscillation"); // include this xml/ (TODO) sdf::ElementPtr tidalHarmonicParams; // Read the tidal oscillation parameter from the SDF file // (TO DO)(TO UNDERSTAND) @@ -700,7 +701,8 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check for (std::vector::iterator it = this->dataPtr->currentStratifiedVelocity.begin(); - it != this->dataPtr->currentStratifiedVelocity.end(); ++it) + it != this->dataPtr->currentStratifiedVelocity.end(); + ++it) // currentStratifiedVelocity values defined where ? (TODO) { gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); currentVel.add_depth(it->W()); diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 2f0082ae..31f899a1 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(gz-common5 REQUIRED COMPONENTS profiler) find_package(gz-sim8 REQUIRED) find_package(geometry_msgs REQUIRED) find_package(dave_interfaces REQUIRED) +find_package(dave_gz_world_plugins REQUIRED) find_package(Boost REQUIRED COMPONENTS system) # Set version variables @@ -26,7 +27,7 @@ add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cc) # Include directories target_include_directories(SphericalCoords PRIVATE include) -target_include_directories(ocean_current_plugin PRIVATE include) +target_include_directories(ocean_current_plugin PRIVATE include ${dave_gz_world_plugins_INCLUDE_DIRS}) # Linking target_link_libraries(SphericalCoords gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -45,6 +46,7 @@ ament_target_dependencies(ocean_current_plugin std_msgs geometry_msgs dave_interfaces + dave_gz_world_plugins ) # Install targets diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index 84b39c61..6ddd1349 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -49,6 +49,8 @@ #include "dave_interfaces/srv/Set_Origin_Spherical_Coord.hpp" #include "dave_interfaces/srv/Set_Stratified_Current_Direction.hpp" #include "dave_interfaces/srv/Set_Stratified_Current_Velocity.hpp" +// #include + // #include "dave_interfaces/srv/Stratified_Current_Database.hpp" // #include "dave_interfaces/srv/Stratified_Current_Velocity.hpp" diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml index 218f37f3..44bab929 100644 --- a/gazebo/dave_ros_gz_plugins/package.xml +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -10,6 +10,7 @@ protobuf-dev rclcpp dave_interfaces + dave_gz_world_plugins sensor_msgs geometry_msgs std_msgs diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index b6c6e9c9..7fb5c95a 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,33 +1,21 @@ #include "dave_ros_gz_plugins/ocean_current_plugin.hh" -#include - +#include +#include #include -#include - #include -#include - -#include -#include - -#include #include - -#include +#include +#include #include #include #include +#include +#include +#include +#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include "gz/plugin/Register.hh" #include "gz/sim/components/World.hh" -// Things to consider/decide for now - -// 1. Is consistency being maintained while the use of "gzerr" and "RCLCPP_ERROR" for error logging -// ? -// 2. Do the Headers included in the code exist ? -// 3. use of gz_bridge and message types. -// 4. Integration with the rest of the system with respect to the use of smart pointers and other -// parameters like behaviour. - GZ_ADD_PLUGIN( dave_ros_gz_plugins::UnderwaterCurrentROSPlugin, gz::sim::System, dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::ISystemConfigure, @@ -54,7 +42,6 @@ struct UnderwaterCurrentROSPlugin::PrivateData set_stratified_current_horz_angle_model; rclcpp::Service::SharedPtr set_stratified_current_vert_angle_model; - std::shared_ptr rosNode; gz::sim::World world; gz::sim::Model model; @@ -96,18 +83,9 @@ void UnderwaterCurrentROSPlugin::Configure( std::chrono::steady_clock::duration lastUpdate{0}; // auto lastUpdate = gz::sim::simTime; - try - { - this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = - this->dataPtr->stratifiedCurrentVelocityTopic + "_database"; - } - catch (const gz::common::Exception & e) - // we can use both "_e" and "e" but "e" is now - // the conventional way to display errors in C++. - { - gzerr << "Error loading plugin: " << e.what() << '\n'; - return; - } + + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = + this->dataPtr->stratifiedCurrentVelocityTopic + "_database"; if (!rclcpp::ok()) { @@ -224,361 +202,367 @@ void UnderwaterCurrentROSPlugin::Update( // Updating for stratified behaviour of Ocean Currents // What is the .size value over here, to be (checked) - for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); i++) { - geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); - velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); - velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); - stratCurrentVelocityMsg.velocities.push_back(velocity); - stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); - } + for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); + i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) + { + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); + velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); + velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); + stratCurrentVelocityMsg.velocities.push_back(velocity); + stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); + } - this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); + this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); - // Generate and publish stratified current database - dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) - { - // Stratified current database entry preparation - geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->stratifiedDatabase[i].X(); - velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); - velocity.z = 0.0; // Assuming z is intentionally set to 0.0 - currentDatabaseMsg.velocities.push_back(velocity); - currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); - } + // Generate and publish stratified current database + dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; + for (int i = 0; i < this->dataPtr->stratifiedDatabase + .size(); // again check with ocean_cureent_world_plugin.cc (TODO) + i++) // read from csv file in ocean_cureent_world_plugin.cc + { + // Stratified current database entry preparation + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->dataPtr->stratifiedDatabase[i].X(); + velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); + velocity.z = 0.0; // Assuming z is intentionally set to 0.0 + currentDatabaseMsg.velocities.push_back(velocity); + currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); + } - if (this->dataPtr->tidalHarmonicFlag) - { - // Tidal harmonic constituents - currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; - currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; - currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; - currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; - currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; - currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; - currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; - currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; - currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; - currentDatabaseMsg.tideConstituents = true; - } - else - { - for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) + if (this->dataPtr + ->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) { - // Tidal oscillation database - currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); - currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); - currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); - currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); - currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); - - currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); + // Tidal harmonic constituents + currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; + currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; + currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; + currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; + currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; + currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; + currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; + currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; + currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; + currentDatabaseMsg.tideConstituents = true; + } + else + { + for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) + { + // Tidal oscillation database + currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); + currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); + currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); + currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); + currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); + + currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); + } + currentDatabaseMsg.tideConstituents = false; } - currentDatabaseMsg.tideConstituents = false; - } - currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; - currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; + currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; + currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; - currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; - currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; - currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; - currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; - currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; + currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; + currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; + currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; + currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; + currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; - this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); + this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); + } } -} -///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -{ - // Advertise the service to update the current velocity model - this->dataPtr->set_current_velocity_model = - this->dataPtr->rosNode->create_service( - "set_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current horizontal angle model - this->dataPtr->set_current_horz_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current vertical angle model - this->dataPtr->set_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, + ///////////////////////////////////////////////// + void UnderwaterCurrentROSPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) + { + // Advertise the service to update the current velocity model + this->dataPtr->set_current_velocity_model = + this->dataPtr->rosNode->create_service( + "set_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current velocity mean value - this->dataPtr->set_current_velocity = - this->dataPtr->rosNode->create_service( - "set_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the stratified current velocity - this->dataPtr->set_stratified_current_velocity = - this->dataPtr->rosNode->create_service( - "set_stratified_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, - this, std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current horizontal angle - this->dataPtr->set_current_horz_angle = - this->dataPtr->rosNode->create_service( - "set_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, + // Advertise the service to update the current horizontal angle model + this->dataPtr->set_current_horz_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current vertical angle model + this->dataPtr->set_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current velocity mean value + this->dataPtr->set_current_velocity = + this->dataPtr->rosNode->create_service( + "set_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current horizontal angle - this->dataPtr->set_stratified_current_horz_angle_model = - this->dataPtr->rosNode->create_service( - "set_stratified_current_horz_angle_model", - std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel, this, std::placeholders::_1, - std::placeholders::_2)); - - // Advertise the service to update the current vertical angle - this->dataPtr->set_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, - std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current vertical angle - this->dataPtr->set_stratified_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_stratified_current_vert_angle_model", - std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel, this, std::placeholders::_1, - std::placeholders::_2)); - - // Update the time tracking for publication - this->dataPtr->lastUpdate = _info.simTime; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); + // Advertise the service to update the stratified current velocity + this->dataPtr->set_stratified_current_velocity = + this->dataPtr->rosNode->create_service( + "set_stratified_current_velocity", + std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, this, std::placeholders::_1, + std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle + this->dataPtr->set_current_horz_angle = + this->dataPtr->rosNode->create_service( + "set_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, + std::placeholders::_1, std::placeholders::_2)); + // Advertise the service to update the stratified current horizontal angle + this->dataPtr->set_stratified_current_horz_angle_model = + this->dataPtr->rosNode->create_service( + "set_stratified_current_horz_angle_model", + std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel, this, std::placeholders::_1, + std::placeholders::_2)); + + // Advertise the service to update the current vertical angle + this->dataPtr->set_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current vertical angle + this->dataPtr->set_stratified_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_stratified_current_vert_angle_model", + std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel, this, std::placeholders::_1, + std::placeholders::_2)); + + // Update the time tracking for publication + this->dataPtr->lastUpdate = _info.simTime; + } + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res) + { + _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); - return true; -} + return true; + } -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) { - _res->success = false; + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + if (_res->success) + { + // Update the database values (new angle, unchanged velocity) + double velocity = hypot( + this->dataPtr->stratifiedDatabase[_req->layer].X(), + this->dataPtr->stratifiedDatabase[_req->layer].Y()); + this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + } return true; } - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); - if (_res->success) + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res) { - // Update the database values (new angle, unchanged velocity) - double velocity = hypot( - this->dataPtr->stratifiedDatabase[_req->layer].X(), - this->dataPtr->stratifiedDatabase[_req->layer].Y()); - this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); + return true; } - return true; -} - -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); - return true; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) { - _res->success = false; + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); return true; } - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); - return true; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - if ( - this->dataPtr->currentVelModel.SetMean(_req->velocity) && - this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && - this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) { - 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 - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; + if ( + this->dataPtr->currentVelModel.SetMean(_req->velocity) && + this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && + this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) + { + 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 + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; + } + else + { + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; + } + return true; } - else + + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) { - gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + { + _res->success = false; + return true; + } + if ( + this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + { + // Update the database values as well + this->dataPtr->stratifiedDatabase[_req->layer].X() = + cos(_req->horizontal_angle) * _req->velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = + sin(_req->horizontal_angle) * _req->velocity; + gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl + << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl + << " Vertical angle [rad] = " << _req.vertical_angle << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; + } + else + { + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; + } + return true; } - return true; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) { - _res->success = false; + _res->mean = this->dataPtr->currentVelModel.mean; + _res->min = this->dataPtr->currentVelModel.min; + _res->max = this->dataPtr->currentVelModel.max; + _res->noise = this->dataPtr->currentVelModel.noiseAmp; + _res->mu = this->dataPtr->currentVelModel.mu; return true; } - if ( - this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && - this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && - this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) { - // Update the database values as well - this->dataPtr->stratifiedDatabase[_req->layer].X() = - cos(_req->horizontal_angle) * _req->velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = - sin(_req->horizontal_angle) * _req->velocity; - gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl - << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl - << " Vertical angle [rad] = " << _req.vertical_angle << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; + _res->mean = this->dataPtr->currentHorzAngleModel.mean; + _res->min = this->dataPtr->currentHorzAngleModel.min; + _res->max = this->dataPtr->currentHorzAngleModel.max; + _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentHorzAngleModel.mu; + return true; } - else + + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) { - gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; + _res->mean = this->dataPtr->currentVertAngleModel.mean; + _res->min = this->dataPtr->currentVertAngleModel.min; + _res->max = this->dataPtr->currentVertAngleModel.max; + _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentVertAngleModel.mu; + return true; } - return true; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->mean = this->dataPtr->currentVelModel.mean; - _res->min = this->dataPtr->currentVelModel.min; - _res->max = this->dataPtr->currentVelModel.max; - _res->noise = this->dataPtr->currentVelModel.noiseAmp; - _res->mu = this->dataPtr->currentVelModel.mu; - return true; -} - -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->mean = this->dataPtr->currentHorzAngleModel.mean; - _res->min = this->dataPtr->currentHorzAngleModel.min; - _res->max = this->dataPtr->currentHorzAngleModel.max; - _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentHorzAngleModel.mu; - return true; -} + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) + { + _res->success = this->dataPtr->currentVelModel.SetModel( + std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, + _req->noise); -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->mean = this->dataPtr->currentVertAngleModel.mean; - _res->min = this->dataPtr->currentVertAngleModel.min; - _res->max = this->dataPtr->currentVertAngleModel.max; - _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentVertAngleModel.mu; - return true; -} + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) + { + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) + model.SetModel( + model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); + } -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->success = this->dataPtr->currentVelModel.SetModel( - std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, - _req->noise); + gzmsg << "Current velocity model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentVelModel.Print(); - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) - { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) - model.SetModel( - model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); + return true; } + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) + { + _res->success = this->dataPtr->currentHorzAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); - gzmsg << "Current velocity model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVelModel.Print(); + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) + { + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; + model.SetModel( + model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); + } - return true; -} -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->success = this->dataPtr->currentHorzAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + gzmsg << "Horizontal angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentHorzAngleModel.Print(); + return true; - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) - { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; - model.SetModel( - model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); + // gzmsg << "Horizontal angle model updated" << std::endl + // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + // this->dataPtr->currentHorzAngleModel.Print(); } - gzmsg << "Horizontal angle model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentHorzAngleModel.Print(); - return true; - - // gzmsg << "Horizontal angle model updated" << std::endl - // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->dataPtr->currentHorzAngleModel.Print(); -} - -///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) -{ - _res->success = this->dataPtr->currentVertAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + ///////////////////////////////////////////////// + bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) + { + _res->success = this->dataPtr->currentVertAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); - gzmsg << "Vertical angle model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVertAngleModel.Print(); - return true; -} + gzmsg << "Vertical angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentVertAngleModel.Print(); + return true; + } -///////////////////////////////////////////////// -GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) + ///////////////////////////////////////////////// + GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) } // namespace dave_ros_gz_plugins // #endif \ No newline at end of file From a241227cf44b4265c79d4e9972cb0f77f3c1efa0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 12 Aug 2024 19:22:04 +0530 Subject: [PATCH 18/79] minor code fixes --- .../ocean_current_model_plugin.hh | 12 +- .../src/ocean_current_model_plugin.cc | 103 +++- .../src/ocean_current_world_plugin.cc | 50 +- .../ocean_current_plugin.hh | 15 - .../src/ocean_current_plugin.cc | 512 +++++++++--------- 5 files changed, 352 insertions(+), 340 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index e2914964..40c7f1ae 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -45,6 +45,15 @@ public: // Function called after the simulation state updates void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + void TransientCurrentPlugin::LoadCurrentVelocityParams( + sdf::ElementPtr _sdf, 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); + /// \brief Check if an entity is enabled or not. /// \param[in] _entity Target entity /// \param[in] _ecm Entity component manager @@ -58,7 +67,7 @@ public: void databaseSubThread(); /// \brief Calculate ocean current using database and vehicle state - void CalculateOceanCurrent(); + void CalculateOceanCurrent(double vehicleDepth); void Gauss_Markov_process_initialize(); @@ -69,6 +78,7 @@ public: void PublishCurrentVelocity(); private: + std::shared_ptr ros_node_; struct PrivateData; std::unique_ptr dataPtr; }; diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index 7d84b772..a89324d8 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -2,15 +2,18 @@ // #include "dave_gz_world_plugins/gauss_markov_process.hh" // #include #include +#include #include #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -67,8 +70,9 @@ struct TransientCurrentPlugin::PrivateData std::mutex lock_; std::chrono::steady_clock::duration rosPublishPeriod{0}; std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr flowVelocityPub; + rclcpp::Publisher::SharedPtr flowVelocityPub; rclcpp::Subscription::SharedPtr databaseSub; + // std::shared_ptr> flowVelocityPub; std::string modelName; /// \brief Gauss-Markov process instance for the velocity components // TODO @@ -118,8 +122,7 @@ struct TransientCurrentPlugin::PrivateData /// \brief Tidal oscillation world start time (GMT) std::array world_start_time; - - /// \brief Private data pointer + gz::sim::Entity modelEntity; }; // constructor @@ -139,12 +142,16 @@ void TransientCurrentPlugin::Configure( gzdbg << "dave_gz_model_plugins::TransientCureentPlugin::Configure on entity: " << _entity << std::endl; + // Make a clone so that we can call non-const methods + sdf::ElementPtr sdfClone = _sdf->Clone(); + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); this->dataPtr->world = gz::sim::World(worldEntity); auto model = gz::sim::Model(_entity); this->dataPtr->model = model; this->dataPtr->modelName = model.Name(_ecm); + this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->modelName, _ecm); // this->dataPtr->sdf = _sdf; // not needed check @@ -167,25 +174,35 @@ void TransientCurrentPlugin::Configure( this->dataPtr->ns = _sdf->Get("namespace"); // Initialize the ROS 2 node - this->dataPtr->ros_node_ = std::make_shared("TransirentCurrentPlugin"); + this->ros_node_ = std::make_shared("TransirentCurrentPlugin"); // Advertise the ROS flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = - this->dataPtr->ros_node_->create_publisher( + this->ros_node_->create_publisher( this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); // Initializing the Gazebo transport node this->dataPtr->gz_node = std::make_shared(); // Advertise the current velocity topic in ROS 2 - this->dataPtr->gz_current_vel_pub[this->dataPtr->currentVelocityTopic] = - this->dataPtr->gz_node->create_publisher( - this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); + this->dataPtr->gz_current_vel_pub = + this->dataPtr->gz_node->Advertise( + this->dataPtr->currentVelocityTopic); // Read topic name of stratified ocean current from SDF - if (_sdf->HasElement("transient_current")) + LoadCurrentVelocityParams(sdfClone, _ecm); +} + +///////////////////////////////////////////////// +void TransientCurrentPlugin::LoadCurrentVelocityParams( + sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm) +{ + // Read topic name of stratified ocean current from SDF + sdf::ElementPtr currentVelocityParams; + if (_sdf->HasElement("transient_current")) // Add this to the sdf file TODO { - sdf::Element currentVelocityParams = _sdf->GetElement("transient_current"); + currentVelocityParams = _sdf->GetElement( + "transient_current"); // tried const sdf::Element switched to using Auto (check) if (currentVelocityParams->HasElement("topic_stratified")) { this->dataPtr->transientCurrentVelocityTopic = @@ -214,14 +231,46 @@ void TransientCurrentPlugin::Init() { // Doing nothing for now } +////////////////////////////////////////// + +gz::sim::Entity TransientCurrentPlugin::GetModelEntity( + const std::string & modelName, gz::sim::EntityComponentManager & ecm) +{ + gz::sim::Entity modelEntity = gz::sim::kNullEntity; + + ecm.Each( + [&](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 TransientCurrentPlugin::GetModelPose( + const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm) +{ + const auto * poseComp = ecm.Component(modelEntity); + if (poseComp) + { + return poseComp->Data(); + } + else + { + gzerr << "Pose component not found for entity: " << modelEntity << std::endl; + return gz::math::Pose3d::Zero; + } +} ///////////////////////////////////////////////// -void TransientCurrentPlugin::CalculateOceanCurrent() +void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) { this->dataPtr->lock_.lock(); - // Update vehicle position - double vehicleDepth = -this->dataPtr->model->WorldPose().Pos().Z(); - if (this->dataPtr->database.size() == 0) { // skip for next time (waiting for valid database subscrition) @@ -286,7 +335,7 @@ void TransientCurrentPlugin::CalculateOceanCurrent() if (this->dataPtr->tideFlag) { // Update tide oscillation - this->dataPtr->time = this->dataPtr->currentTime; + this->dataPtr->time = this->dataPtr->lastUpdate; if (this->dataPtr->tide_Constituents) { @@ -335,9 +384,6 @@ void TransientCurrentPlugin::CalculateOceanCurrent() this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; - // Update time - this->dataPtr->time = this->dataPtr->lastUpdate; - // Update current velocity double velocityNorth = this->dataPtr->currentVelNorthModel.Update((this->dataPtr->time).Double()); @@ -374,7 +420,7 @@ void TransientCurrentPlugin::PublishCurrentVelocity() ¤tVel, gz::math::Vector3d( this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), this->dataPtr->currentVelocity.Z())); - this->dataPtr->gz_current_vel_pub[this->dataPtr->currentVelocityTopic]->Publish(currentVel); + this->dataPtr->gz_current_vel_pub.Publish(currentVel); } ///////////////////////////////////////////////// @@ -540,16 +586,14 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() void TransientCurrentPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - this->dataPtr->currentTime = _info.simTime; - this->dataPtr->time = this->dataPtr->currentTime; + this->dataPtr->time = _info.simTime; this->dataPtr->Gauss_Markov_process_initialize(); // something is wrong here (check) // Subscribe stratified ocean current database this->dataPtr->databaseSub = - this->dataPtr->ros_node_ - ->create_subscription( - this->dataPtr->transientCurrentVelocityTopic, 10, - std::bind(&TransientCurrentPlugin::UpdateDatabase, this, _1)); + this->ros_node_->create_subscription( + this->dataPtr->transientCurrentVelocityTopic, 10, + std::bind(&TransientCurrentPlugin::UpdateDatabase, this, _1)); // Connect the update event callback for ROS and ocean current calculation this->dataPtr->Connect(); @@ -561,7 +605,10 @@ void TransientCurrentPlugin::PreUpdate( void TransientCurrentPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - this->dataPtr->calculateOceanCurrent(); + // Update vehicle position + gz::math::Pose3d vehicle_pos = GetModelPose(this->dataPtr->modelEntity, _ecm); + double vehicleDepth = std::abs(vehicle_pos.Z()); + CalculateOceanCurrent(vehicleDepth); } ///////////////////////////////////////////////// void TransientCurrentPlugin::PostUpdate( @@ -574,10 +621,10 @@ void TransientCurrentPlugin::PostUpdate( // PostPublishCurrentVelocity(); // } this->dataPtr->lastUpdate = _info.simTime; - PostPublishCurrentVelocity(); + PublishCurrentVelocity(); if (!_info.paused) { - rclcpp::spin_some(this->rosNode); + rclcpp::spin_some(this->ros_node_); if (_info.iterations % 1000 == 0) { diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 3b5870dd..131621c7 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -1,20 +1,3 @@ -// Copyright (c) 2016 The UUV 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. - -/// \file ocean_current_world_plugin.cc - #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include @@ -43,11 +26,6 @@ #include "gz/sim/components/World.hh" // #include TODO (235-239) -// using namespace gz; -// using namespace sim; -// using namespace systems; -// using namespace dave_gz_world_plugins; - GZ_ADD_PLUGIN( dave_gz_world_plugins::UnderwaterCurrentPlugin, gz::sim::System, dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemConfigure, @@ -58,13 +36,8 @@ namespace dave_gz_world_plugins { struct UnderwaterCurrentPlugin::PrivateData { - /// \brief Pointer to world gz::sim::World world{gz::sim::kNullEntity}; - - /// \brief Pointer to sdf sdf::ElementPtr sdf; - - /// \brief True if the sea surface is present bool hasSurface; /// \brief Pointer to a node for communication @@ -72,7 +45,7 @@ struct UnderwaterCurrentPlugin::PrivateData /// \brief Map of publishers // gz::transport::Node::Publisher publishers; - std::map publishers; + gz::transport::Node::Publisher gz_node_cvel_world_pub; /// \brief Vehicle Depth Subscriber gz::transport::Node subscriber; @@ -103,20 +76,10 @@ struct UnderwaterCurrentPlugin::PrivateData /// \brief Gauss-Markov process instance for horizontal angle model gz::GaussMarkovProcess currentHorzAngleModel; - - /// \brief Gauss-Markov process instance for vertical angle model gz::GaussMarkovProcess currentVertAngleModel; - - /// \brief Vector of Gauss-Markov process instances for stratified velocity std::vector> stratifiedCurrentModels; - - /// \brief Vector of dateGMT for tidal oscillation std::vector> dateGMT; - - /// \brief Vector of speedcmsec for tidal oscillation std::vector speedcmsec; - - /// \brief Tidal current harmonic constituents bool tidalHarmonicFlag; double M2_amp; @@ -497,7 +460,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() } csvFile.close(); - this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic] = + this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node->Advertise( this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); @@ -677,9 +640,8 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() std::chrono::duration(this->dataPtr->lastUpdate).count(); // Advertise the current velocity & stratified current velocity topics - this->dataPtr->publishers[this->dataPtr->currentVelocityTopic] = - this->dataPtr->gz_node->Advertise( - this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); + this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node->Advertise( + this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); gzmsg << "Current velocity topic name: " << this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic << std::endl; } @@ -692,7 +654,7 @@ void UnderwaterCurrentPlugin::PublishCurrentVelocity() ¤tVel, gz::math::Vector3d( this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), this->dataPtr->currentVelocity.Z())); - this->dataPtr->publishers[this->dataPtr->currentVelocityTopic].Publish(currentVel); + this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); } ///////////////////////////////////////////////// @@ -711,7 +673,7 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() { return; } - this->dataPtr->publishers[this->dataPtr->stratifiedCurrentVelocityTopic].Publish(currentVel); + this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); } ///////////////////////////////////////////////// diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index 6ddd1349..b271e823 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -1,18 +1,3 @@ -// 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. - /// \file ocean_current_plugin.hh /// \brief Publishes the ocean current velocity in ROS messages and cxreates a /// service to alter the flow model in runtime diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 7fb5c95a..85df596f 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,5 +1,5 @@ #include "dave_ros_gz_plugins/ocean_current_plugin.hh" -#include +// #include #include #include #include @@ -12,7 +12,7 @@ #include #include #include -#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" +// #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include "gz/plugin/Register.hh" #include "gz/sim/components/World.hh" @@ -36,12 +36,14 @@ struct UnderwaterCurrentROSPlugin::PrivateData rclcpp::Service::SharedPtr set_current_horz_angle_model; rclcpp::Service::SharedPtr set_current_vert_angle_model; rclcpp::Service::SharedPtr set_current_velocity; + rclcpp::Service::SharedPtr set_current_horz_angle; + rclcpp::Service::SharedPtr set_current_vert_angle; rclcpp::Service::SharedPtr set_stratified_current_velocity; rclcpp::Service::SharedPtr - set_stratified_current_horz_angle_model; + set_stratified_current_horz_angle; rclcpp::Service::SharedPtr - set_stratified_current_vert_angle_model; + set_stratified_current_vert_angle; std::shared_ptr rosNode; gz::sim::World world; gz::sim::Model model; @@ -274,295 +276,301 @@ void UnderwaterCurrentROSPlugin::Update( this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); } } - ///////////////////////////////////////////////// - void UnderwaterCurrentROSPlugin::PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) - { - // Advertise the service to update the current velocity model - this->dataPtr->set_current_velocity_model = - this->dataPtr->rosNode->create_service( - "set_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, +} +///////////////////////////////////////////////// +void UnderwaterCurrentROSPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + // Advertise the service to update the current velocity model + this->dataPtr->set_current_velocity_model = + this->dataPtr->rosNode->create_service( + "set_current_velocity_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle model + this->dataPtr->set_current_horz_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_horz_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current horizontal angle model - this->dataPtr->set_current_horz_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current vertical angle model - this->dataPtr->set_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the current velocity mean value - this->dataPtr->set_current_velocity = - this->dataPtr->rosNode->create_service( - "set_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, + // Advertise the service to update the current vertical angle model + this->dataPtr->set_current_vert_angle_model = + this->dataPtr->rosNode->create_service( + "set_current_vert_angle_model", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current velocity mean value + this->dataPtr->set_current_velocity = + this->dataPtr->rosNode->create_service( + "set_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, + std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current velocity + this->dataPtr->set_stratified_current_velocity = + this->dataPtr->rosNode->create_service( + "set_stratified_current_velocity", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, + this, std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the current horizontal angle //TODO + this->dataPtr->set_current_horz_angle = + this->dataPtr->rosNode->create_service( + "set_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current velocity - this->dataPtr->set_stratified_current_velocity = - this->dataPtr->rosNode->create_service( - "set_stratified_current_velocity", - std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, this, std::placeholders::_1, - std::placeholders::_2)); - - // Advertise the service to update the current horizontal angle - this->dataPtr->set_current_horz_angle = - this->dataPtr->rosNode->create_service( - "set_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, - std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current horizontal angle - this->dataPtr->set_stratified_current_horz_angle_model = - this->dataPtr->rosNode->create_service( - "set_stratified_current_horz_angle_model", - std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel, this, std::placeholders::_1, - std::placeholders::_2)); - - // Advertise the service to update the current vertical angle - this->dataPtr->set_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to update the stratified current vertical angle - this->dataPtr->set_stratified_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "set_stratified_current_vert_angle_model", - std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel, this, std::placeholders::_1, - std::placeholders::_2)); - - // Update the time tracking for publication - this->dataPtr->lastUpdate = _info.simTime; - } - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( - const std::shared_ptr _req, - std::shared_ptr _res) - { - _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); + // Advertise the service to update the current horizontal angle //TODO + this->dataPtr->set_current_vert_angle = + this->dataPtr->rosNode->create_service( + "set_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, + std::placeholders::_1, std::placeholders::_2)); - return true; - } + // Advertise the service to update the stratified current horizontal angle + this->dataPtr->set_stratified_current_horz_angle = + this->dataPtr->rosNode->create_service( + "set_stratified_current_horz_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, + this, std::placeholders::_1, std::placeholders::_2)); + + // // Advertise the service to update the current vertical angle + // this->dataPtr->set_current_vert_angle_model = + // this->dataPtr->rosNode->create_service( + // "set_current_vert_angle_model", std::bind( + // &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, + // std::placeholders::_1, std::placeholders::_2)); + + // Advertise the service to update the stratified current vertical angle + this->dataPtr->set_stratified_current_vert_angle = + this->dataPtr->rosNode->create_service( + "set_stratified_current_vert_angle", std::bind( + &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, + this, std::placeholders::_1, std::placeholders::_2)); + + // Update the time tracking for publication + this->dataPtr->lastUpdate = _info.simTime; +} +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) - { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) - { - _res->success = false; - return true; - } + return true; +} - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); - if (_res->success) - { - // Update the database values (new angle, unchanged velocity) - double velocity = hypot( - this->dataPtr->stratifiedDatabase[_req->layer].X(), - this->dataPtr->stratifiedDatabase[_req->layer].Y()); - this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; - } +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + { + _res->success = false; return true; } - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateVertAngle( - const std::shared_ptr _req, - std::shared_ptr _res) + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + if (_res->success) { - _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); - return true; + // Update the database values (new angle, unchanged velocity) + double velocity = hypot( + this->dataPtr->stratifiedDatabase[_req->layer].X(), + this->dataPtr->stratifiedDatabase[_req->layer].Y()); + this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; } + return true; +} + +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); + return true; +} - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateStratVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) - { - _res->success = false; - return true; - } - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + _res->success = false; return true; } + _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + return true; +} - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if ( + this->dataPtr->currentVelModel.SetMean(_req->velocity) && + this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && + this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) { - if ( - this->dataPtr->currentVelModel.SetMean(_req->velocity) && - this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && - this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) - { - 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 - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; - } - else - { - gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; - } - return true; + 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 + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; } - - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( - const std::shared_ptr _req, - std::shared_ptr _res) + else { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) - { - _res->success = false; - return true; - } - if ( - this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && - this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && - this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) - { - // Update the database values as well - this->dataPtr->stratifiedDatabase[_req->layer].X() = - cos(_req->horizontal_angle) * _req->velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = - sin(_req->horizontal_angle) * _req->velocity; - gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl - << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl - << " Vertical angle [rad] = " << _req.vertical_angle << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; - } - else - { - gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; - } - return true; + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; } + return true; +} - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) { - _res->mean = this->dataPtr->currentVelModel.mean; - _res->min = this->dataPtr->currentVelModel.min; - _res->max = this->dataPtr->currentVelModel.max; - _res->noise = this->dataPtr->currentVelModel.noiseAmp; - _res->mu = this->dataPtr->currentVelModel.mu; + _res->success = false; return true; } - - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + if ( + this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) { - _res->mean = this->dataPtr->currentHorzAngleModel.mean; - _res->min = this->dataPtr->currentHorzAngleModel.min; - _res->max = this->dataPtr->currentHorzAngleModel.max; - _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentHorzAngleModel.mu; - return true; + // Update the database values as well + this->dataPtr->stratifiedDatabase[_req->layer].X() = + cos(_req->horizontal_angle) * _req->velocity; + this->dataPtr->stratifiedDatabase[_req->layer].Y() = + sin(_req->horizontal_angle) * _req->velocity; + gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl + << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl + << " Vertical angle [rad] = " << _req.vertical_angle << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + _res.success = true; } - - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) + else { - _res->mean = this->dataPtr->currentVertAngleModel.mean; - _res->min = this->dataPtr->currentVertAngleModel.min; - _res->max = this->dataPtr->currentVertAngleModel.max; - _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentVertAngleModel.mu; - return true; + gzmsg << "Error while updating the current velocity" << std::endl; + _res.success = false; } + return true; +} - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( - const std::shared_ptr _req, - std::shared_ptr _res) - { - _res->success = this->dataPtr->currentVelModel.SetModel( - std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, - _req->noise); +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->dataPtr->currentVelModel.mean; + _res->min = this->dataPtr->currentVelModel.min; + _res->max = this->dataPtr->currentVelModel.max; + _res->noise = this->dataPtr->currentVelModel.noiseAmp; + _res->mu = this->dataPtr->currentVelModel.mu; + return true; +} - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) - { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) - model.SetModel( - model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); - } +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->dataPtr->currentHorzAngleModel.mean; + _res->min = this->dataPtr->currentHorzAngleModel.min; + _res->max = this->dataPtr->currentHorzAngleModel.max; + _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentHorzAngleModel.mu; + return true; +} - gzmsg << "Current velocity model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVelModel.Print(); +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->mean = this->dataPtr->currentVertAngleModel.mean; + _res->min = this->dataPtr->currentVertAngleModel.min; + _res->max = this->dataPtr->currentVertAngleModel.max; + _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; + _res->mu = this->dataPtr->currentVertAngleModel.mu; + return true; +} - return true; - } - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->dataPtr->currentVelModel.SetModel( + std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, + _req->noise); + + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) { - _res->success = this->dataPtr->currentHorzAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) + model.SetModel( + model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); + } - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) - { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; - model.SetModel( - model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); - } + gzmsg << "Current velocity model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentVelModel.Print(); - gzmsg << "Horizontal angle model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentHorzAngleModel.Print(); - return true; + return true; +} +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->dataPtr->currentHorzAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); - // gzmsg << "Horizontal angle model updated" << std::endl - // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->dataPtr->currentHorzAngleModel.Print(); + for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) + { + gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; + model.SetModel( + model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); } - ///////////////////////////////////////////////// - bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( - const std::shared_ptr _req, - std::shared_ptr _res) - { - _res->success = this->dataPtr->currentVertAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + gzmsg << "Horizontal angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentHorzAngleModel.Print(); + return true; - gzmsg << "Vertical angle model updated" << std::endl - << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVertAngleModel.Print(); - return true; - } + // gzmsg << "Horizontal angle model updated" << std::endl + // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + // this->dataPtr->currentHorzAngleModel.Print(); +} - ///////////////////////////////////////////////// - GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) +///////////////////////////////////////////////// +bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( + const std::shared_ptr _req, + std::shared_ptr _res) +{ + _res->success = this->dataPtr->currentVertAngleModel.SetModel( + _req->mean, _req->min, _req->max, _req->mu, _req->noise); + + gzmsg << "Vertical angle model updated" << std::endl + << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; + this->dataPtr->currentVertAngleModel.Print(); + return true; +} + +///////////////////////////////////////////////// +GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) } // namespace dave_ros_gz_plugins // #endif \ No newline at end of file From ea74a8943e5ad2c8f22d57879dae39eba9a9c1bf Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 13 Aug 2024 00:57:26 +0530 Subject: [PATCH 19/79] minor changes --- .../ocean_current_model_plugin.hh | 10 +++---- .../src/ocean_current_model_plugin.cc | 4 +-- .../gauss_markov_process.hh | 28 ++++--------------- .../ocean_current_world_plugin.hh | 24 ++-------------- .../tidal_oscillation.hh | 28 ++++--------------- .../src/gauss_markov_process.cc | 21 ++------------ .../src/ocean_current_world_plugin.cc | 18 ++++++------ .../src/tidal_oscillation.cc | 21 ++------------ .../ocean_current_plugin.hh | 17 ++--------- .../src/ocean_current_plugin.cc | 3 +- 10 files changed, 38 insertions(+), 136 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 40c7f1ae..9cfd3493 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -1,8 +1,8 @@ -#ifndef OCEAN_CURRENT_MODEL_PLUGIN_HH_ -#define OCEAN_CURRENT_MODEL_PLUGIN_HH_ +#ifndef DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ +#define DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ -// #include "dave_gz_world_plugins/gauss_markov_process.hh" -// #include +#include "dave_gz_world_plugins/gauss_markov_process.hh" +#include "dave_gz_world_plugins/tidal_oscillation.h" #include // #include @@ -84,4 +84,4 @@ private: }; } // namespace dave_gz_model_plugins -#endif // OCEAN_CURRENT_MODEL_PLUGIN_HH_ \ No newline at end of file +#endif // DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index a89324d8..598d91de 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -1,6 +1,4 @@ #include "dave_gz_model_plugins/ocean_current_model_plugin.hh" -// #include "dave_gz_world_plugins/gauss_markov_process.hh" -// #include #include #include #include @@ -23,6 +21,8 @@ #include #include #include +#include "dave_gz_world_plugins/gauss_markov_process.hh" +#include "dave_gz_world_plugins/tidal_oscillation.hh" #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh index 01503ef4..fbc1736e 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/gauss_markov_process.hh @@ -1,23 +1,5 @@ -// Copyright (c) 2016 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. - -/// \file gauss_markov_process.h -/// \brief Implementation of a Gauss-Markov process model - -#ifndef GAUSS_MARKOV_PROCESS_H_ -#define GAUSS_MARKOV_PROCESS_H_ +#ifndef DAVE_GZ_WORLD_PLUGINS__GAUSS_MARKOV_PROCESS_HH_ +#define DAVE_GZ_WORLD_PLUGINS__GAUSS_MARKOV_PROCESS_HH_ #include #include @@ -27,7 +9,7 @@ #include #include -namespace gz +namespace dave_gz_world_plugins { /// \brief Implementation of a Gauss-Markov process to model the current /// velocity and direction according to [1] @@ -99,6 +81,6 @@ public: public: void Print(); }; -} // namespace gz +} // namespace dave_gz_world_plugins -#endif // GAUSS_MARKOV_PROCESS_H_ +#endif // DAVE_GZ_WORLD_PLUGINS__GAUSS_MARKOV_PROCESS_HH_ diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index f0220806..3cc535c3 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -1,23 +1,5 @@ -// Copyright (c) 2016 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. - -/// \file ocean_current_world_plugin.hh -/// \brief Plugin that for the underwater world - -#ifndef OCEAN_CURRENT_WORLD_PLUGIN_H_ -#define OCEAN_CURRENT_WORLD_PLUGIN_H_ +#ifndef DAVE_GZ_WORLD_PLUGINS__OCEAN_CURRENT_WORLD_PLUGIN_HH_ +#define DAVE_GZ_WORLD_PLUGINS__OCEAN_CURRENT_WORLD_PLUGIN_HH_ #include #include @@ -97,4 +79,4 @@ private: }; } // namespace dave_gz_world_plugins -#endif // OCEAN_CURRENT_WORLD_PLUGIN_H_ +#endif // DAVE_GZ_WORLD_PLUGINS__OCEAN_CURRENT_WORLD_PLUGIN_HH_ diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh index 34bf8379..f486bfbe 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/tidal_oscillation.hh @@ -1,23 +1,5 @@ -// Copyright (c) 2016 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. - -/// \file tidal_oscillation.h -/// \brief Interpolation of NOAA data for Tidal Oscillation feature - -#ifndef TIDAL_OSCILLATION_H_ -#define TIDAL_OSCILLATION_H_ +#ifndef DAVE_GZ_WORLD_PLUGINS__TIDAL_OSCILLATION_HH_ +#define DAVE_GZ_WORLD_PLUGINS__TIDAL_OSCILLATION_HH_ #include #include @@ -29,7 +11,7 @@ // #include -namespace gz +namespace dave_gz_world_plugins { /// \brief Interpolation of NOAA data for Tidal Oscillation feature class TidalOscillation @@ -117,6 +99,6 @@ public: public: bool currentType; }; -} // namespace gz +} // namespace dave_gz_world_plugins -#endif // TIDAL_OSCILLATION_H_ +#endif // DAVE_GZ_WORLD_PLUGINS__TIDAL_OSCILLATION_HH_S diff --git a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc index aab94da1..e23d9e64 100644 --- a/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc +++ b/gazebo/dave_gz_world_plugins/src/gauss_markov_process.cc @@ -1,23 +1,6 @@ -// Copyright (c) 2016 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. - -/// \file gauss_markov_process.cc - #include -namespace gz +namespace dave_gz_world_plugins { ///////////////////////////////////////////////// GaussMarkovProcess::GaussMarkovProcess() @@ -105,4 +88,4 @@ void GaussMarkovProcess::Print() << "\tMu = " << this->mu << std::endl << "\tNoise Amp. = " << this->noiseAmp << std::endl; } -} // namespace gz +} // namespace dave_gz_world_plugins diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 131621c7..ebebc5fc 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -72,12 +72,12 @@ struct UnderwaterCurrentPlugin::PrivateData std::string ns; /// \brief Gauss-Markov process instance for the current velocity - gz::GaussMarkovProcess currentVelModel; + dave_gz_world_plugins::GaussMarkovProcess currentVelModel; /// \brief Gauss-Markov process instance for horizontal angle model - gz::GaussMarkovProcess currentHorzAngleModel; - gz::GaussMarkovProcess currentVertAngleModel; - std::vector> stratifiedCurrentModels; + dave_gz_world_plugins::GaussMarkovProcess currentHorzAngleModel; + dave_gz_world_plugins::GaussMarkovProcess currentVertAngleModel; + std::vector> stratifiedCurrentModels; std::vector> dateGMT; std::vector speedcmsec; bool tidalHarmonicFlag; @@ -109,7 +109,7 @@ struct UnderwaterCurrentPlugin::PrivateData bool tideFlag; /// \brief Tidal Oscillation interpolation model - gz::TidalOscillation tide; + dave_gz_world_plugins::TidalOscillation tide; /// \brief Last update time stamp std::chrono::steady_clock::duration lastUpdate{0}; @@ -424,7 +424,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() // Means are the database-specified magnitudes & angles, and // the other values come from the constant current models // TODO: Vertical angle currently set to 0 (not in database) - gz::GaussMarkovProcess magnitudeModel; + dave_gz_world_plugins::GaussMarkovProcess magnitudeModel; magnitudeModel.mean = hypot(row[1], row[0]); magnitudeModel.var = magnitudeModel.mean; magnitudeModel.max = this->dataPtr->currentVelModel.max; @@ -434,7 +434,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() // magnitudeModel.lastUpdate = this->dataPtr->lastUpdate; magnitudeModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - gz::GaussMarkovProcess hAngleModel; + dave_gz_world_plugins::GaussMarkovProcess hAngleModel; hAngleModel.mean = atan2(row[1], row[0]); hAngleModel.var = hAngleModel.mean; hAngleModel.max = M_PI; @@ -443,7 +443,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() hAngleModel.noiseAmp = this->dataPtr->currentHorzAngleModel.noiseAmp; hAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - gz::GaussMarkovProcess vAngleModel; + dave_gz_world_plugins::GaussMarkovProcess vAngleModel; vAngleModel.mean = 0.0; vAngleModel.var = vAngleModel.mean; vAngleModel.max = M_PI / 2.0; @@ -452,7 +452,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() vAngleModel.noiseAmp = this->dataPtr->currentVertAngleModel.noiseAmp; vAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - std::vector depthModels; + std::vector depthModels; depthModels.push_back(magnitudeModel); depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); diff --git a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc index 517b0cef..d3f94ad3 100644 --- a/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc +++ b/gazebo/dave_gz_world_plugins/src/tidal_oscillation.cc @@ -1,20 +1,3 @@ -// Copyright (c) 2016 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. - -/// \file tidal_oscillation.cc - #include #include @@ -50,7 +33,7 @@ double calcdatenum(int year, int mon, int day, int hour, int imin, int sec, int } } // namespace std -namespace gz +namespace dave_gz_world_plugins { ///////////////////////////////////////////////// TidalOscillation::TidalOscillation() {} @@ -179,4 +162,4 @@ std::pair TidalOscillation::Update(double _time, double _current return currents; // in m/s } -} // namespace gz +} // namespace dave_gz_world_plugins diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index b271e823..c28199f2 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -2,19 +2,8 @@ /// \brief Publishes the ocean current velocity in ROS messages and cxreates a /// service to alter the flow model in runtime -#ifndef OCEAN_CURRENT_PLUGIN_H_ -#define OCEAN_CURRENT_PLUGIN_H_ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include +#ifndef DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ +#define DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ #include #include @@ -119,4 +108,4 @@ private: }; } // namespace dave_ros_gz_plugins -#endif // OCEAN_CURRENT_PLUGIN_H_ +#endif // DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 85df596f..803bc73b 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,4 +1,5 @@ #include "dave_ros_gz_plugins/ocean_current_plugin.hh" +#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" // #include #include #include @@ -12,7 +13,7 @@ #include #include #include -// #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" + #include "gz/plugin/Register.hh" #include "gz/sim/components/World.hh" From 2e48a9ce528022131b71558ac72089687f56a9c0 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Tue, 13 Aug 2024 23:33:46 -0700 Subject: [PATCH 20/79] Changes to CMakeLists.txt --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 27 ++++++++--- .../ocean_current_model_plugin.hh | 4 +- gazebo/dave_gz_world_plugins/CMakeLists.txt | 46 +++++++++++++++---- gazebo/dave_ros_gz_plugins/CMakeLists.txt | 40 +++++++++++----- 4 files changed, 86 insertions(+), 31 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index dc636cd7..4dae05d5 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -26,7 +26,13 @@ add_library(ocean_current_model_plugin SHARED src/ocean_current_model_plugin.cc ) -target_include_directories(ocean_current_model_plugin PRIVATE include ${dave_gz_world_plugins_INCLUDE_DIRS}) +target_include_directories(ocean_current_model_plugin + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) target_link_libraries(ocean_current_model_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -41,16 +47,23 @@ ament_target_dependencies(ocean_current_model_plugin dave_gz_world_plugins ) -# Install targets -install(TARGETS ocean_current_model_plugin - DESTINATION lib/${PROJECT_NAME} -) - # Install headers install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) +# Install targets +install( + TARGETS ocean_current_model_plugin + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Environment hooks ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 9cfd3493..7aa48092 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -2,7 +2,7 @@ #define DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ #include "dave_gz_world_plugins/gauss_markov_process.hh" -#include "dave_gz_world_plugins/tidal_oscillation.h" +#include "dave_gz_world_plugins/tidal_oscillation.hh" #include // #include @@ -84,4 +84,4 @@ private: }; } // namespace dave_gz_model_plugins -#endif // DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ \ No newline at end of file +#endif // DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 49d7b559..8e10d625 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -33,9 +33,28 @@ src/gauss_markov_process.cc) add_library(tidal_oscillation SHARED src/tidal_oscillation.cc) -target_include_directories(ocean_current_world_plugin PRIVATE include ${dave_interfaces_INCLUDE_DIRS}) -target_include_directories(gauss_markov_process PRIVATE include) -target_include_directories(tidal_oscillation PRIVATE include) +target_include_directories(ocean_current_world_plugin + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + include ${dave_interfaces_INCLUDE_DIRS} + ) +target_include_directories(gauss_markov_process + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) +target_include_directories(tidal_oscillation + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) target_link_libraries(ocean_current_world_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -65,16 +84,23 @@ ament_target_dependencies(tidal_oscillation dave_interfaces ) -# Install targets -install(TARGETS ocean_current_world_plugin gauss_markov_process tidal_oscillation - DESTINATION lib/${PROJECT_NAME} -) - # Install headers install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) +# Install targets +install( + TARGETS ocean_current_world_plugin gauss_markov_process tidal_oscillation + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Environment hooks ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") @@ -86,4 +112,4 @@ if(BUILD_TESTING) endif() # Configure ament -ament_package() \ No newline at end of file +ament_package() diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 31f899a1..908b404d 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -26,8 +26,22 @@ add_library(SphericalCoords SHARED src/SphericalCoords.cc) add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cc) # Include directories -target_include_directories(SphericalCoords PRIVATE include) -target_include_directories(ocean_current_plugin PRIVATE include ${dave_gz_world_plugins_INCLUDE_DIRS}) +target_include_directories(SphericalCoords + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) +target_include_directories(ocean_current_plugin + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + include ${dave_gz_world_plugins_INCLUDE_DIRS} + include ${dave_interfaces_INCLUDE_DIRS} + ) # Linking target_link_libraries(SphericalCoords gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) @@ -49,20 +63,22 @@ ament_target_dependencies(ocean_current_plugin dave_gz_world_plugins ) -# Install targets -install(TARGETS SphericalCoords - DESTINATION lib/${PROJECT_NAME} +# Install headers +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) # Install targets -install(TARGETS ocean_current_plugin - DESTINATION lib/${PROJECT_NAME} +install( + TARGETS SphericalCoords ocean_current_plugin + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -# Install headers -install(DIRECTORY include/ - DESTINATION include/ -) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) # Environment hooks ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") @@ -74,4 +90,4 @@ if(BUILD_TESTING) endif() # Configure ament -ament_package() \ No newline at end of file +ament_package() From 6085c312bff7e4fc0df019ccb521e1f413fcccef Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Tue, 13 Aug 2024 23:42:30 -0700 Subject: [PATCH 21/79] minor change --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index 4dae05d5..bab19b2e 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -31,7 +31,7 @@ target_include_directories(ocean_current_model_plugin $ $ PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/src ) target_link_libraries(ocean_current_model_plugin From 42927b6ad915217c33bcb055a896f697790751fe Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Wed, 14 Aug 2024 00:17:09 -0700 Subject: [PATCH 22/79] minor changes --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 19 ++++++++-- gazebo/dave_gz_world_plugins/CMakeLists.txt | 39 +++++++++++++++------ gazebo/dave_ros_gz_plugins/CMakeLists.txt | 26 +++++++++++--- 3 files changed, 66 insertions(+), 18 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index bab19b2e..75ed0b06 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -32,10 +32,11 @@ target_include_directories(ocean_current_model_plugin $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src - ) +) target_link_libraries(ocean_current_model_plugin - gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) # Specify dependencies for ocean_current_plugin using ament_target_dependencies @@ -59,10 +60,22 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies( + gz-cmake3 + gz-plugin2 + gz-common5 + gz-sim8 + geometry_msgs + std_msgs + dave_interfaces + dave_gz_world_plugins + rclcpp +) # Environment hooks ament_environment_hooks( diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 8e10d625..d5a96a27 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -27,11 +27,14 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "compiling against Gazebo Harmonic") add_library(ocean_current_world_plugin SHARED -src/ocean_current_world_plugin.cc) + src/ocean_current_world_plugin.cc +) add_library(gauss_markov_process SHARED -src/gauss_markov_process.cc) + src/gauss_markov_process.cc +) add_library(tidal_oscillation SHARED -src/tidal_oscillation.cc) + src/tidal_oscillation.cc +) target_include_directories(ocean_current_world_plugin PUBLIC @@ -40,28 +43,31 @@ target_include_directories(ocean_current_world_plugin PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src include ${dave_interfaces_INCLUDE_DIRS} - ) +) target_include_directories(gauss_markov_process PUBLIC $ $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src - ) +) target_include_directories(tidal_oscillation PUBLIC $ $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src - ) +) target_link_libraries(ocean_current_world_plugin -gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) target_link_libraries(gauss_markov_process -gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) target_link_libraries(tidal_oscillation -gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) ament_target_dependencies(ocean_current_world_plugin rclcpp @@ -96,10 +102,23 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies( + gz-cmake3 + gz-plugin2 + gz-msgs10 + gz-common5 + gz-sim8 + geometry_msgs + sensor_msgs + std_msgs + dave_interfaces + rclcpp +) # Environment hooks ament_environment_hooks( diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index 908b404d..d6208fb7 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -32,7 +32,7 @@ target_include_directories(SphericalCoords $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src - ) +) target_include_directories(ocean_current_plugin PUBLIC $ @@ -41,11 +41,15 @@ target_include_directories(ocean_current_plugin ${CMAKE_CURRENT_SOURCE_DIR}/src include ${dave_gz_world_plugins_INCLUDE_DIRS} include ${dave_interfaces_INCLUDE_DIRS} - ) +) # Linking -target_link_libraries(SphericalCoords gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) -target_link_libraries(ocean_current_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) +target_link_libraries(SphericalCoords + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) +target_link_libraries(ocean_current_plugin + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) # Specify dependencies using ament_target_dependencies ament_target_dependencies(SphericalCoords @@ -75,10 +79,22 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies( + gz-cmake3 + gz-plugin2 + gz-common5 + gz-sim8 + geometry_msgs + std_msgs + dave_interfaces + dave_gz_world_plugins + rclcpp +) # Environment hooks ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") From 11e08e01760e472ab0cb373de4256577a6a48bb0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 15 Aug 2024 12:19:56 +0530 Subject: [PATCH 23/79] sucessfully building model and world plugins :) --- .../msg/StratifiedCurrentDatabase.msg | 44 +-- .../ocean_current_model_plugin.hh | 15 +- .../src/ocean_current_model_plugin.cc | 236 ++++++++------- gazebo/dave_gz_world_plugins/CMakeLists.txt | 1 + .../ocean_current_world_plugin.hh | 1 - .../src/ocean_current_world_plugin.cc | 17 +- .../ocean_current_plugin.hh | 17 +- .../src/ocean_current_plugin.cc | 283 +++++++++--------- 8 files changed, 305 insertions(+), 309 deletions(-) diff --git a/dave_interfaces/msg/StratifiedCurrentDatabase.msg b/dave_interfaces/msg/StratifiedCurrentDatabase.msg index 97143aad..2fa77113 100644 --- a/dave_interfaces/msg/StratifiedCurrentDatabase.msg +++ b/dave_interfaces/msg/StratifiedCurrentDatabase.msg @@ -7,37 +7,37 @@ float32[] depths geometry_msgs/Vector3[] velocities # Tide time (GMT) -int16[] timegmtyear -int16[] timegmtmonth -int16[] timegmtday -int16[] timegmthour -int16[] timegmtminute +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 m2amp -float32 m2phase -float32 m2speed -float32 s2amp -float32 s2phase -float32 s2speed -float32 n2amp -float32 n2phase -float32 n2speed +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 ebbdirection -float32 flooddirection +float32 ebb_direction +float32 flood_direction # World start time (GMT) -int16 worldstarttimeyear -int16 worldstarttimemonth -int16 worldstarttimeday -int16 worldstarttimehour -int16 worldstarttimeminute +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) @@ -80,4 +80,4 @@ int16 worldstarttimeminute # int16 worldStartTimeMonth # int16 worldStartTimeDay # int16 worldStartTimeHour -# int16 worldStartTimeMinute +# int16 worldStartTimeMinute \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 7aa48092..65c0f250 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -3,6 +3,8 @@ #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" #include // #include @@ -17,8 +19,6 @@ #include #include #include -#include "dave_interfaces/msg/Stratified_Current_Database.hpp" -#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" namespace dave_gz_model_plugins { @@ -45,8 +45,7 @@ public: // Function called after the simulation state updates void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - void TransientCurrentPlugin::LoadCurrentVelocityParams( - sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm); + void LoadCurrentVelocityParams(sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm); gz::math::Pose3d GetModelPose( const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm); @@ -69,13 +68,15 @@ public: /// \brief Calculate ocean current using database and vehicle state void CalculateOceanCurrent(double vehicleDepth); - void Gauss_Markov_process_initialize(); + void Gauss_Markov_process_initialize( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); /// \brief Convey model state from gazebo topic to outside - void UpdateDatabase(); + void UpdateDatabase(const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg); /// \brief Publish ocean current - void PublishCurrentVelocity(); + void PublishCurrentVelocity(const gz::sim::UpdateInfo & _info); private: std::shared_ptr ros_node_; diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index 598d91de..c11e97de 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -1,4 +1,5 @@ #include "dave_gz_model_plugins/ocean_current_model_plugin.hh" +#include #include #include #include @@ -23,10 +24,12 @@ #include #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" #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -// #include //TODO +#include //TODO #include #include #include @@ -43,14 +46,6 @@ namespace dave_gz_model_plugins { struct TransientCurrentPlugin::PrivateData { - // starts here - - /// \brief Check if an entity is enabled or not. - /// \param[in] _entity Target entity - /// \param[in] _ecm Entity component manager - /// \return True if buoyancy should be applied. - // bool IsEnabled(Entity _entity, const EntityComponentManager & _ecm) const; - // Initialize any necessary states before the plugin starts virtual void Init(); std::string transientCurrentVelocityTopic; // Declare the variable (updated) @@ -76,9 +71,9 @@ struct TransientCurrentPlugin::PrivateData std::string modelName; /// \brief Gauss-Markov process instance for the velocity components // TODO - // gz::GaussMarkovProcess currentVelNorthModel; // TODO - // gz::GaussMarkovProcess currentVelEastModel; // TODO - // gz::GaussMarkovProcess currentVelDownModel; // TODO + dave_gz_world_plugins::GaussMarkovProcess currentVelNorthModel; // TODO + dave_gz_world_plugins::GaussMarkovProcess currentVelEastModel; // TODO + dave_gz_world_plugins::GaussMarkovProcess currentVelDownModel; // TODO /// \brief Gauss-Markov noise double noiseAmp_North; @@ -91,7 +86,7 @@ struct TransientCurrentPlugin::PrivateData std::vector database; /// \brief Tidal Oscillation interpolation model - // gz::TidalOscillation tide; + dave_gz_world_plugins::TidalOscillation tide; /// \brief Tidal Oscillation flag bool tideFlag; @@ -123,9 +118,9 @@ struct TransientCurrentPlugin::PrivateData /// \brief Tidal oscillation world start time (GMT) std::array world_start_time; gz::sim::Entity modelEntity; + std::chrono::steady_clock::duration time; }; -// constructor TransientCurrentPlugin::TransientCurrentPlugin() : dataPtr(std::make_unique()) {} TransientCurrentPlugin::~TransientCurrentPlugin() = default; @@ -139,6 +134,12 @@ void TransientCurrentPlugin::Configure( rclcpp::init(0, nullptr); } + // Initialize the ROS 2 node + this->ros_node_ = std::make_shared("TransirentCurrentPlugin"); + + // Initializing the Gazebo transport node + this->dataPtr->gz_node = std::make_shared(); + gzdbg << "dave_gz_model_plugins::TransientCureentPlugin::Configure on entity: " << _entity << std::endl; @@ -150,13 +151,17 @@ void TransientCurrentPlugin::Configure( auto model = gz::sim::Model(_entity); this->dataPtr->model = model; - this->dataPtr->modelName = model.Name(_ecm); - this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->modelName, _ecm); - - // this->dataPtr->sdf = _sdf; // not needed check - gzmsg << "Loading transient ocean current model plugin..." << std::endl; + // Read the namespace for topics and services + if (!_sdf->HasElement("namespace")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->modelName = _sdf->Get("namespace"); + if (_sdf->HasElement("flow_velocity_topic")) { this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic"); @@ -170,27 +175,22 @@ void TransientCurrentPlugin::Configure( } gzmsg << "Transient velocity topic name for " << this->dataPtr->modelName << " : " << this->dataPtr->currentVelocityTopic << std::endl; - // Read the namespace for topics and services - this->dataPtr->ns = _sdf->Get("namespace"); - // Initialize the ROS 2 node - this->ros_node_ = std::make_shared("TransirentCurrentPlugin"); + this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->modelName, _ecm); // Advertise the ROS flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->ros_node_->create_publisher( this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); - // Initializing the Gazebo transport node - this->dataPtr->gz_node = std::make_shared(); - - // Advertise the current velocity topic in ROS 2 + // Advertise the current velocity topic in gazebo this->dataPtr->gz_current_vel_pub = - this->dataPtr->gz_node->Advertise( - this->dataPtr->currentVelocityTopic); + this->dataPtr->gz_node->Advertise(this->dataPtr->currentVelocityTopic); // Read topic name of stratified ocean current from SDF LoadCurrentVelocityParams(sdfClone, _ecm); + Gauss_Markov_process_initialize( + _entity, _sdf, _ecm, _eventMgr); // something is wrong here (check) } ///////////////////////////////////////////////// @@ -335,7 +335,6 @@ void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) if (this->dataPtr->tideFlag) { // Update tide oscillation - this->dataPtr->time = this->dataPtr->lastUpdate; if (this->dataPtr->tide_Constituents) { @@ -359,7 +358,7 @@ void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) this->dataPtr->tide.worldStartTime = this->dataPtr->world_start_time; this->dataPtr->tide.Initiate(this->dataPtr->tide_Constituents); std::pair currents = - this->dataPtr->tide.Update((this->dataPtr->time).Double(), northCurrent); + this->dataPtr->tide.Update((this->dataPtr->time).count(), northCurrent); this->dataPtr->currentVelNorthModel.mean = currents.first; this->dataPtr->currentVelEastModel.mean = currents.second; this->dataPtr->currentVelDownModel.mean = 0.0; @@ -372,12 +371,18 @@ void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) } // Change min max accordingly - currentVelNorthModel.max = currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; - currentVelNorthModel.min = currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; - currentVelEastModel.max = currentVelEastModel.mean + this->dataPtr->noiseAmp_East; - currentVelEastModel.min = currentVelEastModel.mean - this->dataPtr->noiseAmp_East; - currentVelDownModel.max = currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; - currentVelDownModel.min = currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; + this->dataPtr->currentVelNorthModel.max = + this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; + this->dataPtr->currentVelNorthModel.min = + this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; + this->dataPtr->currentVelEastModel.max = + this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; + this->dataPtr->currentVelEastModel.min = + this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; + this->dataPtr->currentVelDownModel.max = + this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; + this->dataPtr->currentVelDownModel.min = + this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; // Assign values to the model this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; @@ -386,45 +391,48 @@ void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) // Update current velocity double velocityNorth = - this->dataPtr->currentVelNorthModel.Update((this->dataPtr->time).Double()); + this->dataPtr->currentVelNorthModel.Update((this->dataPtr->time).count()); // Update current horizontal direction around z axis of flow frame - double velocityEast = this->dataPtr->currentVelEastModel.Update((this->dataPtr->time).Double()); + double velocityEast = this->dataPtr->currentVelEastModel.Update((this->dataPtr->time).count()); // Update current horizontal direction around z axis of flow frame - double velocityDown = this->dataPtr->currentVelDownModel.Update((this->dataPtr->time).Double()); + double velocityDown = this->dataPtr->currentVelDownModel.Update((this->dataPtr->time).count()); // Update current Velocity this->dataPtr->currentVelocity = gz::math::Vector3d(velocityNorth, velocityEast, velocityDown); - - // Update time stamp - this->dataPtr->lastUpdate = (this->dataPtr->time).Double(); } this->dataPtr->lock_.unlock(); } ///////////////////////////////////////////////// -void TransientCurrentPlugin::PublishCurrentVelocity() +void TransientCurrentPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & _info) { - geometry_msgs::TwistStamped flowVelMsg; - flowVelMsg.header.stamp = rclcpp::Time().now(); + geometry_msgs::msg::TwistStamped flowVelMsg; + flowVelMsg.header.stamp.sec = + std::chrono::duration_cast(_info.simTime).count(); flowVelMsg.header.frame_id = "/world"; flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); - this->dataPtr->flowVelocityPub.publish(flowVelMsg); + this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish Gazebo topic according to the vehicle depth - msgs::Vector3d currentVel; - msgs::Set( - ¤tVel, gz::math::Vector3d( - this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), - this->dataPtr->currentVelocity.Z())); + gz::msgs::Vector3d currentVel; + // msgs::Set( + // ¤tVel, gz::math::Vector3d( + // this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), + // this->dataPtr->currentVelocity.Z())); + + currentVel.set_x(this->dataPtr->currentVelocity.X()); + currentVel.set_y(this->dataPtr->currentVelocity.Y()); + currentVel.set_z(this->dataPtr->currentVelocity.Z()); this->dataPtr->gz_current_vel_pub.Publish(currentVel); } ///////////////////////////////////////////////// -TransientCurrentPlugin::UpdateDatabase() +void TransientCurrentPlugin::UpdateDatabase( + const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg) { this->dataPtr->lock_.lock(); @@ -438,79 +446,84 @@ TransientCurrentPlugin::UpdateDatabase() { this->dataPtr->timeGMT.clear(); this->dataPtr->tideVelocities.clear(); - if (_msg->tideConstituents == true) + if (_msg->tideconstituents == true) { - this->dataPtr->M2_amp = _msg->M2amp; - this->dataPtr->M2_phase = _msg->M2phase; - this->dataPtr->M2_speed = _msg->M2speed; - this->dataPtr->S2_amp = _msg->S2amp; - this->dataPtr->S2_phase = _msg->S2phase; - this->dataPtr->S2_speed = _msg->S2speed; - this->dataPtr->N2_amp = _msg->N2amp; - this->dataPtr->N2_phase = _msg->N2phase; - this->dataPtr->N2_speed = _msg->N2speed; + this->dataPtr->M2_amp = _msg->m2_amp; + this->dataPtr->M2_phase = _msg->m2_phase; + this->dataPtr->M2_speed = _msg->m2_speed; + this->dataPtr->S2_amp = _msg->s2_amp; + this->dataPtr->S2_phase = _msg->s2_phase; + this->dataPtr->S2_speed = _msg->s2_speed; + this->dataPtr->N2_amp = _msg->n2_amp; + this->dataPtr->N2_phase = _msg->n2_phase; + this->dataPtr->N2_speed = _msg->n2_speed; this->dataPtr->tide_Constituents = true; } else { std::array tmpDateVals; - for (int i = 0; i < _msg->timeGMTYear.size(); i++) + for (int i = 0; i < _msg->time_gmt_year.size(); i++) { - tmpDateVals[0] = _msg->timeGMTYear[i]; - tmpDateVals[1] = _msg->timeGMTMonth[i]; - tmpDateVals[2] = _msg->timeGMTDay[i]; - tmpDateVals[3] = _msg->timeGMTHour[i]; - tmpDateVals[4] = _msg->timeGMTMinute[i]; + tmpDateVals[0] = _msg->time_gmt_year[i]; + tmpDateVals[1] = _msg->time_gmt_month[i]; + tmpDateVals[2] = _msg->time_gmt_day[i]; + tmpDateVals[3] = _msg->time_gmt_hour[i]; + tmpDateVals[4] = _msg->time_gmt_minute[i]; this->dataPtr->timeGMT.push_back(tmpDateVals); - this->dataPtr->tideVelocities.push_back(_msg->tideVelocities[i]); + this->dataPtr->tideVelocities.push_back(_msg->tidevelocities[i]); } this->dataPtr->tide_Constituents = false; } - this->dataPtr->ebbDirection = _msg->ebbDirection; - this->dataPtr->floodDirection = _msg->floodDirection; - this->dataPtr->world_start_time[0] = _msg->worldStartTimeYear; - this->dataPtr->world_start_time[1] = _msg->worldStartTimeMonth; - this->dataPtr->world_start_time[2] = _msg->worldStartTimeDay; - this->dataPtr->world_start_time[3] = _msg->worldStartTimeHour; - this->dataPtr->world_start_time[4] = _msg->worldStartTimeMinute; + this->dataPtr->ebbDirection = _msg->ebb_direction; + this->dataPtr->floodDirection = _msg->flood_direction; + this->dataPtr->world_start_time[0] = _msg->world_start_time_year; + this->dataPtr->world_start_time[1] = _msg->world_start_time_month; + this->dataPtr->world_start_time[2] = _msg->world_start_time_day; + this->dataPtr->world_start_time[3] = _msg->world_start_time_hour; + this->dataPtr->world_start_time[4] = _msg->world_start_time_minute; } this->dataPtr->lock_.unlock(); } ///////////////////////////////////////////////// (check this,dpr) -void TransientCurrentPlugin::Gauss_Markov_process_initialize() +void TransientCurrentPlugin::Gauss_Markov_process_initialize( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { // Read Gauss-Markov parameters + sdf::ElementPtr currentVelocityParams; // initialize velocity_north_model parameters if (currentVelocityParams->HasElement("velocity_north")) { - sdf::Element elem = currentVelocityParams->GetElement("velocity_north"); + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); if (elem->HasElement("mean")) { - this->currentVelNorthModel.mean = 0.0; + this->dataPtr->currentVelNorthModel.mean = 0.0; } if (elem->HasElement("mu")) { - this->currentVelNorthModel.mu = 0.0; + this->dataPtr->currentVelNorthModel.mu = 0.0; } if (elem->HasElement("noiseAmp")) { - this->noiseAmp_North = elem->Get("noiseAmp"); + this->dataPtr->noiseAmp_North = elem->Get("noiseAmp"); } - this->currentVelNorthModel.min = this->currentVelNorthModel.mean - this->noiseAmp_North; - this->currentVelNorthModel.max = this->currentVelNorthModel.mean + this->noiseAmp_North; + this->dataPtr->currentVelNorthModel.min = + this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; + this->dataPtr->currentVelNorthModel.max = + this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; if (elem->HasElement("noiseFreq")) { - this->noiseFreq_North = elem->Get("noiseFreq"); + this->dataPtr->noiseFreq_North = elem->Get("noiseFreq"); } - this->currentVelNorthModel.noiseAmp = this->noiseFreq_North; + this->dataPtr->currentVelNorthModel.noiseAmp = this->dataPtr->noiseFreq_North; } this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; - gzmsg << "For vehicle " << this->dataPtr->model->GetName() + gzmsg << "For vehicle " << this->dataPtr->modelName << " -> Current north-direction velocity [m/s] " << "Gauss-Markov process model:" << std::endl; this->dataPtr->currentVelNorthModel.Print(); @@ -518,30 +531,32 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() // initialize velocity_east_model parameters if (currentVelocityParams->HasElement("velocity_east")) { - sdf::Element elem = currentVelocityParams->GetElement("velocity_east"); + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); if (elem->HasElement("mean")) { - this->currentVelEastModel.mean = 0.0; + this->dataPtr->currentVelEastModel.mean = 0.0; } if (elem->HasElement("mu")) { - this->currentVelEastModel.mu = 0.0; + this->dataPtr->currentVelEastModel.mu = 0.0; } if (elem->HasElement("noiseAmp")) { - this->noiseAmp_East = elem->Get("noiseAmp"); + this->dataPtr->noiseAmp_East = elem->Get("noiseAmp"); } - this->currentVelEastModel.min = this->currentVelEastModel.mean - this->noiseAmp_East; - this->currentVelEastModel.max = this->currentVelEastModel.mean + this->noiseAmp_East; + this->dataPtr->currentVelEastModel.min = + this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; + this->dataPtr->currentVelEastModel.max = + this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; if (elem->HasElement("noiseFreq")) { - this->noiseFreq_East = elem->Get("noiseFreq"); + this->dataPtr->noiseFreq_East = elem->Get("noiseFreq"); } - this->currentVelEastModel.noiseAmp = this->noiseFreq_East; + this->dataPtr->currentVelEastModel.noiseAmp = this->dataPtr->noiseFreq_East; } this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; - gzmsg << "For vehicle " << this->dataPtr->model->GetName() + gzmsg << "For vehicle " << this->dataPtr->modelName << " -> Current east-direction velocity [m/s] " << "Gauss-Markov process model:" << std::endl; this->dataPtr->currentVelEastModel.Print(); @@ -552,23 +567,25 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_down"); if (elem->HasElement("mean")) { - this->currentVelDownModel.mean = 0.0; + this->dataPtr->currentVelDownModel.mean = 0.0; } if (elem->HasElement("mu")) { - this->currentVelDownModel.mu = 0.0; + this->dataPtr->currentVelDownModel.mu = 0.0; } if (elem->HasElement("noiseAmp")) { - this->noiseAmp_Down = elem->Get("noiseAmp"); + this->dataPtr->noiseAmp_Down = elem->Get("noiseAmp"); } - this->currentVelDownModel.min = this->currentVelDownModel.mean - this->noiseAmp_Down; - this->currentVelDownModel.max = this->currentVelDownModel.mean + this->noiseAmp_Down; + this->dataPtr->currentVelDownModel.min = + this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; + this->dataPtr->currentVelDownModel.max = + this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; if (elem->HasElement("noiseFreq")) { - this->noiseFreq_Down = elem->Get("noiseFreq"); + this->dataPtr->noiseFreq_Down = elem->Get("noiseFreq"); } - this->currentVelDownModel.noiseAmp = this->noiseFreq_Down; + this->dataPtr->currentVelDownModel.noiseAmp = this->dataPtr->noiseFreq_Down; } this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; @@ -578,22 +595,20 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize() // this->dataPtr->lastUpdate = _info.simTime; This isn't needed because the last update is being // initialised to 0 and is being updated at postupdate - this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.Double(); - this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.Double(); - this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.Double(); + this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.count(); + this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.count(); + this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.count(); } ///////////////////////////////////////////////// void TransientCurrentPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { this->dataPtr->time = _info.simTime; - this->dataPtr->Gauss_Markov_process_initialize(); // something is wrong here (check) - // Subscribe stratified ocean current database this->dataPtr->databaseSub = this->ros_node_->create_subscription( this->dataPtr->transientCurrentVelocityTopic, 10, - std::bind(&TransientCurrentPlugin::UpdateDatabase, this, _1)); + std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); // Connect the update event callback for ROS and ocean current calculation this->dataPtr->Connect(); @@ -608,6 +623,7 @@ void TransientCurrentPlugin::Update( // Update vehicle position gz::math::Pose3d vehicle_pos = GetModelPose(this->dataPtr->modelEntity, _ecm); double vehicleDepth = std::abs(vehicle_pos.Z()); + this->dataPtr->time = this->dataPtr->lastUpdate; CalculateOceanCurrent(vehicleDepth); } ///////////////////////////////////////////////// @@ -621,7 +637,7 @@ void TransientCurrentPlugin::PostUpdate( // PostPublishCurrentVelocity(); // } this->dataPtr->lastUpdate = _info.simTime; - PublishCurrentVelocity(); + PublishCurrentVelocity(_info); if (!_info.paused) { rclcpp::spin_some(this->ros_node_); diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index d5a96a27..598555bd 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -74,6 +74,7 @@ ament_target_dependencies(ocean_current_world_plugin std_msgs geometry_msgs dave_interfaces + ament_index_cpp ) ament_target_dependencies(gauss_markov_process diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index 3cc535c3..00a34e9c 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -19,7 +19,6 @@ namespace dave_gz_world_plugins { - /// \brief Class for the underwater current plugin // typedef const boost::shared_ptr AnyPtr; diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index ebebc5fc..b345c701 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -3,13 +3,12 @@ #include #include +#include // TODO (235-239) #include #include #include #include #include -#include -#include #include #include #include @@ -21,10 +20,11 @@ #include #include #include +#include "dave_gz_world_plugins/gauss_markov_process.hh" +#include "dave_gz_world_plugins/tidal_oscillation.hh" #include "gz/plugin/Register.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/World.hh" -// #include TODO (235-239) GZ_ADD_PLUGIN( dave_gz_world_plugins::UnderwaterCurrentPlugin, gz::sim::System, @@ -197,12 +197,11 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() gzmsg << "Tidal harmonic constituents " << "configuration found" << std::endl; this->dataPtr->tidalHarmonicFlag = true; } - // else - // { - // this->dataPtr->tidalFilePath = - // ament_index_cpp::get_package_share_directory("dave_worlds") + - // "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; - // } TODO + else + { + this->dataPtr->tidalFilePath = ament_index_cpp::get_package_share_directory("dave_worlds") + + "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; + } } // Read the tidal oscillation direction from the SDF file diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index c28199f2..d08d805c 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -1,7 +1,3 @@ -/// \file ocean_current_plugin.hh -/// \brief Publishes the ocean current velocity in ROS messages and cxreates a -/// service to alter the flow model in runtime - #ifndef DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ #define DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ @@ -44,17 +40,16 @@ public: const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + // void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - // Documentation inherited - void PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; + // // Documentation inherited + // void PreUpdate( + // const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; // Documentation inherited - void PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - void UpdateHorzAngle( + bool UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res); diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 803bc73b..72b50ea5 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -54,7 +54,7 @@ struct UnderwaterCurrentROSPlugin::PrivateData std::string stratifiedCurrentVelocityTopic; std::string stratifiedCurrentVelocityDatabaseTopic; std::string currentVelocityTopic; - std::string ns; + std::string model_namespace; rclcpp::Publisher::SharedPtr flowVelocityPub; rclcpp::Publisher::SharedPtr stratifiedCurrentVelocityPub; @@ -76,32 +76,32 @@ void UnderwaterCurrentROSPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { + if (!rclcpp::ok()) + { + gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your " + "ROS 2 environment.\n"; + return; + } + this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin"); auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); this->dataPtr->world = gz::sim::World(worldEntity); + this->dataPtr->entity = _entity; + this->dataPtr->model = gz::sim::Model(_entity); this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); this->dataPtr->sdf = _sdf; - std::chrono::steady_clock::duration lastUpdate{0}; - - // auto lastUpdate = gz::sim::simTime; this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = this->dataPtr->stratifiedCurrentVelocityTopic + "_database"; - if (!rclcpp::ok()) - { - gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your " - "ROS 2 environment.\n"; - return; - } - - this->dataPtr->ns = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + this->dataPtr->model_namespace = + _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; // Initialising ROS 2 node this->dataPtr->rosNode = - std::make_shared("underwater_current_ros_plugin", this->dataPtr->ns); + std::make_shared("underwater_current_ros_plugin", this->dataPtr->model_namespace); // Create and advertise Messages // Advertise the flow velocity as a stamped twist message @@ -127,38 +127,6 @@ void UnderwaterCurrentROSPlugin::Configure( // check for the number of arg being passed to the function - // Advertise the service to get the current velocity model - this->dataPtr->get_current_velocity_model = - this->dataPtr->rosNode->create_service( - "get_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to get the current horizontal angle model - this->dataPtr->get_current_horz_angle_model = - this->dataPtr->rosNode->create_service( - "get_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, - std::placeholders::_1, std::placeholders::_2)); - - // Advertise the service to get the current vertical angle model - this->dataPtr->get_current_vert_angle_model = - this->dataPtr->rosNode->create_service( - "get_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, - std::placeholders::_1, std::placeholders::_2)); - - // Connect to the Gazebo Harmonic update event - // this->dataPtr->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & - // info) - // { this->dataPtr->OnUpdateCurrentVel(info); - // }); -} - -///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = this->dataPtr->rosNode->create_service( @@ -179,109 +147,7 @@ void UnderwaterCurrentROSPlugin::PreUpdate( "get_current_vert_angle_model", std::bind( &UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); -} -///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::Update( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -{ - if (_info.simTime > this->dataPtr->lastUpdate) - { - // Generate and publish current velocity according to the vehicle depth - geometry_msgs::msg::TwistStamped flowVelMsg; - flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); - flowVelMsg.header.frame_id = - "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions - flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); - flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); - flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); - - this->dataPtr->flowVelocityPub->publish(flowVelMsg); - - // Generate and publish stratified current velocity - dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; - stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); - stratCurrentVelocityMsg.header.frame_id = "world"; - - // Updating for stratified behaviour of Ocean Currents - // What is the .size value over here, to be (checked) - { - for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); - i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) - { - geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); - velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); - velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); - stratCurrentVelocityMsg.velocities.push_back(velocity); - stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); - } - - this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); - - // Generate and publish stratified current database - dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; i < this->dataPtr->stratifiedDatabase - .size(); // again check with ocean_cureent_world_plugin.cc (TODO) - i++) // read from csv file in ocean_cureent_world_plugin.cc - { - // Stratified current database entry preparation - geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->stratifiedDatabase[i].X(); - velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); - velocity.z = 0.0; // Assuming z is intentionally set to 0.0 - currentDatabaseMsg.velocities.push_back(velocity); - currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); - } - - if (this->dataPtr - ->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) - { - // Tidal harmonic constituents - currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; - currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; - currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; - currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; - currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; - currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; - currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; - currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; - currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; - currentDatabaseMsg.tideConstituents = true; - } - else - { - for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) - { - // Tidal oscillation database - currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); - currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); - currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); - currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); - currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); - - currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); - } - currentDatabaseMsg.tideConstituents = false; - } - - currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; - currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; - - currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; - currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; - currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; - currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; - currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; - - this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); - } - } -} -///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -{ // Advertise the service to update the current velocity model this->dataPtr->set_current_velocity_model = this->dataPtr->rosNode->create_service( @@ -352,9 +218,29 @@ void UnderwaterCurrentROSPlugin::PostUpdate( &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, this, std::placeholders::_1, std::placeholders::_2)); - // Update the time tracking for publication - this->dataPtr->lastUpdate = _info.simTime; + // Connect to the Gazebo Harmonic update event + // this->dataPtr->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & + // info) + // { this->dataPtr->OnUpdateCurrentVel(info); + // }); } + +///////////////////////////////////////////////// +// void UnderwaterCurrentROSPlugin::PreUpdate( +// const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +// { +// } +// ///////////////////////////////////////////////// +// void UnderwaterCurrentROSPlugin::Update( +// const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +// { +// if (_info.simTime > this->dataPtr->lastUpdate) +// { +// // TODO put in postupdate +// } +// } + +// namespace dave_ros_gz_plugins ///////////////////////////////////////////////// bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( const std::shared_ptr _req, @@ -570,6 +456,105 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( return true; } +///////////////////////////////////////////////// +void UnderwaterCurrentROSPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + // Generate and publish current velocity according to the vehicle depth + geometry_msgs::msg::TwistStamped flowVelMsg; + flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); + flowVelMsg.header.frame_id = + "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions + + flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + + this->dataPtr->flowVelocityPub->publish(flowVelMsg); + + // + + // Generate and publish stratified current velocity + dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; + stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); + stratCurrentVelocityMsg.header.frame_id = "world"; + + // Updating for stratified behaviour of Ocean Currents + // What is the .size value over here, to be (checked) + + for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); + i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) + { + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); + velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); + velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); + stratCurrentVelocityMsg.velocities.push_back(velocity); + stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); + } + + this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); + + // Generate and publish stratified current database + dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; + for (int i = 0; i < this->dataPtr->stratifiedDatabase + .size(); // again check with ocean_cureent_world_plugin.cc (TODO) + i++) // read from csv file in ocean_cureent_world_plugin.cc + { + // Stratified current database entry preparation + geometry_msgs::msg::Vector3 velocity; + velocity.x = this->dataPtr->stratifiedDatabase[i].X(); + velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); + velocity.z = 0.0; // Assuming z is intentionally set to 0.0 + currentDatabaseMsg.velocities.push_back(velocity); + currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); + } + + if (this->dataPtr->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) + { + // Tidal harmonic constituents + currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; + currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; + currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; + currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; + currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; + currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; + currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; + currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; + currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; + currentDatabaseMsg.tideConstituents = true; + } + else + { + for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) + { + // Tidal oscillation database + currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); + currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); + currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); + currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); + currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); + + currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); + } + currentDatabaseMsg.tideConstituents = false; + } + + currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; + currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; + + currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; + currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; + currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; + currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; + currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; + + this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); + + // Update the time tracking for publication + this->dataPtr->lastUpdate = _info.simTime; +} + ///////////////////////////////////////////////// GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) } // namespace dave_ros_gz_plugins From 20dae530af54835334f58e8d7a20a292749c6043 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 16 Aug 2024 20:33:59 +0530 Subject: [PATCH 24/79] Building Sucessfully git status --- .../msg/StratifiedCurrentDatabase.msg | 43 +-- .../ocean_current_world_plugin.hh | 48 +++- .../src/ocean_current_world_plugin.cc | 272 ++++++++---------- .../ocean_current_plugin.hh | 3 +- .../src/ocean_current_plugin.cc | 232 ++++++++------- 5 files changed, 301 insertions(+), 297 deletions(-) diff --git a/dave_interfaces/msg/StratifiedCurrentDatabase.msg b/dave_interfaces/msg/StratifiedCurrentDatabase.msg index 2fa77113..b6fc2549 100644 --- a/dave_interfaces/msg/StratifiedCurrentDatabase.msg +++ b/dave_interfaces/msg/StratifiedCurrentDatabase.msg @@ -39,45 +39,4 @@ 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) - -# # Publishes depths and velocities read from the csv database - -# # Depths -# float32[] depths - -# # Velocities -# geometry_msgs/Vector3[] velocities - -# # Tide time (GMT) -# int16[] timeGMTYear -# int16[] timeGMTMonth -# int16[] timeGMTDay -# int16[] timeGMTHour -# int16[] timeGMTMinute - -# # Tide velocities -# float32[] tideVelocities - -# # Tide constituents -# bool tideConstituents -# float32 M2amp -# float32 M2phase -# float32 M2speed -# float32 S2amp -# float32 S2phase -# float32 S2speed -# float32 N2amp -# float32 N2phase -# float32 N2speed - -# # Tide direction -# float32 ebbDirection -# float32 floodDirection - -# # World start time (GMT) -# int16 worldStartTimeYear -# int16 worldStartTimeMonth -# int16 worldStartTimeDay -# int16 worldStartTimeHour -# int16 worldStartTimeMinute \ No newline at end of file +# Everything is written in lowercase according to ROS 2 conventions old(v) \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index 00a34e9c..60f98f9d 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -7,6 +7,7 @@ #include "dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.pb.h" // #include +#include #include #include #include @@ -32,7 +33,6 @@ class UnderwaterCurrentPlugin : public gz::sim::System, // public gz::sim::WorldPlugin { - /// \brief Class constructor public: UnderwaterCurrentPlugin(); ~UnderwaterCurrentPlugin(); @@ -70,6 +70,52 @@ public: /// \brief Publish stratified oceqan current velocity void PublishStratifiedCurrentVelocity(); + struct SharedData + { + dave_gz_world_plugins::GaussMarkovProcess currentHorzAngleModel; + /// \brief Gauss-Markov process instance for the current velocity + dave_gz_world_plugins::GaussMarkovProcess currentVelModel; + dave_gz_world_plugins::GaussMarkovProcess currentVertAngleModel; + + /// \brief Vector for read stratified current database values + std::vector stratifiedDatabase; // check + std::vector> stratifiedCurrentModels; + bool tidalHarmonicFlag; + + /// \brief Current linear velocity vector + gz::math::Vector3d currentVelocity; + + /// \brief Vector of current depth-specific linear velocity vectors + std::vector currentStratifiedVelocity; + + double M2_amp; + double M2_phase; + double M2_speed; + double S2_amp; + double S2_phase; + double S2_speed; + double N2_amp; + double N2_phase; + double N2_speed; + + /// \brief Tidal oscillation mean ebb direction + double ebbDirection; + + /// \brief Tidal oscillation mean flood direction + double floodDirection; + + /// \brief Tidal oscillation world start time (GMT) + int world_start_time_day; + int world_start_time_month; + int world_start_time_year; + int world_start_time_hour; + int world_start_time_minute; + + std::vector> dateGMT; + std::vector speedcmsec; + }; + std::unique_ptr sharedDataPtr; + private: // std::shared_ptr ros_node_; diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index b345c701..825893d7 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -34,6 +34,7 @@ GZ_ADD_PLUGIN( namespace dave_gz_world_plugins { + struct UnderwaterCurrentPlugin::PrivateData { gz::sim::World world{gz::sim::kNullEntity}; @@ -65,46 +66,9 @@ struct UnderwaterCurrentPlugin::PrivateData /// \brief Tidal Oscillation Database file path for txt file std::string tidalFilePath; - /// \brief Vector for read stratified current database values - std::vector stratifiedDatabase; - /// \brief Namespace for topics and services std::string ns; - /// \brief Gauss-Markov process instance for the current velocity - dave_gz_world_plugins::GaussMarkovProcess currentVelModel; - - /// \brief Gauss-Markov process instance for horizontal angle model - dave_gz_world_plugins::GaussMarkovProcess currentHorzAngleModel; - dave_gz_world_plugins::GaussMarkovProcess currentVertAngleModel; - std::vector> stratifiedCurrentModels; - std::vector> dateGMT; - std::vector speedcmsec; - bool tidalHarmonicFlag; - - double M2_amp; - double M2_phase; - double M2_speed; - double S2_amp; - double S2_phase; - double S2_speed; - double N2_amp; - double N2_phase; - double N2_speed; - - /// \brief Tidal oscillation mean ebb direction - double ebbDirection; - - /// \brief Tidal oscillation mean flood direction - double floodDirection; - - /// \brief Tidal oscillation world start time (GMT) - int world_start_time_day; - int world_start_time_month; - int world_start_time_year; - int world_start_time_hour; - int world_start_time_minute; - /// \brief Tidal Oscillation flag bool tideFlag; @@ -114,12 +78,6 @@ struct UnderwaterCurrentPlugin::PrivateData /// \brief Last update time stamp std::chrono::steady_clock::duration lastUpdate{0}; - /// \brief Current linear velocity vector - gz::math::Vector3d currentVelocity; - - /// \brief Vector of current depth-specific linear velocity vectors - std::vector currentStratifiedVelocity; - /// \brief File path for stratified current database std::string db_path; @@ -127,7 +85,11 @@ struct UnderwaterCurrentPlugin::PrivateData }; ///////////////////////////////////////////////// -UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() : dataPtr(std::make_unique()) {} +UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() +: dataPtr(std::make_unique()), sharedDataPtr(std::make_unique()) +{ + // this->sharedDataPtr = std::make_unique(); +} ///////////////////////////////////////////////// UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->dataPtr->rosNode.reset(); } @@ -159,9 +121,12 @@ void UnderwaterCurrentPlugin::Configure( LoadGlobalCurrentConfig(); LoadStratifiedCurrentDatabase(); - if (this->dataPtr->sdf->HasElement("tidal_oscillation")) + if (_sdf->HasElement("tidal_oscillation")) { - LoadTidalOscillationDatabase(); + if (_sdf->Get("tidal_oscillation")) + { + LoadTidalOscillationDatabase(); + } } // Connect the update event. This isn't needed it seems (check) @@ -177,7 +142,7 @@ void UnderwaterCurrentPlugin::Configure( void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { this->dataPtr->tideFlag = true; - this->dataPtr->tidalHarmonicFlag = false; + this->sharedDataPtr->tidalHarmonicFlag = false; sdf::ElementPtr tidalOscillationParams = this->dataPtr->sdf->GetElement("tidal_oscillation"); // include this xml/ (TODO) @@ -195,7 +160,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { tidalHarmonicParams = tidalOscillationParams->GetElement("harmonic_constituents"); gzmsg << "Tidal harmonic constituents " << "configuration found" << std::endl; - this->dataPtr->tidalHarmonicFlag = true; + this->sharedDataPtr->tidalHarmonicFlag = true; } else { @@ -211,8 +176,8 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { sdf::ElementPtr elem = tidalOscillationParams->GetElement("mean_direction"); GZ_ASSERT(elem->HasElement("ebb"), "Tidal mean ebb direction not defined"); - this->dataPtr->ebbDirection = elem->Get("ebb"); - this->dataPtr->floodDirection = elem->Get("flood"); + this->sharedDataPtr->ebbDirection = elem->Get("ebb"); + this->sharedDataPtr->floodDirection = elem->Get("flood"); GZ_ASSERT(elem->HasElement("flood"), "Tidal mean flood direction not defined"); } @@ -224,48 +189,51 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() { sdf::ElementPtr elem = tidalOscillationParams->GetElement("world_start_time_GMT"); GZ_ASSERT(elem->HasElement("day"), "World start time (day) not defined"); - this->dataPtr->world_start_time_day = elem->Get("day"); + this->sharedDataPtr->world_start_time_day = elem->Get("day"); GZ_ASSERT(elem->HasElement("month"), "World start time (month) not defined"); - this->dataPtr->world_start_time_month = elem->Get("month"); + this->sharedDataPtr->world_start_time_month = elem->Get("month"); GZ_ASSERT(elem->HasElement("year"), "World start time (year) not defined"); - this->dataPtr->world_start_time_year = elem->Get("year"); + this->sharedDataPtr->world_start_time_year = elem->Get("year"); GZ_ASSERT(elem->HasElement("hour"), "World start time (hour) not defined"); - this->dataPtr->world_start_time_hour = elem->Get("hour"); + this->sharedDataPtr->world_start_time_hour = elem->Get("hour"); if (elem->HasElement("minute")) { - this->dataPtr->world_start_time_minute = elem->Get("minute"); + this->sharedDataPtr->world_start_time_minute = elem->Get("minute"); } else { - this->dataPtr->world_start_time_minute = 0; + this->sharedDataPtr->world_start_time_minute = 0; } } - if (this->dataPtr->tidalHarmonicFlag) + if (this->sharedDataPtr->tidalHarmonicFlag) { // Read harmonic constituents GZ_ASSERT(tidalHarmonicParams->HasElement("M2"), "Harcomnic constituents M2 not found"); sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2"); - this->dataPtr->M2_amp = M2Params->Get("amp"); - this->dataPtr->M2_phase = M2Params->Get("phase"); - this->dataPtr->M2_speed = M2Params->Get("speed"); + this->sharedDataPtr->M2_amp = M2Params->Get("amp"); + this->sharedDataPtr->M2_phase = M2Params->Get("phase"); + this->sharedDataPtr->M2_speed = M2Params->Get("speed"); GZ_ASSERT(tidalHarmonicParams->HasElement("S2"), "Harcomnic constituents S2 not found"); sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2"); - this->dataPtr->S2_amp = S2Params->Get("amp"); - this->dataPtr->S2_phase = S2Params->Get("phase"); - this->dataPtr->S2_speed = S2Params->Get("speed"); + this->sharedDataPtr->S2_amp = S2Params->Get("amp"); + this->sharedDataPtr->S2_phase = S2Params->Get("phase"); + this->sharedDataPtr->S2_speed = S2Params->Get("speed"); GZ_ASSERT(tidalHarmonicParams->HasElement("N2"), "Harcomnic constituents N2 not found"); sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2"); - this->dataPtr->N2_amp = N2Params->Get("amp"); - this->dataPtr->N2_phase = N2Params->Get("phase"); - this->dataPtr->N2_speed = N2Params->Get("speed"); + this->sharedDataPtr->N2_amp = N2Params->Get("amp"); + this->sharedDataPtr->N2_phase = N2Params->Get("phase"); + this->sharedDataPtr->N2_speed = N2Params->Get("speed"); gzmsg << "Tidal harmonic constituents loaded : " << std::endl; - gzmsg << "M2 amp: " << this->dataPtr->M2_amp << " phase: " << this->dataPtr->M2_phase - << " speed: " << this->dataPtr->M2_speed << std::endl; - gzmsg << "S2 amp: " << this->dataPtr->S2_amp << " phase: " << this->dataPtr->S2_phase - << " speed: " << this->dataPtr->S2_speed << std::endl; - gzmsg << "N2 amp: " << this->dataPtr->N2_amp << " phase: " << this->dataPtr->N2_phase - << " speed: " << this->dataPtr->N2_speed << std::endl; + gzmsg << "M2 amp: " << this->sharedDataPtr->M2_amp + << " phase: " << this->sharedDataPtr->M2_phase + << " speed: " << this->sharedDataPtr->M2_speed << std::endl; + gzmsg << "S2 amp: " << this->sharedDataPtr->S2_amp + << " phase: " << this->sharedDataPtr->S2_phase + << " speed: " << this->sharedDataPtr->S2_speed << std::endl; + gzmsg << "N2 amp: " << this->sharedDataPtr->N2_amp + << " phase: " << this->sharedDataPtr->N2_phase + << " speed: " << this->sharedDataPtr->N2_speed << std::endl; } else { @@ -307,21 +275,22 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() tmpDateArray[2] = std::stoi(row[0].substr(8, 10)); tmpDateArray[3] = std::stoi(row[0].substr(11, 13)); tmpDateArray[4] = std::stoi(row[0].substr(14, 16)); - this->dataPtr->dateGMT.push_back(tmpDateArray); + this->sharedDataPtr->dateGMT.push_back(tmpDateArray); - this->dataPtr->speedcmsec.push_back(stold(row[2], &sz)); + this->sharedDataPtr->speedcmsec.push_back(stold(row[2], &sz)); } } csvFile.close(); // Eliminate data with same consecutive type std::vector duplicated; - for (int i = 0; i < this->dataPtr->dateGMT.size() - 1; i++) + for (int i = 0; i < this->sharedDataPtr->dateGMT.size() - 1; i++) { // delete latter if same sign if ( - ((this->dataPtr->speedcmsec[i] > 0) - (this->dataPtr->speedcmsec[i] < 0)) == - ((this->dataPtr->speedcmsec[i + 1] > 0) - (this->dataPtr->speedcmsec[i + 1] < 0))) + ((this->sharedDataPtr->speedcmsec[i] > 0) - (this->sharedDataPtr->speedcmsec[i] < 0)) == + ((this->sharedDataPtr->speedcmsec[i + 1] > 0) - + (this->sharedDataPtr->speedcmsec[i + 1] < 0))) { duplicated.push_back(i + 1); } @@ -329,9 +298,10 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() int eraseCount = 0; for (int i = 0; i < duplicated.size(); i++) { - this->dataPtr->dateGMT.erase(this->dataPtr->dateGMT.begin() + duplicated[i] - eraseCount); - this->dataPtr->speedcmsec.erase( - this->dataPtr->speedcmsec.begin() + duplicated[i] - eraseCount); + this->sharedDataPtr->dateGMT.erase( + this->sharedDataPtr->dateGMT.begin() + duplicated[i] - eraseCount); + this->sharedDataPtr->speedcmsec.erase( + this->sharedDataPtr->speedcmsec.begin() + duplicated[i] - eraseCount); eraseCount++; } } @@ -417,7 +387,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() read.X() = row[0]; read.Y() = row[1]; read.Z() = row[2]; - this->dataPtr->stratifiedDatabase.push_back(read); + this->sharedDataPtr->stratifiedDatabase.push_back(read); // Create Gauss-Markov processes for the stratified currents // Means are the database-specified magnitudes & angles, and @@ -426,10 +396,10 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() dave_gz_world_plugins::GaussMarkovProcess magnitudeModel; magnitudeModel.mean = hypot(row[1], row[0]); magnitudeModel.var = magnitudeModel.mean; - magnitudeModel.max = this->dataPtr->currentVelModel.max; + magnitudeModel.max = this->sharedDataPtr->currentVelModel.max; magnitudeModel.min = 0.0; - magnitudeModel.mu = this->dataPtr->currentVelModel.mu; - magnitudeModel.noiseAmp = this->dataPtr->currentVelModel.noiseAmp; + magnitudeModel.mu = this->sharedDataPtr->currentVelModel.mu; + magnitudeModel.noiseAmp = this->sharedDataPtr->currentVelModel.noiseAmp; // magnitudeModel.lastUpdate = this->dataPtr->lastUpdate; magnitudeModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); @@ -438,8 +408,8 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() hAngleModel.var = hAngleModel.mean; hAngleModel.max = M_PI; hAngleModel.min = -M_PI; - hAngleModel.mu = this->dataPtr->currentHorzAngleModel.mu; - hAngleModel.noiseAmp = this->dataPtr->currentHorzAngleModel.noiseAmp; + hAngleModel.mu = this->sharedDataPtr->currentHorzAngleModel.mu; + hAngleModel.noiseAmp = this->sharedDataPtr->currentHorzAngleModel.noiseAmp; hAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); dave_gz_world_plugins::GaussMarkovProcess vAngleModel; @@ -447,15 +417,15 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() vAngleModel.var = vAngleModel.mean; vAngleModel.max = M_PI / 2.0; vAngleModel.min = -M_PI / 2.0; - vAngleModel.mu = this->dataPtr->currentVertAngleModel.mu; - vAngleModel.noiseAmp = this->dataPtr->currentVertAngleModel.noiseAmp; + vAngleModel.mu = this->sharedDataPtr->currentVertAngleModel.mu; + vAngleModel.noiseAmp = this->sharedDataPtr->currentVertAngleModel.noiseAmp; vAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); std::vector depthModels; depthModels.push_back(magnitudeModel); depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); - this->dataPtr->stratifiedCurrentModels.push_back(depthModels); + this->sharedDataPtr->stratifiedCurrentModels.push_back(depthModels); } csvFile.close(); @@ -497,45 +467,46 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity"); if (elem->HasElement("mean")) { - this->dataPtr->currentVelModel.mean = elem->Get("mean"); + this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->dataPtr->currentVelModel.min = elem->Get("min"); + this->sharedDataPtr->currentVelModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->dataPtr->currentVelModel.max = elem->Get("max"); + this->sharedDataPtr->currentVelModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->dataPtr->currentVelModel.mu = elem->Get("mu"); + this->sharedDataPtr->currentVelModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->dataPtr->currentVelModel.noiseAmp = elem->Get("noiseAmp"); + this->sharedDataPtr->currentVelModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->dataPtr->currentVelModel.min < this->dataPtr->currentVelModel.max, + this->sharedDataPtr->currentVelModel.min < this->sharedDataPtr->currentVelModel.max, "Invalid current velocity limits"); GZ_ASSERT( - this->dataPtr->currentVelModel.mean >= this->dataPtr->currentVelModel.min, + this->sharedDataPtr->currentVelModel.mean >= this->sharedDataPtr->currentVelModel.min, "Mean velocity must be greater than minimum"); GZ_ASSERT( - this->dataPtr->currentVelModel.mean <= this->dataPtr->currentVelModel.max, + this->sharedDataPtr->currentVelModel.mean <= this->sharedDataPtr->currentVelModel.max, "Mean velocity must be smaller than maximum"); GZ_ASSERT( - this->dataPtr->currentVelModel.mu >= 0 && this->dataPtr->currentVelModel.mu < 1, + this->sharedDataPtr->currentVelModel.mu >= 0 && this->sharedDataPtr->currentVelModel.mu < 1, "Invalid process constant"); GZ_ASSERT( - this->dataPtr->currentVelModel.noiseAmp < 1 && this->dataPtr->currentVelModel.noiseAmp >= 0, + this->sharedDataPtr->currentVelModel.noiseAmp < 1 && + this->sharedDataPtr->currentVelModel.noiseAmp >= 0, "Noise amplitude has to be smaller than 1"); } - this->dataPtr->currentVelModel.var = this->dataPtr->currentVelModel.mean; + this->sharedDataPtr->currentVelModel.var = this->sharedDataPtr->currentVelModel.mean; gzmsg << "Current velocity [m/s] Gauss-Markov process model:" << std::endl; - this->dataPtr->currentVelModel.Print(); + this->sharedDataPtr->currentVelModel.Print(); if (currentVelocityParams->HasElement("horizontal_angle")) { @@ -543,46 +514,50 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() if (elem->HasElement("mean")) { - this->dataPtr->currentHorzAngleModel.mean = elem->Get("mean"); + this->sharedDataPtr->currentHorzAngleModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->dataPtr->currentHorzAngleModel.min = elem->Get("min"); + this->sharedDataPtr->currentHorzAngleModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->dataPtr->currentHorzAngleModel.max = elem->Get("max"); + this->sharedDataPtr->currentHorzAngleModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->dataPtr->currentHorzAngleModel.mu = elem->Get("mu"); + this->sharedDataPtr->currentHorzAngleModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->dataPtr->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); + this->sharedDataPtr->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->dataPtr->currentHorzAngleModel.min < this->dataPtr->currentHorzAngleModel.max, + this->sharedDataPtr->currentHorzAngleModel.min < + this->sharedDataPtr->currentHorzAngleModel.max, "Invalid current horizontal angle limits"); GZ_ASSERT( - this->dataPtr->currentHorzAngleModel.mean >= this->dataPtr->currentHorzAngleModel.min, + this->sharedDataPtr->currentHorzAngleModel.mean >= + this->sharedDataPtr->currentHorzAngleModel.min, "Mean horizontal angle must be greater than minimum"); GZ_ASSERT( - this->dataPtr->currentHorzAngleModel.mean <= this->dataPtr->currentHorzAngleModel.max, + this->sharedDataPtr->currentHorzAngleModel.mean <= + this->sharedDataPtr->currentHorzAngleModel.max, "Mean horizontal angle must be smaller than maximum"); GZ_ASSERT( - this->dataPtr->currentHorzAngleModel.mu >= 0 && this->dataPtr->currentHorzAngleModel.mu < 1, + this->sharedDataPtr->currentHorzAngleModel.mu >= 0 && + this->sharedDataPtr->currentHorzAngleModel.mu < 1, "Invalid process constant"); GZ_ASSERT( - this->dataPtr->currentHorzAngleModel.noiseAmp < 1 && - this->dataPtr->currentHorzAngleModel.noiseAmp >= 0, + this->sharedDataPtr->currentHorzAngleModel.noiseAmp < 1 && + this->sharedDataPtr->currentHorzAngleModel.noiseAmp >= 0, "Noise amplitude for horizontal angle has to be between 0 and 1"); } - this->dataPtr->currentHorzAngleModel.var = this->dataPtr->currentHorzAngleModel.mean; + this->sharedDataPtr->currentHorzAngleModel.var = this->sharedDataPtr->currentHorzAngleModel.mean; gzmsg << "Current velocity horizontal angle [rad] Gauss-Markov process model:" << std::endl; - this->dataPtr->currentHorzAngleModel.Print(); + this->sharedDataPtr->currentHorzAngleModel.Print(); if (currentVelocityParams->HasElement("vertical_angle")) { @@ -590,52 +565,56 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() if (elem->HasElement("mean")) { - this->dataPtr->currentVertAngleModel.mean = elem->Get("mean"); + this->sharedDataPtr->currentVertAngleModel.mean = elem->Get("mean"); } if (elem->HasElement("min")) { - this->dataPtr->currentVertAngleModel.min = elem->Get("min"); + this->sharedDataPtr->currentVertAngleModel.min = elem->Get("min"); } if (elem->HasElement("max")) { - this->dataPtr->currentVertAngleModel.max = elem->Get("max"); + this->sharedDataPtr->currentVertAngleModel.max = elem->Get("max"); } if (elem->HasElement("mu")) { - this->dataPtr->currentVertAngleModel.mu = elem->Get("mu"); + this->sharedDataPtr->currentVertAngleModel.mu = elem->Get("mu"); } if (elem->HasElement("noiseAmp")) { - this->dataPtr->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); + this->sharedDataPtr->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); } GZ_ASSERT( - this->dataPtr->currentVertAngleModel.min < this->dataPtr->currentVertAngleModel.max, + this->sharedDataPtr->currentVertAngleModel.min < + this->sharedDataPtr->currentVertAngleModel.max, "Invalid current vertical angle limits"); GZ_ASSERT( - this->dataPtr->currentVertAngleModel.mean >= this->dataPtr->currentVertAngleModel.min, + this->sharedDataPtr->currentVertAngleModel.mean >= + this->sharedDataPtr->currentVertAngleModel.min, "Mean vertical angle must be greater than minimum"); GZ_ASSERT( - this->dataPtr->currentVertAngleModel.mean <= this->dataPtr->currentVertAngleModel.max, + this->sharedDataPtr->currentVertAngleModel.mean <= + this->sharedDataPtr->currentVertAngleModel.max, "Mean vertical angle must be smaller than maximum"); GZ_ASSERT( - this->dataPtr->currentVertAngleModel.mu >= 0 && this->dataPtr->currentVertAngleModel.mu < 1, + this->sharedDataPtr->currentVertAngleModel.mu >= 0 && + this->sharedDataPtr->currentVertAngleModel.mu < 1, "Invalid process constant"); GZ_ASSERT( - this->dataPtr->currentVertAngleModel.noiseAmp < 1 && - this->dataPtr->currentVertAngleModel.noiseAmp >= 0, + this->sharedDataPtr->currentVertAngleModel.noiseAmp < 1 && + this->sharedDataPtr->currentVertAngleModel.noiseAmp >= 0, "Noise amplitude for vertical angle has to be between 0 and 1"); } - this->dataPtr->currentVertAngleModel.var = this->dataPtr->currentVertAngleModel.mean; + this->sharedDataPtr->currentVertAngleModel.var = this->sharedDataPtr->currentVertAngleModel.mean; gzmsg << "Current velocity vertical angle [rad] Gauss-Markov process model:" << std::endl; - this->dataPtr->currentVertAngleModel.Print(); + this->sharedDataPtr->currentVertAngleModel.Print(); - this->dataPtr->currentVelModel.lastUpdate = + this->sharedDataPtr->currentVelModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - this->dataPtr->currentHorzAngleModel.lastUpdate = + this->sharedDataPtr->currentHorzAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - this->dataPtr->currentVertAngleModel.lastUpdate = + this->sharedDataPtr->currentVertAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); // Advertise the current velocity & stratified current velocity topics @@ -650,9 +629,10 @@ void UnderwaterCurrentPlugin::PublishCurrentVelocity() { gz::msgs::Vector3d currentVel; gz::msgs::Set( - ¤tVel, gz::math::Vector3d( - this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), - this->dataPtr->currentVelocity.Z())); + ¤tVel, + gz::math::Vector3d( + this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), + this->sharedDataPtr->currentVelocity.Z())); this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); } @@ -661,8 +641,8 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() { dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check for (std::vector::iterator it = - this->dataPtr->currentStratifiedVelocity.begin(); - it != this->dataPtr->currentStratifiedVelocity.end(); + this->sharedDataPtr->currentStratifiedVelocity.begin(); + it != this->sharedDataPtr->currentStratifiedVelocity.end(); ++it) // currentStratifiedVelocity values defined where ? (TODO) { gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); @@ -692,30 +672,30 @@ void UnderwaterCurrentPlugin::Update( // model // Update current velocity - double currentVelMag = this->dataPtr->currentVelModel.Update(time); + double currentVelMag = this->sharedDataPtr->currentVelModel.Update(time); // Update current horizontal direction around z axis of flow frame - double horzAngle = this->dataPtr->currentHorzAngleModel.Update(time); + double horzAngle = this->sharedDataPtr->currentHorzAngleModel.Update(time); // Update current horizontal direction around z axis of flow frame - double vertAngle = this->dataPtr->currentVertAngleModel.Update(time); + double vertAngle = this->sharedDataPtr->currentVertAngleModel.Update(time); // Generating the current velocity vector as in the North-East-Down frame - this->dataPtr->currentVelocity = gz::math::Vector3d( + this->sharedDataPtr->currentVelocity = gz::math::Vector3d( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle)); // Generate the depth-specific velocities - this->dataPtr->currentStratifiedVelocity.clear(); - for (int i = 0; i < this->dataPtr->stratifiedDatabase.size(); i++) + this->sharedDataPtr->currentStratifiedVelocity.clear(); + for (int i = 0; i < this->sharedDataPtr->stratifiedDatabase.size(); i++) { - double depth = this->dataPtr->stratifiedDatabase[i].Z(); - currentVelMag = this->dataPtr->stratifiedCurrentModels[i][0].Update(time); - horzAngle = this->dataPtr->stratifiedCurrentModels[i][1].Update(time); - vertAngle = this->dataPtr->stratifiedCurrentModels[i][2].Update(time); + double depth = this->sharedDataPtr->stratifiedDatabase[i].Z(); + currentVelMag = this->sharedDataPtr->stratifiedCurrentModels[i][0].Update(time); + horzAngle = this->sharedDataPtr->stratifiedCurrentModels[i][1].Update(time); + vertAngle = this->sharedDataPtr->stratifiedCurrentModels[i][2].Update(time); gz::math::Vector4d depthVel( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); - this->dataPtr->currentStratifiedVelocity.push_back(depthVel); + this->sharedDataPtr->currentStratifiedVelocity.push_back(depthVel); } } diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh index d08d805c..fbcdc215 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh @@ -9,6 +9,7 @@ #include #include +#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include "dave_interfaces/msg/Stratified_Current_Database.hpp" #include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" #include "dave_interfaces/srv/Get_Current_Model.hpp" @@ -19,7 +20,6 @@ #include "dave_interfaces/srv/Set_Origin_Spherical_Coord.hpp" #include "dave_interfaces/srv/Set_Stratified_Current_Direction.hpp" #include "dave_interfaces/srv/Set_Stratified_Current_Velocity.hpp" -// #include // #include "dave_interfaces/srv/Stratified_Current_Database.hpp" // #include "dave_interfaces/srv/Stratified_Current_Velocity.hpp" @@ -28,7 +28,6 @@ namespace dave_ros_gz_plugins { class UnderwaterCurrentROSPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, public gz::sim::ISystemPostUpdate { public: diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 72b50ea5..39cf577d 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,5 +1,7 @@ #include "dave_ros_gz_plugins/ocean_current_plugin.hh" +#include "dave_gz_world_plugins/gauss_markov_process.hh" #include "dave_gz_world_plugins/ocean_current_world_plugin.hh" +// #include "dave_gz_world_plugins/UnderwaterCurrentPlugin.hh" // #include #include #include @@ -10,7 +12,9 @@ #include #include #include +#include #include +#include #include #include @@ -24,6 +28,7 @@ GZ_ADD_PLUGIN( namespace dave_ros_gz_plugins { +; struct UnderwaterCurrentROSPlugin::PrivateData { // std::string db_path; (check)(TODO) @@ -66,10 +71,7 @@ struct UnderwaterCurrentROSPlugin::PrivateData UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() : dataPtr(std::make_unique()) { } -UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() -{ - this->dataPtr->rosNode.reset(); // Properly handle ROS 2 node shutdown -} +UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() {} ///////////////////////////////////////////////// void UnderwaterCurrentROSPlugin::Configure( @@ -246,8 +248,8 @@ bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->dataPtr->currentHorzAngleModel.SetMean(_req->angle); - + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + _res->success = data.currentHorzAngleModel.SetMean(_req->angle); return true; } @@ -256,21 +258,22 @@ bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + if (_req->layer >= data.stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + _res->success = data.stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); if (_res->success) { // Update the database values (new angle, unchanged velocity) - double velocity = hypot( - this->dataPtr->stratifiedDatabase[_req->layer].X(), - this->dataPtr->stratifiedDatabase[_req->layer].Y()); - this->dataPtr->stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + double velocity = + hypot(data.stratifiedDatabase[_req->layer].X(), data.stratifiedDatabase[_req->layer].Y()); + data.stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + data.stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; } return true; } @@ -280,7 +283,8 @@ bool UnderwaterCurrentROSPlugin::UpdateVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->dataPtr->currentVertAngleModel.SetMean(_req->angle); + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + _res->success = data.currentVertAngleModel.SetMean(_req->angle); return true; } @@ -289,12 +293,14 @@ bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + if (_req->layer >= data.stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + _res->success = data.stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); return true; } @@ -303,21 +309,23 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + if ( - this->dataPtr->currentVelModel.SetMean(_req->velocity) && - this->dataPtr->currentHorzAngleModel.SetMean(_req->horizontal_angle) && - this->dataPtr->currentVertAngleModel.SetMean(_req->vertical_angle)) + data.currentVelModel.SetMean(_req->velocity) && + data.currentHorzAngleModel.SetMean(_req->horizontal_angle) && + data.currentVertAngleModel.SetMean(_req->vertical_angle)) { - 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 + 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 << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; + _res->success = true; } else { gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; + _res->success = false; } return true; } @@ -327,31 +335,31 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - if (_req->layer >= this->dataPtr->stratifiedDatabase.size()) + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + if (_req->layer >= data.stratifiedDatabase.size()) { _res->success = false; return true; } if ( - this->dataPtr->stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && - this->dataPtr->stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && - this->dataPtr->stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + data.stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + data.stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + data.stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) { // Update the database values as well - this->dataPtr->stratifiedDatabase[_req->layer].X() = - cos(_req->horizontal_angle) * _req->velocity; - this->dataPtr->stratifiedDatabase[_req->layer].Y() = - sin(_req->horizontal_angle) * _req->velocity; - gzmsg << "Layer " << _req.layer << " current velocity [m/s] = " << _req.velocity << std::endl - << " Horizontal angle [rad] = " << _req.horizontal_angle << std::endl - << " Vertical angle [rad] = " << _req.vertical_angle << std::endl + data.stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; + data.stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; + gzmsg << "Layer " << _req->layer << " current velocity [m/s] = " << _req->velocity << std::endl + << " Horizontal angle [rad] = " << _req->horizontal_angle << std::endl + << " Vertical angle [rad] = " << _req->vertical_angle << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - _res.success = true; + _res->success = true; } else { gzmsg << "Error while updating the current velocity" << std::endl; - _res.success = false; + _res->success = false; } return true; } @@ -361,11 +369,13 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->dataPtr->currentVelModel.mean; - _res->min = this->dataPtr->currentVelModel.min; - _res->max = this->dataPtr->currentVelModel.max; - _res->noise = this->dataPtr->currentVelModel.noiseAmp; - _res->mu = this->dataPtr->currentVelModel.mu; + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->mean = data.currentVelModel.mean; + _res->min = data.currentVelModel.min; + _res->max = data.currentVelModel.max; + _res->noise = data.currentVelModel.noiseAmp; + _res->mu = data.currentVelModel.mu; return true; } @@ -374,11 +384,13 @@ bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->dataPtr->currentHorzAngleModel.mean; - _res->min = this->dataPtr->currentHorzAngleModel.min; - _res->max = this->dataPtr->currentHorzAngleModel.max; - _res->noise = this->dataPtr->currentHorzAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentHorzAngleModel.mu; + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->mean = data.currentHorzAngleModel.mean; + _res->min = data.currentHorzAngleModel.min; + _res->max = data.currentHorzAngleModel.max; + _res->noise = data.currentHorzAngleModel.noiseAmp; + _res->mu = data.currentHorzAngleModel.mu; return true; } @@ -387,11 +399,13 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->mean = this->dataPtr->currentVertAngleModel.mean; - _res->min = this->dataPtr->currentVertAngleModel.min; - _res->max = this->dataPtr->currentVertAngleModel.max; - _res->noise = this->dataPtr->currentVertAngleModel.noiseAmp; - _res->mu = this->dataPtr->currentVertAngleModel.mu; + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->mean = data.currentVertAngleModel.mean; + _res->min = data.currentVertAngleModel.min; + _res->max = data.currentVertAngleModel.max; + _res->noise = data.currentVertAngleModel.noiseAmp; + _res->mu = data.currentVertAngleModel.mu; return true; } @@ -400,20 +414,23 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->dataPtr->currentVelModel.SetModel( + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->success = data.currentVelModel.SetModel( std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) + for (int i = 0; i < data.stratifiedCurrentModels.size(); i++) { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][0]; //(updated) + dave_gz_world_plugins::GaussMarkovProcess & model = + data.stratifiedCurrentModels[i][0]; //(updated) model.SetModel( model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); } gzmsg << "Current velocity model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVelModel.Print(); + data.currentVelModel.Print(); return true; } @@ -422,19 +439,21 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->dataPtr->currentHorzAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->success = + data.currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); - for (int i = 0; i < this->dataPtr->stratifiedCurrentModels.size(); i++) + for (int i = 0; i < data.stratifiedCurrentModels.size(); i++) { - gz::GaussMarkovProcess & model = this->dataPtr->stratifiedCurrentModels[i][1]; + dave_gz_world_plugins::GaussMarkovProcess & model = data.stratifiedCurrentModels[i][1]; model.SetModel( model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); } gzmsg << "Horizontal angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentHorzAngleModel.Print(); + data.currentHorzAngleModel.Print(); return true; // gzmsg << "Horizontal angle model updated" << std::endl @@ -447,12 +466,14 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - _res->success = this->dataPtr->currentVertAngleModel.SetModel( - _req->mean, _req->min, _req->max, _req->mu, _req->noise); + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + + _res->success = + data.currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); gzmsg << "Vertical angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - this->dataPtr->currentVertAngleModel.Print(); + data.currentVertAngleModel.Print(); return true; } @@ -460,15 +481,16 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( void UnderwaterCurrentROSPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { + dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; // Generate and publish current velocity according to the vehicle depth geometry_msgs::msg::TwistStamped flowVelMsg; flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); flowVelMsg.header.frame_id = "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions - flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); - flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); - flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + flowVelMsg.twist.linear.x = data.currentVelocity.X(); + flowVelMsg.twist.linear.y = data.currentVelocity.Y(); + flowVelMsg.twist.linear.z = data.currentVelocity.Z(); this->dataPtr->flowVelocityPub->publish(flowVelMsg); @@ -482,72 +504,72 @@ void UnderwaterCurrentROSPlugin::PostUpdate( // Updating for stratified behaviour of Ocean Currents // What is the .size value over here, to be (checked) - for (size_t i = 0; i < this->dataPtr->currentStratifiedVelocity.size(); + for (size_t i = 0; i < data.currentStratifiedVelocity.size(); i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) { geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->currentStratifiedVelocity[i].X(); - velocity.y = this->dataPtr->currentStratifiedVelocity[i].Y(); - velocity.z = this->dataPtr->currentStratifiedVelocity[i].Z(); + velocity.x = data.currentStratifiedVelocity[i].X(); + velocity.y = data.currentStratifiedVelocity[i].Y(); + velocity.z = data.currentStratifiedVelocity[i].Z(); stratCurrentVelocityMsg.velocities.push_back(velocity); - stratCurrentVelocityMsg.depths.push_back(this->dataPtr->currentStratifiedVelocity[i].W()); + stratCurrentVelocityMsg.depths.push_back(data.currentStratifiedVelocity[i].W()); } this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; i < this->dataPtr->stratifiedDatabase - .size(); // again check with ocean_cureent_world_plugin.cc (TODO) - i++) // read from csv file in ocean_cureent_world_plugin.cc + for (int i = 0; + i < data.stratifiedDatabase.size(); // again check with ocean_cureent_world_plugin.cc (TODO) + i++) // read from csv file in ocean_cureent_world_plugin.cc { // Stratified current database entry preparation geometry_msgs::msg::Vector3 velocity; - velocity.x = this->dataPtr->stratifiedDatabase[i].X(); - velocity.y = this->dataPtr->stratifiedDatabase[i].Y(); + velocity.x = data.stratifiedDatabase[i].X(); + velocity.y = data.stratifiedDatabase[i].Y(); velocity.z = 0.0; // Assuming z is intentionally set to 0.0 currentDatabaseMsg.velocities.push_back(velocity); - currentDatabaseMsg.depths.push_back(this->dataPtr->stratifiedDatabase[i].Z()); + currentDatabaseMsg.depths.push_back(data.stratifiedDatabase[i].Z()); } - if (this->dataPtr->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) + if (data.tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) { // Tidal harmonic constituents - currentDatabaseMsg.m2amp = this->dataPtr->M2_amp; - currentDatabaseMsg.m2phase = this->dataPtr->M2_phase; - currentDatabaseMsg.m2speed = this->dataPtr->M2_speed; - currentDatabaseMsg.s2amp = this->dataPtr->S2_amp; - currentDatabaseMsg.s2phase = this->dataPtr->S2_phase; - currentDatabaseMsg.s2speed = this->dataPtr->S2_speed; - currentDatabaseMsg.n2amp = this->dataPtr->N2_amp; - currentDatabaseMsg.n2phase = this->dataPtr->N2_phase; - currentDatabaseMsg.n2speed = this->dataPtr->N2_speed; - currentDatabaseMsg.tideConstituents = true; + currentDatabaseMsg.m2_amp = data.M2_amp; + currentDatabaseMsg.m2_phase = data.M2_phase; + currentDatabaseMsg.m2_speed = data.M2_speed; + currentDatabaseMsg.s2_amp = data.S2_amp; + currentDatabaseMsg.s2_phase = data.S2_phase; + currentDatabaseMsg.s2_speed = data.S2_speed; + currentDatabaseMsg.n2_amp = data.N2_amp; + currentDatabaseMsg.n2_phase = data.N2_phase; + currentDatabaseMsg.n2_speed = data.N2_speed; + currentDatabaseMsg.tideconstituents = true; } else { - for (int i = 0; i < this->dataPtr->dateGMT.size(); i++) + for (int i = 0; i < data.dateGMT.size(); i++) { // Tidal oscillation database - currentDatabaseMsg.timegmtyear.push_back(this->dataPtr->dateGMT[i][0]); - currentDatabaseMsg.timegmtmonth.push_back(this->dataPtr->dateGMT[i][1]); - currentDatabaseMsg.timegmtday.push_back(this->dataPtr->dateGMT[i][2]); - currentDatabaseMsg.timegmthour.push_back(this->dataPtr->dateGMT[i][3]); - currentDatabaseMsg.timegmtminute.push_back(this->dataPtr->dateGMT[i][4]); + currentDatabaseMsg.time_gmt_year.push_back(data.dateGMT[i][0]); + currentDatabaseMsg.time_gmt_month.push_back(data.dateGMT[i][1]); + currentDatabaseMsg.time_gmt_day.push_back(data.dateGMT[i][2]); + currentDatabaseMsg.time_gmt_hour.push_back(data.dateGMT[i][3]); + currentDatabaseMsg.time_gmt_minute.push_back(data.dateGMT[i][4]); - currentDatabaseMsg.tidevelocities.push_back(this->dataPtr->speedcmsec[i]); + currentDatabaseMsg.tidevelocities.push_back(data.speedcmsec[i]); } - currentDatabaseMsg.tideConstituents = false; + currentDatabaseMsg.tideconstituents = false; } - currentDatabaseMsg.ebbdirection = this->dataPtr->ebbDirection; - currentDatabaseMsg.flooddirection = this->dataPtr->floodDirection; + currentDatabaseMsg.ebb_direction = data.ebbDirection; + currentDatabaseMsg.flood_direction = data.floodDirection; - currentDatabaseMsg.worldstarttimeyear = this->dataPtr->world_start_time_year; - currentDatabaseMsg.worldstarttimemonth = this->dataPtr->world_start_time_month; - currentDatabaseMsg.worldstarttimeday = this->dataPtr->world_start_time_day; - currentDatabaseMsg.worldstarttimehour = this->dataPtr->world_start_time_hour; - currentDatabaseMsg.worldstarttimeminute = this->dataPtr->world_start_time_minute; + currentDatabaseMsg.world_start_time_year = data.world_start_time_year; + currentDatabaseMsg.world_start_time_month = data.world_start_time_month; + currentDatabaseMsg.world_start_time_day = data.world_start_time_day; + currentDatabaseMsg.world_start_time_hour = data.world_start_time_hour; + currentDatabaseMsg.world_start_time_minute = data.world_start_time_minute; this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); @@ -555,8 +577,6 @@ void UnderwaterCurrentROSPlugin::PostUpdate( this->dataPtr->lastUpdate = _info.simTime; } -///////////////////////////////////////////////// -GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentROSPlugin) } // namespace dave_ros_gz_plugins // #endif \ No newline at end of file From a209450bab1aff2f3f2e0cc37cda2069307eaad4 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 18 Aug 2024 00:28:21 +0530 Subject: [PATCH 25/79] pre final code --- .../ocean_current_model_plugin.hh | 8 +-- .../src/ocean_current_model_plugin.cc | 55 ++++++------------- .../src/ocean_current_world_plugin.cc | 11 ++-- .../src/ocean_current_plugin.cc | 7 +-- 4 files changed, 25 insertions(+), 56 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 65c0f250..628a18a3 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -24,7 +24,6 @@ namespace dave_gz_model_plugins { class TransientCurrentPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, public gz::sim::ISystemUpdate, public gz::sim::ISystemPostUpdate { @@ -37,9 +36,6 @@ public: const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - // Function called before the simulation state updates - void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); // Function called after the simulation state updates @@ -68,9 +64,7 @@ public: /// \brief Calculate ocean current using database and vehicle state void CalculateOceanCurrent(double vehicleDepth); - void Gauss_Markov_process_initialize( - const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + void Gauss_Markov_process_initialize(const std::shared_ptr & _sdf); /// \brief Convey model state from gazebo topic to outside void UpdateDatabase(const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg); diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index c11e97de..13a236d8 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -29,7 +29,7 @@ #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -#include //TODO +#include #include #include #include @@ -38,7 +38,6 @@ GZ_ADD_PLUGIN( dave_gz_model_plugins::TransientCurrentPlugin, gz::sim::System, dave_gz_model_plugins::TransientCurrentPlugin::ISystemConfigure, - dave_gz_model_plugins::TransientCurrentPlugin::ISystemPreUpdate, dave_gz_model_plugins::TransientCurrentPlugin::ISystemUpdate, dave_gz_model_plugins::TransientCurrentPlugin::ISystemPostUpdate) @@ -48,7 +47,7 @@ struct TransientCurrentPlugin::PrivateData { // Initialize any necessary states before the plugin starts virtual void Init(); - std::string transientCurrentVelocityTopic; // Declare the variable (updated) + std::string transientCurrentVelocityTopic; gz::sim::World world{gz::sim::kNullEntity}; gz::sim::Entity entity{gz::sim::kNullEntity}; gz::sim::Model model{gz::sim::kNullEntity}; @@ -70,10 +69,10 @@ struct TransientCurrentPlugin::PrivateData // std::shared_ptr> flowVelocityPub; std::string modelName; - /// \brief Gauss-Markov process instance for the velocity components // TODO - dave_gz_world_plugins::GaussMarkovProcess currentVelNorthModel; // TODO - dave_gz_world_plugins::GaussMarkovProcess currentVelEastModel; // TODO - dave_gz_world_plugins::GaussMarkovProcess currentVelDownModel; // TODO + /// \brief Gauss-Markov process instance for the velocity components + dave_gz_world_plugins::GaussMarkovProcess currentVelNorthModel; + dave_gz_world_plugins::GaussMarkovProcess currentVelEastModel; + dave_gz_world_plugins::GaussMarkovProcess currentVelDownModel; /// \brief Gauss-Markov noise double noiseAmp_North; @@ -187,10 +186,16 @@ void TransientCurrentPlugin::Configure( this->dataPtr->gz_current_vel_pub = this->dataPtr->gz_node->Advertise(this->dataPtr->currentVelocityTopic); + // Subscribe stratified ocean current database + this->dataPtr->databaseSub = + this->ros_node_->create_subscription( + this->dataPtr->transientCurrentVelocityTopic, 10, + std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); + // Read topic name of stratified ocean current from SDF LoadCurrentVelocityParams(sdfClone, _ecm); - Gauss_Markov_process_initialize( - _entity, _sdf, _ecm, _eventMgr); // something is wrong here (check) + Gauss_Markov_process_initialize(_sdf); + gzmsg << "Transient current model plugin loaded!" << std::endl; } ///////////////////////////////////////////////// @@ -201,8 +206,7 @@ void TransientCurrentPlugin::LoadCurrentVelocityParams( sdf::ElementPtr currentVelocityParams; if (_sdf->HasElement("transient_current")) // Add this to the sdf file TODO { - currentVelocityParams = _sdf->GetElement( - "transient_current"); // tried const sdf::Element switched to using Auto (check) + currentVelocityParams = _sdf->GetElement("transient_current"); if (currentVelocityParams->HasElement("topic_stratified")) { this->dataPtr->transientCurrentVelocityTopic = @@ -419,11 +423,6 @@ void TransientCurrentPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & // Generate and publish Gazebo topic according to the vehicle depth gz::msgs::Vector3d currentVel; - // msgs::Set( - // ¤tVel, gz::math::Vector3d( - // this->dataPtr->currentVelocity.X(), this->dataPtr->currentVelocity.Y(), - // this->dataPtr->currentVelocity.Z())); - currentVel.set_x(this->dataPtr->currentVelocity.X()); currentVel.set_y(this->dataPtr->currentVelocity.Y()); currentVel.set_z(this->dataPtr->currentVelocity.Z()); @@ -487,10 +486,9 @@ void TransientCurrentPlugin::UpdateDatabase( this->dataPtr->lock_.unlock(); } -///////////////////////////////////////////////// (check this,dpr) +///////////////////////////////////////////////// void TransientCurrentPlugin::Gauss_Markov_process_initialize( - const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) + const std::shared_ptr & _sdf) { // Read Gauss-Markov parameters sdf::ElementPtr currentVelocityParams; @@ -593,29 +591,10 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize( << "Gauss-Markov process model:" << std::endl; this->dataPtr->currentVelDownModel.Print(); - // this->dataPtr->lastUpdate = _info.simTime; This isn't needed because the last update is being - // initialised to 0 and is being updated at postupdate this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.count(); this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.count(); this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.count(); } -///////////////////////////////////////////////// -void TransientCurrentPlugin::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ - this->dataPtr->time = _info.simTime; - // Subscribe stratified ocean current database - this->dataPtr->databaseSub = - this->ros_node_->create_subscription( - this->dataPtr->transientCurrentVelocityTopic, 10, - std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); - - // Connect the update event callback for ROS and ocean current calculation - this->dataPtr->Connect(); - - gzmsg << "Transient current model plugin loaded!" << std::endl; -} - ///////////////////////////////////////////////// void TransientCurrentPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 825893d7..00b78170 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -128,11 +128,10 @@ void UnderwaterCurrentPlugin::Configure( LoadTidalOscillationDatabase(); } } - - // Connect the update event. This isn't needed it seems (check) - // this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin( - // boost::bind(&UnderwaterCurrentPlugin::Update, - // this, _1)); + else + { + gzmsg << "Tidal oscillation not enabled" << std::endl; + } gzmsg << "Underwater current plugin loaded!" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; @@ -148,7 +147,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() this->dataPtr->sdf->GetElement("tidal_oscillation"); // include this xml/ (TODO) sdf::ElementPtr tidalHarmonicParams; - // Read the tidal oscillation parameter from the SDF file // (TO DO)(TO UNDERSTAND) + // Read the tidal oscillation parameter from the SDF file // (TODO)(TO UNDERSTAND) if (tidalOscillationParams->HasElement("databasefilePath")) { this->dataPtr->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 39cf577d..97557e52 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -1,8 +1,4 @@ #include "dave_ros_gz_plugins/ocean_current_plugin.hh" -#include "dave_gz_world_plugins/gauss_markov_process.hh" -#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" -// #include "dave_gz_world_plugins/UnderwaterCurrentPlugin.hh" -// #include #include #include #include @@ -17,6 +13,8 @@ #include #include #include +#include "dave_gz_world_plugins/gauss_markov_process.hh" +#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" #include "gz/plugin/Register.hh" #include "gz/sim/components/World.hh" @@ -28,7 +26,6 @@ GZ_ADD_PLUGIN( namespace dave_ros_gz_plugins { -; struct UnderwaterCurrentROSPlugin::PrivateData { // std::string db_path; (check)(TODO) From dea4ff953fac403cb77367908932fbc19284f4f9 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 17:36:38 +0900 Subject: [PATCH 26/79] gazebo source install script --- ...jazzy-binary-gz-harmonic-source-install.sh | 115 ++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 extras/ros-jazzy-binary-gz-harmonic-source-install.sh diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh new file mode 100644 index 00000000..b482a3c2 --- /dev/null +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -0,0 +1,115 @@ +#!/bin/bash +# Script to install development tools and libraries for robotics and simulation +echo +echo -e "\033[94m============================================================\033[0m" +echo -e "\033[94m== One-liner Installation Script for ROS-Gazebo Framework ==\033[0m" +echo -e "\033[94m============================================================\033[0m" +echo -e "Requirements: Ubuntu 24.04 LTS Noble" +echo -e "\033[94m============================================================\033[0m" + +echo +echo -e "\033[96m(1/4) ------------- Updating the System ----------------\033[0m" +echo "Performing full system upgrade (this might take a while)..." +sudo sudo apt update && apt full-upgrade -y + +echo +echo -e "\033[96m(2/4) ------------ Install Dependencies ---------------\033[0m" +echo -e "\033[34mInstalling essential tools and libraries...\033[0m" +sudo apt install -y \ + build-essential \ + cmake \ + cppcheck \ + curl \ + git \ + gnupg \ + libeigen3-dev \ + libgles2-mesa-dev \ + lsb-release \ + pkg-config \ + protobuf-compiler \ + python3-dbg \ + python3-pip \ + python3-venv \ + qtbase5-dev \ + ruby \ + software-properties-common \ + sudo \ + cppzmq-dev \ + wget + +echo +echo -e "\033[96m(3/4) ------------ Install Package Keys ---------------\033[0m" +echo -e "\033[34mInstalling Signing Keys for ROS and Gazebo...\033[0m" +# Remove keyring if exists to avoid conflicts +sudo rm -f /usr/share/keyrings/ros2-latest-archive-keyring.gpg && \ + sudo rm -rf /etc/apt/sources.list.d/ros2-latest.list +# Get Keys +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg +sudo wget https://packages.osrfoundation.org/gazebo.gpg \ + -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + +ARCH=$(dpkg --print-architecture) +# shellcheck disable=SC1091 +UBUNTU_CODENAME=$( . /etc/os-release && echo "$UBUNTU_CODENAME") +REPO="deb [arch=$ARCH signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ +http://packages.ros.org/ros2/ubuntu $UBUNTU_CODENAME main" +echo "$REPO" | sudo tee /etc/apt/sources.list.d/ros2.list >/dev/null + +DISTRO=$(lsb_release -cs) +REPO="deb [arch=$ARCH signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \ +http://packages.osrfoundation.org/gazebo/ubuntu-stable $DISTRO main" +echo "$REPO" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list >/dev/null + +echo +echo -e "\033[96m(4/4) ------------ Install ROS-Gazebo ---------------\033[0m" +DIST=jazzy + +echo -e "\033[34mInstalling ROS Gazebo framework...\033[0m" +sudo apt update && apt install -y \ + python3-rosdep \ + python3-rosinstall-generator \ + python3-vcstool \ + ros-$DIST-desktop \ + ros-$DIST-ros-gz \ + ros-$DIST-gz-ros2-control \ + ros-$DIST-effort-controllers \ + ros-$DIST-geographic-info \ + ros-$DIST-joint-state-publisher \ + ros-$DIST-joy-teleop \ + ros-$DIST-key-teleop \ + ros-$DIST-moveit-planners \ + ros-$DIST-moveit-simple-controller-manager \ + ros-$DIST-moveit-ros-visualization \ + ros-$DIST-robot-localization \ + ros-$DIST-ros2-controllers \ + ros-$DIST-teleop-tools \ + ros-$DIST-urdfdom-py \ + ros-dev-tools + +echo +echo -e "\033[34mSetting up Gazebo source directory...\033[0m" +mkdir -p /opt/gazebo/src && cd /opt/gazebo/src || exit +curl -O https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/collection-harmonic.yaml +vcs import < collection-harmonic.yaml + +echo -e "\033[34mUpdating package list and installing dependencies...\033[0m" +# shellcheck disable=SC2046,SC2006 +sudo apt -y install $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/gz\|sdf/d' | tr '\n' ' ') + +echo -e "\033[34mBuilding the project with colcon...\033[0m" +cd /opt/gazebo || exit +# Build gz-physics with limited cores to avoid memory issues +MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics +# Full build +colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install + +echo +echo -e "\033[32m============================================================\033[0m" +echo -e "\033[32mROS-Gazebo Framework Installation completed. Awesome! 🤘🚀 \033[0m" +echo -e "Following command will set-up ROS-Gazebo environment variables to run it" +echo -e "\033[95msource /opt/ros/jazzy/setup.bash\033[0m" +echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" +echo -e "\033[95mexport PYTHONPATH=\$PYTHONPATH:/opt/gazebo/install/lib/python\033[0m" +echo -e "You may check ROS, and Gazebo version installed with \033[33mprintenv ROS_DISTRO\033[0m and \033[33mecho \$GZ_VERSION\033[0m" +echo From ce110039e17a740837949f8be9ccdfddf252ef96 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 19:15:38 +0900 Subject: [PATCH 27/79] specify gz-phyics version --- extras/ros-jazzy-binary-gz-harmonic-source-install.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh index b482a3c2..7fb496fb 100644 --- a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -100,7 +100,7 @@ sudo apt -y install $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' echo -e "\033[34mBuilding the project with colcon...\033[0m" cd /opt/gazebo || exit # Build gz-physics with limited cores to avoid memory issues -MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics +MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics7 # Full build colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install From 9ee8fe11628431bdc019ded4f71b1ec99d408f0a Mon Sep 17 00:00:00 2001 From: CHOI WOEN-SUG Date: Thu, 8 Aug 2024 09:03:05 +0900 Subject: [PATCH 28/79] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 3a9ee3c2..161efa22 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ # DAVE +[![Publish a Docker image (AMD64; Common X86_64 Linux Machine)](https://github.com/IOES-Lab/dave/actions/workflows/docker-amd64.yml/badge.svg)](https://github.com/IOES-Lab/dave/actions/workflows/docker-amd64.yml) +[![Publish a Docker image (ARM64; Apple Silicon)](https://github.com/IOES-Lab/dave/actions/workflows/docker-arm64v8.yml/badge.svg?branch=ros2)](https://github.com/IOES-Lab/dave/actions/workflows/docker-arm64v8.yml) + For contribution, do `pip3 install pre-commit && pre-commit install && pre-commit run --all-files` before commit. From 35b2f052d69a2eff3012e2732214c7acb20ce4e4 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 15:39:37 +0900 Subject: [PATCH 29/79] source install test --- .docker/jazzy.arm64v8.dockerfile | 5 +++-- .github/workflows/docker-amd64.yml | 1 + .github/workflows/docker-arm64v8.yml | 1 + .../ros-jazzy-binary-gz-harmonic-source-install.sh | 14 ++++++++++++++ 4 files changed, 19 insertions(+), 2 deletions(-) diff --git a/.docker/jazzy.arm64v8.dockerfile b/.docker/jazzy.arm64v8.dockerfile index 2dde1ef7..df0dbf2d 100644 --- a/.docker/jazzy.arm64v8.dockerfile +++ b/.docker/jazzy.arm64v8.dockerfile @@ -82,12 +82,12 @@ FROM woensugchoi/ubuntu-arm-rdp-base:latest ARG USER=docker # ROS-Gazebo arg -ARG BRANCH="ros2" +ARG BRANCH="dockertest" ARG ROS_DISTRO="jazzy" # Install ROS-Gazebo framework ADD https://raw.githubusercontent.com/IOES-Lab/dave/$BRANCH/\ -extras/ros-jazzy-gz-harmonic-install.sh install.sh +extras/ros-jazzy-binary-gz-harmonic-source-install.sh install.sh RUN bash install.sh # Set up Dave workspace @@ -129,6 +129,7 @@ RUN touch /ros_entrypoint.sh && sed --in-place --expression \ # Set User as user USER docker RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ + echo "source ~/gazebo/install/setup.bash" >> ~/.bashrc && \ echo "if [ -d ~/HOST ]; then chown docker:docker ~/HOST; fi" \ >> ~/.bashrc diff --git a/.github/workflows/docker-amd64.yml b/.github/workflows/docker-amd64.yml index 91bcdb27..e0f9423d 100644 --- a/.github/workflows/docker-amd64.yml +++ b/.github/workflows/docker-amd64.yml @@ -7,6 +7,7 @@ on: tags: ["*"] branches: - ros2 + - dockertest env: IMAGE_NAME: ${{ github.repository }} diff --git a/.github/workflows/docker-arm64v8.yml b/.github/workflows/docker-arm64v8.yml index f4ac0e1b..5b39d6e5 100644 --- a/.github/workflows/docker-arm64v8.yml +++ b/.github/workflows/docker-arm64v8.yml @@ -7,6 +7,7 @@ on: tags: ["*"] branches: - ros2 + - dockertest env: IMAGE_NAME: ${{ github.repository }} diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh index 7fb496fb..2fe5f3b7 100644 --- a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -89,7 +89,11 @@ sudo apt update && apt install -y \ echo echo -e "\033[34mSetting up Gazebo source directory...\033[0m" +<<<<<<< HEAD mkdir -p /opt/gazebo/src && cd /opt/gazebo/src || exit +======= +mkdir -p ~/gazebo/src && cd ~/gazebo/src || exit +>>>>>>> 3179fd4 (source install test) curl -O https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/collection-harmonic.yaml vcs import < collection-harmonic.yaml @@ -98,9 +102,15 @@ echo -e "\033[34mUpdating package list and installing dependencies...\033[0m" sudo apt -y install $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/gz\|sdf/d' | tr '\n' ' ') echo -e "\033[34mBuilding the project with colcon...\033[0m" +<<<<<<< HEAD cd /opt/gazebo || exit # Build gz-physics with limited cores to avoid memory issues MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics7 +======= +cd ~/gazebo || exit +# Build gz-physics with limited cores to avoid memory issues +MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics +>>>>>>> 3179fd4 (source install test) # Full build colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install @@ -109,7 +119,11 @@ echo -e "\033[32m============================================================\03 echo -e "\033[32mROS-Gazebo Framework Installation completed. Awesome! 🤘🚀 \033[0m" echo -e "Following command will set-up ROS-Gazebo environment variables to run it" echo -e "\033[95msource /opt/ros/jazzy/setup.bash\033[0m" +<<<<<<< HEAD echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" echo -e "\033[95mexport PYTHONPATH=\$PYTHONPATH:/opt/gazebo/install/lib/python\033[0m" +======= +echo -e "\033[95msource ~/gazebo/install/setup.bash\033[0m" +>>>>>>> 3179fd4 (source install test) echo -e "You may check ROS, and Gazebo version installed with \033[33mprintenv ROS_DISTRO\033[0m and \033[33mecho \$GZ_VERSION\033[0m" echo From b8423a4d804b0dee6b301d3832b5b76deb454e0c Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 16:07:05 +0900 Subject: [PATCH 30/79] amd --- .docker/jazzy.amd64.dockerfile | 7 ++++--- .../ros-jazzy-binary-gz-harmonic-source-install.sh | 12 ++++++++++++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/.docker/jazzy.amd64.dockerfile b/.docker/jazzy.amd64.dockerfile index 8ce4615f..5984df5e 100644 --- a/.docker/jazzy.amd64.dockerfile +++ b/.docker/jazzy.amd64.dockerfile @@ -1,6 +1,6 @@ ARG ROS_DISTRO="jazzy" FROM osrf/ros:$ROS_DISTRO-desktop-full -ARG BRANCH="ros2" +ARG BRANCH="dockertest" # Install Utilities # hadolint ignore=DL3008 @@ -39,7 +39,7 @@ RUN adduser --shell /bin/bash --disabled-password --gecos '' $USER \ # Install ROS-Gazebo framework ADD https://raw.githubusercontent.com/IOES-Lab/dave/$BRANCH/\ -extras/ros-jazzy-gz-harmonic-install.sh install.sh +extras/ros-jazzy-binary-gz-harmonic-source-install.sh install.sh RUN bash install.sh # Set up Dave workspace @@ -66,4 +66,5 @@ RUN touch /ros_entrypoint.sh && sed --in-place --expression \ # Set User as user USER $USER -RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc +RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ + echo "source /opt/gazebo/install/setup.bash" >> ~/.bashrc diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh index 2fe5f3b7..978a310e 100644 --- a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -90,10 +90,14 @@ sudo apt update && apt install -y \ echo echo -e "\033[34mSetting up Gazebo source directory...\033[0m" <<<<<<< HEAD +<<<<<<< HEAD mkdir -p /opt/gazebo/src && cd /opt/gazebo/src || exit ======= mkdir -p ~/gazebo/src && cd ~/gazebo/src || exit >>>>>>> 3179fd4 (source install test) +======= +mkdir -p /opt/gazebo/src && cd /opt/gazebo/src || exit +>>>>>>> 22941bc (amd) curl -O https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/collection-harmonic.yaml vcs import < collection-harmonic.yaml @@ -103,11 +107,15 @@ sudo apt -y install $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' echo -e "\033[34mBuilding the project with colcon...\033[0m" <<<<<<< HEAD +<<<<<<< HEAD cd /opt/gazebo || exit # Build gz-physics with limited cores to avoid memory issues MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics7 ======= cd ~/gazebo || exit +======= +cd /opt/gazebo || exit +>>>>>>> 22941bc (amd) # Build gz-physics with limited cores to avoid memory issues MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics >>>>>>> 3179fd4 (source install test) @@ -120,10 +128,14 @@ echo -e "\033[32mROS-Gazebo Framework Installation completed. Awesome! 🤘🚀 echo -e "Following command will set-up ROS-Gazebo environment variables to run it" echo -e "\033[95msource /opt/ros/jazzy/setup.bash\033[0m" <<<<<<< HEAD +<<<<<<< HEAD echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" echo -e "\033[95mexport PYTHONPATH=\$PYTHONPATH:/opt/gazebo/install/lib/python\033[0m" ======= echo -e "\033[95msource ~/gazebo/install/setup.bash\033[0m" >>>>>>> 3179fd4 (source install test) +======= +echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" +>>>>>>> 22941bc (amd) echo -e "You may check ROS, and Gazebo version installed with \033[33mprintenv ROS_DISTRO\033[0m and \033[33mecho \$GZ_VERSION\033[0m" echo From fe611e1726dfe065f2ab2f0aa15ff8922e6cb41a Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 17:10:27 +0900 Subject: [PATCH 31/79] clean up --- .docker/jazzy.amd64.dockerfile | 2 +- .docker/jazzy.arm64v8.dockerfile | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.docker/jazzy.amd64.dockerfile b/.docker/jazzy.amd64.dockerfile index 5984df5e..14d1ca26 100644 --- a/.docker/jazzy.amd64.dockerfile +++ b/.docker/jazzy.amd64.dockerfile @@ -43,7 +43,7 @@ extras/ros-jazzy-binary-gz-harmonic-source-install.sh install.sh RUN bash install.sh # Set up Dave workspace -ENV DAVE_WS /root/ws_dave +ENV DAVE_WS=/opt/ws_dave WORKDIR $DAVE_WS/src ADD https://raw.githubusercontent.com/IOES-Lab/dave/$BRANCH/\ diff --git a/.docker/jazzy.arm64v8.dockerfile b/.docker/jazzy.arm64v8.dockerfile index df0dbf2d..88d8cf30 100644 --- a/.docker/jazzy.arm64v8.dockerfile +++ b/.docker/jazzy.arm64v8.dockerfile @@ -78,7 +78,7 @@ # Using the pre-built image for above commented out dockerfile code lines # hadolint ignore=DL3007 -FROM woensugchoi/ubuntu-arm-rdp-base:latest +FROM --platform=linux/arm64 woensugchoi/ubuntu-arm-rdp-base:latest ARG USER=docker # ROS-Gazebo arg @@ -91,7 +91,7 @@ extras/ros-jazzy-binary-gz-harmonic-source-install.sh install.sh RUN bash install.sh # Set up Dave workspace -ENV ROS_UNDERLAY /home/$USER/dave_ws/install +ENV ROS_UNDERLAY=/home/$USER/dave_ws/install WORKDIR $ROS_UNDERLAY/../src ADD https://raw.githubusercontent.com/IOES-Lab/dave/$BRANCH/\ @@ -129,7 +129,7 @@ RUN touch /ros_entrypoint.sh && sed --in-place --expression \ # Set User as user USER docker RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ - echo "source ~/gazebo/install/setup.bash" >> ~/.bashrc && \ + echo "source /opt/gazebo/install/setup.bash" >> ~/.bashrc && \ echo "if [ -d ~/HOST ]; then chown docker:docker ~/HOST; fi" \ >> ~/.bashrc From b3640cfe73ad0bcdb8dd4d5ef845a8754d9cdeac Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 17:26:47 +0900 Subject: [PATCH 32/79] add entry and pythonpath --- .docker/jazzy.amd64.dockerfile | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/.docker/jazzy.amd64.dockerfile b/.docker/jazzy.amd64.dockerfile index 14d1ca26..1f068951 100644 --- a/.docker/jazzy.amd64.dockerfile +++ b/.docker/jazzy.amd64.dockerfile @@ -62,7 +62,15 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" && \ # source entrypoint setup RUN touch /ros_entrypoint.sh && sed --in-place --expression \ - '$i source "$DAVE_WS/install/setup.bash"' /ros_entrypoint.sh + '$i source "/opt/ws_dave/install/setup.bash"' /ros_entrypoint.sh + +# Source ROS and Gazebo +RUN sed --in-place --expression \ +'$i source "/opt/ros/jazzy/setup.bash"' /ros_entrypoint.sh && \ +sed --in-place --expression \ +'$i source "/opt/gazebo/install/setup.bash"' /ros_entrypoint.sh && \ +sed --in-place --expression \ +'$i export PYTHONPATH=$PYTHONPATH:/opt/gazebo/install/lib/python' /ros_entrypoint.sh # Set User as user USER $USER From 5273020dda38a3c62c4398a119b9df274faa9617 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 17:37:26 +0900 Subject: [PATCH 33/79] minor fix --- extras/ros-jazzy-binary-gz-harmonic-source-install.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh index 978a310e..ba89779b 100644 --- a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -131,11 +131,14 @@ echo -e "\033[95msource /opt/ros/jazzy/setup.bash\033[0m" <<<<<<< HEAD echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" echo -e "\033[95mexport PYTHONPATH=\$PYTHONPATH:/opt/gazebo/install/lib/python\033[0m" +<<<<<<< HEAD ======= echo -e "\033[95msource ~/gazebo/install/setup.bash\033[0m" >>>>>>> 3179fd4 (source install test) ======= echo -e "\033[95msource /opt/gazebo/install/setup.bash\033[0m" >>>>>>> 22941bc (amd) +======= +>>>>>>> b9dd381 (minor fix) echo -e "You may check ROS, and Gazebo version installed with \033[33mprintenv ROS_DISTRO\033[0m and \033[33mecho \$GZ_VERSION\033[0m" echo From f16e9d72618822f7c1d9c1fe2b275c8cbe84648f Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 17:40:54 +0900 Subject: [PATCH 34/79] finalize --- .docker/jazzy.amd64.dockerfile | 2 +- .docker/jazzy.arm64v8.dockerfile | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.docker/jazzy.amd64.dockerfile b/.docker/jazzy.amd64.dockerfile index 1f068951..8841efd9 100644 --- a/.docker/jazzy.amd64.dockerfile +++ b/.docker/jazzy.amd64.dockerfile @@ -1,6 +1,6 @@ ARG ROS_DISTRO="jazzy" FROM osrf/ros:$ROS_DISTRO-desktop-full -ARG BRANCH="dockertest" +ARG BRANCH="ros2" # Install Utilities # hadolint ignore=DL3008 diff --git a/.docker/jazzy.arm64v8.dockerfile b/.docker/jazzy.arm64v8.dockerfile index 88d8cf30..47a7cdaf 100644 --- a/.docker/jazzy.arm64v8.dockerfile +++ b/.docker/jazzy.arm64v8.dockerfile @@ -82,7 +82,7 @@ FROM --platform=linux/arm64 woensugchoi/ubuntu-arm-rdp-base:latest ARG USER=docker # ROS-Gazebo arg -ARG BRANCH="dockertest" +ARG BRANCH="ros2" ARG ROS_DISTRO="jazzy" # Install ROS-Gazebo framework @@ -130,6 +130,7 @@ RUN touch /ros_entrypoint.sh && sed --in-place --expression \ USER docker RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ echo "source /opt/gazebo/install/setup.bash" >> ~/.bashrc && \ + echo "export PYTHONPATH=$PYTHONPATH:/opt/gazebo/install/lib/python" >> ~/.bashrc && \ echo "if [ -d ~/HOST ]; then chown docker:docker ~/HOST; fi" \ >> ~/.bashrc From 35eb686f81b321e48c2a78c0b637440474285662 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 7 Aug 2024 19:13:58 +0900 Subject: [PATCH 35/79] specify gz-physics version --- extras/ros-jazzy-binary-gz-harmonic-source-install.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh index ba89779b..be5d9f5e 100644 --- a/extras/ros-jazzy-binary-gz-harmonic-source-install.sh +++ b/extras/ros-jazzy-binary-gz-harmonic-source-install.sh @@ -111,6 +111,7 @@ echo -e "\033[34mBuilding the project with colcon...\033[0m" cd /opt/gazebo || exit # Build gz-physics with limited cores to avoid memory issues MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics7 +<<<<<<< HEAD ======= cd ~/gazebo || exit ======= @@ -119,6 +120,8 @@ cd /opt/gazebo || exit # Build gz-physics with limited cores to avoid memory issues MAKEFLAGS="-j2" colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-up-to gz-physics >>>>>>> 3179fd4 (source install test) +======= +>>>>>>> a4627f9 (specify gz-physics version) # Full build colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install From 61266fb0e0b1ea6db2abdd6b1de9467d2f84ed06 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Thu, 8 Aug 2024 08:59:21 +0900 Subject: [PATCH 36/79] testing self-hosted runner --- .github/workflows/docker-amd64.yml | 3 ++- .github/workflows/docker-arm64v8.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker-amd64.yml b/.github/workflows/docker-amd64.yml index e0f9423d..c3f1d3fc 100644 --- a/.github/workflows/docker-amd64.yml +++ b/.github/workflows/docker-amd64.yml @@ -16,7 +16,8 @@ env: jobs: build-and-push-to-docker-hub: - runs-on: ubuntu-latest + # runs-on: ubuntu-latest + runs-on: self-hosted permissions: contents: read packages: write diff --git a/.github/workflows/docker-arm64v8.yml b/.github/workflows/docker-arm64v8.yml index 5b39d6e5..78be0a99 100644 --- a/.github/workflows/docker-arm64v8.yml +++ b/.github/workflows/docker-arm64v8.yml @@ -16,7 +16,8 @@ env: jobs: build-and-push-to-docker-hub: - runs-on: ubuntu-latest + # runs-on: ubuntu-latest + runs-on: self-hosted permissions: contents: read packages: write From 5885978a1826f049cb63387ce2559c055bb1f779 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Thu, 8 Aug 2024 17:17:21 +0900 Subject: [PATCH 37/79] arm build takes longer than 6 hours --- .github/workflows/docker-arm64v8.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/docker-arm64v8.yml b/.github/workflows/docker-arm64v8.yml index 78be0a99..206a682b 100644 --- a/.github/workflows/docker-arm64v8.yml +++ b/.github/workflows/docker-arm64v8.yml @@ -18,6 +18,7 @@ jobs: build-and-push-to-docker-hub: # runs-on: ubuntu-latest runs-on: self-hosted + timeout-minutes: 1440 permissions: contents: read packages: write From 413782cea4faa4d4e707d3a67a669c5951e50caf Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 24 Jul 2024 11:45:13 +0530 Subject: [PATCH 38/79] added sub_sea_pressure_sensor_plugin --- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 13 +- .../sea_pressure_sensor.hh | 37 ++++ .../src/sea_pressure_sensor.cc | 182 ++++++++++++++++++ 3 files changed, 231 insertions(+), 1 deletion(-) create mode 100644 gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh create mode 100644 gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index b5a522e5..98877da8 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -23,9 +23,11 @@ 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}) @@ -33,6 +35,9 @@ target_link_libraries(UsblTransceiver target_link_libraries(UsblTransponder 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 @@ -48,8 +53,14 @@ ament_target_dependencies(UsblTransponder std_msgs ) +ament_target_dependencies(sea_pressure_sensor + dave_interfaces + rclcpp + std_msgs +) + # Install targets -install(TARGETS UsblTransceiver UsblTransponder +install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh new file mode 100644 index 00000000..7a56b7d1 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh @@ -0,0 +1,37 @@ +#ifndef dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ +#define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ + +// #include +#include +#include +#include +#include + +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 & _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); + +private: + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_gz_sensor_plugins + +#endif // __UUV_SUBSEA_PRESSURE_ROS_PLUGIN_HH__ \ No newline at end of file diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc new file mode 100644 index 00000000..f22d0116 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -0,0 +1,182 @@ +#include "dave_gz_sensor_plugins/Sea_pressure_sensor.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 +{ + std::string ns; + gz::sim::EntityComponentManager * ecm; + /// \brief The world +public: + gz::sim::World world{gz::sim::kNullEntity}; + + /// \brief The world name; +public: + std::string worldName; + double saturation; + +public: + std::chrono::steady_clock::duration lastMeasurementTime{0}; + bool estimateDepth; + double standardPressure; + double kPaPerM; + std::shared_ptr < rclcpp::Publisher rosSensorOutputPub; + std::shared_ptr gazeboNode; + std::shared_ptr rosNode; + std::shared_ptr < gz::transport::Publisher gazeboSensorOutputPub; + std::string robotNamespace; + bool gzMsgEnabled; + double noiseAmp; + double noiseSigma; + double lastMeasurementTime; +}; + +SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique()) +{ +} + +SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {} + +void SubseaPressureSensorPlugin::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & /*_eventManager*/) +{ + this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); + if (!this->dataPtr->world.Valid(_ecm)) + { + gzerr << "World entity not found" << std::endl; + return; + } + + if (_sdf->HasElement("saturation")) + { + this->dataPtr->saturation = _sdf->Get("saturation"); + } + else + { + this->dataPtr->saturation = 3000; + } + + if (_sdf->HasElement("estimate_depth_on")) + { + this->dataPtr->estimateDepth = _sdf->Get("estimate_depth_on"); + } + else + { + this->dataPtr->estimateDepth = false; + } + + if (_sdf->HasElement("standard_pressure")) + { + this->dataPtr->standardPressure = _sdf->Get("standard_pressure"); + } + else + { + this->dataPtr->standardPressure = 101.325; + } + + if (_sdf->HasElement("kPa_per_meter")) + { + this->dataPtr->kPaPerM = _sdf->Get("kPa_per_meter"); + } + else + { + this->dataPtr->kPaPerM = 9.80638; + } + + this->dataPtr->rosSensorOutputPub = + this->rosNode->create_publisher( + this->dataPtr->sensorOutputTopic, rclcpp::QoS(10).reliable()); + + if (this->dataPtr->gzMsgEnabled) + { + this->dataPtr->gazeboSensorOutputPub = this->gazeboNode->Advertise( + this->dataPtr->robotNamespace + "/" + this->dataPtr->sensorOutputTopic); + } +} + +void SubseaPressureSensorPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + this->dataPtr->PublishState(); + + if (!this->dataPtr->EnableMeasurement(_info)) + { + return; + } + + const gz::sim::components::WorldPose * pComp = + _ecm.Component(this->dataPtr->linkEntity); + gz::math::Vector3d pos; + if (pComp) + { + pos = pComp->Data().Pos(); + } + + double depth = std::abs(pos.Z()); + double pressure = this->dataPtr->standardPressure; + if (depth >= 0) + { + pressure += depth * this->dataPtr->kPaPerM; + } + + pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp); + + double inferredDepth = 0.0; + if (this->dataPtr->estimateDepth) + { + inferredDepth = (pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM; + } +} + +void SubseaPressureSensorPlugin::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + this->dataPtr->PublishState(); + + if (!this->dataPtr->EnableMeasurement(_info)) + { + return; + } + + if (this->dataPtr->gzMsgEnabled) + { + sensor_msgs::msgs::Pressure gazeboMsg; + gazeboMsg.set_pressure(pressure); + gazeboMsg.set_stddev(this->dataPtr->noiseSigma); + if (this->dataPtr->estimateDepth) + { + gazeboMsg.set_depth(inferredDepth); + } + this->dataPtr->gazeboSensorOutputPub->Publish(gazeboMsg); + } + + sensor_msgs::msg::FluidPressure rosMsg; + rosMsg.header.stamp.sec = _info.simTime.sec; + rosMsg.header.stamp.nanosec = _info.simTime.nsec; + this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); + rosMsg.fluid_pressure = pressure; + rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; + this->dataPtr->rosSensorOutputPub->publish(rosMsg); + this->dataPtr->lastMeasurementTime = _info.simTime; +} + +} // namespace dave_gz_sensor_plugins \ No newline at end of file From 65286f1641e260eeb7b5b036f6216dd5a65e8fe1 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 24 Jul 2024 17:28:12 +0530 Subject: [PATCH 39/79] added support for proto --- dave_interfaces/CMakeLists.txt | 30 ++++++++- .../msgs/SensorPressure.proto | 26 ++++++++ gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 9 +++ gazebo/dave_gz_sensor_plugins/package.xml | 2 + .../src/sea_pressure_sensor.cc | 62 +++++++++++++++---- 5 files changed, 117 insertions(+), 12 deletions(-) create mode 100644 dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index d32bb9b7..60ed1dc3 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -12,7 +12,14 @@ 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) +find_package(gz-msgs10 REQUIRED) # new to proto + + +# Define a variable 'GZ_MSGS_VER' holding the version number +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +# Define a variable 'MSGS_PROTOS' listing the .proto files +set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" @@ -76,4 +83,25 @@ ament_export_dependencies(rosidl_default_runtime) # Install CMake package configuration ament_export_include_directories(include) +# Example of custom messages that depend on gz.msgs +set(MSGS_PROTOS + ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) +gz_msgs_generate_messages( + # The cmake target to be generated for libraries/executables to link + TARGET pressure_sensor_msgs_gen + # The protobuf package to generate (Typically based on the path) + PROTO_PACKAGE "pressure_sensor_msgs.msgs" + # The path to the base directory of the proto files + # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) + MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + # List of proto files to generate + MSGS_PROTOS ${MSGS_PROTOS} + # List of message targets this library imports from + # DEPENDENCIES gz_msgs_gen + # Dependency on gz-msgs + DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}-msgs) + ament_package() diff --git a/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto b/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto new file mode 100644 index 00000000..6c2c7c05 --- /dev/null +++ b/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto @@ -0,0 +1,26 @@ +// Copyright (c) 2016 The UUV 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. + +syntax = "proto3"; +package pressure_sensor_msgs.msgs; + +message Pressure +{ + // required double pressure = 1 [default = 0.0]; + // required double stddev = 2 [default = -1.0]; + optional double pressure = 1 [default = 0.0]; + optional double stddev = 2 [default = -1.0]; + optional double depth = 3 [default = -1.0]; +} \ No newline at end of file diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index 98877da8..b9215523 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8) # Define the project name project(dave_gz_sensor_plugins) +# project(my_custom_package VERSION 0.0.1) # Find required packages find_package(ament_cmake REQUIRED) @@ -13,11 +14,17 @@ 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(gz-msgs10 REQUIRED) # new to proto # 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}) # new to proto + +# DEPENDENCIES(gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}) # new to proto + message(STATUS "Compiling against Gazebo Harmonic") @@ -42,6 +49,7 @@ 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 @@ -57,6 +65,7 @@ ament_target_dependencies(sea_pressure_sensor dave_interfaces rclcpp std_msgs + sensor_msgs ) # Install targets diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml index b1c657d6..aadf5439 100644 --- a/gazebo/dave_gz_sensor_plugins/package.xml +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -8,6 +8,8 @@ ament_cmake ament_lint_auto dave_interfaces + protobuf + protobuf dave_interfaces ament_cmake diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index f22d0116..4566024b 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -10,6 +11,14 @@ #include #include #include +#include +#include +#include +// #include "sensor_msgs.msgs" +#include "dave_interfaces/proto/SensorPressure.proto" +// #include +// #include +#include GZ_ADD_PLUGIN( dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System, @@ -38,19 +47,25 @@ struct SubseaPressureSensorPlugin::PrivateData bool estimateDepth; double standardPressure; double kPaPerM; - std::shared_ptr < rclcpp::Publisher rosSensorOutputPub; - std::shared_ptr gazeboNode; + std::shared_ptr> rosSensorOutputPub; + std::shared_ptr gazeboNode; std::shared_ptr rosNode; - std::shared_ptr < gz::transport::Publisher gazeboSensorOutputPub; + std::shared_ptr> + gazeboSensorOutputPub; // Advertise()) { + dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); + dataPtr->gazeboNode = std::make_shared(); + // dataPtr->sensorOutputTopic = "sensor_output_topic"; // 1 (check) + // dataPtr->gzMsgEnabled = true; + // dataPtr->noiseAmp = 0.1; + // dataPtr->noiseSigma = 0.01; } SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {} @@ -60,6 +75,12 @@ void SubseaPressureSensorPlugin::Configure( gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & /*_eventManager*/) { this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); + // this->dataPtr->sensorOutputTopic = "sensor_output_topic"; + + this->dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); // 2 (check) + this->dataPtr->gazeboNode = std::make_shared(); + // this->dataPtr->gazeboNode->Init(); + if (!this->dataPtr->world.Valid(_ecm)) { gzerr << "World entity not found" << std::endl; @@ -102,21 +123,30 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->kPaPerM = 9.80638; } + // this->dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); // 3 + // (check) this->dataPtr->rosSensorOutputPub = - this->rosNode->create_publisher( - this->dataPtr->sensorOutputTopic, rclcpp::QoS(10).reliable()); + this->dataPtr->rosNode->create_publisher( + "sensor_output_topic", rclcpp::QoS(10).reliable()); if (this->dataPtr->gzMsgEnabled) { - this->dataPtr->gazeboSensorOutputPub = this->gazeboNode->Advertise( - this->dataPtr->robotNamespace + "/" + this->dataPtr->sensorOutputTopic); + this->dataPtr->gazeboSensorOutputPub = + this->dataPtr->gazeboNode->create_publisher( + this->dataPtr->robotNamespace + "/" + "sensor_output_topic", 1); } + // { + // this->dataPtr->gazeboSensorOutputPub = + // this->gazeboNode->Advertise( + // this->dataPtr->robotNamespace + "/" + "sensor_output_topic"); + // } (check the issue is that the sensor output topic is not defined) hence for now the custom + // argument is passed. The same is done with the rosnode. } void SubseaPressureSensorPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - this->dataPtr->PublishState(); + // this->dataPtr->PublishState(); if (!this->dataPtr->EnableMeasurement(_info)) { @@ -150,7 +180,7 @@ void SubseaPressureSensorPlugin::PreUpdate( void SubseaPressureSensorPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - this->dataPtr->PublishState(); + // this->dataPtr->PublishState(); if (!this->dataPtr->EnableMeasurement(_info)) { @@ -159,7 +189,7 @@ void SubseaPressureSensorPlugin::PostUpdate( if (this->dataPtr->gzMsgEnabled) { - sensor_msgs::msgs::Pressure gazeboMsg; + pressure_sensor_msgs::msgs::Pressure gazeboMsg; gazeboMsg.set_pressure(pressure); gazeboMsg.set_stddev(this->dataPtr->noiseSigma); if (this->dataPtr->estimateDepth) @@ -177,6 +207,16 @@ void SubseaPressureSensorPlugin::PostUpdate( rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; this->dataPtr->rosSensorOutputPub->publish(rosMsg); this->dataPtr->lastMeasurementTime = _info.simTime; + + if (!_info.paused) + { + rclcpp::spin_some(this->ros_node_); + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_ros_gz_plugins::SubseaPressureSensorPlugin::PostUpdate" << std::endl; + } + } } } // namespace dave_gz_sensor_plugins \ No newline at end of file From 41fbeaeed63e4383317cf95f75e13f3193094b59 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 25 Jul 2024 13:38:51 +0530 Subject: [PATCH 40/79] updated cmake for review --- dave_interfaces/CMakeLists.txt | 67 +++++++++++++++---- .../msgs/SensorPressure.proto | 13 ++-- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 29 +++++++- .../src/sea_pressure_sensor.cc | 20 +++--- 4 files changed, 98 insertions(+), 31 deletions(-) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 60ed1dc3..5d2c252a 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -14,13 +14,6 @@ find_package(rosidl_default_generators REQUIRED) find_package(gz-cmake3 REQUIRED) find_package(gz-msgs10 REQUIRED) # new to proto - -# Define a variable 'GZ_MSGS_VER' holding the version number -set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) - -# Define a variable 'MSGS_PROTOS' listing the .proto files -set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) - rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" "msg/UsblResponse.msg" @@ -79,16 +72,62 @@ gz_msgs_generate_messages( DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} ) -ament_export_dependencies(rosidl_default_runtime) -# Install CMake package configuration -ament_export_include_directories(include) +# Define a variable 'GZ_MSGS_VER' holding the version number +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +# Define a variable 'MSGS_PROTOS' listing the .proto files +set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) + +# Define the GZ_DATA_INSTALL_DIR variable +# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos +install( + DIRECTORY gz + DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto + COMPONENT proto + FILES_MATCHING PATTERN "*.proto") + +# Example of custom messages that depend on gz.msgs +set(MSGS_PROTOS + ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) +gz_msgs_generate_messages( + # The cmake target to be generated for libraries/executables to link + TARGET msgs # pressure_sensor_msgs_gen + # The protobuf package to generate (Typically based on the path) + PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" + # The path to the base directory of the proto files + # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) + MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + # List of proto files to generate + MSGS_PROTOS ${MSGS_PROTOS} + # List of message targets this library imports from + # DEPENDENCIES gz_msgs_gen + # Dependency on gz-msgs + DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} +) + +# new to proto +################################################ + +# Define a variable 'GZ_MSGS_VER' holding the version number +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +# Define a variable 'MSGS_PROTOS' listing the .proto files +set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) + +# Define the GZ_DATA_INSTALL_DIR variable +# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos +install( + DIRECTORY gz + DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto + COMPONENT proto + FILES_MATCHING PATTERN "*.proto") # Example of custom messages that depend on gz.msgs set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) gz_msgs_generate_messages( # The cmake target to be generated for libraries/executables to link - TARGET pressure_sensor_msgs_gen + TARGET msgs # pressure_sensor_msgs_gen # The protobuf package to generate (Typically based on the path) PROTO_PACKAGE "pressure_sensor_msgs.msgs" # The path to the base directory of the proto files @@ -101,7 +140,11 @@ gz_msgs_generate_messages( # Dependency on gz-msgs DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} ) +# target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}-msgs) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}-msgs) +################################################ +ament_export_dependencies(rosidl_default_runtime) +# Install CMake package configuration +ament_export_include_directories(include) ament_package() diff --git a/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto b/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto index 6c2c7c05..718a72ff 100644 --- a/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto +++ b/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto @@ -16,11 +16,8 @@ syntax = "proto3"; package pressure_sensor_msgs.msgs; -message Pressure -{ - // required double pressure = 1 [default = 0.0]; - // required double stddev = 2 [default = -1.0]; - optional double pressure = 1 [default = 0.0]; - optional double stddev = 2 [default = -1.0]; - optional double depth = 3 [default = -1.0]; -} \ No newline at end of file +message Pressure { + double pressure = 1; + double stddev = 2; + double depth = 3; +} diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index b9215523..e55a377c 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -15,6 +15,7 @@ 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) # new to proto # Set version variables @@ -23,9 +24,6 @@ set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) # new to proto -# DEPENDENCIES(gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}) # new to proto - - message(STATUS "Compiling against Gazebo Harmonic") add_library(UsblTransceiver SHARED src/UsblTransceiver.cc) @@ -88,5 +86,30 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +################################ +# # Protobuf generation +# set(PROTO_FILES dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) +# protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${PROTO_FILES}) + +# add_library(pressure_sensor_msgs_gen SHARED ${PROTO_SRCS} ${PROTO_HDRS}) +# target_link_libraries(pressure_sensor_msgs_gen ${PROTOBUF_LIBRARIES}) + +# # Specify dependencies for your library +# ament_target_dependencies(pressure_sensor_msgs_gen +# rclcpp +# std_msgs +# geometry_msgs +# sensor_msgs +# dave_interfaces +# ) + +# include_directories(${PROTOBUF_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR}) + +# # Install targets +# install(TARGETS pressure_sensor_msgs_gen +# DESTINATION lib/${PROJECT_NAME} +# ) +################################ + # Configure ament ament_package() diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 4566024b..0afb9948 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -12,12 +12,13 @@ #include #include #include +#include // Add this line to include the header file #include #include // #include "sensor_msgs.msgs" -#include "dave_interfaces/proto/SensorPressure.proto" -// #include -// #include +#include "dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.pb.h" // (check) +// trusted #include #include +// #include GZ_ADD_PLUGIN( @@ -28,7 +29,6 @@ GZ_ADD_PLUGIN( namespace dave_gz_sensor_plugins { - struct SubseaPressureSensorPlugin::PrivateData { std::string ns; @@ -47,10 +47,14 @@ struct SubseaPressureSensorPlugin::PrivateData bool estimateDepth; double standardPressure; double kPaPerM; - std::shared_ptr> rosSensorOutputPub; - std::shared_ptr gazeboNode; - std::shared_ptr rosNode; - std::shared_ptr> + rclcpp::Publisher rosSensorOutputPub; + rclcpp::Node gazeboNode; + rclcpp::Node rosNode; + rclcpp::Publisher + // std::shared_ptr> rosSensorOutputPub; + // std::shared_ptr gazeboNode; + // std::shared_ptr rosNode; + // std::shared_ptr> gazeboSensorOutputPub; // Advertise Date: Thu, 25 Jul 2024 15:53:54 -0300 Subject: [PATCH 41/79] fix include issue --- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 24 +++++++++++-------- .../src/sea_pressure_sensor.cc | 13 ++++++++-- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index e55a377c..d468fc7f 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -1,8 +1,5 @@ cmake_minimum_required(VERSION 3.8) - -# Define the project name project(dave_gz_sensor_plugins) -# project(my_custom_package VERSION 0.0.1) # Find required packages find_package(ament_cmake REQUIRED) @@ -17,12 +14,15 @@ find_package(dave_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(Protobuf REQUIRED) find_package(gz-msgs10 REQUIRED) # new to proto +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}) # new to proto +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") @@ -32,17 +32,21 @@ 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_include_directories(sea_pressure_sensor PRIVATE include + ${dave_interfaces_INCLUDE_DIRS} +) 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}) - + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +) # Specify dependencies for FullSystem using ament_target_dependencies ament_target_dependencies(UsblTransceiver @@ -78,7 +82,8 @@ 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) @@ -111,5 +116,4 @@ endif() # ) ################################ -# Configure ament ament_package() diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 0afb9948..8d9cef2a 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -1,4 +1,4 @@ -#include "dave_gz_sensor_plugins/Sea_pressure_sensor.hh" +#include "dave_gz_sensor_plugins/sea_pressure_sensor.hh" #include #include #include @@ -13,10 +13,11 @@ #include #include #include // Add this line to include the header file +#include // Add this line to include the header file #include #include // #include "sensor_msgs.msgs" -#include "dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.pb.h" // (check) +#include // (check) // trusted #include #include // #include @@ -50,6 +51,14 @@ struct SubseaPressureSensorPlugin::PrivateData rclcpp::Publisher rosSensorOutputPub; rclcpp::Node gazeboNode; rclcpp::Node rosNode; + rclcpp::Publisher + // std::shared_ptr> rosSensorOutputPub; + // std::shared_ptr gazeboNode; + // std::shared_ptr rosNode; + // std::shared_ptr> + rclcpp::Publisher rosSensorOutputPub; + rclcpp::Node gazeboNode; + rclcpp::Node rosNode; rclcpp::Publisher // std::shared_ptr> rosSensorOutputPub; // std::shared_ptr gazeboNode; From 04fbb43e87f04dd9e15251baff8781bc7226142d Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 26 Jul 2024 12:56:49 +0530 Subject: [PATCH 42/79] removed duplicates --- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index d468fc7f..463b452b 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -13,15 +13,12 @@ find_package(geometry_msgs REQUIRED) find_package(dave_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(Protobuf REQUIRED) -find_package(gz-msgs10 REQUIRED) # new to proto -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}) # new to proto set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") From 339dae0e1b7c8033a04e37fc9575c45514e2d002 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 27 Jul 2024 16:28:55 +0530 Subject: [PATCH 43/79] Successfully compiling code --- .../sea_pressure_sensor.hh | 2 + .../src/sea_pressure_sensor.cc | 188 ++++++++++++------ 2 files changed, 131 insertions(+), 59 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh index 7a56b7d1..cabb6a73 100644 --- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh @@ -2,10 +2,12 @@ #define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ // #include +#include #include #include #include #include +#include namespace dave_gz_sensor_plugins { diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 8d9cef2a..81366f97 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -1,4 +1,6 @@ #include "dave_gz_sensor_plugins/sea_pressure_sensor.hh" +#include +#include #include #include #include @@ -12,15 +14,9 @@ #include #include #include -#include // Add this line to include the header file -#include // Add this line to include the header file +#include #include #include -// #include "sensor_msgs.msgs" -#include // (check) -// trusted #include #include -// -#include GZ_ADD_PLUGIN( dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System, @@ -48,34 +44,23 @@ struct SubseaPressureSensorPlugin::PrivateData bool estimateDepth; double standardPressure; double kPaPerM; - rclcpp::Publisher rosSensorOutputPub; - rclcpp::Node gazeboNode; - rclcpp::Node rosNode; - rclcpp::Publisher - // std::shared_ptr> rosSensorOutputPub; - // std::shared_ptr gazeboNode; - // std::shared_ptr rosNode; - // std::shared_ptr> - rclcpp::Publisher rosSensorOutputPub; - rclcpp::Node gazeboNode; - rclcpp::Node rosNode; - rclcpp::Publisher - // std::shared_ptr> rosSensorOutputPub; - // std::shared_ptr gazeboNode; - // std::shared_ptr rosNode; - // std::shared_ptr> - gazeboSensorOutputPub; // Advertise gazeboNode; + // gz::transport::Node::Publisher gazeboSensorOutputPub; + gz::transport::Node::Publisher gazeboSensorOutputPub; + rclcpp::Publisher::SharedPtr rosSensorOutputPub; + std::shared_ptr rosNode; std::string robotNamespace; bool gzMsgEnabled; double noiseAmp; double noiseSigma; + double inferredDepth; + double pressure; + // auto pos; }; SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique()) { - dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); - dataPtr->gazeboNode = std::make_shared(); - // dataPtr->sensorOutputTopic = "sensor_output_topic"; // 1 (check) + dataPtr->inferredDepth = 0.0; // dataPtr->gzMsgEnabled = true; // dataPtr->noiseAmp = 0.1; // dataPtr->noiseSigma = 0.01; @@ -136,8 +121,10 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->kPaPerM = 9.80638; } - // this->dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); // 3 - // (check) + // Initialize the Gazebo node + this->dataPtr->gazeboNode = std::make_shared(); + // this->dataPtr->gazeboNode->Init(); + this->dataPtr->rosSensorOutputPub = this->dataPtr->rosNode->create_publisher( "sensor_output_topic", rclcpp::QoS(10).reliable()); @@ -145,8 +132,8 @@ void SubseaPressureSensorPlugin::Configure( if (this->dataPtr->gzMsgEnabled) { this->dataPtr->gazeboSensorOutputPub = - this->dataPtr->gazeboNode->create_publisher( - this->dataPtr->robotNamespace + "/" + "sensor_output_topic", 1); + this->dataPtr->gazeboNode->Advertise( + this->dataPtr->robotNamespace + "/" + "sensor_output_topic"); } // { // this->dataPtr->gazeboSensorOutputPub = @@ -155,38 +142,80 @@ void SubseaPressureSensorPlugin::Configure( // } (check the issue is that the sensor output topic is not defined) hence for now the custom // argument is passed. The same is done with the rosnode. } +////////////////////////////////////////// -void SubseaPressureSensorPlugin::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +gz::math::Pose3d GetWorldPose( + const gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm) { - // this->dataPtr->PublishState(); - - if (!this->dataPtr->EnableMeasurement(_info)) + // Ensure the WorldPose component exists + if (!_ecm.Component(_entity)) { - return; + _ecm.CreateComponent(_entity, gz::sim::components::WorldPose()); } - const gz::sim::components::WorldPose * pComp = - _ecm.Component(this->dataPtr->linkEntity); - gz::math::Vector3d pos; - if (pComp) + // Get the WorldPose component + const auto * worldPoseComp = _ecm.Component(_entity); + if (worldPoseComp) + { + return worldPoseComp->Data(); + } + else { - pos = pComp->Data().Pos(); + gzerr << "WorldPose component not found for entity: " << _entity << std::endl; + return gz::math::Pose3d::Zero; } +} + +void SubseaPressureSensorPlugin::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + // this->dataPtr->PublishState(); + + // if (!this->dataPtr->EnableMeasurement(_info)) + // { + // return; + // } + + // const gz::sim::components::WorldPose * pComp = + // _ecm.Component(_entity); // check + // gz::math::Vector3d pos; + // if (pComp) + // { + // pos = pComp->Data().Pos(); + // } + const gz::sim::Entity & _entity = _ecm.EntityByComponents(gz::sim::components::WorldPose()); + // gz::math::Vector3 pos; + auto pos = GetWorldPose(_entity, _ecm); + + // gz::math::Pose3d pose = GetWorldPose(_entity, _ecm); + // gz::math::Vector3d pos = pose.Pos(); + //////////// + // const gz::sim::components::WorldPose * worldPoseComp = + // _ecm.Component(this->dataPtr->entity); + + // if (worldPoseComp) + // { + // gz::math::Pose3d worldPose = worldPoseComp->Data(); + // // Use 'worldPose' as needed + // } + //////////// double depth = std::abs(pos.Z()); - double pressure = this->dataPtr->standardPressure; + this->dataPtr->pressure = this->dataPtr->standardPressure; if (depth >= 0) { - pressure += depth * this->dataPtr->kPaPerM; + this->dataPtr->pressure += depth * this->dataPtr->kPaPerM; } - pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp); + // not adding gaussian noise for now + // pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp); + this->dataPtr->pressure += this->dataPtr->noiseAmp; - double inferredDepth = 0.0; + // double inferredDepth = 0.0; if (this->dataPtr->estimateDepth) { - inferredDepth = (pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM; + this->dataPtr->inferredDepth = + (this->dataPtr->pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM; } } @@ -195,35 +224,38 @@ void SubseaPressureSensorPlugin::PostUpdate( { // this->dataPtr->PublishState(); - if (!this->dataPtr->EnableMeasurement(_info)) - { - return; - } + // if (!this->dataPtr->EnableMeasurement(_info)) + // { + // return; + // } if (this->dataPtr->gzMsgEnabled) { pressure_sensor_msgs::msgs::Pressure gazeboMsg; - gazeboMsg.set_pressure(pressure); + gazeboMsg.set_pressure(this->dataPtr->pressure); gazeboMsg.set_stddev(this->dataPtr->noiseSigma); if (this->dataPtr->estimateDepth) { - gazeboMsg.set_depth(inferredDepth); + gazeboMsg.set_depth(this->dataPtr->inferredDepth); } - this->dataPtr->gazeboSensorOutputPub->Publish(gazeboMsg); + this->dataPtr->gazeboSensorOutputPub.Publish(gazeboMsg); } sensor_msgs::msg::FluidPressure rosMsg; - rosMsg.header.stamp.sec = _info.simTime.sec; - rosMsg.header.stamp.nanosec = _info.simTime.nsec; + // rosMsg.header.stamp.sec = _info.simTime.sec; + // rosMsg.header.stamp.nanosec = _info.simTime.nsec; + rosMsg.header.stamp.sec = std::chrono::duration_cast(_info.simTime).count(); + rosMsg.header.stamp.nanosec = + std::chrono::duration_cast(_info.simTime).count() % 1000000000; this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); - rosMsg.fluid_pressure = pressure; + rosMsg.fluid_pressure = this->dataPtr->pressure; rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; this->dataPtr->rosSensorOutputPub->publish(rosMsg); this->dataPtr->lastMeasurementTime = _info.simTime; if (!_info.paused) { - rclcpp::spin_some(this->ros_node_); + rclcpp::spin_some(this->dataPtr->rosNode); if (_info.iterations % 1000 == 0) { @@ -232,4 +264,42 @@ void SubseaPressureSensorPlugin::PostUpdate( } } -} // namespace dave_gz_sensor_plugins \ No newline at end of file +} // namespace dave_gz_sensor_plugins + +///////////////////////////////////////////////// +// bool SubseaPressureSensorPlugin::EnableMeasurement(const gz::sim::UpdateInfo & _info) const +// { +// common::Time current_time = _info.simTime; +// double dt = (current_time - this->lastMeasurementTime).Double(); +// return dt >= 1.0 / this->updateRate && this->isReferenceInit && this->isOn.data; +// } + +///////////////////////////////////////////////// +// this->noiseModels["default"]: A map that likely holds different noise models, with “default” +// being a standard Gaussian distribution function. + +// double ROSBasePlugin::GetGaussianNoise(double _amp) +// { +// return _amp * this->noiseModels["default"](this->rndGen); +// } + +///////////////////////////////////////////////// +// bool SubseaPressureSensorPlugin::GetSDFParam( +// const std::shared_ptr & _sdf, const std::string & name, T & param, +// const T & default_value, const bool & verbose = false) +// { +// if (sdf->HasElement(name)) +// { +// param = sdf->GetElement(name)->Get(); +// return true; +// } +// else +// { +// param = default_value; +// if (verbose) +// { +// gzerr << "[uuv_sensor_plugins] Please specify a value for parameter \"" << name << "\".\n"; +// } +// } +// return false; +// } \ No newline at end of file From f58fa120d316b730e735231a8cd378dd72a1143a Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 4 Aug 2024 23:07:50 +0530 Subject: [PATCH 44/79] Pre-Final fixes --- .../src/sea_pressure_sensor.cc | 32 +++++++++---------- .../dave_sea_pressor_sensor_tutorial.world | 0 2 files changed, 16 insertions(+), 16 deletions(-) create mode 100644 models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 81366f97..00444b98 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -28,18 +28,16 @@ namespace dave_gz_sensor_plugins { struct SubseaPressureSensorPlugin::PrivateData { +public: std::string ns; gz::sim::EntityComponentManager * ecm; /// \brief The world -public: gz::sim::World world{gz::sim::kNullEntity}; /// \brief The world name; -public: std::string worldName; double saturation; -public: std::chrono::steady_clock::duration lastMeasurementTime{0}; bool estimateDepth; double standardPressure; @@ -61,9 +59,9 @@ struct SubseaPressureSensorPlugin::PrivateData SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique()) { dataPtr->inferredDepth = 0.0; - // dataPtr->gzMsgEnabled = true; - // dataPtr->noiseAmp = 0.1; - // dataPtr->noiseSigma = 0.01; + dataPtr->gzMsgEnabled = true; // consider changing to false + dataPtr->noiseAmp = 0.0; + dataPtr->noiseSigma = 3.0; } SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {} @@ -75,9 +73,11 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); // this->dataPtr->sensorOutputTopic = "sensor_output_topic"; + // Initialize the ROS 2 node this->dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); // 2 (check) + + // Initialize the Gazebo node this->dataPtr->gazeboNode = std::make_shared(); - // this->dataPtr->gazeboNode->Init(); if (!this->dataPtr->world.Valid(_ecm)) { @@ -91,7 +91,7 @@ void SubseaPressureSensorPlugin::Configure( } else { - this->dataPtr->saturation = 3000; + this->dataPtr->saturation = 3000; // original xacro file has it as range set at 30000 } if (_sdf->HasElement("estimate_depth_on")) @@ -121,8 +121,6 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->kPaPerM = 9.80638; } - // Initialize the Gazebo node - this->dataPtr->gazeboNode = std::make_shared(); // this->dataPtr->gazeboNode->Init(); this->dataPtr->rosSensorOutputPub = @@ -185,7 +183,7 @@ void SubseaPressureSensorPlugin::PreUpdate( // } const gz::sim::Entity & _entity = _ecm.EntityByComponents(gz::sim::components::WorldPose()); // gz::math::Vector3 pos; - auto pos = GetWorldPose(_entity, _ecm); + auto pos = GetWorldPose(_entity, _ecm); // check if the var name would be worldPoseComp // gz::math::Pose3d pose = GetWorldPose(_entity, _ecm); // gz::math::Vector3d pos = pose.Pos(); @@ -199,7 +197,7 @@ void SubseaPressureSensorPlugin::PreUpdate( // // Use 'worldPose' as needed // } //////////// - + // Depth data is extracted from the model and pressure is calculated double depth = std::abs(pos.Z()); this->dataPtr->pressure = this->dataPtr->standardPressure; if (depth >= 0) @@ -209,10 +207,10 @@ void SubseaPressureSensorPlugin::PreUpdate( // not adding gaussian noise for now // pressure += this->dataPtr->GetGaussianNoise(this->dataPtr->noiseAmp); - this->dataPtr->pressure += this->dataPtr->noiseAmp; + this->dataPtr->pressure += this->dataPtr->noiseAmp; // noiseAmp is 0.0 // double inferredDepth = 0.0; - if (this->dataPtr->estimateDepth) + if (this->dataPtr->estimateDepth) // estimateDepth is false by default { this->dataPtr->inferredDepth = (this->dataPtr->pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM; @@ -244,9 +242,11 @@ void SubseaPressureSensorPlugin::PostUpdate( sensor_msgs::msg::FluidPressure rosMsg; // rosMsg.header.stamp.sec = _info.simTime.sec; // rosMsg.header.stamp.nanosec = _info.simTime.nsec; - rosMsg.header.stamp.sec = std::chrono::duration_cast(_info.simTime).count(); + rosMsg.header.stamp.sec = + std::chrono::duration_cast(_info.simTime).count(); // Time in seconds rosMsg.header.stamp.nanosec = - std::chrono::duration_cast(_info.simTime).count() % 1000000000; + std::chrono::duration_cast(_info.simTime).count() % + 1000000000; // Time in nanoseconds this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); rosMsg.fluid_pressure = this->dataPtr->pressure; rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; diff --git a/models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world b/models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world new file mode 100644 index 00000000..e69de29b From e6615d13642bfbe1c9f2543cdbda16bfbb7b70fb Mon Sep 17 00:00:00 2001 From: hmoyen Date: Thu, 25 Jul 2024 17:56:03 -0300 Subject: [PATCH 45/79] fix material issues --- .../dave_worlds/worlds/dave_Santorini.world | 14 +- .../worlds/dave_bimanual_example.world | 14 +- .../worlds/dave_electrical_mating.world | 18 +- .../worlds/dave_graded_seabed.world | 12 +- .../dave_worlds/worlds/dave_integrated.world | 16 +- .../worlds/dave_ocean_models.world | 12 +- .../dave_worlds/worlds/dave_ocean_waves.world | 12 +- .../dave_ocean_waves_mossy_ground.world | 13 +- .../dave_ocean_waves_transient_current.world | 11 +- .../worlds/dave_plug_and_socket.world | 16 +- models/dave_worlds/worlds/new_dvl.world | 414 ++++++++++++++++++ 11 files changed, 490 insertions(+), 62 deletions(-) create mode 100644 models/dave_worlds/worlds/new_dvl.world diff --git a/models/dave_worlds/worlds/dave_Santorini.world b/models/dave_worlds/worlds/dave_Santorini.world index 865d5001..7d172e8a 100644 --- a/models/dave_worlds/worlds/dave_Santorini.world +++ b/models/dave_worlds/worlds/dave_Santorini.world @@ -68,13 +68,13 @@ false - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_bimanual_example.world b/models/dave_worlds/worlds/dave_bimanual_example.world index 461200b9..1b2f1e50 100644 --- a/models/dave_worlds/worlds/dave_bimanual_example.world +++ b/models/dave_worlds/worlds/dave_bimanual_example.world @@ -66,7 +66,7 @@ - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame 0 0 0 0 0 0 @@ -113,12 +113,12 @@ 3.0 - - - + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + 0.1 0.1 0.1 1.0 + 0.0 0.0 0.0 1.0 + diff --git a/models/dave_worlds/worlds/dave_electrical_mating.world b/models/dave_worlds/worlds/dave_electrical_mating.world index efa0d543..18434f24 100644 --- a/models/dave_worlds/worlds/dave_electrical_mating.world +++ b/models/dave_worlds/worlds/dave_electrical_mating.world @@ -58,13 +58,13 @@ false - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + @@ -73,11 +73,11 @@ - + true diff --git a/models/dave_worlds/worlds/dave_graded_seabed.world b/models/dave_worlds/worlds/dave_graded_seabed.world index b6a4f90f..1b4690a8 100644 --- a/models/dave_worlds/worlds/dave_graded_seabed.world +++ b/models/dave_worlds/worlds/dave_graded_seabed.world @@ -81,11 +81,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_integrated.world b/models/dave_worlds/worlds/dave_integrated.world index bff398fa..dde9496d 100644 --- a/models/dave_worlds/worlds/dave_integrated.world +++ b/models/dave_worlds/worlds/dave_integrated.world @@ -77,11 +77,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + @@ -107,11 +109,11 @@ -191.06 115.8 -155.0 0 -1.570777 0 - + true diff --git a/models/dave_worlds/worlds/dave_ocean_models.world b/models/dave_worlds/worlds/dave_ocean_models.world index 28a4cbb4..c1c293e0 100644 --- a/models/dave_worlds/worlds/dave_ocean_models.world +++ b/models/dave_worlds/worlds/dave_ocean_models.world @@ -83,11 +83,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_ocean_waves.world b/models/dave_worlds/worlds/dave_ocean_waves.world index ca0de8dd..920c9f59 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves.world +++ b/models/dave_worlds/worlds/dave_ocean_waves.world @@ -83,11 +83,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world index 1bb9ca82..88beb352 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world +++ b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world @@ -83,12 +83,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world index f69dcd30..1fd5c19f 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world +++ b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world @@ -67,10 +67,13 @@ false - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + diff --git a/models/dave_worlds/worlds/dave_plug_and_socket.world b/models/dave_worlds/worlds/dave_plug_and_socket.world index a48b6065..bdb3b1f5 100644 --- a/models/dave_worlds/worlds/dave_plug_and_socket.world +++ b/models/dave_worlds/worlds/dave_plug_and_socket.world @@ -58,11 +58,13 @@ false - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - 0 0 0 0 0 0 - + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + @@ -72,11 +74,11 @@ 0 0 0 0 0 0 - + true diff --git a/models/dave_worlds/worlds/new_dvl.world b/models/dave_worlds/worlds/new_dvl.world new file mode 100644 index 00000000..f231fac1 --- /dev/null +++ b/models/dave_worlds/worlds/new_dvl.world @@ -0,0 +1,414 @@ + + + + + + + + 0.0 1.0 1.0 + + 0.0 0.7 0.8 + + false + + + + 0.001 + 1.0 + + + + + + + + + + + + + + + + + 56.71897669633431 + 3.515625 + + + + + + + 50 0 150 0 0 0 + 1 1 1 1 + 0.1 0.1 0.1 1 + 0.3 0.3 -1 + false + + + + 20 0 -90 0 0 0 + 0.9 0.9 0.9 1 + 0.2 0.2 0.2 1 + -1 0 0 + false + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame + + 0 0 0 0 0 0 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Waves + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap + 0 0 -100 0 0 0 + + + + true + 21 -1 -98.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 0.03 + 3.0 + + + + + + + 0.03 + 3.0 + + + + + + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase with Inertia + 20 0 -98 0 0 0 + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase Distorted + 22 -0.5 -100 0 0 1.5708 + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 18.7 3.77 -97.5 0 0.29 -0.715 + + 0.25 + 2500 + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + + true + false + 4000000 + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + + 1025 + + 0 + 1 + + + + + + From 8e30757f759074c2a18f0f8cbaac473d2c88ce15 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 6 Aug 2024 22:47:46 +0530 Subject: [PATCH 46/79] Final code for Sea_Pressure_sensor_plugin --- .../sea_pressure_sensor.hh | 11 ++ .../src/sea_pressure_sensor.cc | 138 ++++++++---------- .../description/rexrov/model.sdf | 21 ++- 3 files changed, 92 insertions(+), 78 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh index cabb6a73..86b61d0b 100644 --- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh @@ -2,6 +2,7 @@ #define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ // #include +#include #include #include #include @@ -30,7 +31,17 @@ public: 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 rosNode; struct PrivateData; std::unique_ptr dataPtr; }; diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 00444b98..79b0c9b7 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -5,11 +5,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -17,6 +19,7 @@ #include #include #include +#include "gz/sim/components/Model.hh" GZ_ADD_PLUGIN( dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System, @@ -29,61 +32,62 @@ namespace dave_gz_sensor_plugins struct SubseaPressureSensorPlugin::PrivateData { public: - std::string ns; - gz::sim::EntityComponentManager * ecm; /// \brief The world gz::sim::World world{gz::sim::kNullEntity}; - /// \brief The world name; std::string worldName; double saturation; - + gz::sim::EntityComponentManager * ecm = nullptr; std::chrono::steady_clock::duration lastMeasurementTime{0}; - bool estimateDepth; - double standardPressure; - double kPaPerM; + bool estimateDepth = false; + double standardPressure = 101.325; + double kPaPerM = 9.80638; std::shared_ptr gazeboNode; - // gz::transport::Node::Publisher gazeboSensorOutputPub; gz::transport::Node::Publisher gazeboSensorOutputPub; rclcpp::Publisher::SharedPtr rosSensorOutputPub; std::shared_ptr rosNode; std::string robotNamespace; - bool gzMsgEnabled; - double noiseAmp; - double noiseSigma; - double inferredDepth; - double pressure; - // auto pos; + bool gzMsgEnabled = false; + double noiseAmp = 0.0; + double noiseSigma = 3.0; + double inferredDepth = 0.0; + double pressure = 0.0; + std::string modelName; + gz::sim::Model model; + gz::sim::Entity modelEntity; }; SubseaPressureSensorPlugin::SubseaPressureSensorPlugin() : dataPtr(std::make_unique()) { - dataPtr->inferredDepth = 0.0; - dataPtr->gzMsgEnabled = true; // consider changing to false - dataPtr->noiseAmp = 0.0; - dataPtr->noiseSigma = 3.0; } SubseaPressureSensorPlugin::~SubseaPressureSensorPlugin() {} void SubseaPressureSensorPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & /*_eventManager*/) + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); - // this->dataPtr->sensorOutputTopic = "sensor_output_topic"; + 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->dataPtr->rosNode = std::make_shared("subsea_pressure_sensor"); // 2 (check) + this->rosNode = std::make_shared("subsea_pressure_sensor"); // Initialize the Gazebo node - this->dataPtr->gazeboNode = std::make_shared(); + this->dataPtr->gazeboNode = std::make_shared(); // check idts it's needed - if (!this->dataPtr->world.Valid(_ecm)) + if (!_sdf->HasElement("namespace")) { - gzerr << "World entity not found" << std::endl; + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; return; } + this->dataPtr->robotNamespace = _sdf->Get("namespace"); if (_sdf->HasElement("saturation")) { @@ -122,9 +126,10 @@ void SubseaPressureSensorPlugin::Configure( } // this->dataPtr->gazeboNode->Init(); + this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->robotNamespace, _ecm); // check TODO this->dataPtr->rosSensorOutputPub = - this->dataPtr->rosNode->create_publisher( + this->rosNode->create_publisher( "sensor_output_topic", rclcpp::QoS(10).reliable()); if (this->dataPtr->gzMsgEnabled) @@ -133,33 +138,39 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->gazeboNode->Advertise( this->dataPtr->robotNamespace + "/" + "sensor_output_topic"); } - // { - // this->dataPtr->gazeboSensorOutputPub = - // this->gazeboNode->Advertise( - // this->dataPtr->robotNamespace + "/" + "sensor_output_topic"); - // } (check the issue is that the sensor output topic is not defined) hence for now the custom - // argument is passed. The same is done with the rosnode. } ////////////////////////////////////////// -gz::math::Pose3d GetWorldPose( - const gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm) +gz::sim::Entity SubseaPressureSensorPlugin::GetModelEntity( + const std::string & modelName, gz::sim::EntityComponentManager & ecm) { - // Ensure the WorldPose component exists - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, gz::sim::components::WorldPose()); - } + gz::sim::Entity modelEntity = gz::sim::kNullEntity; + + ecm.Each( + [&](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; +} - // Get the WorldPose component - const auto * worldPoseComp = _ecm.Component(_entity); - if (worldPoseComp) +gz::math::Pose3d SubseaPressureSensorPlugin::GetModelPose( + const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm) +{ + const auto * poseComp = ecm.Component(modelEntity); + if (poseComp) { - return worldPoseComp->Data(); + return poseComp->Data(); } else { - gzerr << "WorldPose component not found for entity: " << _entity << std::endl; + gzerr << "Pose component not found for entity: " << modelEntity << std::endl; return gz::math::Pose3d::Zero; } } @@ -167,38 +178,15 @@ gz::math::Pose3d GetWorldPose( void SubseaPressureSensorPlugin::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - // this->dataPtr->PublishState(); + // Get model pose - // if (!this->dataPtr->EnableMeasurement(_info)) - // { - // return; - // } + gz::math::Pose3d sea_pressure_sensor_pos = GetModelPose(this->dataPtr->modelEntity, _ecm); - // const gz::sim::components::WorldPose * pComp = - // _ecm.Component(_entity); // check - // gz::math::Vector3d pos; - // if (pComp) - // { - // pos = pComp->Data().Pos(); - // } - const gz::sim::Entity & _entity = _ecm.EntityByComponents(gz::sim::components::WorldPose()); + // const gz::sim::Entity & _entity = _ecm.EntityByComponents(gz::sim::components::WorldPose()); // gz::math::Vector3 pos; - auto pos = GetWorldPose(_entity, _ecm); // check if the var name would be worldPoseComp - - // gz::math::Pose3d pose = GetWorldPose(_entity, _ecm); - // gz::math::Vector3d pos = pose.Pos(); - //////////// - // const gz::sim::components::WorldPose * worldPoseComp = - // _ecm.Component(this->dataPtr->entity); + // auto pos = GetWorldPose(_entity, _ecm); - // if (worldPoseComp) - // { - // gz::math::Pose3d worldPose = worldPoseComp->Data(); - // // Use 'worldPose' as needed - // } - //////////// - // Depth data is extracted from the model and pressure is calculated - double depth = std::abs(pos.Z()); + double depth = std::abs(sea_pressure_sensor_pos.Z()); this->dataPtr->pressure = this->dataPtr->standardPressure; if (depth >= 0) { @@ -240,14 +228,12 @@ void SubseaPressureSensorPlugin::PostUpdate( } sensor_msgs::msg::FluidPressure rosMsg; - // rosMsg.header.stamp.sec = _info.simTime.sec; - // rosMsg.header.stamp.nanosec = _info.simTime.nsec; rosMsg.header.stamp.sec = std::chrono::duration_cast(_info.simTime).count(); // Time in seconds rosMsg.header.stamp.nanosec = std::chrono::duration_cast(_info.simTime).count() % 1000000000; // Time in nanoseconds - this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); + // this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); rosMsg.fluid_pressure = this->dataPtr->pressure; rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; this->dataPtr->rosSensorOutputPub->publish(rosMsg); @@ -255,7 +241,7 @@ void SubseaPressureSensorPlugin::PostUpdate( if (!_info.paused) { - rclcpp::spin_some(this->dataPtr->rosNode); + rclcpp::spin_some(this->rosNode); if (_info.iterations % 1000 == 0) { diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 0b9bb15b..f5b91146 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -1,4 +1,4 @@ - + @@ -584,5 +584,22 @@ thruster8_joint + + + rexrov + + sea_pressure + + 10 + + 3.0 + 0.0 + false + 101.325 + 9.80638 + + - + \ No newline at end of file From 922e4945afe408b46e1fbf5996dd2413e5c42fd3 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 6 Aug 2024 23:40:00 +0530 Subject: [PATCH 47/79] minor Update --- gazebo/dave_gz_sensor_plugins/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml index aadf5439..70c3aeb7 100644 --- a/gazebo/dave_gz_sensor_plugins/package.xml +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -3,6 +3,7 @@ dave_gz_sensor_plugins 0.0.0 DAVE sensor plugins + Gaurav kumar Helena Moyen Apache 2.0 ament_cmake From c3e4d3381de55e5b9913e115f26fac1faca5fae3 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 8 Aug 2024 19:55:34 +0530 Subject: [PATCH 48/79] updated code with reccomended changes --- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 31 +--- .../sea_pressure_sensor.hh | 1 - gazebo/dave_gz_sensor_plugins/package.xml | 1 + .../src/sea_pressure_sensor.cc | 140 ++++++------------ .../description/rexrov/model.sdf | 5 +- 5 files changed, 52 insertions(+), 126 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index 463b452b..3fbb5cbc 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -29,9 +29,7 @@ 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 - ${dave_interfaces_INCLUDE_DIRS} -) +target_include_directories(sea_pressure_sensor PRIVATE include) target_link_libraries(UsblTransceiver gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} @@ -61,10 +59,10 @@ ament_target_dependencies(UsblTransponder ) ament_target_dependencies(sea_pressure_sensor - dave_interfaces rclcpp std_msgs sensor_msgs + geometry_msgs ) # Install targets @@ -88,29 +86,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -################################ -# # Protobuf generation -# set(PROTO_FILES dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) -# protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${PROTO_FILES}) - -# add_library(pressure_sensor_msgs_gen SHARED ${PROTO_SRCS} ${PROTO_HDRS}) -# target_link_libraries(pressure_sensor_msgs_gen ${PROTOBUF_LIBRARIES}) - -# # Specify dependencies for your library -# ament_target_dependencies(pressure_sensor_msgs_gen -# rclcpp -# std_msgs -# geometry_msgs -# sensor_msgs -# dave_interfaces -# ) - -# include_directories(${PROTOBUF_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR}) - -# # Install targets -# install(TARGETS pressure_sensor_msgs_gen -# DESTINATION lib/${PROJECT_NAME} -# ) -################################ - ament_package() diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh index 86b61d0b..132a46d2 100644 --- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh @@ -3,7 +3,6 @@ // #include #include -#include #include #include #include diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml index 70c3aeb7..f8e6da97 100644 --- a/gazebo/dave_gz_sensor_plugins/package.xml +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -6,6 +6,7 @@ Gaurav kumar Helena Moyen Apache 2.0 + geometry_msgs ament_cmake ament_lint_auto dave_interfaces diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index 79b0c9b7..eccb40d5 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -1,11 +1,11 @@ #include "dave_gz_sensor_plugins/sea_pressure_sensor.hh" -#include +#include #include +#include #include #include #include #include -#include #include #include #include @@ -19,7 +19,6 @@ #include #include #include -#include "gz/sim/components/Model.hh" GZ_ADD_PLUGIN( dave_gz_sensor_plugins::SubseaPressureSensorPlugin, gz::sim::System, @@ -32,28 +31,23 @@ namespace dave_gz_sensor_plugins struct SubseaPressureSensorPlugin::PrivateData { public: - /// \brief The world - gz::sim::World world{gz::sim::kNullEntity}; - /// \brief The world name; - std::string worldName; double saturation; gz::sim::EntityComponentManager * ecm = nullptr; std::chrono::steady_clock::duration lastMeasurementTime{0}; - bool estimateDepth = false; + bool estimateDepth; double standardPressure = 101.325; double kPaPerM = 9.80638; std::shared_ptr gazeboNode; - gz::transport::Node::Publisher gazeboSensorOutputPub; - rclcpp::Publisher::SharedPtr rosSensorOutputPub; + gz::transport::Node::Publisher gz_pressure_sensor_pub; + rclcpp::Publisher::SharedPtr ros_pressure_sensor_pub; + rclcpp::Publisher::SharedPtr ros_depth_estimate_pub; std::shared_ptr rosNode; std::string robotNamespace; - bool gzMsgEnabled = false; double noiseAmp = 0.0; double noiseSigma = 3.0; double inferredDepth = 0.0; double pressure = 0.0; std::string modelName; - gz::sim::Model model; gz::sim::Entity modelEntity; }; @@ -79,7 +73,7 @@ void SubseaPressureSensorPlugin::Configure( this->rosNode = std::make_shared("subsea_pressure_sensor"); // Initialize the Gazebo node - this->dataPtr->gazeboNode = std::make_shared(); // check idts it's needed + this->dataPtr->gazeboNode = std::make_shared(); if (!_sdf->HasElement("namespace")) { @@ -95,7 +89,7 @@ void SubseaPressureSensorPlugin::Configure( } else { - this->dataPtr->saturation = 3000; // original xacro file has it as range set at 30000 + this->dataPtr->saturation = 3000; } if (_sdf->HasElement("estimate_depth_on")) @@ -104,7 +98,7 @@ void SubseaPressureSensorPlugin::Configure( } else { - this->dataPtr->estimateDepth = false; + this->dataPtr->estimateDepth = true; } if (_sdf->HasElement("standard_pressure")) @@ -126,18 +120,22 @@ void SubseaPressureSensorPlugin::Configure( } // this->dataPtr->gazeboNode->Init(); - this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->robotNamespace, _ecm); // check TODO + this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->robotNamespace, _ecm); - this->dataPtr->rosSensorOutputPub = + this->dataPtr->ros_pressure_sensor_pub = this->rosNode->create_publisher( - "sensor_output_topic", rclcpp::QoS(10).reliable()); + this->dataPtr->robotNamespace + "/" + "Pressure", rclcpp::QoS(10).reliable()); - if (this->dataPtr->gzMsgEnabled) + if (this->dataPtr->estimateDepth) { - this->dataPtr->gazeboSensorOutputPub = - this->dataPtr->gazeboNode->Advertise( - this->dataPtr->robotNamespace + "/" + "sensor_output_topic"); + this->dataPtr->ros_depth_estimate_pub = + this->rosNode->create_publisher( + this->dataPtr->robotNamespace + "/" + "Pressure_depth", rclcpp::QoS(10).reliable()); } + + this->dataPtr->gz_pressure_sensor_pub = + this->dataPtr->gazeboNode->Advertise( + this->dataPtr->robotNamespace + "/" + "Pressure"); } ////////////////////////////////////////// @@ -179,13 +177,7 @@ 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); - - // const gz::sim::Entity & _entity = _ecm.EntityByComponents(gz::sim::components::WorldPose()); - // gz::math::Vector3 pos; - // auto pos = GetWorldPose(_entity, _ecm); - double depth = std::abs(sea_pressure_sensor_pos.Z()); this->dataPtr->pressure = this->dataPtr->standardPressure; if (depth >= 0) @@ -193,12 +185,12 @@ void SubseaPressureSensorPlugin::PreUpdate( this->dataPtr->pressure += depth * this->dataPtr->kPaPerM; } - // not adding gaussian noise for now + // 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) // estimateDepth is false by default + if (this->dataPtr->estimateDepth) { this->dataPtr->inferredDepth = (this->dataPtr->pressure - this->dataPtr->standardPressure) / this->dataPtr->kPaPerM; @@ -208,36 +200,36 @@ void SubseaPressureSensorPlugin::PreUpdate( void SubseaPressureSensorPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - // this->dataPtr->PublishState(); + this->dataPtr->lastMeasurementTime = _info.simTime; - // if (!this->dataPtr->EnableMeasurement(_info)) - // { - // return; - // } + // 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); - if (this->dataPtr->gzMsgEnabled) - { - pressure_sensor_msgs::msgs::Pressure gazeboMsg; - gazeboMsg.set_pressure(this->dataPtr->pressure); - gazeboMsg.set_stddev(this->dataPtr->noiseSigma); - if (this->dataPtr->estimateDepth) - { - gazeboMsg.set_depth(this->dataPtr->inferredDepth); - } - this->dataPtr->gazeboSensorOutputPub.Publish(gazeboMsg); - } + // Publishing the pressure message + this->dataPtr->gz_pressure_sensor_pub.Publish(gzPressureMsg); - sensor_msgs::msg::FluidPressure rosMsg; - rosMsg.header.stamp.sec = + // Publishing Sea_Pressure on Ros Topic + sensor_msgs::msg::FluidPressure rosPressureMsg; + rosPressureMsg.header.stamp.sec = std::chrono::duration_cast(_info.simTime).count(); // Time in seconds - rosMsg.header.stamp.nanosec = + rosPressureMsg.header.stamp.nanosec = std::chrono::duration_cast(_info.simTime).count() % 1000000000; // Time in nanoseconds - // this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); - rosMsg.fluid_pressure = this->dataPtr->pressure; - rosMsg.variance = this->dataPtr->noiseSigma * this->dataPtr->noiseSigma; - this->dataPtr->rosSensorOutputPub->publish(rosMsg); - this->dataPtr->lastMeasurementTime = _info.simTime; + 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(this->dataPtr->lastMeasurementTime).count(); + this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg); + } if (!_info.paused) { @@ -250,42 +242,4 @@ void SubseaPressureSensorPlugin::PostUpdate( } } -} // namespace dave_gz_sensor_plugins - -///////////////////////////////////////////////// -// bool SubseaPressureSensorPlugin::EnableMeasurement(const gz::sim::UpdateInfo & _info) const -// { -// common::Time current_time = _info.simTime; -// double dt = (current_time - this->lastMeasurementTime).Double(); -// return dt >= 1.0 / this->updateRate && this->isReferenceInit && this->isOn.data; -// } - -///////////////////////////////////////////////// -// this->noiseModels["default"]: A map that likely holds different noise models, with “default” -// being a standard Gaussian distribution function. - -// double ROSBasePlugin::GetGaussianNoise(double _amp) -// { -// return _amp * this->noiseModels["default"](this->rndGen); -// } - -///////////////////////////////////////////////// -// bool SubseaPressureSensorPlugin::GetSDFParam( -// const std::shared_ptr & _sdf, const std::string & name, T & param, -// const T & default_value, const bool & verbose = false) -// { -// if (sdf->HasElement(name)) -// { -// param = sdf->GetElement(name)->Get(); -// return true; -// } -// else -// { -// param = default_value; -// if (verbose) -// { -// gzerr << "[uuv_sensor_plugins] Please specify a value for parameter \"" << name << "\".\n"; -// } -// } -// return false; -// } \ No newline at end of file +} // namespace dave_gz_sensor_plugins \ No newline at end of file diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index f5b91146..16edd254 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -593,10 +593,9 @@ sea_pressure 10 - 3.0 - 0.0 - false + + true 101.325 9.80638 From f49f229199736389be402c831b530d739f858429 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 8 Aug 2024 21:11:56 +0530 Subject: [PATCH 49/79] Minor changes --- .../include/dave_gz_sensor_plugins/sea_pressure_sensor.hh | 1 - models/dave_robot_models/description/rexrov/model.sdf | 3 --- 2 files changed, 4 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh index 132a46d2..d729f360 100644 --- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/sea_pressure_sensor.hh @@ -1,7 +1,6 @@ #ifndef dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ #define dave_gz_sensor_plugins__SUBSEA_PRESSURE_SENSOR_HH_ -// #include #include #include #include diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 16edd254..4e4e5d61 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -589,12 +589,9 @@ filename="sea_pressure_sensor" name=" dave_gz_sensor_plugins::SubseaPressureSensorPlugin"> rexrov - sea_pressure - 10 3.0 - true 101.325 9.80638 From 428b382b934d6733934bdc72c69001adc99de58c Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 9 Aug 2024 10:15:55 +0530 Subject: [PATCH 50/79] Removed Proto --- dave_interfaces/CMakeLists.txt | 73 ------------------- .../msgs/SensorPressure.proto | 23 ------ 2 files changed, 96 deletions(-) delete mode 100644 dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 5d2c252a..0bd9904b 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -11,8 +11,6 @@ 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) # new to proto rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" @@ -72,77 +70,6 @@ gz_msgs_generate_messages( DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} ) -# Define a variable 'GZ_MSGS_VER' holding the version number -set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) - -# Define a variable 'MSGS_PROTOS' listing the .proto files -set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) - -# Define the GZ_DATA_INSTALL_DIR variable -# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos -install( - DIRECTORY gz - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto - COMPONENT proto - FILES_MATCHING PATTERN "*.proto") - -# Example of custom messages that depend on gz.msgs -set(MSGS_PROTOS - ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) -gz_msgs_generate_messages( - # The cmake target to be generated for libraries/executables to link - TARGET msgs # pressure_sensor_msgs_gen - # The protobuf package to generate (Typically based on the path) - PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" - # The path to the base directory of the proto files - # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) - MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto - # List of proto files to generate - MSGS_PROTOS ${MSGS_PROTOS} - # List of message targets this library imports from - # DEPENDENCIES gz_msgs_gen - # Dependency on gz-msgs - DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} -) - -# new to proto -################################################ - -# Define a variable 'GZ_MSGS_VER' holding the version number -set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) - -# Define a variable 'MSGS_PROTOS' listing the .proto files -set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) - -# Define the GZ_DATA_INSTALL_DIR variable -# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos -install( - DIRECTORY gz - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto - COMPONENT proto - FILES_MATCHING PATTERN "*.proto") - -# Example of custom messages that depend on gz.msgs -set(MSGS_PROTOS - ${CMAKE_CURRENT_SOURCE_DIR}/proto/pressure_sensor_msgs/msgs/SensorPressure.proto) -gz_msgs_generate_messages( - # The cmake target to be generated for libraries/executables to link - TARGET msgs # pressure_sensor_msgs_gen - # The protobuf package to generate (Typically based on the path) - PROTO_PACKAGE "pressure_sensor_msgs.msgs" - # The path to the base directory of the proto files - # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) - MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto - # List of proto files to generate - MSGS_PROTOS ${MSGS_PROTOS} - # List of message targets this library imports from - # DEPENDENCIES gz_msgs_gen - # Dependency on gz-msgs - DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} -) -# target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}-msgs) - -################################################ ament_export_dependencies(rosidl_default_runtime) # Install CMake package configuration ament_export_include_directories(include) diff --git a/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto b/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto deleted file mode 100644 index 718a72ff..00000000 --- a/dave_interfaces/proto/pressure_sensor_msgs/msgs/SensorPressure.proto +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) 2016 The UUV 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. - -syntax = "proto3"; -package pressure_sensor_msgs.msgs; - -message Pressure { - double pressure = 1; - double stddev = 2; - double depth = 3; -} From 9b1a7b290f6780600c76fde35301194c740e2a80 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Fri, 9 Aug 2024 17:20:26 +0900 Subject: [PATCH 51/79] remove empty file --- models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world diff --git a/models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world b/models/dave_worlds/worlds/dave_sea_pressor_sensor_tutorial.world deleted file mode 100644 index e69de29b..00000000 From e731f5c7982117fbabe0c1591b5d2823e888a51c Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Fri, 9 Aug 2024 17:33:23 +0900 Subject: [PATCH 52/79] add lints and build checks for PR too --- .github/workflows/jazzy-source-build.yml | 6 ++++-- .github/workflows/lint.yml | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml index dd341ef9..5822b6b9 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -16,9 +16,11 @@ on: - "**/package.xml" - "**/CMakeLists.txt" - "dave.jazzy.repos" + pull_request: + types: [opened, reopened] schedule: - # Run every day to detect flakiness and broken dependencies - - cron: "03 3 * * *" + # Run every week to detect flakiness and broken dependencies + - cron: "03 3 * * 1" env: ros_distro: jazzy diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 48ca25a8..1684173c 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -7,6 +7,8 @@ on: tags: ["*"] branches: - ros2 + pull_request: + types: [opened, reopened] jobs: # ament_lint: From 408e8a382b9481f8f95e2717d0d64668357122d7 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 3 Sep 2024 22:58:44 +0530 Subject: [PATCH 53/79] minor changes and modification of sdf --- dave_interfaces/CMakeLists.txt | 4 +- .../launch/robot_in_world.launch.py | 3 +- gazebo/dave_gz_model_plugins/CMakeLists.txt | 144 ++++++++++++++---- .../ocean_current_model_plugin.hh | 4 +- gazebo/dave_gz_model_plugins/package.xml | 2 +- .../src/ocean_current_model_plugin.cc | 30 ++-- .../ocean_current_world_plugin.hh | 7 - .../src/ocean_current_world_plugin.cc | 12 +- .../src/ocean_current_plugin.cc | 9 +- .../description/rexrov/model.sdf | 29 +++- .../worlds/transientOceanCurrentDatabase.csv | 2 +- 11 files changed, 176 insertions(+), 70 deletions(-) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 0bd9904b..6cdf513d 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/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" @@ -74,4 +76,4 @@ ament_export_dependencies(rosidl_default_runtime) # Install CMake package configuration ament_export_include_directories(include) -ament_package() +ament_package() \ No newline at end of file diff --git a/examples/dave_robot_launch/launch/robot_in_world.launch.py b/examples/dave_robot_launch/launch/robot_in_world.launch.py index 26c21afc..17f0f6e5 100644 --- a/examples/dave_robot_launch/launch/robot_in_world.launch.py +++ b/examples/dave_robot_launch/launch/robot_in_world.launch.py @@ -39,7 +39,8 @@ def launch_setup(context, *args, **kwargs): gz_args.append(" -r") if debug.perform(context) == "true": gz_args.append(" -v ") - gz_args.append(verbose.perform(context)) + if verbose.perform(context) == "true": + gz_args.append(" -v ") # Include the first launch file gz_sim_launch = IncludeLaunchDescription( diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index 75ed0b06..b982b936 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -26,19 +26,12 @@ add_library(ocean_current_model_plugin SHARED src/ocean_current_model_plugin.cc ) -target_include_directories(ocean_current_model_plugin - PUBLIC - $ - $ - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src -) +target_include_directories(ocean_current_model_plugin PRIVATE include) target_link_libraries(ocean_current_model_plugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) - # Specify dependencies for ocean_current_plugin using ament_target_dependencies ament_target_dependencies(ocean_current_model_plugin rclcpp @@ -48,33 +41,14 @@ ament_target_dependencies(ocean_current_model_plugin dave_gz_world_plugins ) -# Install headers -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - # Install targets -install( - TARGETS ocean_current_model_plugin - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include +install(TARGETS ocean_current_model_plugin + DESTINATION lib/${PROJECT_NAME} ) -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - gz-cmake3 - gz-plugin2 - gz-common5 - gz-sim8 - geometry_msgs - std_msgs - dave_interfaces - dave_gz_world_plugins - rclcpp +# Install headers +install(DIRECTORY include/ + DESTINATION include/ ) # Environment hooks @@ -90,3 +64,109 @@ endif() ament_package() + +# cmake_minimum_required(VERSION 3.8) + +# project(dave_gz_model_plugins) + +# 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(dave_gz_world_plugins REQUIRED) + +# # Set version variables with gazebo Harmonic +# 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(ocean_current_model_plugin SHARED +# src/ocean_current_model_plugin.cc +# ) + +# # target_include_directories(ocean_current_model_plugin +# # PUBLIC +# # $ +# # $ +# # PRIVATE +# # ${CMAKE_CURRENT_SOURCE_DIR}/src +# # ) + + +# target_include_directories(ocean_current_model_plugin PRIVATE include) + +# target_link_libraries(ocean_current_model_plugin +# gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +# ) + + +# # Specify dependencies for ocean_current_plugin using ament_target_dependencies +# ament_target_dependencies(ocean_current_model_plugin +# rclcpp +# std_msgs +# geometry_msgs +# dave_interfaces +# dave_gz_world_plugins +# ) + +# # Install targets +# install(TARGETS ocean_current_model_plugin +# DESTINATION lib/${PROJECT_NAME} +# ) + +# # Install headers +# install(DIRECTORY include/ +# DESTINATION include/ +# ) + +# # # Install headers +# # install(DIRECTORY include/ +# # DESTINATION include/${PROJECT_NAME} +# # ) + +# # # Install targets +# # install( +# # TARGETS ocean_current_model_plugin +# # EXPORT export_${PROJECT_NAME} +# # LIBRARY DESTINATION lib +# # ARCHIVE DESTINATION lib +# # RUNTIME DESTINATION bin +# # INCLUDES DESTINATION include +# # ) + +# # ament_export_include_directories("include/${PROJECT_NAME}") +# # ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +# # ament_export_dependencies( +# # gz-cmake3 +# # gz-plugin2 +# # gz-common5 +# # gz-sim8 +# # geometry_msgs +# # std_msgs +# # dave_interfaces +# # dave_gz_world_plugins +# # rclcpp +# # ) + +# # Environment hooks +# ament_environment_hooks( +# "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" +# ) + +# # Following directives are used when testing. +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +# endif() + +# ament_package() + diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh index 628a18a3..c1a47a48 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh @@ -67,7 +67,9 @@ public: void Gauss_Markov_process_initialize(const std::shared_ptr & _sdf); /// \brief Convey model state from gazebo topic to outside - void UpdateDatabase(const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg); + // void UpdateDatabase(const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg); + void UpdateDatabase( + const std::shared_ptr & _msg); /// \brief Publish ocean current void PublishCurrentVelocity(const gz::sim::UpdateInfo & _info); diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index 8e6fde01..46a5a0ad 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -2,7 +2,7 @@ dave_gz_model_plugins 0.0.0 - The dave_gz_model_plugins + Dave Model Plugins Gaurav Kumar Apache 2.0 ament_cmake diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index 13a236d8..b96284df 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -46,16 +46,13 @@ namespace dave_gz_model_plugins struct TransientCurrentPlugin::PrivateData { // Initialize any necessary states before the plugin starts - virtual void Init(); std::string transientCurrentVelocityTopic; gz::sim::World world{gz::sim::kNullEntity}; gz::sim::Entity entity{gz::sim::kNullEntity}; gz::sim::Model model{gz::sim::kNullEntity}; gz::sim::Entity modelLink{gz::sim::kNullEntity}; std::string ns; - virtual void Connect(); std::shared_ptr gz_node; - // std::map publishers; gz::transport::Node::Publisher gz_current_vel_pub; std::string currentVelocityTopic; std::chrono::steady_clock::duration lastUpdate{0}; @@ -66,7 +63,6 @@ struct TransientCurrentPlugin::PrivateData std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr flowVelocityPub; rclcpp::Subscription::SharedPtr databaseSub; - // std::shared_ptr> flowVelocityPub; std::string modelName; /// \brief Gauss-Markov process instance for the velocity components @@ -187,11 +183,14 @@ void TransientCurrentPlugin::Configure( this->dataPtr->gz_node->Advertise(this->dataPtr->currentVelocityTopic); // Subscribe stratified ocean current database + // this->dataPtr->databaseSub = + // this->ros_node_->create_subscription( + // this->dataPtr->transientCurrentVelocityTopic, 10, + // std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); this->dataPtr->databaseSub = this->ros_node_->create_subscription( this->dataPtr->transientCurrentVelocityTopic, 10, std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); - // Read topic name of stratified ocean current from SDF LoadCurrentVelocityParams(sdfClone, _ecm); Gauss_Markov_process_initialize(_sdf); @@ -230,11 +229,7 @@ void TransientCurrentPlugin::LoadCurrentVelocityParams( } } } -///////////////////////////////////////////////// -void TransientCurrentPlugin::Init() -{ - // Doing nothing for now -} + ////////////////////////////////////////// gz::sim::Entity TransientCurrentPlugin::GetModelEntity( @@ -430,8 +425,10 @@ void TransientCurrentPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & } ///////////////////////////////////////////////// +// void TransientCurrentPlugin::UpdateDatabase( +// const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg) void TransientCurrentPlugin::UpdateDatabase( - const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg) + const std::shared_ptr & _msg) { this->dataPtr->lock_.lock(); @@ -595,6 +592,17 @@ void TransientCurrentPlugin::Gauss_Markov_process_initialize( this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.count(); this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.count(); } + +///////////////////////////////////////////////// +// void TransientCurrentPlugin::ApllyModelForces(){} + +///////////////////////////////////////////////// +// void TransientCurrentPlugin::PreUpdate(){ + +// // Apply force to the model + +// } + ///////////////////////////////////////////////// void TransientCurrentPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh index 60f98f9d..34049e60 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh @@ -27,7 +27,6 @@ namespace dave_gz_world_plugins // public WorldPlugin, class UnderwaterCurrentPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, public gz::sim::ISystemUpdate, public gz::sim::ISystemPostUpdate // public gz::sim::WorldPlugin @@ -45,12 +44,6 @@ public: // Documentation inherited. virtual void Init(); - /// \brief Update the simulation state. - /// \param[in] _info Information used in the update event. - // void Update(const common::UpdateInfo & _info); (check if this is needed) - - void PreUpdate(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc index 00b78170..214ec120 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc @@ -29,7 +29,7 @@ GZ_ADD_PLUGIN( dave_gz_world_plugins::UnderwaterCurrentPlugin, gz::sim::System, dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemConfigure, - dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemPreUpdate, + dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemUpdate, dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemPostUpdate) namespace dave_gz_world_plugins @@ -99,8 +99,8 @@ void UnderwaterCurrentPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { - GZ_ASSERT(_entity != NULL, "World pointer is invalid"); - GZ_ASSERT(_sdf != NULL, "SDF pointer is invalid"); + // GZ_ASSERT(_entity != gz::sim::kNullEntity, "World pointer is invalid"); + // GZ_ASSERT(_sdf != nullptr, "SDF pointer is invalid"); this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); if (!this->dataPtr->world.Valid(_ecm)) // check @@ -654,12 +654,6 @@ void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); } -///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ -} - ///////////////////////////////////////////////// void UnderwaterCurrentPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc index 97557e52..c0a06bad 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc @@ -77,9 +77,10 @@ void UnderwaterCurrentROSPlugin::Configure( { if (!rclcpp::ok()) { - gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your " - "ROS 2 environment.\n"; - return; + rclcpp::init(0, nullptr); + // gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your + // " + // "ROS 2 environment.\n"; } this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin"); @@ -124,8 +125,6 @@ void UnderwaterCurrentROSPlugin::Configure( // serviceCallback method of to be used directly as a ROS 2 service handler without // having to wrap it in another function that manually passes the request and response. - // check for the number of arg being passed to the function - // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = this->dataPtr->rosNode->create_service( diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 4e4e5d61..a4281317 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -587,7 +587,7 @@ + name="dave_gz_sensor_plugins::SubseaPressureSensorPlugin"> rexrov sea_pressure 10 @@ -597,5 +597,32 @@ 9.80638 + + + + rexrov + + + $(arg namespace)/base_link + + stratified_current_velocity + + 0.3 + 0.0 + + + 0.3 + 0.0 + + + 0.3 + 0.0 + + + false + + \ No newline at end of file diff --git a/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv b/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv index 34795da8..7cb26ec6 100644 --- a/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv +++ b/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv @@ -12,4 +12,4 @@ Information about this data can be written here (in one line without comma),, 0.01,0.02,80 0.01,0.02,90 0.01,0.02,100 -0.01,0.0,110 +0.01,0.0,110 \ No newline at end of file From 085b4eb625048d5dc978480cd808a7a251f814b3 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Fri, 20 Sep 2024 02:03:52 +0530 Subject: [PATCH 54/79] testing --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 2 ++ .../src/ocean_current_model_plugin.cc | 9 ++++----- models/dave_robot_models/description/rexrov/model.sdf | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index b982b936..dc612020 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -25,6 +25,7 @@ message(STATUS "Compiling against Gazebo Harmonic") add_library(ocean_current_model_plugin SHARED src/ocean_current_model_plugin.cc ) +set_property(TARGET ocean_current_model_plugin PROPERTY CXX_STANDARD 17) target_include_directories(ocean_current_model_plugin PRIVATE include) @@ -56,6 +57,7 @@ ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" ) + # Following directives are used when testing. if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc index b96284df..2195b40b 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc @@ -1,8 +1,11 @@ #include "dave_gz_model_plugins/ocean_current_model_plugin.hh" #include #include +#include #include #include +#include +#include #include #include #include @@ -18,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -29,11 +33,6 @@ #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -#include -#include -#include -#include - // Register plugin GZ_ADD_PLUGIN( dave_gz_model_plugins::TransientCurrentPlugin, gz::sim::System, diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index a4281317..2b33380c 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -600,7 +600,7 @@ rexrov From 42f989d06f8d70e1b94658c960ad9da20718ed70 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 7 Oct 2024 02:39:37 +0530 Subject: [PATCH 55/79] working ocean current model plugin --- gazebo/dave_gz_model_plugins/CMakeLists.txt | 131 +--- ...l_plugin.hh => OceanCurrentModelPlugin.hh} | 65 +- ...l_plugin.cc => OceanCurrentModelPlugin.cc} | 606 +++++++++--------- .../description/rexrov/model.sdf | 16 +- 4 files changed, 357 insertions(+), 461 deletions(-) rename gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/{ocean_current_model_plugin.hh => OceanCurrentModelPlugin.hh} (55%) rename gazebo/dave_gz_model_plugins/src/{ocean_current_model_plugin.cc => OceanCurrentModelPlugin.cc} (67%) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index dc612020..e1c6d471 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -1,7 +1,7 @@ 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) @@ -12,9 +12,11 @@ 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) #r +find_package(gz-msgs10 REQUIRED) #r find_package(dave_gz_world_plugins REQUIRED) -# Set version variables with gazebo Harmonic +# 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}) @@ -22,19 +24,16 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") -add_library(ocean_current_model_plugin SHARED - src/ocean_current_model_plugin.cc -) -set_property(TARGET ocean_current_model_plugin PROPERTY CXX_STANDARD 17) +add_library(OceanCurrentModelPlugin SHARED src/OceanCurrentModelPlugin.cc) -target_include_directories(ocean_current_model_plugin PRIVATE include) +target_include_directories(OceanCurrentModelPlugin PRIVATE include) -target_link_libraries(ocean_current_model_plugin +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(ocean_current_model_plugin +ament_target_dependencies(OceanCurrentModelPlugin rclcpp std_msgs geometry_msgs @@ -43,7 +42,7 @@ ament_target_dependencies(ocean_current_model_plugin ) # Install targets -install(TARGETS ocean_current_model_plugin +install(TARGETS OceanCurrentModelPlugin DESTINATION lib/${PROJECT_NAME} ) @@ -57,118 +56,10 @@ ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" ) - -# Following directives are used when testing. +# Testing setup if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -ament_package() - - -# cmake_minimum_required(VERSION 3.8) - -# project(dave_gz_model_plugins) - -# 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(dave_gz_world_plugins REQUIRED) - -# # Set version variables with gazebo Harmonic -# 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(ocean_current_model_plugin SHARED -# src/ocean_current_model_plugin.cc -# ) - -# # target_include_directories(ocean_current_model_plugin -# # PUBLIC -# # $ -# # $ -# # PRIVATE -# # ${CMAKE_CURRENT_SOURCE_DIR}/src -# # ) - - -# target_include_directories(ocean_current_model_plugin PRIVATE include) - -# target_link_libraries(ocean_current_model_plugin -# gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} -# ) - - -# # Specify dependencies for ocean_current_plugin using ament_target_dependencies -# ament_target_dependencies(ocean_current_model_plugin -# rclcpp -# std_msgs -# geometry_msgs -# dave_interfaces -# dave_gz_world_plugins -# ) - -# # Install targets -# install(TARGETS ocean_current_model_plugin -# DESTINATION lib/${PROJECT_NAME} -# ) - -# # Install headers -# install(DIRECTORY include/ -# DESTINATION include/ -# ) - -# # # Install headers -# # install(DIRECTORY include/ -# # DESTINATION include/${PROJECT_NAME} -# # ) - -# # # Install targets -# # install( -# # TARGETS ocean_current_model_plugin -# # EXPORT export_${PROJECT_NAME} -# # LIBRARY DESTINATION lib -# # ARCHIVE DESTINATION lib -# # RUNTIME DESTINATION bin -# # INCLUDES DESTINATION include -# # ) - -# # ament_export_include_directories("include/${PROJECT_NAME}") -# # ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -# # ament_export_dependencies( -# # gz-cmake3 -# # gz-plugin2 -# # gz-common5 -# # gz-sim8 -# # geometry_msgs -# # std_msgs -# # dave_interfaces -# # dave_gz_world_plugins -# # rclcpp -# # ) - -# # Environment hooks -# ament_environment_hooks( -# "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in" -# ) - -# # Following directives are used when testing. -# if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -# endif() - -# ament_package() - +ament_package() \ No newline at end of file diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh similarity index 55% rename from gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh rename to gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh index c1a47a48..a15a9786 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/ocean_current_model_plugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh @@ -1,77 +1,68 @@ -#ifndef DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ -#define DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ +#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 // #include #include #include -#include -#include -#include #include #include +#include + +#include +#include #include + +#include #include -#include namespace dave_gz_model_plugins { -class TransientCurrentPlugin : public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemUpdate, - public gz::sim::ISystemPostUpdate +class OceanCurrentModelPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemUpdate, + public gz::sim::ISystemPostUpdate { public: - TransientCurrentPlugin(); // constructor - ~TransientCurrentPlugin(); // Destructor + OceanCurrentModelPlugin(); + ~OceanCurrentModelPlugin() override; + + // ---------------------------------------------- - // Configure the plugin with necessary entities and component managers void Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - - // Function called after the simulation state updates - void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - void LoadCurrentVelocityParams(sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm); - gz::math::Pose3d GetModelPose( - const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm); + void UpdateDatabase( + const std::shared_ptr & _msg); gz::sim::Entity GetModelEntity( const std::string & modelName, gz::sim::EntityComponentManager & ecm); - /// \brief Check if an entity is enabled or not. - /// \param[in] _entity Target entity - /// \param[in] _ecm Entity component manager - /// \return True if buoyancy should be applied. - // bool IsEnabled(gz::sim::Entity & _entity, gz::sim::EntityComponentManager & _ecm) const; + gz::math::Pose3d GetModelPose( + const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm); - // Initialize any necessary states before the plugin starts - virtual void Init(); + // ---------------------------------------------- - /// \brief ROS helper function that processes messages - void databaseSubThread(); + void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - /// \brief Calculate ocean current using database and vehicle state void CalculateOceanCurrent(double vehicleDepth); - void Gauss_Markov_process_initialize(const std::shared_ptr & _sdf); + // ---------------------------------------------- - /// \brief Convey model state from gazebo topic to outside - // void UpdateDatabase(const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg); - void UpdateDatabase( - const std::shared_ptr & _msg); + // Function called after the simulation state updates + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - /// \brief Publish ocean current void PublishCurrentVelocity(const gz::sim::UpdateInfo & _info); private: @@ -81,4 +72,4 @@ private: }; } // namespace dave_gz_model_plugins -#endif // DAVE_GZ_MODEL_PLUGINS__OCEAN_CURRENT_MODEL_PLUGIN_HH_ +#endif // DAVE_GZ_MODEL_PLUGINS__OCEANCURRENTMODELPLUGIN_HH_ diff --git a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc similarity index 67% rename from gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc rename to gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index 2195b40b..d9fb93d4 100644 --- a/gazebo/dave_gz_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -1,11 +1,10 @@ -#include "dave_gz_model_plugins/ocean_current_model_plugin.hh" +// Plugin includes +#include "dave_gz_model_plugins/OceanCurrentModelPlugin.hh" +#include "dave_gz_world_plugins/gauss_markov_process.hh" +#include "dave_gz_world_plugins/tidal_oscillation.hh" + +// Gazebo includes #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -18,58 +17,80 @@ #include #include #include -#include -#include + +// ROS2 includes +#include +#include #include -#include #include + +// Message interfaces includes +#include "dave_interfaces/msg/Stratified_Current_Database.hpp" +#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" + +// Standard library includes +#include +#include +#include +#include +#include +#include #include #include #include -#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" + +// Common utilities #include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -// Register plugin GZ_ADD_PLUGIN( - dave_gz_model_plugins::TransientCurrentPlugin, gz::sim::System, - dave_gz_model_plugins::TransientCurrentPlugin::ISystemConfigure, - dave_gz_model_plugins::TransientCurrentPlugin::ISystemUpdate, - dave_gz_model_plugins::TransientCurrentPlugin::ISystemPostUpdate) + dave_gz_model_plugins::OceanCurrentModelPlugin, gz::sim::System, + dave_gz_model_plugins::OceanCurrentModelPlugin::ISystemConfigure, + dave_gz_model_plugins::OceanCurrentModelPlugin::ISystemUpdate, + dave_gz_model_plugins::OceanCurrentModelPlugin::ISystemPostUpdate) namespace dave_gz_model_plugins { -struct TransientCurrentPlugin::PrivateData +struct OceanCurrentModelPlugin::PrivateData { - // Initialize any necessary states before the plugin starts - std::string transientCurrentVelocityTopic; + // Gazebo Simulation Related Variables gz::sim::World world{gz::sim::kNullEntity}; gz::sim::Entity entity{gz::sim::kNullEntity}; gz::sim::Model model{gz::sim::kNullEntity}; gz::sim::Entity modelLink{gz::sim::kNullEntity}; - std::string ns; + gz::sim::Entity modelEntity; + std::string modelName; + + // Transport and Communication std::shared_ptr gz_node; gz::transport::Node::Publisher gz_current_vel_pub; - std::string currentVelocityTopic; - std::chrono::steady_clock::duration lastUpdate{0}; - int lastDepthIndex = 0; - gz::math::Vector3d currentVelocity; - std::mutex lock_; - std::chrono::steady_clock::duration rosPublishPeriod{0}; std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr flowVelocityPub; rclcpp::Subscription::SharedPtr databaseSub; - std::string modelName; - /// \brief Gauss-Markov process instance for the velocity components + // Topics + std::string currentVelocityTopic; + std::string transientCurrentVelocityTopic; + + // Time and Periods + std::chrono::steady_clock::duration lastUpdate{0}; + std::chrono::steady_clock::duration rosPublishPeriod{0}; + std::chrono::steady_clock::duration time; + + // Lock for Synchronization + std::mutex lock_; + + // Current Velocity + gz::math::Vector3d currentVelocity; + int lastDepthIndex = 0; + std::vector database; + + // Gauss-Markov Process Models dave_gz_world_plugins::GaussMarkovProcess currentVelNorthModel; dave_gz_world_plugins::GaussMarkovProcess currentVelEastModel; dave_gz_world_plugins::GaussMarkovProcess currentVelDownModel; - /// \brief Gauss-Markov noise + // Noise Amplitudes and Frequencies double noiseAmp_North; double noiseAmp_East; double noiseAmp_Down; @@ -77,85 +98,75 @@ struct TransientCurrentPlugin::PrivateData double noiseFreq_East; double noiseFreq_Down; - std::vector database; - - /// \brief Tidal Oscillation interpolation model + // Tidal Oscillation Model dave_gz_world_plugins::TidalOscillation tide; - /// \brief Tidal Oscillation flag + // Tidal Oscillation Settings and Flags bool tideFlag; - - /// \brief Vector to read timeGMT - std::vector> timeGMT; - - /// \brief Vector to read tideVelocities - std::vector tideVelocities; - - /// \brief Tidal current harmonic constituents bool tide_Constituents; - double M2_amp; - double M2_phase; - double M2_speed; - double S2_amp; - double S2_phase; - double S2_speed; - double N2_amp; - double N2_phase; - double N2_speed; - - /// \brief Tidal oscillation mean ebb direction - double ebbDirection; + double M2_amp, M2_phase, M2_speed; + double S2_amp, S2_phase, S2_speed; + double N2_amp, N2_phase, N2_speed; - /// \brief Tidal oscillation mean flood direction + // Tidal Oscillation Directions + double ebbDirection; double floodDirection; - /// \brief Tidal oscillation world start time (GMT) + // Tidal Oscillation World Start Time (GMT) std::array world_start_time; - gz::sim::Entity modelEntity; - std::chrono::steady_clock::duration time; + std::vector> timeGMT; + + // Tidal Velocities + std::vector tideVelocities; }; -TransientCurrentPlugin::TransientCurrentPlugin() : dataPtr(std::make_unique()) {} +OceanCurrentModelPlugin::OceanCurrentModelPlugin() : dataPtr(std::make_unique()) {} + +OceanCurrentModelPlugin::~OceanCurrentModelPlugin() = default; -TransientCurrentPlugin::~TransientCurrentPlugin() = default; +// ---------------------------------------------- -void TransientCurrentPlugin::Configure( +///////////////////////////////////////////////// +void OceanCurrentModelPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { + // Check if ROS is initialized; if not, initialize it if (!rclcpp::ok()) { rclcpp::init(0, nullptr); } // Initialize the ROS 2 node - this->ros_node_ = std::make_shared("TransirentCurrentPlugin"); + this->ros_node_ = std::make_shared("OceanCurrentModelPlugin"); - // Initializing the Gazebo transport node + // Initialize the Gazebo transport node this->dataPtr->gz_node = std::make_shared(); - gzdbg << "dave_gz_model_plugins::TransientCureentPlugin::Configure on entity: " << _entity + gzdbg << "dave_gz_model_plugins::OceanCurrentModelPlugin::Configure on entity: " << _entity << std::endl; - // Make a clone so that we can call non-const methods + // Clone the SDF element to allow calling non-const methods sdf::ElementPtr sdfClone = _sdf->Clone(); + // Get the world entity from the Entity Component Manager (ECM) auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); this->dataPtr->world = gz::sim::World(worldEntity); + // Get the model entity auto model = gz::sim::Model(_entity); this->dataPtr->model = model; - gzmsg << "Loading transient ocean current model plugin..." << std::endl; + gzmsg << "Loading ocean current model plugin..." << std::endl; // Read the namespace for topics and services if (!_sdf->HasElement("namespace")) { - gzerr << "Missing required parameter , " - << "plugin will not be initialized." << std::endl; + gzerr << "Missing required parameter , plugin will not be initialized." << std::endl; return; } this->dataPtr->modelName = _sdf->Get("namespace"); + // Set the flow velocity topic from SDF or use the default topic name if (_sdf->HasElement("flow_velocity_topic")) { this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic"); @@ -164,45 +175,47 @@ void TransientCurrentPlugin::Configure( { this->dataPtr->currentVelocityTopic = "hydrodynamics/current_velocity/" + this->dataPtr->modelName; - gzerr << "Empty flow_velocity_topic for transient_current model plugin." - << "Default topicName definition is used" << std::endl; + gzwarn << "Empty flow_velocity_topic for transient_current model plugin. Default topicName " + "definition is used." + << std::endl; } gzmsg << "Transient velocity topic name for " << this->dataPtr->modelName << " : " << this->dataPtr->currentVelocityTopic << std::endl; + // Get the model entity based on the model name this->dataPtr->modelEntity = GetModelEntity(this->dataPtr->modelName, _ecm); + // Read the stratified ocean current topic name from the SDF + LoadCurrentVelocityParams(sdfClone, _ecm); + + // Initialize the Gauss-Markov process + // Gauss_Markov_process_initialize(_sdf); + // Advertise the ROS flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->ros_node_->create_publisher( this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); - // Advertise the current velocity topic in gazebo + // Advertise the current velocity topic in Gazebo this->dataPtr->gz_current_vel_pub = this->dataPtr->gz_node->Advertise(this->dataPtr->currentVelocityTopic); - // Subscribe stratified ocean current database - // this->dataPtr->databaseSub = - // this->ros_node_->create_subscription( - // this->dataPtr->transientCurrentVelocityTopic, 10, - // std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); + // Subscribe to the stratified ocean current database this->dataPtr->databaseSub = this->ros_node_->create_subscription( this->dataPtr->transientCurrentVelocityTopic, 10, - std::bind(&TransientCurrentPlugin::UpdateDatabase, this, std::placeholders::_1)); - // Read topic name of stratified ocean current from SDF - LoadCurrentVelocityParams(sdfClone, _ecm); - Gauss_Markov_process_initialize(_sdf); + std::bind(&OceanCurrentModelPlugin::UpdateDatabase, this, std::placeholders::_1)); + gzmsg << "Transient current model plugin loaded!" << std::endl; } ///////////////////////////////////////////////// -void TransientCurrentPlugin::LoadCurrentVelocityParams( +void OceanCurrentModelPlugin::LoadCurrentVelocityParams( sdf::ElementPtr _sdf, gz::sim::EntityComponentManager & _ecm) { // Read topic name of stratified ocean current from SDF sdf::ElementPtr currentVelocityParams; - if (_sdf->HasElement("transient_current")) // Add this to the sdf file TODO + if (_sdf->HasElement("transient_current")) { currentVelocityParams = _sdf->GetElement("transient_current"); if (currentVelocityParams->HasElement("topic_stratified")) @@ -217,8 +230,114 @@ void TransientCurrentPlugin::LoadCurrentVelocityParams( "/hydrodynamics/stratified_current_velocity_database"; } + // Read Gauss-Markov parameters + // sdf::ElementPtr currentVelocityDirection; + + // initialize velocity_north_model parameters + if (currentVelocityParams->HasElement("velocity_north")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); + if (elem->HasElement("mean")) + { + this->dataPtr->currentVelNorthModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->dataPtr->currentVelNorthModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->dataPtr->noiseAmp_North = elem->Get("noiseAmp"); + } + this->dataPtr->currentVelNorthModel.min = + this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; + this->dataPtr->currentVelNorthModel.max = + this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; + if (elem->HasElement("noiseFreq")) + { + this->dataPtr->noiseFreq_North = elem->Get("noiseFreq"); + } + this->dataPtr->currentVelNorthModel.noiseAmp = this->dataPtr->noiseFreq_North; + } + + this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; + gzmsg << "For vehicle " << this->dataPtr->modelName + << " -> Current north-direction velocity [m/s] " + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelNorthModel.Print(); + + // initialize velocity_east_model parameters + if (currentVelocityParams->HasElement("velocity_east")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); + if (elem->HasElement("mean")) + { + this->dataPtr->currentVelEastModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->dataPtr->currentVelEastModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->dataPtr->noiseAmp_East = elem->Get("noiseAmp"); + } + this->dataPtr->currentVelEastModel.min = + this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; + this->dataPtr->currentVelEastModel.max = + this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; + if (elem->HasElement("noiseFreq")) + { + this->dataPtr->noiseFreq_East = elem->Get("noiseFreq"); + } + this->dataPtr->currentVelEastModel.noiseAmp = this->dataPtr->noiseFreq_East; + } + + this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; + gzmsg << "For vehicle " << this->dataPtr->modelName + << " -> Current east-direction velocity [m/s] " + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelEastModel.Print(); + + // initialize velocity_down_model parameters + if (currentVelocityParams->HasElement("velocity_down")) + { + sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_down"); + if (elem->HasElement("mean")) + { + this->dataPtr->currentVelDownModel.mean = 0.0; + } + if (elem->HasElement("mu")) + { + this->dataPtr->currentVelDownModel.mu = 0.0; + } + if (elem->HasElement("noiseAmp")) + { + this->dataPtr->noiseAmp_Down = elem->Get("noiseAmp"); + } + this->dataPtr->currentVelDownModel.min = + this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; + this->dataPtr->currentVelDownModel.max = + this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; + if (elem->HasElement("noiseFreq")) + { + this->dataPtr->noiseFreq_Down = elem->Get("noiseFreq"); + } + this->dataPtr->currentVelDownModel.noiseAmp = this->dataPtr->noiseFreq_Down; + } + + this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; + gzmsg << "For vehicle " << this->dataPtr->modelName + << " -> Current down-direction velocity [m/s]" + << "Gauss-Markov process model:" << std::endl; + this->dataPtr->currentVelDownModel.Print(); + + this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.count(); + this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.count(); + this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.count(); + // Tidal Oscillation - if (_sdf->HasElement("tide_oscillation")) // TODO + if (_sdf->HasElement("tide_oscillation")) { this->dataPtr->tideFlag = _sdf->Get("tide_oscillation"); } @@ -229,9 +348,64 @@ void TransientCurrentPlugin::LoadCurrentVelocityParams( } } -////////////////////////////////////////// +void OceanCurrentModelPlugin::UpdateDatabase( + const std::shared_ptr & _msg) +{ + this->dataPtr->lock_.lock(); -gz::sim::Entity TransientCurrentPlugin::GetModelEntity( + this->dataPtr->database.clear(); + for (int i = 0; i < _msg->depths.size(); i++) + { + gz::math::Vector3d data(_msg->velocities[i].x, _msg->velocities[i].y, _msg->depths[i]); + this->dataPtr->database.push_back(data); + } + if (this->dataPtr->tideFlag) + { + this->dataPtr->timeGMT.clear(); + this->dataPtr->tideVelocities.clear(); + if (_msg->tideconstituents == true) + { + this->dataPtr->M2_amp = _msg->m2_amp; + this->dataPtr->M2_phase = _msg->m2_phase; + this->dataPtr->M2_speed = _msg->m2_speed; + this->dataPtr->S2_amp = _msg->s2_amp; + this->dataPtr->S2_phase = _msg->s2_phase; + this->dataPtr->S2_speed = _msg->s2_speed; + this->dataPtr->N2_amp = _msg->n2_amp; + this->dataPtr->N2_phase = _msg->n2_phase; + this->dataPtr->N2_speed = _msg->n2_speed; + this->dataPtr->tide_Constituents = true; + } + else + { + std::array tmpDateVals; + for (int i = 0; i < _msg->time_gmt_year.size(); i++) + { + tmpDateVals[0] = _msg->time_gmt_year[i]; + tmpDateVals[1] = _msg->time_gmt_month[i]; + tmpDateVals[2] = _msg->time_gmt_day[i]; + tmpDateVals[3] = _msg->time_gmt_hour[i]; + tmpDateVals[4] = _msg->time_gmt_minute[i]; + + this->dataPtr->timeGMT.push_back(tmpDateVals); + this->dataPtr->tideVelocities.push_back(_msg->tidevelocities[i]); + } + this->dataPtr->tide_Constituents = false; + } + this->dataPtr->ebbDirection = _msg->ebb_direction; + this->dataPtr->floodDirection = _msg->flood_direction; + this->dataPtr->world_start_time[0] = _msg->world_start_time_year; + this->dataPtr->world_start_time[1] = _msg->world_start_time_month; + this->dataPtr->world_start_time[2] = _msg->world_start_time_day; + this->dataPtr->world_start_time[3] = _msg->world_start_time_hour; + this->dataPtr->world_start_time[4] = _msg->world_start_time_minute; + } + + this->dataPtr->lock_.unlock(); +} + +////////////////////////////////////////// +gz::sim::Entity OceanCurrentModelPlugin::GetModelEntity( const std::string & modelName, gz::sim::EntityComponentManager & ecm) { gz::sim::Entity modelEntity = gz::sim::kNullEntity; @@ -249,8 +423,9 @@ gz::sim::Entity TransientCurrentPlugin::GetModelEntity( return modelEntity; } + ///////////////////////////////////////////////// -gz::math::Pose3d TransientCurrentPlugin::GetModelPose( +gz::math::Pose3d OceanCurrentModelPlugin::GetModelPose( const gz::sim::Entity & modelEntity, gz::sim::EntityComponentManager & ecm) { const auto * poseComp = ecm.Component(modelEntity); @@ -264,8 +439,22 @@ gz::math::Pose3d TransientCurrentPlugin::GetModelPose( return gz::math::Pose3d::Zero; } } + +// ---------------------------------------------- + ///////////////////////////////////////////////// -void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) +void OceanCurrentModelPlugin::Update( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + // Update vehicle position + gz::math::Pose3d vehicle_pos = GetModelPose(this->dataPtr->modelEntity, _ecm); + double vehicleDepth = std::abs(vehicle_pos.Z()); + this->dataPtr->time = this->dataPtr->lastUpdate; + CalculateOceanCurrent(vehicleDepth); +} + +///////////////////////////////////////////////// +void OceanCurrentModelPlugin::CalculateOceanCurrent(double vehicleDepth) { this->dataPtr->lock_.lock(); @@ -403,217 +592,11 @@ void TransientCurrentPlugin::CalculateOceanCurrent(double vehicleDepth) this->dataPtr->lock_.unlock(); } -///////////////////////////////////////////////// -void TransientCurrentPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & _info) -{ - geometry_msgs::msg::TwistStamped flowVelMsg; - flowVelMsg.header.stamp.sec = - std::chrono::duration_cast(_info.simTime).count(); - flowVelMsg.header.frame_id = "/world"; - flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); - flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); - flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); - this->dataPtr->flowVelocityPub->publish(flowVelMsg); - - // Generate and publish Gazebo topic according to the vehicle depth - gz::msgs::Vector3d currentVel; - currentVel.set_x(this->dataPtr->currentVelocity.X()); - currentVel.set_y(this->dataPtr->currentVelocity.Y()); - currentVel.set_z(this->dataPtr->currentVelocity.Z()); - this->dataPtr->gz_current_vel_pub.Publish(currentVel); -} - -///////////////////////////////////////////////// -// void TransientCurrentPlugin::UpdateDatabase( -// const dave_interfaces::msg::StratifiedCurrentDatabase::ConstPtr & _msg) -void TransientCurrentPlugin::UpdateDatabase( - const std::shared_ptr & _msg) -{ - this->dataPtr->lock_.lock(); - - this->dataPtr->database.clear(); - for (int i = 0; i < _msg->depths.size(); i++) - { - gz::math::Vector3d data(_msg->velocities[i].x, _msg->velocities[i].y, _msg->depths[i]); - this->dataPtr->database.push_back(data); - } - if (this->dataPtr->tideFlag) - { - this->dataPtr->timeGMT.clear(); - this->dataPtr->tideVelocities.clear(); - if (_msg->tideconstituents == true) - { - this->dataPtr->M2_amp = _msg->m2_amp; - this->dataPtr->M2_phase = _msg->m2_phase; - this->dataPtr->M2_speed = _msg->m2_speed; - this->dataPtr->S2_amp = _msg->s2_amp; - this->dataPtr->S2_phase = _msg->s2_phase; - this->dataPtr->S2_speed = _msg->s2_speed; - this->dataPtr->N2_amp = _msg->n2_amp; - this->dataPtr->N2_phase = _msg->n2_phase; - this->dataPtr->N2_speed = _msg->n2_speed; - this->dataPtr->tide_Constituents = true; - } - else - { - std::array tmpDateVals; - for (int i = 0; i < _msg->time_gmt_year.size(); i++) - { - tmpDateVals[0] = _msg->time_gmt_year[i]; - tmpDateVals[1] = _msg->time_gmt_month[i]; - tmpDateVals[2] = _msg->time_gmt_day[i]; - tmpDateVals[3] = _msg->time_gmt_hour[i]; - tmpDateVals[4] = _msg->time_gmt_minute[i]; - - this->dataPtr->timeGMT.push_back(tmpDateVals); - this->dataPtr->tideVelocities.push_back(_msg->tidevelocities[i]); - } - this->dataPtr->tide_Constituents = false; - } - this->dataPtr->ebbDirection = _msg->ebb_direction; - this->dataPtr->floodDirection = _msg->flood_direction; - this->dataPtr->world_start_time[0] = _msg->world_start_time_year; - this->dataPtr->world_start_time[1] = _msg->world_start_time_month; - this->dataPtr->world_start_time[2] = _msg->world_start_time_day; - this->dataPtr->world_start_time[3] = _msg->world_start_time_hour; - this->dataPtr->world_start_time[4] = _msg->world_start_time_minute; - } - - this->dataPtr->lock_.unlock(); -} - -///////////////////////////////////////////////// -void TransientCurrentPlugin::Gauss_Markov_process_initialize( - const std::shared_ptr & _sdf) -{ - // Read Gauss-Markov parameters - sdf::ElementPtr currentVelocityParams; - - // initialize velocity_north_model parameters - if (currentVelocityParams->HasElement("velocity_north")) - { - sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); - if (elem->HasElement("mean")) - { - this->dataPtr->currentVelNorthModel.mean = 0.0; - } - if (elem->HasElement("mu")) - { - this->dataPtr->currentVelNorthModel.mu = 0.0; - } - if (elem->HasElement("noiseAmp")) - { - this->dataPtr->noiseAmp_North = elem->Get("noiseAmp"); - } - this->dataPtr->currentVelNorthModel.min = - this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; - this->dataPtr->currentVelNorthModel.max = - this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; - if (elem->HasElement("noiseFreq")) - { - this->dataPtr->noiseFreq_North = elem->Get("noiseFreq"); - } - this->dataPtr->currentVelNorthModel.noiseAmp = this->dataPtr->noiseFreq_North; - } - - this->dataPtr->currentVelNorthModel.var = this->dataPtr->currentVelNorthModel.mean; - gzmsg << "For vehicle " << this->dataPtr->modelName - << " -> Current north-direction velocity [m/s] " - << "Gauss-Markov process model:" << std::endl; - this->dataPtr->currentVelNorthModel.Print(); - - // initialize velocity_east_model parameters - if (currentVelocityParams->HasElement("velocity_east")) - { - sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); - if (elem->HasElement("mean")) - { - this->dataPtr->currentVelEastModel.mean = 0.0; - } - if (elem->HasElement("mu")) - { - this->dataPtr->currentVelEastModel.mu = 0.0; - } - if (elem->HasElement("noiseAmp")) - { - this->dataPtr->noiseAmp_East = elem->Get("noiseAmp"); - } - this->dataPtr->currentVelEastModel.min = - this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; - this->dataPtr->currentVelEastModel.max = - this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; - if (elem->HasElement("noiseFreq")) - { - this->dataPtr->noiseFreq_East = elem->Get("noiseFreq"); - } - this->dataPtr->currentVelEastModel.noiseAmp = this->dataPtr->noiseFreq_East; - } - - this->dataPtr->currentVelEastModel.var = this->dataPtr->currentVelEastModel.mean; - gzmsg << "For vehicle " << this->dataPtr->modelName - << " -> Current east-direction velocity [m/s] " - << "Gauss-Markov process model:" << std::endl; - this->dataPtr->currentVelEastModel.Print(); - - // initialize velocity_down_model parameters - if (currentVelocityParams->HasElement("velocity_down")) - { - sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_down"); - if (elem->HasElement("mean")) - { - this->dataPtr->currentVelDownModel.mean = 0.0; - } - if (elem->HasElement("mu")) - { - this->dataPtr->currentVelDownModel.mu = 0.0; - } - if (elem->HasElement("noiseAmp")) - { - this->dataPtr->noiseAmp_Down = elem->Get("noiseAmp"); - } - this->dataPtr->currentVelDownModel.min = - this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; - this->dataPtr->currentVelDownModel.max = - this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; - if (elem->HasElement("noiseFreq")) - { - this->dataPtr->noiseFreq_Down = elem->Get("noiseFreq"); - } - this->dataPtr->currentVelDownModel.noiseAmp = this->dataPtr->noiseFreq_Down; - } - - this->dataPtr->currentVelDownModel.var = this->dataPtr->currentVelDownModel.mean; - gzmsg << "For vehicle " << this->dataPtr->modelName << " -> Current down-direction velocity [m/s]" - << "Gauss-Markov process model:" << std::endl; - this->dataPtr->currentVelDownModel.Print(); - - this->dataPtr->currentVelNorthModel.lastUpdate = this->dataPtr->lastUpdate.count(); - this->dataPtr->currentVelEastModel.lastUpdate = this->dataPtr->lastUpdate.count(); - this->dataPtr->currentVelDownModel.lastUpdate = this->dataPtr->lastUpdate.count(); -} - -///////////////////////////////////////////////// -// void TransientCurrentPlugin::ApllyModelForces(){} - -///////////////////////////////////////////////// -// void TransientCurrentPlugin::PreUpdate(){ - -// // Apply force to the model -// } +// ---------------------------------------------- ///////////////////////////////////////////////// -void TransientCurrentPlugin::Update( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ - // Update vehicle position - gz::math::Pose3d vehicle_pos = GetModelPose(this->dataPtr->modelEntity, _ecm); - double vehicleDepth = std::abs(vehicle_pos.Z()); - this->dataPtr->time = this->dataPtr->lastUpdate; - CalculateOceanCurrent(vehicleDepth); -} -///////////////////////////////////////////////// -void TransientCurrentPlugin::PostUpdate( +void OceanCurrentModelPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Publish the Current Velocity. TODO find if we really need this @@ -630,8 +613,33 @@ void TransientCurrentPlugin::PostUpdate( if (_info.iterations % 1000 == 0) { - gzmsg << "dave_gz_model_plugins::TransientCurrentPlugin::PostUpdate" << std::endl; + gzmsg << "dave_gz_model_plugins::OceanCurrentModelPlugin::PostUpdate" << std::endl; } } } -} // namespace dave_gz_model_plugins \ No newline at end of file + +///////////////////////////////////////////////// +void OceanCurrentModelPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & _info) +{ + geometry_msgs::msg::TwistStamped flowVelMsg; + flowVelMsg.header.stamp.sec = + std::chrono::duration_cast(_info.simTime).count(); + flowVelMsg.header.frame_id = "/world"; + flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + this->dataPtr->flowVelocityPub->publish(flowVelMsg); + + // Generate and publish Gazebo topic according to the vehicle depth + gz::msgs::Vector3d currentVel; + currentVel.set_x(this->dataPtr->currentVelocity.X()); + currentVel.set_y(this->dataPtr->currentVelocity.Y()); + currentVel.set_z(this->dataPtr->currentVelocity.Z()); + this->dataPtr->gz_current_vel_pub.Publish(currentVel); +} + +} // namespace dave_gz_model_plugins + +// Notes - +// 1. Relplace "TransientCurrentPlugin" with OceanCurrentModelPlugin +// 2. \ No newline at end of file diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 2b33380c..962f0f29 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -600,29 +600,35 @@ + filename="OceanCurrentModelPlugin" + name="dave_gz_model_plugins::OceanCurrentModelPlugin"> rexrov - - + hydrodynamics/current_velocity + base_link $(arg namespace)/base_link stratified_current_velocity + 0.0 + 0.0 0.3 0.0 + 0.0 + 0.0 0.3 0.0 + 0.0 + 0.0 0.3 0.0 false - + \ No newline at end of file From cb27f22648142f1b5421294ca423653821ba9cb6 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 7 Oct 2024 04:28:10 +0530 Subject: [PATCH 56/79] major restructuring and refreshed code --- gazebo/dave_gz_world_plugins/CMakeLists.txt | 12 +- ...d_plugin.hh => OceanCurrentWorldPlugin.hh} | 90 ++++---- ...d_plugin.cc => OceanCurrentWorldPlugin.cc} | 210 +++++++++--------- gazebo/dave_ros_gz_plugins/CMakeLists.txt | 10 +- ...urrent_plugin.hh => OceanCurrentPlugin.hh} | 33 ++- ...urrent_plugin.cc => OceanCurrentPlugin.cc} | 198 ++++++++++------- models/dave_worlds/worlds/ocp.world | 207 +++++++++++++++++ 7 files changed, 508 insertions(+), 252 deletions(-) rename gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/{ocean_current_world_plugin.hh => OceanCurrentWorldPlugin.hh} (56%) rename gazebo/dave_gz_world_plugins/src/{ocean_current_world_plugin.cc => OceanCurrentWorldPlugin.cc} (86%) rename gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/{ocean_current_plugin.hh => OceanCurrentPlugin.hh} (85%) rename gazebo/dave_ros_gz_plugins/src/{ocean_current_plugin.cc => OceanCurrentPlugin.cc} (80%) create mode 100755 models/dave_worlds/worlds/ocp.world diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 598555bd..d71c1633 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -26,8 +26,8 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "compiling against Gazebo Harmonic") -add_library(ocean_current_world_plugin SHARED - src/ocean_current_world_plugin.cc +add_library(OceanCurrentWorldPlugin SHARED + src/OceanCurrentWorldPlugin.cc ) add_library(gauss_markov_process SHARED src/gauss_markov_process.cc @@ -36,7 +36,7 @@ add_library(tidal_oscillation SHARED src/tidal_oscillation.cc ) -target_include_directories(ocean_current_world_plugin +target_include_directories(OceanCurrentWorldPlugin PUBLIC $ $ @@ -59,7 +59,7 @@ target_include_directories(tidal_oscillation ${CMAKE_CURRENT_SOURCE_DIR}/src ) -target_link_libraries(ocean_current_world_plugin +target_link_libraries(OceanCurrentWorldPlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) target_link_libraries(gauss_markov_process @@ -69,7 +69,7 @@ target_link_libraries(tidal_oscillation gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) -ament_target_dependencies(ocean_current_world_plugin +ament_target_dependencies(OceanCurrentWorldPlugin rclcpp std_msgs geometry_msgs @@ -98,7 +98,7 @@ install(DIRECTORY include/ # Install targets install( - TARGETS ocean_current_world_plugin gauss_markov_process tidal_oscillation + TARGETS OceanCurrentWorldPlugin gauss_markov_process tidal_oscillation EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh similarity index 56% rename from gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh rename to gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 34049e60..f4ccb31d 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/ocean_current_world_plugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -25,7 +25,7 @@ namespace dave_gz_world_plugins // typedef const boost::shared_ptr AnyPtr; // public WorldPlugin, -class UnderwaterCurrentPlugin : public gz::sim::System, +class OceanCurrentWorldPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemUpdate, public gz::sim::ISystemPostUpdate @@ -33,79 +33,81 @@ class UnderwaterCurrentPlugin : public gz::sim::System, { public: - UnderwaterCurrentPlugin(); - ~UnderwaterCurrentPlugin(); + OceanCurrentWorldPlugin(); + ~OceanCurrentWorldPlugin(); + + // ---------------------------------------------- - // Documentation inherited. void Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); + virtual void LoadTidalOscillationDatabase(); + + virtual void LoadStratifiedCurrentDatabase(); + + virtual void LoadGlobalCurrentConfig(); + // Documentation inherited. virtual void Init(); - void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + // ---------------------------------------------- - void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - /// \brief Load global current velocity configuration - virtual void LoadGlobalCurrentConfig(); + // ---------------------------------------------- - /// \brief Load stratified current velocity database - virtual void LoadStratifiedCurrentDatabase(); - - /// \brief Load tidal oscillation database - virtual void LoadTidalOscillationDatabase(); + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - /// \brief Publish current velocity and the pose of its frame void PublishCurrentVelocity(); - /// \brief Publish stratified oceqan current velocity void PublishStratifiedCurrentVelocity(); + // ---------------------------------------------- struct SharedData { - dave_gz_world_plugins::GaussMarkovProcess currentHorzAngleModel; - /// \brief Gauss-Markov process instance for the current velocity - dave_gz_world_plugins::GaussMarkovProcess currentVelModel; - dave_gz_world_plugins::GaussMarkovProcess currentVertAngleModel; - - /// \brief Vector for read stratified current database values - std::vector stratifiedDatabase; // check - std::vector> stratifiedCurrentModels; - bool tidalHarmonicFlag; + // Gauss-Markov process instances for current models + dave_gz_world_plugins::GaussMarkovProcess currentHorzAngleModel; // Horizontal angle + dave_gz_world_plugins::GaussMarkovProcess currentVelModel; // Velocity + dave_gz_world_plugins::GaussMarkovProcess currentVertAngleModel; // Vertical angle - /// \brief Current linear velocity vector - gz::math::Vector3d currentVelocity; + // Stratified current database and models + std::vector stratifiedDatabase; // Database values for stratified currents + std::vector> + stratifiedCurrentModels; // Stratified current models - /// \brief Vector of current depth-specific linear velocity vectors - std::vector currentStratifiedVelocity; + // Tidal harmonic flag + bool tidalHarmonicFlag; - double M2_amp; - double M2_phase; - double M2_speed; - double S2_amp; - double S2_phase; - double S2_speed; - double N2_amp; - double N2_phase; - double N2_speed; + // Current velocity information + gz::math::Vector3d currentVelocity; // Current linear velocity vector + std::vector + currentStratifiedVelocity; // Depth-specific linear velocity vectors for stratified current - /// \brief Tidal oscillation mean ebb direction - double ebbDirection; + // Tidal harmonic data (M2, S2, N2 constituents) + double M2_amp, M2_phase, M2_speed; // M2 tidal constituent + double S2_amp, S2_phase, S2_speed; // S2 tidal constituent + double N2_amp, N2_phase, N2_speed; // N2 tidal constituent - /// \brief Tidal oscillation mean flood direction - double floodDirection; + // Tidal oscillation mean directions + double ebbDirection; // Mean ebb direction + double floodDirection; // Mean flood direction - /// \brief Tidal oscillation world start time (GMT) + // Tidal oscillation world start time (GMT) int world_start_time_day; int world_start_time_month; int world_start_time_year; int world_start_time_hour; int world_start_time_minute; - std::vector> dateGMT; - std::vector speedcmsec; + // Topics + std::string currentVelocityTopic; + std::string stratifiedCurrentVelocityTopic; + // std::string vehicleDepthTopic; + + // Date and speed information + std::vector> dateGMT; // Date in GMT (year, month, day, hour, minute) + std::vector speedcmsec; // Speed in cm/sec }; std::unique_ptr sharedDataPtr; diff --git a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc similarity index 86% rename from gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc rename to gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 214ec120..01b25375 100644 --- a/gazebo/dave_gz_world_plugins/src/ocean_current_world_plugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -1,14 +1,11 @@ -#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" +// Include the necessary headers for Ocean Current Plugin +#include "dave_gz_world_plugins/OceanCurrentWorldPlugin.hh" + +// Include messages for Stratified Current Velocity #include +// Include Gazebo and Ignition libraries #include -#include -#include // TODO (235-239) -#include -#include -#include -#include -#include #include #include #include @@ -16,86 +13,89 @@ #include #include #include +#include +#include #include + +// Boost libraries for string manipulation, binding, and shared pointers +#include +#include +#include +#include + +// Other necessary libraries +#include +#include #include -#include #include -#include "dave_gz_world_plugins/gauss_markov_process.hh" -#include "dave_gz_world_plugins/tidal_oscillation.hh" -#include "gz/plugin/Register.hh" -#include "gz/sim/components/Model.hh" -#include "gz/sim/components/World.hh" + +// AMENT library for package directory functions +#include + +// Custom plugin headers +#include "dave_gz_world_plugins/gauss_markov_process.hh" // For Gauss-Markov process +#include "dave_gz_world_plugins/tidal_oscillation.hh" // For tidal oscillation modeling GZ_ADD_PLUGIN( - dave_gz_world_plugins::UnderwaterCurrentPlugin, gz::sim::System, - dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemConfigure, - dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemUpdate, - dave_gz_world_plugins::UnderwaterCurrentPlugin::ISystemPostUpdate) + dave_gz_world_plugins::OceanCurrentWorldPlugin, gz::sim::System, + dave_gz_world_plugins::OceanCurrentWorldPlugin::ISystemConfigure, + dave_gz_world_plugins::OceanCurrentWorldPlugin::ISystemUpdate, + dave_gz_world_plugins::OceanCurrentWorldPlugin::ISystemPostUpdate) namespace dave_gz_world_plugins { -struct UnderwaterCurrentPlugin::PrivateData +struct OceanCurrentWorldPlugin::PrivateData { - gz::sim::World world{gz::sim::kNullEntity}; - sdf::ElementPtr sdf; - bool hasSurface; - - /// \brief Pointer to a node for communication - std::shared_ptr gz_node; - - /// \brief Map of publishers - // gz::transport::Node::Publisher publishers; - gz::transport::Node::Publisher gz_node_cvel_world_pub; - - /// \brief Vehicle Depth Subscriber - gz::transport::Node subscriber; - - /// \brief Current velocity topic - std::string currentVelocityTopic; + // Gazebo Simulation and World Components + gz::sim::World world{gz::sim::kNullEntity}; // Gazebo world instance + sdf::ElementPtr sdf; // Pointer to the SDF element + bool hasSurface; // Flag to check if surface is present - /// \brief Stratified Ocean current topic - std::string stratifiedCurrentVelocityTopic; + // Communication Node for Gazebo + std::shared_ptr gz_node; // Transport node for communication - /// \brief Vehicle depth topic - std::string vehicleDepthTopic; + // Publishers + gz::transport::Node::Publisher gz_node_cvel_world_pub; // Publisher for current velocity - /// \brief Ocean Current Database file path for csv file - std::string databaseFilePath; + // Subscribers + gz::transport::Node subscriber; // Subscriber for vehicle depth - /// \brief Tidal Oscillation Database file path for txt file - std::string tidalFilePath; + // Database File Paths + std::string databaseFilePath; // Path to ocean current database (CSV file) + std::string tidalFilePath; // Path to tidal oscillation database (TXT file) - /// \brief Namespace for topics and services - std::string ns; + // Namespace for Topics and Services + std::string ns; // Namespace for topics and services - /// \brief Tidal Oscillation flag + // Tidal Oscillation Variables bool tideFlag; - - /// \brief Tidal Oscillation interpolation model dave_gz_world_plugins::TidalOscillation tide; - /// \brief Last update time stamp + // Timing std::chrono::steady_clock::duration lastUpdate{0}; - /// \brief File path for stratified current database - std::string db_path; + // Stratified Current Database Path + std::string db_path; // Path to the stratified current database + // ROS 2 Node std::shared_ptr rosNode; }; ///////////////////////////////////////////////// -UnderwaterCurrentPlugin::UnderwaterCurrentPlugin() +OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() : dataPtr(std::make_unique()), sharedDataPtr(std::make_unique()) { // this->sharedDataPtr = std::make_unique(); } ///////////////////////////////////////////////// -UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin() { this->dataPtr->rosNode.reset(); } +OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() { this->dataPtr->rosNode.reset(); } + +// ---------------------------------------------- ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::Configure( +void OceanCurrentWorldPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { @@ -138,16 +138,15 @@ void UnderwaterCurrentPlugin::Configure( } ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() +void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() { this->dataPtr->tideFlag = true; this->sharedDataPtr->tidalHarmonicFlag = false; - sdf::ElementPtr tidalOscillationParams = - this->dataPtr->sdf->GetElement("tidal_oscillation"); // include this xml/ (TODO) + sdf::ElementPtr tidalOscillationParams = this->dataPtr->sdf->GetElement("tidal_oscillation"); sdf::ElementPtr tidalHarmonicParams; - // Read the tidal oscillation parameter from the SDF file // (TODO)(TO UNDERSTAND) + // Read the tidal oscillation parameter from the SDF file if (tidalOscillationParams->HasElement("databasefilePath")) { this->dataPtr->tidalFilePath = tidalOscillationParams->Get("databasefilePath"); @@ -307,7 +306,7 @@ void UnderwaterCurrentPlugin::LoadTidalOscillationDatabase() } ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() +void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() { GZ_ASSERT( this->dataPtr->sdf->HasElement("transient_current"), @@ -316,16 +315,16 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() if (transientCurrentParams->HasElement("topic_stratified")) { - this->dataPtr->stratifiedCurrentVelocityTopic = + this->sharedDataPtr->stratifiedCurrentVelocityTopic = transientCurrentParams->Get("topic_stratified"); } else { - this->dataPtr->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; + this->sharedDataPtr->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; } GZ_ASSERT( - !this->dataPtr->stratifiedCurrentVelocityTopic.empty(), + !this->sharedDataPtr->stratifiedCurrentVelocityTopic.empty(), "Empty stratified ocean current velocity topic"); // Read the depth dependent ocean current file path from the SDF file @@ -430,14 +429,15 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase() this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node->Advertise( - this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic); + this->dataPtr->ns + "/" + this->sharedDataPtr->stratifiedCurrentVelocityTopic); gzmsg << "Stratified current velocity topic name: " - << this->dataPtr->ns + "/" + this->dataPtr->stratifiedCurrentVelocityTopic << std::endl; + << this->dataPtr->ns + "/" + this->sharedDataPtr->stratifiedCurrentVelocityTopic + << std::endl; } ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() +void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() { // NOTE: The plugin currently requires stratified current, so the // global current set up in this method is potentially @@ -452,14 +452,15 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() // Read the topic names from the SDF file if (currentVelocityParams->HasElement("topic")) { - this->dataPtr->currentVelocityTopic = currentVelocityParams->Get("topic"); + this->sharedDataPtr->currentVelocityTopic = currentVelocityParams->Get("topic"); } else { - this->dataPtr->currentVelocityTopic = "current_velocity"; + this->sharedDataPtr->currentVelocityTopic = "current_velocity"; } - GZ_ASSERT(!this->dataPtr->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); + GZ_ASSERT( + !this->sharedDataPtr->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); if (currentVelocityParams->HasElement("velocity")) { @@ -618,44 +619,15 @@ void UnderwaterCurrentPlugin::LoadGlobalCurrentConfig() // Advertise the current velocity & stratified current velocity topics this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node->Advertise( - this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic); + this->dataPtr->ns + "/" + this->sharedDataPtr->currentVelocityTopic); gzmsg << "Current velocity topic name: " - << this->dataPtr->ns + "/" + this->dataPtr->currentVelocityTopic << std::endl; + << this->dataPtr->ns + "/" + this->sharedDataPtr->currentVelocityTopic << std::endl; } -///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::PublishCurrentVelocity() -{ - gz::msgs::Vector3d currentVel; - gz::msgs::Set( - ¤tVel, - gz::math::Vector3d( - this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), - this->sharedDataPtr->currentVelocity.Z())); - this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); -} - -///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity() -{ - dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check - for (std::vector::iterator it = - this->sharedDataPtr->currentStratifiedVelocity.begin(); - it != this->sharedDataPtr->currentStratifiedVelocity.end(); - ++it) // currentStratifiedVelocity values defined where ? (TODO) - { - gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); - currentVel.add_depth(it->W()); - } - if (currentVel.velocity_size() == 0) - { - return; - } - this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); -} +// ---------------------------------------------- ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::Update( +void OceanCurrentWorldPlugin::Update( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { // Update the time @@ -692,8 +664,41 @@ void UnderwaterCurrentPlugin::Update( } } +// ---------------------------------------------- + +///////////////////////////////////////////////// +void OceanCurrentWorldPlugin::PublishCurrentVelocity() +{ + gz::msgs::Vector3d currentVel; + gz::msgs::Set( + ¤tVel, + gz::math::Vector3d( + this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), + this->sharedDataPtr->currentVelocity.Z())); + this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); +} + +///////////////////////////////////////////////// +void OceanCurrentWorldPlugin::PublishStratifiedCurrentVelocity() +{ + dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check + for (std::vector::iterator it = + this->sharedDataPtr->currentStratifiedVelocity.begin(); + it != this->sharedDataPtr->currentStratifiedVelocity.end(); + ++it) // currentStratifiedVelocity values defined where ? (TODO) + { + gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); + currentVel.add_depth(it->W()); + } + if (currentVel.velocity_size() == 0) + { + return; + } + this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); +} + ///////////////////////////////////////////////// -void UnderwaterCurrentPlugin::PostUpdate( +void OceanCurrentWorldPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { // Update time stamp @@ -701,4 +706,5 @@ void UnderwaterCurrentPlugin::PostUpdate( PublishCurrentVelocity(); PublishStratifiedCurrentVelocity(); } + } // namespace dave_gz_world_plugins \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt index d6208fb7..b009436c 100644 --- a/gazebo/dave_ros_gz_plugins/CMakeLists.txt +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -23,7 +23,7 @@ message(STATUS "Compiling against Gazebo Harmonic") # Libraries add_library(SphericalCoords SHARED src/SphericalCoords.cc) -add_library(ocean_current_plugin SHARED src/ocean_current_plugin.cc) +add_library(OceanCurrentPlugin SHARED src/OceanCurrentPlugin.cc) # Include directories target_include_directories(SphericalCoords @@ -33,7 +33,7 @@ target_include_directories(SphericalCoords PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ) -target_include_directories(ocean_current_plugin +target_include_directories(OceanCurrentPlugin PUBLIC $ $ @@ -47,7 +47,7 @@ target_include_directories(ocean_current_plugin target_link_libraries(SphericalCoords gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) -target_link_libraries(ocean_current_plugin +target_link_libraries(OceanCurrentPlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) @@ -59,7 +59,7 @@ ament_target_dependencies(SphericalCoords std_msgs ) -ament_target_dependencies(ocean_current_plugin +ament_target_dependencies(OceanCurrentPlugin rclcpp std_msgs geometry_msgs @@ -74,7 +74,7 @@ install(DIRECTORY include/ # Install targets install( - TARGETS SphericalCoords ocean_current_plugin + TARGETS SphericalCoords OceanCurrentPlugin EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh similarity index 85% rename from gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh rename to gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh index fbcdc215..cb40a3ae 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/ocean_current_plugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh @@ -1,17 +1,26 @@ #ifndef DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ #define DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ +// Gazebo Simulation Related Headers #include + +// ROS 2 Headers #include #include +// Standard Library Headers #include #include #include -#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" +// Plugin-specific Headers +#include "dave_gz_world_plugins/OceanCurrentWorldPlugin.hh" + +// Dave Interfaces: Message Types #include "dave_interfaces/msg/Stratified_Current_Database.hpp" #include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" + +// Dave Interfaces: Service Types #include "dave_interfaces/srv/Get_Current_Model.hpp" #include "dave_interfaces/srv/Get_Origin_Spherical_Coord.hpp" #include "dave_interfaces/srv/Set_Current_Direction.hpp" @@ -26,28 +35,32 @@ namespace dave_ros_gz_plugins { -class UnderwaterCurrentROSPlugin : public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemPostUpdate +class OceanCurrentPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { public: - UnderwaterCurrentROSPlugin(); - ~UnderwaterCurrentROSPlugin(); + OceanCurrentPlugin(); + ~OceanCurrentPlugin(); + + // ---------------------------------------------- - // Documentation inherited void Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - // void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + // ---------------------------------------------- - // // Documentation inherited + // void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); // void PreUpdate( // const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; - // Documentation inherited + // ---------------------------------------------- + void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); + // ---------------------------------------------- + bool UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res); diff --git a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc similarity index 80% rename from gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc rename to gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index c0a06bad..dc6acc35 100644 --- a/gazebo/dave_ros_gz_plugins/src/ocean_current_plugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -1,62 +1,90 @@ -#include "dave_ros_gz_plugins/ocean_current_plugin.hh" +// Standard Library Headers #include -#include #include #include +#include +#include + +// Boost Library Header +#include + +// ROS 2 Headers #include +#include +#include +#include + +// Gazebo Physics and Simulation Headers #include +#include #include #include #include -#include -#include -#include -#include -#include +#include + +// Dave Gazebo World Plugins +#include "dave_gz_world_plugins/OceanCurrentWorldPlugin.hh" #include "dave_gz_world_plugins/gauss_markov_process.hh" -#include "dave_gz_world_plugins/ocean_current_world_plugin.hh" -#include "gz/plugin/Register.hh" -#include "gz/sim/components/World.hh" +// Dave ROS-Gazebo Plugin +#include "dave_ros_gz_plugins/OceanCurrentPlugin.hh" GZ_ADD_PLUGIN( - dave_ros_gz_plugins::UnderwaterCurrentROSPlugin, gz::sim::System, - dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::ISystemConfigure, - dave_ros_gz_plugins::UnderwaterCurrentROSPlugin::ISystemPostUpdate) + dave_ros_gz_plugins::OceanCurrentPlugin, gz::sim::System, + dave_ros_gz_plugins::OceanCurrentPlugin::ISystemConfigure, + dave_ros_gz_plugins::OceanCurrentPlugin::ISystemPostUpdate) namespace dave_ros_gz_plugins { -struct UnderwaterCurrentROSPlugin::PrivateData +struct OceanCurrentPlugin::PrivateData { // std::string db_path; (check)(TODO) // this->dataPtr->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); - std::chrono::steady_clock::duration lastUpdate{0}; // // rclcpp::Service::SharedPtr; //Template + + // Time management + std::chrono::steady_clock::duration lastUpdate{0}; + + // ROS 2 Services for Current Models rclcpp::Service::SharedPtr get_current_velocity_model; rclcpp::Service::SharedPtr get_current_horz_angle_model; rclcpp::Service::SharedPtr get_current_vert_angle_model; rclcpp::Service::SharedPtr set_current_velocity_model; rclcpp::Service::SharedPtr set_current_horz_angle_model; rclcpp::Service::SharedPtr set_current_vert_angle_model; + + // ROS 2 Services for Current Velocity and Directions rclcpp::Service::SharedPtr set_current_velocity; rclcpp::Service::SharedPtr set_current_horz_angle; rclcpp::Service::SharedPtr set_current_vert_angle; + + // ROS 2 Services for Stratified Current Models rclcpp::Service::SharedPtr set_stratified_current_velocity; rclcpp::Service::SharedPtr set_stratified_current_horz_angle; rclcpp::Service::SharedPtr set_stratified_current_vert_angle; + + // ROS Node std::shared_ptr rosNode; + + // Gazebo Simulation Objects gz::sim::World world; gz::sim::Model model; gz::sim::Entity entity; // Added entity member + + // Model and SDF Parameters std::string modelName; std::shared_ptr sdf; - std::string stratifiedCurrentVelocityTopic; + + // Topic Names for Publishing Data + // std::string stratifiedCurrentVelocityTopic; std::string stratifiedCurrentVelocityDatabaseTopic; - std::string currentVelocityTopic; + // std::string currentVelocityTopic; std::string model_namespace; + + // ROS 2 Publishers rclcpp::Publisher::SharedPtr flowVelocityPub; rclcpp::Publisher::SharedPtr stratifiedCurrentVelocityPub; @@ -65,13 +93,11 @@ struct UnderwaterCurrentROSPlugin::PrivateData }; ///////////////////////////////////////////////// -UnderwaterCurrentROSPlugin::UnderwaterCurrentROSPlugin() : dataPtr(std::make_unique()) -{ -} -UnderwaterCurrentROSPlugin::~UnderwaterCurrentROSPlugin() {} +OceanCurrentPlugin::OceanCurrentPlugin() : dataPtr(std::make_unique()) {} +OceanCurrentPlugin::~OceanCurrentPlugin() {} ///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::Configure( +void OceanCurrentPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { @@ -79,27 +105,35 @@ void UnderwaterCurrentROSPlugin::Configure( { rclcpp::init(0, nullptr); // gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your - // " - // "ROS 2 environment.\n"; + // ROS 2 environment."; } + // Initialize the ROS 2 node this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin"); + + // Retrieve the Gazebo world entity auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); this->dataPtr->world = gz::sim::World(worldEntity); + // Set model entity this->dataPtr->entity = _entity; - this->dataPtr->model = gz::sim::Model(_entity); this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); + + // Save the SDF pointer this->dataPtr->sdf = _sdf; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + + // Set the topic for the stratified current velocity database this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = - this->dataPtr->stratifiedCurrentVelocityTopic + "_database"; + data.stratifiedCurrentVelocityTopic + "_database"; + // Retrieve the model namespace from the SDF if it exists this->dataPtr->model_namespace = _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; - // Initialising ROS 2 node + // Reinitialize the ROS 2 node with the model namespace this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin", this->dataPtr->model_namespace); @@ -107,114 +141,108 @@ void UnderwaterCurrentROSPlugin::Configure( // Advertise the flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->dataPtr->rosNode->create_publisher( - this->dataPtr->currentVelocityTopic, rclcpp::QoS(10)); + data.currentVelocityTopic, rclcpp::QoS(10)); // Advertise the stratified ocean current message this->dataPtr->stratifiedCurrentVelocityPub = this->dataPtr->rosNode->create_publisher( - this->dataPtr->stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); + data.stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); // Advertise the stratified ocean current database message this->dataPtr->stratifiedCurrentDatabasePub = this->dataPtr->rosNode->create_publisher( this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); - // Create and advertise services - // In this setup : std::placeholders::_1 and std::placeholders::_2 are used to represent the - // request and response objects that the service callback expects to receive. This allows the - // serviceCallback method of to be used directly as a ROS 2 service handler without - // having to wrap it in another function that manually passes the request and response. - // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = this->dataPtr->rosNode->create_service( "get_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentVelocityModel, this, + &OceanCurrentPlugin::GetCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current horizontal angle model this->dataPtr->get_current_horz_angle_model = this->dataPtr->rosNode->create_service( "get_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel, this, + &OceanCurrentPlugin::GetCurrentHorzAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to get the current vertical angle model this->dataPtr->get_current_vert_angle_model = this->dataPtr->rosNode->create_service( "get_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel, this, + &OceanCurrentPlugin::GetCurrentVertAngleModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current velocity model this->dataPtr->set_current_velocity_model = this->dataPtr->rosNode->create_service( "set_current_velocity_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel, this, + &OceanCurrentPlugin::UpdateCurrentVelocityModel, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle model this->dataPtr->set_current_horz_angle_model = this->dataPtr->rosNode->create_service( "set_current_horz_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); + &OceanCurrentPlugin::UpdateCurrentHorzAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current vertical angle model this->dataPtr->set_current_vert_angle_model = this->dataPtr->rosNode->create_service( "set_current_vert_angle_model", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel, - this, std::placeholders::_1, std::placeholders::_2)); + &OceanCurrentPlugin::UpdateCurrentVertAngleModel, this, + std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current velocity mean value this->dataPtr->set_current_velocity = this->dataPtr->rosNode->create_service( "set_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateCurrentVelocity, this, + &OceanCurrentPlugin::UpdateCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current velocity this->dataPtr->set_stratified_current_velocity = this->dataPtr->rosNode->create_service( "set_stratified_current_velocity", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity, - this, std::placeholders::_1, std::placeholders::_2)); + &OceanCurrentPlugin::UpdateStratCurrentVelocity, this, + std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle //TODO this->dataPtr->set_current_horz_angle = this->dataPtr->rosNode->create_service( - "set_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateHorzAngle, this, - std::placeholders::_1, std::placeholders::_2)); + "set_current_horz_angle", + std::bind( + &OceanCurrentPlugin::UpdateHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the current horizontal angle //TODO this->dataPtr->set_current_vert_angle = this->dataPtr->rosNode->create_service( - "set_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateVertAngle, this, - std::placeholders::_1, std::placeholders::_2)); + "set_current_vert_angle", + std::bind( + &OceanCurrentPlugin::UpdateVertAngle, this, std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current horizontal angle this->dataPtr->set_stratified_current_horz_angle = this->dataPtr->rosNode->create_service( "set_stratified_current_horz_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratHorzAngle, - this, std::placeholders::_1, std::placeholders::_2)); + &OceanCurrentPlugin::UpdateStratHorzAngle, this, + std::placeholders::_1, std::placeholders::_2)); // // Advertise the service to update the current vertical angle // this->dataPtr->set_current_vert_angle_model = // this->dataPtr->rosNode->create_service( // "set_current_vert_angle_model", std::bind( - // &UnderwaterCurrentROSPlugin::UpdateVertAngleModel, this, + // &OceanCurrentPlugin::UpdateVertAngleModel, this, // std::placeholders::_1, std::placeholders::_2)); // Advertise the service to update the stratified current vertical angle this->dataPtr->set_stratified_current_vert_angle = this->dataPtr->rosNode->create_service( "set_stratified_current_vert_angle", std::bind( - &UnderwaterCurrentROSPlugin::UpdateStratVertAngle, - this, std::placeholders::_1, std::placeholders::_2)); + &OceanCurrentPlugin::UpdateStratVertAngle, this, + std::placeholders::_1, std::placeholders::_2)); // Connect to the Gazebo Harmonic update event // this->dataPtr->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & @@ -224,12 +252,12 @@ void UnderwaterCurrentROSPlugin::Configure( } ///////////////////////////////////////////////// -// void UnderwaterCurrentROSPlugin::PreUpdate( +// void OceanCurrentPlugin::PreUpdate( // const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) // { // } // ///////////////////////////////////////////////// -// void UnderwaterCurrentROSPlugin::Update( +// void OceanCurrentPlugin::Update( // const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) // { // if (_info.simTime > this->dataPtr->lastUpdate) @@ -240,21 +268,21 @@ void UnderwaterCurrentROSPlugin::Configure( // namespace dave_ros_gz_plugins ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateHorzAngle( +bool OceanCurrentPlugin::UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->success = data.currentHorzAngleModel.SetMean(_req->angle); return true; } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( +bool OceanCurrentPlugin::UpdateStratHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; if (_req->layer >= data.stratifiedDatabase.size()) { @@ -275,21 +303,21 @@ bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateVertAngle( +bool OceanCurrentPlugin::UpdateVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->success = data.currentVertAngleModel.SetMean(_req->angle); return true; } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( +bool OceanCurrentPlugin::UpdateStratVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; if (_req->layer >= data.stratifiedDatabase.size()) { @@ -301,11 +329,11 @@ bool UnderwaterCurrentROSPlugin::UpdateStratVertAngle( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( +bool OceanCurrentPlugin::UpdateCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; if ( data.currentVelModel.SetMean(_req->velocity) && @@ -327,11 +355,11 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocity( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( +bool OceanCurrentPlugin::UpdateStratCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; if (_req->layer >= data.stratifiedDatabase.size()) { @@ -361,11 +389,11 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( +bool OceanCurrentPlugin::GetCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->mean = data.currentVelModel.mean; _res->min = data.currentVelModel.min; @@ -376,11 +404,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVelocityModel( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( +bool OceanCurrentPlugin::GetCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->mean = data.currentHorzAngleModel.mean; _res->min = data.currentHorzAngleModel.min; @@ -391,11 +419,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( +bool OceanCurrentPlugin::GetCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->mean = data.currentVertAngleModel.mean; _res->min = data.currentVertAngleModel.min; @@ -406,11 +434,11 @@ bool UnderwaterCurrentROSPlugin::GetCurrentVertAngleModel( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( +bool OceanCurrentPlugin::UpdateCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->success = data.currentVelModel.SetModel( std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, @@ -431,11 +459,11 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel( return true; } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( +bool OceanCurrentPlugin::UpdateCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->success = data.currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); @@ -458,11 +486,11 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel( } ///////////////////////////////////////////////// -bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( +bool OceanCurrentPlugin::UpdateCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; _res->success = data.currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); @@ -474,10 +502,10 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVertAngleModel( } ///////////////////////////////////////////////// -void UnderwaterCurrentROSPlugin::PostUpdate( +void OceanCurrentPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - dave_gz_world_plugins::UnderwaterCurrentPlugin::SharedData data; + dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; // Generate and publish current velocity according to the vehicle depth geometry_msgs::msg::TwistStamped flowVelMsg; flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world new file mode 100755 index 00000000..5af7a0fd --- /dev/null +++ b/models/dave_worlds/worlds/ocp.world @@ -0,0 +1,207 @@ + + + + + + + + 0.01 0.01 0.01 1.0 + + + 12 + + + 1 + + + 0.001 + 1.0 + + + + + + + + + + + 56.71897669633431 + 3.515625 + + + + + 50 0 150 0 0 0 + 1 1 1 1 + .1 .1 .1 1 + 0.3 0.3 -1 + false + + + + -50 0 -150 0 0 0 + 0.6 0.6 0.6 1 + 0 0 0 1 + -0.3 -0.3 -1 + false + + + + -100 500 -20 0 0 0 + 0.8 0.8 0.8 1 + 1 1 1 1 + -1 -1 0 + false + + + + -150 -130 50 0 0 0 + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + 0.5 0.5 -1 + false + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Coast Water + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap + 0 0 -95 0 0 0 + + + + + 1025 + + 0 + 1 + + + + + + hydrodynamics + + current_velocity + + 0 + 0 + 5 + 0.0 + 0.0 + + + + 0 + -3.141592653589793238 + 3.141592653589793238 + 0.0 + 0.0 + + + + 0 + -3.141592653589793238 + 3.141592653589793238 + 0.0 + 0.0 + + + + + stratified_current_velocity + + transientOceanCurrentDatabase.csv + + + + + + + ACT1951_1_Annual_2021.csv + + + + + + + + + + + + + 69 + 255 + + + + + 4 + 3 + 2021 + 15 + 0 + + + + + + \ No newline at end of file From df383b3bf59831df49af601119554046e14437a0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 10 Oct 2024 04:45:17 +0530 Subject: [PATCH 57/79] fixed issues with OCWP --- dave_interfaces/CMakeLists.txt | 4 ++++ gazebo/dave_gz_world_plugins/CMakeLists.txt | 19 +++++++++++---- .../OceanCurrentWorldPlugin.hh | 21 ++++++----------- .../src/OceanCurrentWorldPlugin.cc | 23 ++++++++----------- .../dave_ros_gz_plugins/OceanCurrentPlugin.hh | 2 +- .../src/OceanCurrentPlugin.cc | 2 +- models/dave_worlds/worlds/ocp.world | 12 +++++----- 7 files changed, 43 insertions(+), 40 deletions(-) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 6cdf513d..0cd9ad73 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -59,13 +59,17 @@ set(MSGS_PROTOS gz_msgs_generate_messages( # The cmake target to be generated for libraries/executables to link TARGET msgs # pressure_sensor_msgs_gen + # TARGET dave_gz_world_plugins_msgs # The protobuf package to generate (Typically based on the path) PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" + # The path to the base directory of the proto files # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + # List of proto files to generate MSGS_PROTOS ${MSGS_PROTOS} + # List of message targets this library imports from # DEPENDENCIES gz_msgs_gen # Dependency on gz-msgs diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index d71c1633..f96971bc 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -59,8 +59,16 @@ target_include_directories(tidal_oscillation ${CMAKE_CURRENT_SOURCE_DIR}/src ) +# target_link_libraries(OceanCurrentWorldPlugin +# gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} +# ) + target_link_libraries(OceanCurrentWorldPlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + gauss_markov_process # Add this line + tidal_oscillation + # dave_interfaces + # dave_gz_world_plugins_msgs ) target_link_libraries(gauss_markov_process gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} @@ -75,6 +83,7 @@ ament_target_dependencies(OceanCurrentWorldPlugin geometry_msgs dave_interfaces ament_index_cpp + gz-msgs${GZ_MSGS_VER} ) ament_target_dependencies(gauss_markov_process @@ -91,11 +100,6 @@ ament_target_dependencies(tidal_oscillation dave_interfaces ) -# Install headers -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - # Install targets install( TARGETS OceanCurrentWorldPlugin gauss_markov_process tidal_oscillation @@ -106,6 +110,11 @@ install( INCLUDES DESTINATION include ) +# Install headers +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index f4ccb31d..936c19d7 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -15,26 +15,20 @@ #include #include +#include #include #include namespace dave_gz_world_plugins { -/// \brief Class for the underwater current plugin - -// typedef const boost::shared_ptr AnyPtr; - -// public WorldPlugin, class OceanCurrentWorldPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemUpdate, public gz::sim::ISystemPostUpdate -// public gz::sim::WorldPlugin - { public: OceanCurrentWorldPlugin(); - ~OceanCurrentWorldPlugin(); + ~OceanCurrentWorldPlugin() override; // ---------------------------------------------- @@ -42,14 +36,11 @@ public: const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr); - virtual void LoadTidalOscillationDatabase(); + void LoadTidalOscillationDatabase(); - virtual void LoadStratifiedCurrentDatabase(); + void LoadStratifiedCurrentDatabase(); - virtual void LoadGlobalCurrentConfig(); - - // Documentation inherited. - virtual void Init(); + void LoadGlobalCurrentConfig(); // ---------------------------------------------- @@ -64,6 +55,7 @@ public: void PublishStratifiedCurrentVelocity(); // ---------------------------------------------- + struct SharedData { // Gauss-Markov process instances for current models @@ -117,6 +109,7 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; + } // namespace dave_gz_world_plugins #endif // DAVE_GZ_WORLD_PLUGINS__OCEAN_CURRENT_WORLD_PLUGIN_HH_ diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 01b25375..661a753d 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -44,7 +44,6 @@ GZ_ADD_PLUGIN( namespace dave_gz_world_plugins { - struct OceanCurrentWorldPlugin::PrivateData { // Gazebo Simulation and World Components @@ -82,7 +81,6 @@ struct OceanCurrentWorldPlugin::PrivateData std::shared_ptr rosNode; }; -///////////////////////////////////////////////// OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() : dataPtr(std::make_unique()), sharedDataPtr(std::make_unique()) { @@ -90,11 +88,11 @@ OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() } ///////////////////////////////////////////////// -OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() { this->dataPtr->rosNode.reset(); } +OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() = default; +// OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() { this->dataPtr->rosNode.reset(); } // ---------------------------------------------- -///////////////////////////////////////////////// void OceanCurrentWorldPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) @@ -137,7 +135,6 @@ void OceanCurrentWorldPlugin::Configure( << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; } -///////////////////////////////////////////////// void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() { this->dataPtr->tideFlag = true; @@ -334,7 +331,8 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() } else { - this->dataPtr->databaseFilePath = "transientOceanCurrentDatabase.csv"; + this->dataPtr->databaseFilePath = + "/Users/gaurav/dave_ws/src/dave/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv"; } GZ_ASSERT( @@ -665,17 +663,15 @@ void OceanCurrentWorldPlugin::Update( } // ---------------------------------------------- - ///////////////////////////////////////////////// void OceanCurrentWorldPlugin::PublishCurrentVelocity() { - gz::msgs::Vector3d currentVel; + gz::msgs::Vector3d currVel; gz::msgs::Set( - ¤tVel, - gz::math::Vector3d( - this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), - this->sharedDataPtr->currentVelocity.Z())); - this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); + &currVel, gz::math::Vector3d( + this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), + this->sharedDataPtr->currentVelocity.Z())); + this->dataPtr->gz_node_cvel_world_pub.Publish(currVel); } ///////////////////////////////////////////////// @@ -706,5 +702,6 @@ void OceanCurrentWorldPlugin::PostUpdate( PublishCurrentVelocity(); PublishStratifiedCurrentVelocity(); } +// ---------------------------------------------- } // namespace dave_gz_world_plugins \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh index cb40a3ae..2290d876 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh @@ -41,7 +41,7 @@ class OceanCurrentPlugin : public gz::sim::System, { public: OceanCurrentPlugin(); - ~OceanCurrentPlugin(); + ~OceanCurrentPlugin() override; // ---------------------------------------------- diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index dc6acc35..401b7c87 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -94,7 +94,7 @@ struct OceanCurrentPlugin::PrivateData ///////////////////////////////////////////////// OceanCurrentPlugin::OceanCurrentPlugin() : dataPtr(std::make_unique()) {} -OceanCurrentPlugin::~OceanCurrentPlugin() {} +OceanCurrentPlugin::~OceanCurrentPlugin() = default; ///////////////////////////////////////////////// void OceanCurrentPlugin::Configure( diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index 5af7a0fd..f16dda0e 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -17,7 +17,7 @@ - + 0.01 0.01 0.01 1.0 @@ -116,7 +116,7 @@ - + hydrodynamics current_velocity @@ -149,7 +149,7 @@ stratified_current_velocity - transientOceanCurrentDatabase.csv + /Users/gaurav/dave_ws/src/dave/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv @@ -194,9 +194,9 @@ - 4 - 3 - 2021 + 9 + 9 + 2024 15 0 From c449d635262b986d1866af5020c5546f84edf0cb Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 23 Oct 2024 20:39:05 +0900 Subject: [PATCH 58/79] proto is linked --- .clang-format-ignore | 1 + dave_interfaces/CMakeLists.txt | 37 ------------------- gazebo/dave_gz_world_plugins/CMakeLists.txt | 36 ++++++++++++++++-- .../msgs/StratifiedCurrentVelocity.proto | 0 .../src/OceanCurrentWorldPlugin.cc | 8 ++-- models/dave_worlds/worlds/ocp.world | 2 +- 6 files changed, 40 insertions(+), 44 deletions(-) create mode 100644 .clang-format-ignore rename {dave_interfaces => gazebo/dave_gz_world_plugins}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto (100%) diff --git a/.clang-format-ignore b/.clang-format-ignore new file mode 100644 index 00000000..880b22f6 --- /dev/null +++ b/.clang-format-ignore @@ -0,0 +1 @@ +*.proto \ No newline at end of file diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 0cd9ad73..4fc1583a 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -39,43 +39,6 @@ install(FILES DESTINATION share/${PROJECT_NAME} ) -# Define a variable 'GZ_MSGS_VER' holding the version number -set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) - -# Define a variable 'MSGS_PROTOS' listing the .proto files -set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) - -# Define the GZ_DATA_INSTALL_DIR variable -# DESTINATION ${GZ_DATA_INSTALL_DIR}/protos -install( - DIRECTORY gz - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto - COMPONENT proto - FILES_MATCHING PATTERN "*.proto") - -# Example of custom messages that depend on gz.msgs -set(MSGS_PROTOS - ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) -gz_msgs_generate_messages( - # The cmake target to be generated for libraries/executables to link - TARGET msgs # pressure_sensor_msgs_gen - # TARGET dave_gz_world_plugins_msgs - # The protobuf package to generate (Typically based on the path) - PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" - - # The path to the base directory of the proto files - # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) - MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto - - # List of proto files to generate - MSGS_PROTOS ${MSGS_PROTOS} - - # List of message targets this library imports from - # DEPENDENCIES gz_msgs_gen - # Dependency on gz-msgs - DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} -) - ament_export_dependencies(rosidl_default_runtime) # Install CMake package configuration ament_export_include_directories(include) diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index f96971bc..a25e9e00 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -26,6 +26,37 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) message(STATUS "compiling against Gazebo Harmonic") +# Define a variable 'GZ_MSGS_VER' holding the version number +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +# Define a variable 'MSGS_PROTOS' listing the .proto files +set(MSGS_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto) + +# Example of custom messages that depend on gz.msgs +set(MSGS_PROTOS + ${CMAKE_CURRENT_SOURCE_DIR}/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto +) + +gz_msgs_generate_messages( + # The cmake target to be generated for libraries/executables to link + TARGET msgs # pressure_sensor_msgs_gen + # TARGET dave_gz_world_plugins_msgs + # The protobuf package to generate (Typically based on the path) + PROTO_PACKAGE "dave_gz_world_plugins_msgs.msgs" + + # The path to the base directory of the proto files + # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) + MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + + # List of proto files to generate + MSGS_PROTOS ${MSGS_PROTOS} + + # List of message targets this library imports from + # DEPENDENCIES gz_msgs_gen + # Dependency on gz-msgs + DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} +) + add_library(OceanCurrentWorldPlugin SHARED src/OceanCurrentWorldPlugin.cc ) @@ -67,8 +98,7 @@ target_link_libraries(OceanCurrentWorldPlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} gauss_markov_process # Add this line tidal_oscillation - # dave_interfaces - # dave_gz_world_plugins_msgs + dave_gz_world_plugins-msgs ) target_link_libraries(gauss_markov_process gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} @@ -102,7 +132,7 @@ ament_target_dependencies(tidal_oscillation # Install targets install( - TARGETS OceanCurrentWorldPlugin gauss_markov_process tidal_oscillation + TARGETS OceanCurrentWorldPlugin gauss_markov_process tidal_oscillation dave_gz_world_plugins-msgs EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto b/gazebo/dave_gz_world_plugins/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto similarity index 100% rename from dave_interfaces/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto rename to gazebo/dave_gz_world_plugins/proto/dave_gz_world_plugins_msgs/msgs/StratifiedCurrentVelocity.proto diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 661a753d..e6284e4b 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -327,12 +327,14 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() // Read the depth dependent ocean current file path from the SDF file if (transientCurrentParams->HasElement("databasefilePath")) { - this->dataPtr->databaseFilePath = transientCurrentParams->Get("databasefilePath"); + this->dataPtr->databaseFilePath = ament_index_cpp::get_package_share_directory("dave_worlds") + + "/worlds/" + + transientCurrentParams->Get("databasefilePath"); } else { - this->dataPtr->databaseFilePath = - "/Users/gaurav/dave_ws/src/dave/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv"; + this->dataPtr->databaseFilePath = ament_index_cpp::get_package_share_directory("dave_worlds") + + "/worlds/transientOceanCurrentDatabase.csv"; } GZ_ASSERT( diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index f16dda0e..194510c3 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -149,7 +149,7 @@ stratified_current_velocity - /Users/gaurav/dave_ws/src/dave/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv + transientOceanCurrentDatabase.csv From c812c510769aab1a0a59e2c5e0046e64a47bd554 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 17 Nov 2024 01:24:15 +0530 Subject: [PATCH 59/79] few fixes --- .../src/OceanCurrentModelPlugin.cc | 6 +- .../OceanCurrentWorldPlugin.hh | 2 +- .../src/OceanCurrentWorldPlugin.cc | 357 ++++++++++++------ .../dave_ros_gz_plugins/OceanCurrentPlugin.hh | 4 +- .../src/OceanCurrentPlugin.cc | 30 +- .../description/rexrov/model.sdf | 2 +- models/dave_worlds/worlds/ocp.world | 3 + 7 files changed, 280 insertions(+), 124 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index d9fb93d4..f5ad6dc3 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -169,7 +169,7 @@ void OceanCurrentModelPlugin::Configure( // Set the flow velocity topic from SDF or use the default topic name if (_sdf->HasElement("flow_velocity_topic")) { - this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic"); + this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic") + "/" + this->dataPtr->modelName; } else { @@ -639,7 +639,3 @@ void OceanCurrentModelPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & } } // namespace dave_gz_model_plugins - -// Notes - -// 1. Relplace "TransientCurrentPlugin" with OceanCurrentModelPlugin -// 2. \ No newline at end of file diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 936c19d7..4917f731 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -101,7 +101,7 @@ public: std::vector> dateGMT; // Date in GMT (year, month, day, hour, minute) std::vector speedcmsec; // Speed in cm/sec }; - std::unique_ptr sharedDataPtr; + std::shared_ptr sharedDataPtr; private: // std::shared_ptr ros_node_; diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index e6284e4b..17ab811d 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -6,6 +6,7 @@ // Include Gazebo and Ignition libraries #include +#include #include #include #include @@ -57,6 +58,10 @@ struct OceanCurrentWorldPlugin::PrivateData // Publishers gz::transport::Node::Publisher gz_node_cvel_world_pub; // Publisher for current velocity + // Publishers + gz::transport::Node::Publisher + gz_node_scvel_world_pub; // Publisher for Stratified current velocity + // Subscribers gz::transport::Node subscriber; // Subscriber for vehicle depth @@ -82,7 +87,7 @@ struct OceanCurrentWorldPlugin::PrivateData }; OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() -: dataPtr(std::make_unique()), sharedDataPtr(std::make_unique()) +: dataPtr(std::make_unique()), sharedDataPtr(std::shared_ptr()) { // this->sharedDataPtr = std::make_unique(); } @@ -165,32 +170,80 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() } // Read the tidal oscillation direction from the SDF file - GZ_ASSERT( - tidalOscillationParams->HasElement("mean_direction"), "Tidal mean direction not defined"); + if (!(tidalOscillationParams->HasElement("mean_direction"))) + { + gzerr << "Tidal mean direction not defined" << std::endl; + } + if (tidalOscillationParams->HasElement("mean_direction")) { sdf::ElementPtr elem = tidalOscillationParams->GetElement("mean_direction"); - GZ_ASSERT(elem->HasElement("ebb"), "Tidal mean ebb direction not defined"); - this->sharedDataPtr->ebbDirection = elem->Get("ebb"); - this->sharedDataPtr->floodDirection = elem->Get("flood"); - GZ_ASSERT(elem->HasElement("flood"), "Tidal mean flood direction not defined"); + if (elem->HasElement("ebb")) + { + this->sharedDataPtr->ebbDirection = elem->Get("ebb"); + } + else + { + gzerr << "Tidal mean ebb direction not defined" << std::endl; + } + + if (!(elem->HasElement("flood"))) + { + this->sharedDataPtr->floodDirection = elem->Get("flood"); + } + else + { + gzerr << "Tidal mean flood direction not defined" << std::endl; + } } // Read the world start time (GMT) from the SDF file - GZ_ASSERT( - tidalOscillationParams->HasElement("world_start_time_GMT"), - "World start time (GMT) not defined"); + if (!(tidalOscillationParams->HasElement("world_start_time_GMT"))) + { + gzerr << "World start time (GMT) not defined" << std::endl; + } + if (tidalOscillationParams->HasElement("world_start_time_GMT")) { sdf::ElementPtr elem = tidalOscillationParams->GetElement("world_start_time_GMT"); - GZ_ASSERT(elem->HasElement("day"), "World start time (day) not defined"); - this->sharedDataPtr->world_start_time_day = elem->Get("day"); - GZ_ASSERT(elem->HasElement("month"), "World start time (month) not defined"); - this->sharedDataPtr->world_start_time_month = elem->Get("month"); - GZ_ASSERT(elem->HasElement("year"), "World start time (year) not defined"); - this->sharedDataPtr->world_start_time_year = elem->Get("year"); - GZ_ASSERT(elem->HasElement("hour"), "World start time (hour) not defined"); - this->sharedDataPtr->world_start_time_hour = elem->Get("hour"); + + if (elem->HasElement("day")) + { + this->sharedDataPtr->world_start_time_day = elem->Get("day"); + } + else + { + gzerr << "World start time (day) not defined" << std::endl; + } + + if (elem->HasElement("month")) + { + this->sharedDataPtr->world_start_time_month = elem->Get("month"); + } + else + { + gzerr << "World start time (month) not defined" << std::endl; + } + + if (elem->HasElement("year")) + { + this->sharedDataPtr->world_start_time_year = elem->Get("year"); + } + else + { + gzerr << "World start time (year) not defined" << std::endl; + } + + if (elem->HasElement("hour")) + { + this->sharedDataPtr->world_start_time_hour = elem->Get("hour"); + } + else + + { + gzerr << "World start time (hour) not defined" << std::endl; + } + if (elem->HasElement("minute")) { this->sharedDataPtr->world_start_time_minute = elem->Get("minute"); @@ -204,21 +257,42 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() if (this->sharedDataPtr->tidalHarmonicFlag) { // Read harmonic constituents - GZ_ASSERT(tidalHarmonicParams->HasElement("M2"), "Harcomnic constituents M2 not found"); - sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2"); - this->sharedDataPtr->M2_amp = M2Params->Get("amp"); - this->sharedDataPtr->M2_phase = M2Params->Get("phase"); - this->sharedDataPtr->M2_speed = M2Params->Get("speed"); - GZ_ASSERT(tidalHarmonicParams->HasElement("S2"), "Harcomnic constituents S2 not found"); - sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2"); - this->sharedDataPtr->S2_amp = S2Params->Get("amp"); - this->sharedDataPtr->S2_phase = S2Params->Get("phase"); - this->sharedDataPtr->S2_speed = S2Params->Get("speed"); - GZ_ASSERT(tidalHarmonicParams->HasElement("N2"), "Harcomnic constituents N2 not found"); - sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2"); - this->sharedDataPtr->N2_amp = N2Params->Get("amp"); - this->sharedDataPtr->N2_phase = N2Params->Get("phase"); - this->sharedDataPtr->N2_speed = N2Params->Get("speed"); + if (tidalHarmonicParams->HasElement("M2")) + { + sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2"); + this->sharedDataPtr->M2_amp = M2Params->Get("amp"); + this->sharedDataPtr->M2_phase = M2Params->Get("phase"); + this->sharedDataPtr->M2_speed = M2Params->Get("speed"); + } + else + { + gzerr << "Harcomnic constituents M2 not found" << std::endl; + } + + if (tidalHarmonicParams->HasElement("S2")) + { + sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2"); + this->sharedDataPtr->S2_amp = S2Params->Get("amp"); + this->sharedDataPtr->S2_phase = S2Params->Get("phase"); + this->sharedDataPtr->S2_speed = S2Params->Get("speed"); + } + else + { + gzerr << "Harcomnic constituents S2 not found" << std::endl; + } + + if (tidalHarmonicParams->HasElement("N2")) + { + sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2"); + this->sharedDataPtr->N2_amp = N2Params->Get("amp"); + this->sharedDataPtr->N2_phase = N2Params->Get("phase"); + this->sharedDataPtr->N2_speed = N2Params->Get("speed"); + } + else + { + gzerr << "Harcomnic constituents N2 not found" << std::endl; + } + gzmsg << "Tidal harmonic constituents loaded : " << std::endl; gzmsg << "M2 amp: " << this->sharedDataPtr->M2_amp << " phase: " << this->sharedDataPtr->M2_phase @@ -242,7 +316,11 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() this->dataPtr->tidalFilePath = paths->FindFile(this->dataPtr->tidalFilePath, true); csvFile.open(this->dataPtr->tidalFilePath); } - GZ_ASSERT(csvFile, "Tidal Oscillation database file does not exist"); + + if (!csvFile) + { + gzerr << "Tidal Oscillation database file does not exist" << std::endl; + } gzmsg << "Tidal Oscillation Database loaded : " << this->dataPtr->tidalFilePath << std::endl; @@ -305,11 +383,17 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() ///////////////////////////////////////////////// void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() { - GZ_ASSERT( - this->dataPtr->sdf->HasElement("transient_current"), - "Transient current configuration not available"); - sdf::ElementPtr transientCurrentParams = this->dataPtr->sdf->GetElement("transient_current"); + sdf::ElementPtr transientCurrentParams; + if (this->dataPtr->sdf->HasElement("transient_current")) + { + transientCurrentParams = this->dataPtr->sdf->GetElement("transient_current"); + } + else + { + gzerr << "Transient current configuration not available" << std::endl; + } + if (transientCurrentParams->HasElement("topic_stratified")) { this->sharedDataPtr->stratifiedCurrentVelocityTopic = @@ -320,9 +404,10 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() this->sharedDataPtr->stratifiedCurrentVelocityTopic = "stratified_current_velocity"; } - GZ_ASSERT( - !this->sharedDataPtr->stratifiedCurrentVelocityTopic.empty(), - "Empty stratified ocean current velocity topic"); + if (this->sharedDataPtr->stratifiedCurrentVelocityTopic.empty()) + { + gzerr << "Empty stratified ocean current velocity topic" << std::endl; + } // Read the depth dependent ocean current file path from the SDF file if (transientCurrentParams->HasElement("databasefilePath")) @@ -337,17 +422,13 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() "/worlds/transientOceanCurrentDatabase.csv"; } - GZ_ASSERT( - !this->dataPtr->databaseFilePath.empty(), "Empty stratified ocean current database file path"); + if (this->dataPtr->databaseFilePath.empty()) + { + gzerr << "Empty stratified ocean current database file path" << std::endl; + } gzmsg << this->dataPtr->databaseFilePath << std::endl; - // #if GAZEBO_MAJOR_VERSION >= 8 - // this->dataPtr->lastUpdate = this->dataPtr->world->SimTime(); - // #else - // this->dataPtr->lastUpdate = this->dataPtr->world->GetSimTime(); - // #endif - // Read database std::ifstream csvFile; std::string line; @@ -358,7 +439,11 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() this->dataPtr->databaseFilePath = paths->FindFile(this->dataPtr->databaseFilePath, true); csvFile.open(this->dataPtr->databaseFilePath); } - GZ_ASSERT(csvFile, "Stratified Ocean database file does not exist"); + + if (!csvFile) + { + gzerr << "Stratified Ocean database file does not exist" << std::endl; + } gzmsg << "Statified Ocean Current Database loaded : " << this->dataPtr->databaseFilePath << std::endl; @@ -427,7 +512,7 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() } csvFile.close(); - this->dataPtr->gz_node_cvel_world_pub = + this->dataPtr->gz_node_scvel_world_pub = this->dataPtr->gz_node->Advertise( this->dataPtr->ns + "/" + this->sharedDataPtr->stratifiedCurrentVelocityTopic); @@ -445,9 +530,16 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() // Consider setting it up as one or the other, but not both? // Retrieve the velocity configuration, if existent - GZ_ASSERT( - this->dataPtr->sdf->HasElement("constant_current"), "Current configuration not available"); - sdf::ElementPtr currentVelocityParams = this->dataPtr->sdf->GetElement("constant_current"); + + sdf::ElementPtr currentVelocityParams; + if (this->dataPtr->sdf->HasElement("constant_current")) + { + currentVelocityParams = this->dataPtr->sdf->GetElement("constant_current"); + } + else + { + gzerr << "Current configuration not available" << std::endl; + } // Read the topic names from the SDF file if (currentVelocityParams->HasElement("topic")) @@ -459,8 +551,10 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVelocityTopic = "current_velocity"; } - GZ_ASSERT( - !this->sharedDataPtr->currentVelocityTopic.empty(), "Empty ocean current velocity topic"); + if (this->sharedDataPtr->currentVelocityTopic.empty()) + { + gzerr << "Empty ocean current velocity topic" << std::endl; + } if (currentVelocityParams->HasElement("velocity")) { @@ -486,22 +580,31 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVelModel.noiseAmp = elem->Get("noiseAmp"); } - GZ_ASSERT( - this->sharedDataPtr->currentVelModel.min < this->sharedDataPtr->currentVelModel.max, - "Invalid current velocity limits"); - GZ_ASSERT( - this->sharedDataPtr->currentVelModel.mean >= this->sharedDataPtr->currentVelModel.min, - "Mean velocity must be greater than minimum"); - GZ_ASSERT( - this->sharedDataPtr->currentVelModel.mean <= this->sharedDataPtr->currentVelModel.max, - "Mean velocity must be smaller than maximum"); - GZ_ASSERT( - this->sharedDataPtr->currentVelModel.mu >= 0 && this->sharedDataPtr->currentVelModel.mu < 1, - "Invalid process constant"); - GZ_ASSERT( + if (this->sharedDataPtr->currentVelModel.min < this->sharedDataPtr->currentVelModel.max) + { + gzerr << "Invalid current velocity limits" << std::endl; + } + + if (this->sharedDataPtr->currentVelModel.mean >= this->sharedDataPtr->currentVelModel.min) + { + gzerr << "Mean velocity must be greater than minimum" << std::endl; + } + if (this->sharedDataPtr->currentVelModel.mean <= this->sharedDataPtr->currentVelModel.max) + { + gzerr << "Mean velocity must be smaller than maximum" << std::endl; + } + + if (this->sharedDataPtr->currentVelModel.mu >= 0 && this->sharedDataPtr->currentVelModel.mu < 1) + { + gzerr << "Invalid process constant" << std::endl; + } + + if ( this->sharedDataPtr->currentVelModel.noiseAmp < 1 && - this->sharedDataPtr->currentVelModel.noiseAmp >= 0, - "Noise amplitude has to be smaller than 1"); + this->sharedDataPtr->currentVelModel.noiseAmp >= 0) + { + gzerr << "Noise amplitude has to be smaller than 1" << std::endl; + } } this->sharedDataPtr->currentVelModel.var = this->sharedDataPtr->currentVelModel.mean; @@ -533,26 +636,40 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); } - GZ_ASSERT( + if ( this->sharedDataPtr->currentHorzAngleModel.min < - this->sharedDataPtr->currentHorzAngleModel.max, - "Invalid current horizontal angle limits"); - GZ_ASSERT( + this->sharedDataPtr->currentHorzAngleModel.max) + { + gzerr << "Invalid current horizontal angle limits" << std::endl; + } + + if ( this->sharedDataPtr->currentHorzAngleModel.mean >= - this->sharedDataPtr->currentHorzAngleModel.min, - "Mean horizontal angle must be greater than minimum"); - GZ_ASSERT( + this->sharedDataPtr->currentHorzAngleModel.min) + { + gzerr << "Mean horizontal angle must be greater than minimum" << std::endl; + } + + if ( this->sharedDataPtr->currentHorzAngleModel.mean <= - this->sharedDataPtr->currentHorzAngleModel.max, - "Mean horizontal angle must be smaller than maximum"); - GZ_ASSERT( + this->sharedDataPtr->currentHorzAngleModel.max) + { + gzerr << "Mean horizontal angle must be smaller than maximum" << std::endl; + } + + if ( this->sharedDataPtr->currentHorzAngleModel.mu >= 0 && - this->sharedDataPtr->currentHorzAngleModel.mu < 1, - "Invalid process constant"); - GZ_ASSERT( + this->sharedDataPtr->currentHorzAngleModel.mu < 1) + { + gzerr << "Invalid process constant" << std::endl; + } + + if ( this->sharedDataPtr->currentHorzAngleModel.noiseAmp < 1 && - this->sharedDataPtr->currentHorzAngleModel.noiseAmp >= 0, - "Noise amplitude for horizontal angle has to be between 0 and 1"); + this->sharedDataPtr->currentHorzAngleModel.noiseAmp >= 0) + { + gzerr << "Noise amplitude for horizontal angle has to be between 0 and 1" << std::endl; + } } this->sharedDataPtr->currentHorzAngleModel.var = this->sharedDataPtr->currentHorzAngleModel.mean; @@ -584,26 +701,40 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); } - GZ_ASSERT( + if ( this->sharedDataPtr->currentVertAngleModel.min < - this->sharedDataPtr->currentVertAngleModel.max, - "Invalid current vertical angle limits"); - GZ_ASSERT( + this->sharedDataPtr->currentVertAngleModel.max) + { + gzerr << "Invalid current vertical angle limits" << std::endl; + } + + if ( this->sharedDataPtr->currentVertAngleModel.mean >= - this->sharedDataPtr->currentVertAngleModel.min, - "Mean vertical angle must be greater than minimum"); - GZ_ASSERT( + this->sharedDataPtr->currentVertAngleModel.min) + { + gzerr << "Mean vertical angle must be greater than minimum" << std::endl; + } + + if ( this->sharedDataPtr->currentVertAngleModel.mean <= - this->sharedDataPtr->currentVertAngleModel.max, - "Mean vertical angle must be smaller than maximum"); - GZ_ASSERT( + this->sharedDataPtr->currentVertAngleModel.max) + { + gzerr << "Mean vertical angle must be smaller than maximum" << std::endl; + } + + if ( this->sharedDataPtr->currentVertAngleModel.mu >= 0 && - this->sharedDataPtr->currentVertAngleModel.mu < 1, - "Invalid process constant"); - GZ_ASSERT( + this->sharedDataPtr->currentVertAngleModel.mu < 1) + { + gzerr << "Invalid process constant" << std::endl; + } + + if ( this->sharedDataPtr->currentVertAngleModel.noiseAmp < 1 && - this->sharedDataPtr->currentVertAngleModel.noiseAmp >= 0, - "Noise amplitude for vertical angle has to be between 0 and 1"); + this->sharedDataPtr->currentVertAngleModel.noiseAmp >= 0) + { + gzerr << "Noise amplitude for vertical angle has to be between 0 and 1" << std::endl; + } } this->sharedDataPtr->currentVertAngleModel.var = this->sharedDataPtr->currentVertAngleModel.mean; @@ -637,12 +768,16 @@ void OceanCurrentWorldPlugin::Update( // model // Update current velocity - double currentVelMag = this->sharedDataPtr->currentVelModel.Update(time); + double currentVelMag = this->sharedDataPtr->currentVelModel.Update(time); // (TODO) + // double currentVelMag = 1.00; + // Update current horizontal direction around z axis of flow frame - double horzAngle = this->sharedDataPtr->currentHorzAngleModel.Update(time); + double horzAngle = this->sharedDataPtr->currentHorzAngleModel.Update(time); // (TODO) + // double horzAngle = 0.3; // Update current horizontal direction around z axis of flow frame - double vertAngle = this->sharedDataPtr->currentVertAngleModel.Update(time); + // double vertAngle = 0.3; + double vertAngle = this->sharedDataPtr->currentVertAngleModel.Update(time); // (TODO) // Generating the current velocity vector as in the North-East-Down frame this->sharedDataPtr->currentVelocity = gz::math::Vector3d( @@ -679,20 +814,24 @@ void OceanCurrentWorldPlugin::PublishCurrentVelocity() ///////////////////////////////////////////////// void OceanCurrentWorldPlugin::PublishStratifiedCurrentVelocity() { - dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel; // check + dave_gz_world_plugins_msgs::msgs::StratifiedCurrentVelocity stratcurrentVel; // check for (std::vector::iterator it = this->sharedDataPtr->currentStratifiedVelocity.begin(); it != this->sharedDataPtr->currentStratifiedVelocity.end(); ++it) // currentStratifiedVelocity values defined where ? (TODO) { - gz::msgs::Set(currentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); - currentVel.add_depth(it->W()); + gz::msgs::Set(stratcurrentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); + // auto* velocity = stratcurrentVel.add_velocity(); + // velocity->set_x(it->X()); + // velocity->set_y(it->Y()); + // velocity->set_z(it->Z()); + stratcurrentVel.add_depth(it->W()); } - if (currentVel.velocity_size() == 0) + if (stratcurrentVel.velocity_size() == 0) { return; } - this->dataPtr->gz_node_cvel_world_pub.Publish(currentVel); + this->dataPtr->gz_node_scvel_world_pub.Publish(stratcurrentVel); } ///////////////////////////////////////////////// diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh index 2290d876..f8b73703 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh @@ -17,8 +17,8 @@ #include "dave_gz_world_plugins/OceanCurrentWorldPlugin.hh" // Dave Interfaces: Message Types -#include "dave_interfaces/msg/Stratified_Current_Database.hpp" -#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" +#include "dave_interfaces/msg/stratified_current_database.hpp" +#include "dave_interfaces/msg/stratified_current_velocity.hpp" // Dave Interfaces: Service Types #include "dave_interfaces/srv/Get_Current_Model.hpp" diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index 401b7c87..de0c5c7f 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -126,12 +126,21 @@ void OceanCurrentPlugin::Configure( dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; // Set the topic for the stratified current velocity database - this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = - data.stratifiedCurrentVelocityTopic + "_database"; + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = "stratifiedCurrentVelocityTopic_database"; + // this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = + // data.stratifiedCurrentVelocityTopic + "_database"; // Retrieve the model namespace from the SDF if it exists - this->dataPtr->model_namespace = - _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + // this->dataPtr->model_namespace = + // _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; + if (_sdf->HasElement("namespace")) + { + this->dataPtr->model_namespace = _sdf->Get("namespace"); + } + else + { + this->dataPtr->model_namespace = "hydrodynamics"; + } // Reinitialize the ROS 2 node with the model namespace this->dataPtr->rosNode = @@ -141,17 +150,26 @@ void OceanCurrentPlugin::Configure( // Advertise the flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->dataPtr->rosNode->create_publisher( - data.currentVelocityTopic, rclcpp::QoS(10)); + "currentVelocityTopic", rclcpp::QoS(10)); // Advertise the stratified ocean current message this->dataPtr->stratifiedCurrentVelocityPub = this->dataPtr->rosNode->create_publisher( - data.stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); + "stratifiedCurrentVelocityTopic", rclcpp::QoS(10)); // Advertise the stratified ocean current database message this->dataPtr->stratifiedCurrentDatabasePub = this->dataPtr->rosNode->create_publisher( this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); + // // Advertise the stratified ocean current message + // this->dataPtr->stratifiedCurrentVelocityPub = + // this->dataPtr->rosNode->create_publisher( + // data.stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); + + // // Advertise the stratified ocean current database message + // this->dataPtr->stratifiedCurrentDatabasePub = + // this->dataPtr->rosNode->create_publisher( + // this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 962f0f29..d33a97d0 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -607,7 +607,7 @@ base_link $(arg namespace)/base_link - stratified_current_velocity + 0.0 0.0 diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index 194510c3..cd1eec0b 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -203,5 +203,8 @@ + + hydrodynamics + \ No newline at end of file From 51438263be7f28e6de12fa67b701592e01d3ef1c Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 17 Nov 2024 01:34:51 +0530 Subject: [PATCH 60/79] few fixes2 --- gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc | 3 ++- gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index f5ad6dc3..73e06788 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -169,7 +169,8 @@ void OceanCurrentModelPlugin::Configure( // Set the flow velocity topic from SDF or use the default topic name if (_sdf->HasElement("flow_velocity_topic")) { - this->dataPtr->currentVelocityTopic = _sdf->Get("flow_velocity_topic") + "/" + this->dataPtr->modelName; + this->dataPtr->currentVelocityTopic = + _sdf->Get("flow_velocity_topic") + "/" + this->dataPtr->modelName; } else { diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 17ab811d..ea04f01c 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -393,7 +393,7 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() { gzerr << "Transient current configuration not available" << std::endl; } - + if (transientCurrentParams->HasElement("topic_stratified")) { this->sharedDataPtr->stratifiedCurrentVelocityTopic = From f04faadd62856069bc16cbd43b979a6b9ed5d96e Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 27 Nov 2024 08:20:57 +0530 Subject: [PATCH 61/79] update --- .../src/OceanCurrentWorldPlugin.cc | 71 +++++++++---------- 1 file changed, 32 insertions(+), 39 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index ea04f01c..82cf8684 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -89,7 +89,7 @@ struct OceanCurrentWorldPlugin::PrivateData OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() : dataPtr(std::make_unique()), sharedDataPtr(std::shared_ptr()) { - // this->sharedDataPtr = std::make_unique(); + this->sharedDataPtr = std::shared_ptr(); } ///////////////////////////////////////////////// @@ -559,9 +559,12 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() if (currentVelocityParams->HasElement("velocity")) { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity"); + // this->sharedDataPtr = std::make_shared(); if (elem->HasElement("mean")) { - this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); + // this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); + this->sharedDataPtr->currentVelModel.mean = + std::make_shared(elem->Get("mean")); } if (elem->HasElement("min")) { @@ -580,28 +583,28 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVelModel.noiseAmp = elem->Get("noiseAmp"); } - if (this->sharedDataPtr->currentVelModel.min < this->sharedDataPtr->currentVelModel.max) + if (!(this->sharedDataPtr->currentVelModel.min < this->sharedDataPtr->currentVelModel.max)) { gzerr << "Invalid current velocity limits" << std::endl; } - if (this->sharedDataPtr->currentVelModel.mean >= this->sharedDataPtr->currentVelModel.min) + if (!(this->sharedDataPtr->currentVelModel.mean >= this->sharedDataPtr->currentVelModel.min)) { gzerr << "Mean velocity must be greater than minimum" << std::endl; } - if (this->sharedDataPtr->currentVelModel.mean <= this->sharedDataPtr->currentVelModel.max) + if (!(this->sharedDataPtr->currentVelModel.mean <= this->sharedDataPtr->currentVelModel.max)) { gzerr << "Mean velocity must be smaller than maximum" << std::endl; } - if (this->sharedDataPtr->currentVelModel.mu >= 0 && this->sharedDataPtr->currentVelModel.mu < 1) + if (!(this->sharedDataPtr->currentVelModel.mu >= 0 && + this->sharedDataPtr->currentVelModel.mu < 1)) { gzerr << "Invalid process constant" << std::endl; } - if ( - this->sharedDataPtr->currentVelModel.noiseAmp < 1 && - this->sharedDataPtr->currentVelModel.noiseAmp >= 0) + if (!(this->sharedDataPtr->currentVelModel.noiseAmp < 1 && + this->sharedDataPtr->currentVelModel.noiseAmp >= 0)) { gzerr << "Noise amplitude has to be smaller than 1" << std::endl; } @@ -636,37 +639,32 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentHorzAngleModel.noiseAmp = elem->Get("noiseAmp"); } - if ( - this->sharedDataPtr->currentHorzAngleModel.min < - this->sharedDataPtr->currentHorzAngleModel.max) + if (!(this->sharedDataPtr->currentHorzAngleModel.min < + this->sharedDataPtr->currentHorzAngleModel.max)) { gzerr << "Invalid current horizontal angle limits" << std::endl; } - if ( - this->sharedDataPtr->currentHorzAngleModel.mean >= - this->sharedDataPtr->currentHorzAngleModel.min) + if (!(this->sharedDataPtr->currentHorzAngleModel.mean >= + this->sharedDataPtr->currentHorzAngleModel.min)) { gzerr << "Mean horizontal angle must be greater than minimum" << std::endl; } - if ( - this->sharedDataPtr->currentHorzAngleModel.mean <= - this->sharedDataPtr->currentHorzAngleModel.max) + if (!(this->sharedDataPtr->currentHorzAngleModel.mean <= + this->sharedDataPtr->currentHorzAngleModel.max)) { gzerr << "Mean horizontal angle must be smaller than maximum" << std::endl; } - if ( - this->sharedDataPtr->currentHorzAngleModel.mu >= 0 && - this->sharedDataPtr->currentHorzAngleModel.mu < 1) + if (!(this->sharedDataPtr->currentHorzAngleModel.mu >= 0 && + this->sharedDataPtr->currentHorzAngleModel.mu < 1)) { gzerr << "Invalid process constant" << std::endl; } - if ( - this->sharedDataPtr->currentHorzAngleModel.noiseAmp < 1 && - this->sharedDataPtr->currentHorzAngleModel.noiseAmp >= 0) + if (!(this->sharedDataPtr->currentHorzAngleModel.noiseAmp < 1 && + this->sharedDataPtr->currentHorzAngleModel.noiseAmp >= 0)) { gzerr << "Noise amplitude for horizontal angle has to be between 0 and 1" << std::endl; } @@ -701,37 +699,32 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVertAngleModel.noiseAmp = elem->Get("noiseAmp"); } - if ( - this->sharedDataPtr->currentVertAngleModel.min < - this->sharedDataPtr->currentVertAngleModel.max) + if (!(this->sharedDataPtr->currentVertAngleModel.min < + this->sharedDataPtr->currentVertAngleModel.max)) { gzerr << "Invalid current vertical angle limits" << std::endl; } - if ( - this->sharedDataPtr->currentVertAngleModel.mean >= - this->sharedDataPtr->currentVertAngleModel.min) + if (!(this->sharedDataPtr->currentVertAngleModel.mean >= + this->sharedDataPtr->currentVertAngleModel.min)) { gzerr << "Mean vertical angle must be greater than minimum" << std::endl; } - if ( - this->sharedDataPtr->currentVertAngleModel.mean <= - this->sharedDataPtr->currentVertAngleModel.max) + if (!(this->sharedDataPtr->currentVertAngleModel.mean <= + this->sharedDataPtr->currentVertAngleModel.max)) { gzerr << "Mean vertical angle must be smaller than maximum" << std::endl; } - if ( - this->sharedDataPtr->currentVertAngleModel.mu >= 0 && - this->sharedDataPtr->currentVertAngleModel.mu < 1) + if (!(this->sharedDataPtr->currentVertAngleModel.mu >= 0 && + this->sharedDataPtr->currentVertAngleModel.mu < 1)) { gzerr << "Invalid process constant" << std::endl; } - if ( - this->sharedDataPtr->currentVertAngleModel.noiseAmp < 1 && - this->sharedDataPtr->currentVertAngleModel.noiseAmp >= 0) + if (!(this->sharedDataPtr->currentVertAngleModel.noiseAmp < 1 && + this->sharedDataPtr->currentVertAngleModel.noiseAmp >= 0)) { gzerr << "Noise amplitude for vertical angle has to be between 0 and 1" << std::endl; } From a996b0187daef70879f1684435f21cf394bc4a02 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 23 Dec 2024 20:35:59 +0530 Subject: [PATCH 62/79] loacl changes --- .../OceanCurrentWorldPlugin.hh | 55 ++++++++++++++++++- .../src/OceanCurrentWorldPlugin.cc | 9 ++- .../src/OceanCurrentPlugin.cc | 2 + models/dave_worlds/worlds/ocp.world | 4 +- 4 files changed, 64 insertions(+), 6 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 4917f731..01de5032 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -9,6 +9,7 @@ // #include #include #include +#include #include #include @@ -100,8 +101,60 @@ public: // Date and speed information std::vector> dateGMT; // Date in GMT (year, month, day, hour, minute) std::vector speedcmsec; // Speed in cm/sec + + // ------------------------------------------ + + // // Gauss-Markov process instances for current models + // std::shared_ptr + // currentHorzAngleModel; // Horizontal + // angle + // std::shared_ptr currentVelModel; // Velocity + // std::shared_ptr + // currentVertAngleModel; // Vertical angle + + // // Stratified current database and models + // std::shared_ptr> + // stratifiedDatabase; // Database values for stratified currents + // std::shared_ptr> + // stratifiedCurrentModels; // Stratified current models + + // // Tidal harmonic flag + // std::shared_ptr tidalHarmonicFlag; + + // // Current velocity information + // std::shared_ptr currentVelocity; // Current linear velocity vector + // std::shared_ptr> + // currentStratifiedVelocity; // Depth-specific linear velocity vectors for stratified + // current + + // // Tidal harmonic data (M2, S2, N2 constituents) + // std::shared_ptr M2_amp, M2_phase, M2_speed; // M2 tidal constituent + // std::shared_ptr S2_amp, S2_phase, S2_speed; // S2 tidal constituent + // std::shared_ptr N2_amp, N2_phase, N2_speed; // N2 tidal constituent + + // // Tidal oscillation mean directions + // std::shared_ptr ebbDirection; // Mean ebb direction + // std::shared_ptr floodDirection; // Mean flood direction + + // // Tidal oscillation world start time (GMT) + // std::shared_ptr world_start_time_day; + // std::shared_ptr world_start_time_month; + // std::shared_ptr world_start_time_year; + // std::shared_ptr world_start_time_hour; + // std::shared_ptr world_start_time_minute; + + // // Topics + // std::shared_ptr currentVelocityTopic; + // std::shared_ptr stratifiedCurrentVelocityTopic; + // // std::string vehicleDepthTopic; + + // // Date and speed information + // std::shared_ptr>> + // dateGMT; // Date in GMT (year, month, day, hour, minute) + // std::shared_ptr> speedcmsec; // Speed in cm/sec }; - std::shared_ptr sharedDataPtr; + + std::shared_ptr sharedDataPtr = std::make_shared(); private: // std::shared_ptr ros_node_; diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 82cf8684..ab139140 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include // AMENT library for package directory functions @@ -124,6 +125,8 @@ void OceanCurrentWorldPlugin::Configure( LoadGlobalCurrentConfig(); LoadStratifiedCurrentDatabase(); + LoadGlobalCurrentConfig(); + if (_sdf->HasElement("tidal_oscillation")) { if (_sdf->Get("tidal_oscillation")) @@ -562,9 +565,9 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() // this->sharedDataPtr = std::make_shared(); if (elem->HasElement("mean")) { - // this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); - this->sharedDataPtr->currentVelModel.mean = - std::make_shared(elem->Get("mean")); + this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); + // this->sharedDataPtr->currentVelModel.mean = + // std::make_shared(elem->Get("mean")); } if (elem->HasElement("min")) { diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index de0c5c7f..90af47e8 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include // Boost Library Header diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index cd1eec0b..22e29653 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -203,8 +203,8 @@ - + \ No newline at end of file From 31d77f35b7344ed4c1dd3ba329c19180e0f932d7 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 28 Dec 2024 18:23:39 +0530 Subject: [PATCH 63/79] fixed pointer memory allocation issue --- .../OceanCurrentWorldPlugin.hh | 54 +------------------ .../src/OceanCurrentWorldPlugin.cc | 11 ++-- .../src/OceanCurrentPlugin.cc | 5 -- models/dave_worlds/worlds/ocp.world | 4 +- 4 files changed, 10 insertions(+), 64 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 01de5032..68765759 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -101,60 +101,10 @@ public: // Date and speed information std::vector> dateGMT; // Date in GMT (year, month, day, hour, minute) std::vector speedcmsec; // Speed in cm/sec - - // ------------------------------------------ - - // // Gauss-Markov process instances for current models - // std::shared_ptr - // currentHorzAngleModel; // Horizontal - // angle - // std::shared_ptr currentVelModel; // Velocity - // std::shared_ptr - // currentVertAngleModel; // Vertical angle - - // // Stratified current database and models - // std::shared_ptr> - // stratifiedDatabase; // Database values for stratified currents - // std::shared_ptr> - // stratifiedCurrentModels; // Stratified current models - - // // Tidal harmonic flag - // std::shared_ptr tidalHarmonicFlag; - - // // Current velocity information - // std::shared_ptr currentVelocity; // Current linear velocity vector - // std::shared_ptr> - // currentStratifiedVelocity; // Depth-specific linear velocity vectors for stratified - // current - - // // Tidal harmonic data (M2, S2, N2 constituents) - // std::shared_ptr M2_amp, M2_phase, M2_speed; // M2 tidal constituent - // std::shared_ptr S2_amp, S2_phase, S2_speed; // S2 tidal constituent - // std::shared_ptr N2_amp, N2_phase, N2_speed; // N2 tidal constituent - - // // Tidal oscillation mean directions - // std::shared_ptr ebbDirection; // Mean ebb direction - // std::shared_ptr floodDirection; // Mean flood direction - - // // Tidal oscillation world start time (GMT) - // std::shared_ptr world_start_time_day; - // std::shared_ptr world_start_time_month; - // std::shared_ptr world_start_time_year; - // std::shared_ptr world_start_time_hour; - // std::shared_ptr world_start_time_minute; - - // // Topics - // std::shared_ptr currentVelocityTopic; - // std::shared_ptr stratifiedCurrentVelocityTopic; - // // std::string vehicleDepthTopic; - - // // Date and speed information - // std::shared_ptr>> - // dateGMT; // Date in GMT (year, month, day, hour, minute) - // std::shared_ptr> speedcmsec; // Speed in cm/sec }; - std::shared_ptr sharedDataPtr = std::make_shared(); + // std::shared_ptr sharedDataPtr = std::make_shared(); // (TODO) + std::shared_ptr sharedDataPtr; private: // std::shared_ptr ros_node_; diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index ab139140..eee368e0 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -90,7 +90,7 @@ struct OceanCurrentWorldPlugin::PrivateData OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() : dataPtr(std::make_unique()), sharedDataPtr(std::shared_ptr()) { - this->sharedDataPtr = std::shared_ptr(); + this->sharedDataPtr = std::make_shared(); } ///////////////////////////////////////////////// @@ -547,7 +547,8 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() // Read the topic names from the SDF file if (currentVelocityParams->HasElement("topic")) { - this->sharedDataPtr->currentVelocityTopic = currentVelocityParams->Get("topic"); + this->sharedDataPtr->currentVelocityTopic = + this->dataPtr->ns + "/" + currentVelocityParams->Get("topic"); } else { @@ -746,9 +747,9 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() // Advertise the current velocity & stratified current velocity topics this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node->Advertise( - this->dataPtr->ns + "/" + this->sharedDataPtr->currentVelocityTopic); - gzmsg << "Current velocity topic name: " - << this->dataPtr->ns + "/" + this->sharedDataPtr->currentVelocityTopic << std::endl; + this->sharedDataPtr->currentVelocityTopic); + gzmsg << "Current velocity topic name: " << this->sharedDataPtr->currentVelocityTopic + << std::endl; } // ---------------------------------------------- diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index 90af47e8..b3d39eb4 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -129,12 +129,7 @@ void OceanCurrentPlugin::Configure( // Set the topic for the stratified current velocity database this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = "stratifiedCurrentVelocityTopic_database"; - // this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = - // data.stratifiedCurrentVelocityTopic + "_database"; - // Retrieve the model namespace from the SDF if it exists - // this->dataPtr->model_namespace = - // _sdf->HasElement("namespace") ? _sdf->Get("namespace") : ""; if (_sdf->HasElement("namespace")) { this->dataPtr->model_namespace = _sdf->Get("namespace"); diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index 22e29653..cd1eec0b 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -203,8 +203,8 @@ - + \ No newline at end of file From 1d30cb3bce344a84ad142b17cc9894cb00b211dc Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 28 Dec 2024 20:37:14 +0530 Subject: [PATCH 64/79] code check --- .../src/OceanCurrentWorldPlugin.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index eee368e0..42f2ee82 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -103,9 +103,6 @@ void OceanCurrentWorldPlugin::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { - // GZ_ASSERT(_entity != gz::sim::kNullEntity, "World pointer is invalid"); - // GZ_ASSERT(_sdf != nullptr, "SDF pointer is invalid"); - this->dataPtr->world = gz::sim::World(_ecm.EntityByComponents(gz::sim::components::World())); if (!this->dataPtr->world.Valid(_ecm)) // check { @@ -117,7 +114,7 @@ void OceanCurrentWorldPlugin::Configure( // Read the namespace for topics and services this->dataPtr->ns = _sdf->Get("namespace"); - gzmsg << "Loading underwater world..." << std::endl; + gzmsg << "Loading underwater world plugin" << std::endl; // Initializing the transport node this->dataPtr->gz_node = std::make_shared(); @@ -474,6 +471,8 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() read.Y() = row[1]; read.Z() = row[2]; this->sharedDataPtr->stratifiedDatabase.push_back(read); + // gzmsg << "stratifiedDatabase: " << this->sharedDataPtr->stratifiedDatabase << std::endl; + // //testline (TODO) // Create Gauss-Markov processes for the stratified currents // Means are the database-specified magnitudes & angles, and @@ -512,6 +511,8 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); this->sharedDataPtr->stratifiedCurrentModels.push_back(depthModels); + // gzmsg << "stratifiedCurrentModels: " << this->sharedDataPtr->stratifiedCurrentModels << + // std::endl; //testline (TODO) } csvFile.close(); @@ -781,10 +782,12 @@ void OceanCurrentWorldPlugin::Update( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle)); + gzmsg << "out DepthVel"; // testline (TODO) // Generate the depth-specific velocities this->sharedDataPtr->currentStratifiedVelocity.clear(); for (int i = 0; i < this->sharedDataPtr->stratifiedDatabase.size(); i++) { + gzmsg << "enter DepthVel"; // testline (TODO) double depth = this->sharedDataPtr->stratifiedDatabase[i].Z(); currentVelMag = this->sharedDataPtr->stratifiedCurrentModels[i][0].Update(time); horzAngle = this->sharedDataPtr->stratifiedCurrentModels[i][1].Update(time); @@ -792,6 +795,7 @@ void OceanCurrentWorldPlugin::Update( gz::math::Vector4d depthVel( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); + gzmsg << "DepthVel: " << depthVel << std::endl; // testline (TODO) this->sharedDataPtr->currentStratifiedVelocity.push_back(depthVel); } } From 6ce45292c10f15f6e0d37fa70f31ac85e66cf0ff Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Mon, 13 Jan 2025 08:06:41 +0530 Subject: [PATCH 65/79] changed the tidal_oscillation --- .../dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc | 7 ++----- models/dave_worlds/worlds/ocp.world | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 42f2ee82..7099aa62 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -126,10 +126,7 @@ void OceanCurrentWorldPlugin::Configure( if (_sdf->HasElement("tidal_oscillation")) { - if (_sdf->Get("tidal_oscillation")) - { - LoadTidalOscillationDatabase(); - } + LoadTidalOscillationDatabase(); } else { @@ -165,7 +162,7 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() else { this->dataPtr->tidalFilePath = ament_index_cpp::get_package_share_directory("dave_worlds") + - "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv"; + "/worlds/ACT1951_1_Annual_2021.csv"; } } diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index cd1eec0b..899a7112 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -158,7 +158,7 @@ https://tidesandcurrents.noaa.gov/noaacurrents/Annual?id=ACT1951_1 --> - ACT1951_1_Annual_2021.csv + - - rexrov - sea_pressure - 10 - 3.0 - true - 101.325 - 9.80638 - - diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index 899a7112..4b5df100 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -121,7 +121,7 @@ current_velocity - 0 + 2 0 5 0.0 @@ -129,7 +129,7 @@ - 0 + 1 -3.141592653589793238 3.141592653589793238 0.0 @@ -137,7 +137,7 @@ - 0 + 1.2 -3.141592653589793238 3.141592653589793238 0.0 @@ -149,7 +149,7 @@ stratified_current_velocity - transientOceanCurrentDatabase.csv + transientOceanCurrentDatabase.csv @@ -203,8 +203,8 @@ - + \ No newline at end of file From eca57b87a4018b31fd6eb2b88100280df7048ce0 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 15 Jan 2025 21:38:46 +0530 Subject: [PATCH 68/79] Ocp ocean current working : ) --- .../OceanCurrentWorldPlugin.hh | 4 +- .../src/OceanCurrentWorldPlugin.cc | 32 +- .../src/OceanCurrentPlugin.cc | 348 +++++++++++------- models/dave_worlds/worlds/ocp.world | 4 +- 4 files changed, 252 insertions(+), 136 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 68765759..0c38e214 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -57,6 +57,8 @@ public: // ---------------------------------------------- + static OceanCurrentWorldPlugin * Instance(); + struct SharedData { // Gauss-Markov process instances for current models @@ -107,7 +109,7 @@ public: std::shared_ptr sharedDataPtr; private: - // std::shared_ptr ros_node_; + static OceanCurrentWorldPlugin * singletonInstance; struct PrivateData; std::unique_ptr dataPtr; diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index e1bbe1e0..d0e7d4cd 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -19,10 +19,10 @@ #include // Boost libraries for string manipulation, binding, and shared pointers -// #include -// #include -// #include -// #include +#include +#include +#include +#include // Other necessary libraries #include @@ -88,6 +88,8 @@ struct OceanCurrentWorldPlugin::PrivateData std::shared_ptr rosNode; }; +OceanCurrentWorldPlugin * OceanCurrentWorldPlugin::singletonInstance = nullptr; + OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() : dataPtr(std::make_unique()), sharedDataPtr(std::shared_ptr()) { @@ -95,8 +97,21 @@ OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() } ///////////////////////////////////////////////// -OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() = default; -// OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() { this->dataPtr->rosNode.reset(); } +// OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() = default; + +OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() +{ + // Clear the static pointer if we are the instance + if (OceanCurrentWorldPlugin::singletonInstance == this) + { + OceanCurrentWorldPlugin::singletonInstance = nullptr; + } +} + +OceanCurrentWorldPlugin * OceanCurrentWorldPlugin::Instance() +{ + return OceanCurrentWorldPlugin::singletonInstance; +} // ---------------------------------------------- @@ -134,6 +149,11 @@ void OceanCurrentWorldPlugin::Configure( gzmsg << "Tidal oscillation not enabled" << std::endl; } + if (!OceanCurrentWorldPlugin::singletonInstance) + { + OceanCurrentWorldPlugin::singletonInstance = this; + } + gzmsg << "Underwater current plugin loaded!" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; } diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index b3d39eb4..d84fcaff 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -125,8 +125,26 @@ void OceanCurrentPlugin::Configure( // Save the SDF pointer this->dataPtr->sdf = _sdf; - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin) + { + gzerr + << "The world plugin hasn't been configured or isn't loaded so we can't publish real data yet" + << std::endl; + return; + } + + auto sharedPtr = worldPlugin->sharedDataPtr; + if (!sharedPtr) + { + gzerr + << "The world plugin hasn't been configured or isn't loaded so we can't publish real data yet" + << std::endl; + return; + } + // Access the "real" currentHorzAngleModel + auto & horzModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; // Set the topic for the stratified current velocity database this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = "stratifiedCurrentVelocityTopic_database"; @@ -258,38 +276,24 @@ void OceanCurrentPlugin::Configure( "set_stratified_current_vert_angle", std::bind( &OceanCurrentPlugin::UpdateStratVertAngle, this, std::placeholders::_1, std::placeholders::_2)); - - // Connect to the Gazebo Harmonic update event - // this->dataPtr->rosPublishConnection = _world->OnUpdateBegin([this](const gz::sim::UpdateInfo & - // info) - // { this->dataPtr->OnUpdateCurrentVel(info); - // }); } -///////////////////////////////////////////////// -// void OceanCurrentPlugin::PreUpdate( -// const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -// { -// } -// ///////////////////////////////////////////////// -// void OceanCurrentPlugin::Update( -// const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -// { -// if (_info.simTime > this->dataPtr->lastUpdate) -// { -// // TODO put in postupdate -// } -// } - -// namespace dave_ros_gz_plugins ///////////////////////////////////////////////// bool OceanCurrentPlugin::UpdateHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; - _res->success = data.currentHorzAngleModel.SetMean(_req->angle); + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + auto & horzModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; + _res->success = horzModel.SetMean(_req->angle); return true; + // _res->success = data.currentHorzAngleModel.SetMean(_req->angle); + // return true; } ///////////////////////////////////////////////// @@ -297,22 +301,30 @@ bool OceanCurrentPlugin::UpdateStratHorzAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + + auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase; + auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels; - if (_req->layer >= data.stratifiedDatabase.size()) + if (_req->layer >= stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = data.stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); + _res->success = stratifiedCurrentModels[_req->layer][1].SetMean(_req->angle); if (_res->success) { // Update the database values (new angle, unchanged velocity) double velocity = - hypot(data.stratifiedDatabase[_req->layer].X(), data.stratifiedDatabase[_req->layer].Y()); - data.stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; - data.stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; + hypot(stratifiedDatabase[_req->layer].X(), stratifiedDatabase[_req->layer].Y()); + stratifiedDatabase[_req->layer].X() = cos(_req->angle) * velocity; + stratifiedDatabase[_req->layer].Y() = sin(_req->angle) * velocity; } return true; } @@ -322,8 +334,15 @@ bool OceanCurrentPlugin::UpdateVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; - _res->success = data.currentVertAngleModel.SetMean(_req->angle); + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + + auto & currentVertAngleModel = worldPlugin->sharedDataPtr->currentVertAngleModel; + _res->success = currentVertAngleModel.SetMean(_req->angle); return true; } @@ -332,14 +351,22 @@ bool OceanCurrentPlugin::UpdateStratVertAngle( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } - if (_req->layer >= data.stratifiedDatabase.size()) + auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase; + auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels; + + if (_req->layer >= stratifiedDatabase.size()) { _res->success = false; return true; } - _res->success = data.stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); + _res->success = stratifiedCurrentModels[_req->layer][2].SetMean(_req->angle); return true; } @@ -348,12 +375,22 @@ bool OceanCurrentPlugin::UpdateCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + + // Access the "real" currentHorzAngleModel + auto & currentVelModel = worldPlugin->sharedDataPtr->currentVelModel; + auto & currentHorzAngleModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; + auto & currentVertAngleModel = worldPlugin->sharedDataPtr->currentVertAngleModel; if ( - data.currentVelModel.SetMean(_req->velocity) && - data.currentHorzAngleModel.SetMean(_req->horizontal_angle) && - data.currentVertAngleModel.SetMean(_req->vertical_angle)) + currentVelModel.SetMean(_req->velocity) && + currentHorzAngleModel.SetMean(_req->horizontal_angle) && + currentVertAngleModel.SetMean(_req->vertical_angle)) { gzmsg << "Current velocity [m/s] = " << _req->velocity << std::endl << "Current horizontal angle [rad] = " << _req->horizontal_angle << std::endl @@ -374,21 +411,29 @@ bool OceanCurrentPlugin::UpdateStratCurrentVelocity( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + + auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels; + auto & stratifiedDatabase = worldPlugin->sharedDataPtr->stratifiedDatabase; - if (_req->layer >= data.stratifiedDatabase.size()) + if (_req->layer >= stratifiedDatabase.size()) { _res->success = false; return true; } if ( - data.stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && - data.stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && - data.stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) + stratifiedCurrentModels[_req->layer][0].SetMean(_req->velocity) && + stratifiedCurrentModels[_req->layer][1].SetMean(_req->horizontal_angle) && + stratifiedCurrentModels[_req->layer][2].SetMean(_req->vertical_angle)) { // Update the database values as well - data.stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; - data.stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; + stratifiedDatabase[_req->layer].X() = cos(_req->horizontal_angle) * _req->velocity; + stratifiedDatabase[_req->layer].Y() = sin(_req->horizontal_angle) * _req->velocity; gzmsg << "Layer " << _req->layer << " current velocity [m/s] = " << _req->velocity << std::endl << " Horizontal angle [rad] = " << _req->horizontal_angle << std::endl << " Vertical angle [rad] = " << _req->vertical_angle << std::endl @@ -408,13 +453,20 @@ bool OceanCurrentPlugin::GetCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + // _res->success = false; + return true; + } - _res->mean = data.currentVelModel.mean; - _res->min = data.currentVelModel.min; - _res->max = data.currentVelModel.max; - _res->noise = data.currentVelModel.noiseAmp; - _res->mu = data.currentVelModel.mu; + auto & currentVelModel = worldPlugin->sharedDataPtr->currentVelModel; + + _res->mean = currentVelModel.mean; + _res->min = currentVelModel.min; + _res->max = currentVelModel.max; + _res->noise = currentVelModel.noiseAmp; + _res->mu = currentVelModel.mu; return true; } @@ -423,13 +475,20 @@ bool OceanCurrentPlugin::GetCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + // _res->success = false; + return true; + } - _res->mean = data.currentHorzAngleModel.mean; - _res->min = data.currentHorzAngleModel.min; - _res->max = data.currentHorzAngleModel.max; - _res->noise = data.currentHorzAngleModel.noiseAmp; - _res->mu = data.currentHorzAngleModel.mu; + auto & currentHorzAngleModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; + + _res->mean = currentHorzAngleModel.mean; + _res->min = currentHorzAngleModel.min; + _res->max = currentHorzAngleModel.max; + _res->noise = currentHorzAngleModel.noiseAmp; + _res->mu = currentHorzAngleModel.mu; return true; } @@ -438,13 +497,20 @@ bool OceanCurrentPlugin::GetCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + // _res->success = false; + return true; + } + + auto & currentVertAngleModel = worldPlugin->sharedDataPtr->currentVertAngleModel; - _res->mean = data.currentVertAngleModel.mean; - _res->min = data.currentVertAngleModel.min; - _res->max = data.currentVertAngleModel.max; - _res->noise = data.currentVertAngleModel.noiseAmp; - _res->mu = data.currentVertAngleModel.mu; + _res->mean = currentVertAngleModel.mean; + _res->min = currentVertAngleModel.min; + _res->max = currentVertAngleModel.max; + _res->noise = currentVertAngleModel.noiseAmp; + _res->mu = currentVertAngleModel.mu; return true; } @@ -453,23 +519,30 @@ bool OceanCurrentPlugin::UpdateCurrentVelocityModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } - _res->success = data.currentVelModel.SetModel( + auto & currentVelModel = worldPlugin->sharedDataPtr->currentVelModel; + auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels; + + _res->success = currentVelModel.SetModel( std::max(0.0, _req->mean), std::min(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); - for (int i = 0; i < data.stratifiedCurrentModels.size(); i++) + for (int i = 0; i < stratifiedCurrentModels.size(); i++) { - dave_gz_world_plugins::GaussMarkovProcess & model = - data.stratifiedCurrentModels[i][0]; //(updated) + dave_gz_world_plugins::GaussMarkovProcess & model = stratifiedCurrentModels[i][0]; //(updated) model.SetModel( model.mean, std::max(0.0, _req->min), std::max(0.0, _req->max), _req->mu, _req->noise); } gzmsg << "Current velocity model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - data.currentVelModel.Print(); + currentVelModel.Print(); return true; } @@ -478,26 +551,29 @@ bool OceanCurrentPlugin::UpdateCurrentHorzAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } - _res->success = - data.currentHorzAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + auto & horzModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; + auto & stratifiedCurrentModels = worldPlugin->sharedDataPtr->stratifiedCurrentModels; + + _res->success = horzModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); - for (int i = 0; i < data.stratifiedCurrentModels.size(); i++) + for (int i = 0; i < stratifiedCurrentModels.size(); i++) { - dave_gz_world_plugins::GaussMarkovProcess & model = data.stratifiedCurrentModels[i][1]; + dave_gz_world_plugins::GaussMarkovProcess & model = stratifiedCurrentModels[i][1]; model.SetModel( model.mean, std::max(-M_PI, _req->min), std::min(M_PI, _req->max), _req->mu, _req->noise); } gzmsg << "Horizontal angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - data.currentHorzAngleModel.Print(); + horzModel.Print(); return true; - - // gzmsg << "Horizontal angle model updated" << std::endl - // << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - // this->dataPtr->currentHorzAngleModel.Print(); } ///////////////////////////////////////////////// @@ -505,14 +581,21 @@ bool OceanCurrentPlugin::UpdateCurrentVertAngleModel( const std::shared_ptr _req, std::shared_ptr _res) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin || !worldPlugin->sharedDataPtr) + { + _res->success = false; + return true; + } + + auto & currentVertAngleModel = worldPlugin->sharedDataPtr->currentVertAngleModel; _res->success = - data.currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); + currentVertAngleModel.SetModel(_req->mean, _req->min, _req->max, _req->mu, _req->noise); gzmsg << "Vertical angle model updated" << std::endl << "\tWARNING: Current velocity calculated in the ENU frame" << std::endl; - data.currentVertAngleModel.Print(); + currentVertAngleModel.Print(); return true; } @@ -520,21 +603,32 @@ bool OceanCurrentPlugin::UpdateCurrentVertAngleModel( void OceanCurrentPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - dave_gz_world_plugins::OceanCurrentWorldPlugin::SharedData data; + auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); + if (!worldPlugin) + { + gzerr + << "The world plugin hasn't been configured or isn't loaded so we can't publish real data yet" + << std::endl; + return; + } + + auto sharedPtr = worldPlugin->sharedDataPtr; + if (!sharedPtr) + { + return; + } + // Generate and publish current velocity according to the vehicle depth geometry_msgs::msg::TwistStamped flowVelMsg; flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); - flowVelMsg.header.frame_id = - "world"; // Changed from "/world" to be consistent with ROS 2 TF2 conventions + flowVelMsg.header.frame_id = "world"; - flowVelMsg.twist.linear.x = data.currentVelocity.X(); - flowVelMsg.twist.linear.y = data.currentVelocity.Y(); - flowVelMsg.twist.linear.z = data.currentVelocity.Z(); + flowVelMsg.twist.linear.x = sharedPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = sharedPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = sharedPtr->currentVelocity.Z(); this->dataPtr->flowVelocityPub->publish(flowVelMsg); - // - // Generate and publish stratified current velocity dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); @@ -543,72 +637,72 @@ void OceanCurrentPlugin::PostUpdate( // Updating for stratified behaviour of Ocean Currents // What is the .size value over here, to be (checked) - for (size_t i = 0; i < data.currentStratifiedVelocity.size(); + for (size_t i = 0; i < sharedPtr->currentStratifiedVelocity.size(); i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) { geometry_msgs::msg::Vector3 velocity; - velocity.x = data.currentStratifiedVelocity[i].X(); - velocity.y = data.currentStratifiedVelocity[i].Y(); - velocity.z = data.currentStratifiedVelocity[i].Z(); + velocity.x = sharedPtr->currentStratifiedVelocity[i].X(); + velocity.y = sharedPtr->currentStratifiedVelocity[i].Y(); + velocity.z = sharedPtr->currentStratifiedVelocity[i].Z(); stratCurrentVelocityMsg.velocities.push_back(velocity); - stratCurrentVelocityMsg.depths.push_back(data.currentStratifiedVelocity[i].W()); + stratCurrentVelocityMsg.depths.push_back(sharedPtr->currentStratifiedVelocity[i].W()); } this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; - i < data.stratifiedDatabase.size(); // again check with ocean_cureent_world_plugin.cc (TODO) - i++) // read from csv file in ocean_cureent_world_plugin.cc + for (int i = 0; i < sharedPtr->stratifiedDatabase + .size(); // again check with ocean_cureent_world_plugin.cc (TODO) + i++) // read from csv file in ocean_cureent_world_plugin.cc { // Stratified current database entry preparation geometry_msgs::msg::Vector3 velocity; - velocity.x = data.stratifiedDatabase[i].X(); - velocity.y = data.stratifiedDatabase[i].Y(); + velocity.x = sharedPtr->stratifiedDatabase[i].X(); + velocity.y = sharedPtr->stratifiedDatabase[i].Y(); velocity.z = 0.0; // Assuming z is intentionally set to 0.0 currentDatabaseMsg.velocities.push_back(velocity); - currentDatabaseMsg.depths.push_back(data.stratifiedDatabase[i].Z()); + currentDatabaseMsg.depths.push_back(sharedPtr->stratifiedDatabase[i].Z()); } - if (data.tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) + if (sharedPtr->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) { // Tidal harmonic constituents - currentDatabaseMsg.m2_amp = data.M2_amp; - currentDatabaseMsg.m2_phase = data.M2_phase; - currentDatabaseMsg.m2_speed = data.M2_speed; - currentDatabaseMsg.s2_amp = data.S2_amp; - currentDatabaseMsg.s2_phase = data.S2_phase; - currentDatabaseMsg.s2_speed = data.S2_speed; - currentDatabaseMsg.n2_amp = data.N2_amp; - currentDatabaseMsg.n2_phase = data.N2_phase; - currentDatabaseMsg.n2_speed = data.N2_speed; + currentDatabaseMsg.m2_amp = sharedPtr->M2_amp; + currentDatabaseMsg.m2_phase = sharedPtr->M2_phase; + currentDatabaseMsg.m2_speed = sharedPtr->M2_speed; + currentDatabaseMsg.s2_amp = sharedPtr->S2_amp; + currentDatabaseMsg.s2_phase = sharedPtr->S2_phase; + currentDatabaseMsg.s2_speed = sharedPtr->S2_speed; + currentDatabaseMsg.n2_amp = sharedPtr->N2_amp; + currentDatabaseMsg.n2_phase = sharedPtr->N2_phase; + currentDatabaseMsg.n2_speed = sharedPtr->N2_speed; currentDatabaseMsg.tideconstituents = true; } else { - for (int i = 0; i < data.dateGMT.size(); i++) + for (int i = 0; i < sharedPtr->dateGMT.size(); i++) { // Tidal oscillation database - currentDatabaseMsg.time_gmt_year.push_back(data.dateGMT[i][0]); - currentDatabaseMsg.time_gmt_month.push_back(data.dateGMT[i][1]); - currentDatabaseMsg.time_gmt_day.push_back(data.dateGMT[i][2]); - currentDatabaseMsg.time_gmt_hour.push_back(data.dateGMT[i][3]); - currentDatabaseMsg.time_gmt_minute.push_back(data.dateGMT[i][4]); + currentDatabaseMsg.time_gmt_year.push_back(sharedPtr->dateGMT[i][0]); + currentDatabaseMsg.time_gmt_month.push_back(sharedPtr->dateGMT[i][1]); + currentDatabaseMsg.time_gmt_day.push_back(sharedPtr->dateGMT[i][2]); + currentDatabaseMsg.time_gmt_hour.push_back(sharedPtr->dateGMT[i][3]); + currentDatabaseMsg.time_gmt_minute.push_back(sharedPtr->dateGMT[i][4]); - currentDatabaseMsg.tidevelocities.push_back(data.speedcmsec[i]); + currentDatabaseMsg.tidevelocities.push_back(sharedPtr->speedcmsec[i]); } currentDatabaseMsg.tideconstituents = false; } - currentDatabaseMsg.ebb_direction = data.ebbDirection; - currentDatabaseMsg.flood_direction = data.floodDirection; + currentDatabaseMsg.ebb_direction = sharedPtr->ebbDirection; + currentDatabaseMsg.flood_direction = sharedPtr->floodDirection; - currentDatabaseMsg.world_start_time_year = data.world_start_time_year; - currentDatabaseMsg.world_start_time_month = data.world_start_time_month; - currentDatabaseMsg.world_start_time_day = data.world_start_time_day; - currentDatabaseMsg.world_start_time_hour = data.world_start_time_hour; - currentDatabaseMsg.world_start_time_minute = data.world_start_time_minute; + currentDatabaseMsg.world_start_time_year = sharedPtr->world_start_time_year; + currentDatabaseMsg.world_start_time_month = sharedPtr->world_start_time_month; + currentDatabaseMsg.world_start_time_day = sharedPtr->world_start_time_day; + currentDatabaseMsg.world_start_time_hour = sharedPtr->world_start_time_hour; + currentDatabaseMsg.world_start_time_minute = sharedPtr->world_start_time_minute; this->dataPtr->stratifiedCurrentDatabasePub->publish(currentDatabaseMsg); diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocp.world index 4b5df100..80c45841 100755 --- a/models/dave_worlds/worlds/ocp.world +++ b/models/dave_worlds/worlds/ocp.world @@ -203,8 +203,8 @@ - + \ No newline at end of file From 15ea1164cb09285115ba02760f87de03c9e15fd4 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 16 Jan 2025 03:19:27 +0530 Subject: [PATCH 69/79] corrected incorrect parameter readings --- .../src/OceanCurrentModelPlugin.cc | 72 +++++++++++++++---- 1 file changed, 60 insertions(+), 12 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index 73e06788..fe55f7f4 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -239,10 +239,18 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_north"); if (elem->HasElement("mean")) + { + this->dataPtr->currentVelNorthModel.mean = elem->Get("mean"); + } + else { this->dataPtr->currentVelNorthModel.mean = 0.0; } if (elem->HasElement("mu")) + { + this->dataPtr->currentVelNorthModel.mu = elem->Get("mu"); + } + else { this->dataPtr->currentVelNorthModel.mu = 0.0; } @@ -250,14 +258,22 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { this->dataPtr->noiseAmp_North = elem->Get("noiseAmp"); } - this->dataPtr->currentVelNorthModel.min = - this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; - this->dataPtr->currentVelNorthModel.max = - this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; + else + { + this->dataPtr->noiseAmp_North = 0.0; + } if (elem->HasElement("noiseFreq")) { this->dataPtr->noiseFreq_North = elem->Get("noiseFreq"); } + else + { + this->dataPtr->noiseFreq_North = 0.0; + } + this->dataPtr->currentVelNorthModel.min = + this->dataPtr->currentVelNorthModel.mean - this->dataPtr->noiseAmp_North; + this->dataPtr->currentVelNorthModel.max = + this->dataPtr->currentVelNorthModel.mean + this->dataPtr->noiseAmp_North; this->dataPtr->currentVelNorthModel.noiseAmp = this->dataPtr->noiseFreq_North; } @@ -272,10 +288,18 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_east"); if (elem->HasElement("mean")) + { + this->dataPtr->currentVelEastModel.mean = elem->Get("mean"); + } + else { this->dataPtr->currentVelEastModel.mean = 0.0; } if (elem->HasElement("mu")) + { + this->dataPtr->currentVelEastModel.mu = elem->Get("mu"); + } + else { this->dataPtr->currentVelEastModel.mu = 0.0; } @@ -283,14 +307,22 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { this->dataPtr->noiseAmp_East = elem->Get("noiseAmp"); } - this->dataPtr->currentVelEastModel.min = - this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; - this->dataPtr->currentVelEastModel.max = - this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; + else + { + this->dataPtr->noiseAmp_East = 0.0; + } if (elem->HasElement("noiseFreq")) { this->dataPtr->noiseFreq_East = elem->Get("noiseFreq"); } + else + { + this->dataPtr->noiseFreq_East = 0.0; + } + this->dataPtr->currentVelEastModel.min = + this->dataPtr->currentVelEastModel.mean - this->dataPtr->noiseAmp_East; + this->dataPtr->currentVelEastModel.max = + this->dataPtr->currentVelEastModel.mean + this->dataPtr->noiseAmp_East; this->dataPtr->currentVelEastModel.noiseAmp = this->dataPtr->noiseFreq_East; } @@ -305,10 +337,18 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity_down"); if (elem->HasElement("mean")) + { + this->dataPtr->currentVelDownModel.mean = elem->Get("mean"); + } + else { this->dataPtr->currentVelDownModel.mean = 0.0; } if (elem->HasElement("mu")) + { + this->dataPtr->currentVelDownModel.mu = elem->Get("mu"); + } + else { this->dataPtr->currentVelDownModel.mu = 0.0; } @@ -316,14 +356,22 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( { this->dataPtr->noiseAmp_Down = elem->Get("noiseAmp"); } - this->dataPtr->currentVelDownModel.min = - this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; - this->dataPtr->currentVelDownModel.max = - this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; + else + { + this->dataPtr->noiseAmp_Down = 0.0; + } if (elem->HasElement("noiseFreq")) { this->dataPtr->noiseFreq_Down = elem->Get("noiseFreq"); } + else + { + this->dataPtr->noiseFreq_Down = 0.0; + } + this->dataPtr->currentVelDownModel.min = + this->dataPtr->currentVelDownModel.mean - this->dataPtr->noiseAmp_Down; + this->dataPtr->currentVelDownModel.max = + this->dataPtr->currentVelDownModel.mean + this->dataPtr->noiseAmp_Down; this->dataPtr->currentVelDownModel.noiseAmp = this->dataPtr->noiseFreq_Down; } From f5ea2a297a216e06105e6091f4b3c036091de555 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 18 Jan 2025 07:46:38 +0530 Subject: [PATCH 70/79] minor changes to sdf param --- .../src/OceanCurrentWorldPlugin.cc | 2 +- .../dave_ros_gz_plugins/OceanCurrentPlugin.hh | 8 +--- .../src/OceanCurrentPlugin.cc | 46 ++++++++++++------- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index d0e7d4cd..5bc01491 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -263,7 +263,7 @@ void OceanCurrentWorldPlugin::LoadTidalOscillationDatabase() } else { - this->sharedDataPtr->world_start_time_minute = 0; + gzerr << "World start time (minute) not defined" << std::endl; } } else diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh index f8b73703..cb05e09f 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh @@ -51,12 +51,6 @@ public: // ---------------------------------------------- - // void Update(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); - // void PreUpdate( - // const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; - - // ---------------------------------------------- - void PostUpdate(const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm); // ---------------------------------------------- @@ -110,6 +104,8 @@ public: std::shared_ptr _res); private: + // std::shared_ptr rosNode; + struct PrivateData; std::unique_ptr dataPtr; }; diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index d84fcaff..c2ccba2c 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -12,6 +12,7 @@ // ROS 2 Headers #include +#include #include #include #include @@ -40,10 +41,6 @@ namespace dave_ros_gz_plugins { struct OceanCurrentPlugin::PrivateData { - // std::string db_path; (check)(TODO) - // this->dataPtr->db_path = ament_index_cpp::get_package_share_directory("dave_worlds"); - // // rclcpp::Service::SharedPtr; //Template - // Time management std::chrono::steady_clock::duration lastUpdate{0}; @@ -157,25 +154,26 @@ void OceanCurrentPlugin::Configure( this->dataPtr->model_namespace = "hydrodynamics"; } - // Reinitialize the ROS 2 node with the model namespace - this->dataPtr->rosNode = - std::make_shared("underwater_current_ros_plugin", this->dataPtr->model_namespace); + // Reinitialize the ROS 2 node with the model namespace // TODO: Do we need this confirm with + // woen-sug : ) this->dataPtr->rosNode = + // std::make_shared("underwater_current_ros_plugin", + // this->dataPtr->model_namespace); // Create and advertise Messages // Advertise the flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->dataPtr->rosNode->create_publisher( - "currentVelocityTopic", rclcpp::QoS(10)); + "currentVelocityTopic", 1); // Advertise the stratified ocean current message this->dataPtr->stratifiedCurrentVelocityPub = this->dataPtr->rosNode->create_publisher( - "stratifiedCurrentVelocityTopic", rclcpp::QoS(10)); + "stratifiedCurrentVelocityTopic", 1); // Advertise the stratified ocean current database message this->dataPtr->stratifiedCurrentDatabasePub = this->dataPtr->rosNode->create_publisher( - this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, 1); // // Advertise the stratified ocean current message // this->dataPtr->stratifiedCurrentVelocityPub = // this->dataPtr->rosNode->create_publisher( @@ -603,6 +601,16 @@ bool OceanCurrentPlugin::UpdateCurrentVertAngleModel( void OceanCurrentPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { + if (!_info.paused) + { + rclcpp::spin_some(this->dataPtr->rosNode); + gzmsg << "ros 2 is running" << std::endl; + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_ros_gz_plugins::OceanCurrentPlugin::PostUpdate" << std::endl; + } + } auto * worldPlugin = dave_gz_world_plugins::OceanCurrentWorldPlugin::Instance(); if (!worldPlugin) { @@ -619,7 +627,9 @@ void OceanCurrentPlugin::PostUpdate( } // Generate and publish current velocity according to the vehicle depth - geometry_msgs::msg::TwistStamped flowVelMsg; + // geometry_msgs::msg::TwistStamped flowVelMsg; + auto flowVelMsg = geometry_msgs::msg::TwistStamped(); + flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); flowVelMsg.header.frame_id = "world"; @@ -630,7 +640,10 @@ void OceanCurrentPlugin::PostUpdate( this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish stratified current velocity - dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; + // dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; + + auto stratCurrentVelocityMsg = dave_interfaces::msg::StratifiedCurrentVelocity(); + stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); stratCurrentVelocityMsg.header.frame_id = "world"; @@ -651,10 +664,9 @@ void OceanCurrentPlugin::PostUpdate( this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database - dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; - for (int i = 0; i < sharedPtr->stratifiedDatabase - .size(); // again check with ocean_cureent_world_plugin.cc (TODO) - i++) // read from csv file in ocean_cureent_world_plugin.cc + // dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; + auto currentDatabaseMsg = dave_interfaces::msg::StratifiedCurrentDatabase(); + for (int i = 0; i < sharedPtr->stratifiedDatabase.size(); i++) { // Stratified current database entry preparation geometry_msgs::msg::Vector3 velocity; @@ -665,7 +677,7 @@ void OceanCurrentPlugin::PostUpdate( currentDatabaseMsg.depths.push_back(sharedPtr->stratifiedDatabase[i].Z()); } - if (sharedPtr->tidalHarmonicFlag) // again check with ocean_cureent_world_plugin.cc (TODO) + if (sharedPtr->tidalHarmonicFlag) { // Tidal harmonic constituents currentDatabaseMsg.m2_amp = sharedPtr->M2_amp; From 50b58fa7f482d14c66be18923d93c7e954d010af Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Wed, 22 Jan 2025 21:26:23 +0530 Subject: [PATCH 71/79] code cleanup and fix the msg issue --- .../OceanCurrentWorldPlugin.hh | 1 - .../src/OceanCurrentWorldPlugin.cc | 23 ++++--------------- .../src/OceanCurrentPlugin.cc | 11 +++------ 3 files changed, 7 insertions(+), 28 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index 0c38e214..d0a43bf3 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -105,7 +105,6 @@ public: std::vector speedcmsec; // Speed in cm/sec }; - // std::shared_ptr sharedDataPtr = std::make_shared(); // (TODO) std::shared_ptr sharedDataPtr; private: diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 5bc01491..8e343ed8 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -488,13 +488,11 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() read.Y() = row[1]; read.Z() = row[2]; this->sharedDataPtr->stratifiedDatabase.push_back(read); - // gzmsg << "stratifiedDatabase: " << this->sharedDataPtr->stratifiedDatabase << std::endl; - // //testline (TODO) // Create Gauss-Markov processes for the stratified currents // Means are the database-specified magnitudes & angles, and // the other values come from the constant current models - // TODO: Vertical angle currently set to 0 (not in database) + dave_gz_world_plugins::GaussMarkovProcess magnitudeModel; magnitudeModel.mean = hypot(row[1], row[0]); magnitudeModel.var = magnitudeModel.mean; @@ -527,8 +525,6 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() depthModels.push_back(hAngleModel); depthModels.push_back(vAngleModel); this->sharedDataPtr->stratifiedCurrentModels.push_back(depthModels); - // gzmsg << "stratifiedCurrentModels: " << this->sharedDataPtr->stratifiedCurrentModels << - // std::endl; //testline (TODO) } csvFile.close(); @@ -782,16 +778,13 @@ void OceanCurrentWorldPlugin::Update( // model // Update current velocity - double currentVelMag = this->sharedDataPtr->currentVelModel.Update(time); // (TODO) - // double currentVelMag = 1.00; + double currentVelMag = this->sharedDataPtr->currentVelModel.Update(time); // Update current horizontal direction around z axis of flow frame - double horzAngle = this->sharedDataPtr->currentHorzAngleModel.Update(time); // (TODO) - // double horzAngle = 0.3; + double horzAngle = this->sharedDataPtr->currentHorzAngleModel.Update(time); // Update current horizontal direction around z axis of flow frame - // double vertAngle = 0.3; - double vertAngle = this->sharedDataPtr->currentVertAngleModel.Update(time); // (TODO) + double vertAngle = this->sharedDataPtr->currentVertAngleModel.Update(time); // Generating the current velocity vector as in the North-East-Down frame this->sharedDataPtr->currentVelocity = gz::math::Vector3d( @@ -809,7 +802,6 @@ void OceanCurrentWorldPlugin::Update( gz::math::Vector4d depthVel( currentVelMag * cos(horzAngle) * cos(vertAngle), currentVelMag * sin(horzAngle) * cos(vertAngle), currentVelMag * sin(vertAngle), depth); - // gzmsg << "DepthVel: " << depthVel << std::endl; // testline (TODO) this->sharedDataPtr->currentStratifiedVelocity.push_back(depthVel); } } @@ -824,8 +816,6 @@ void OceanCurrentWorldPlugin::PublishCurrentVelocity() this->sharedDataPtr->currentVelocity.X(), this->sharedDataPtr->currentVelocity.Y(), this->sharedDataPtr->currentVelocity.Z())); this->dataPtr->gz_node_cvel_world_pub.Publish(currVel); - // gzmsg << this->sharedDataPtr->currentVelocity << std::endl; TODO : Remove after testing - // gzmsg << "currvel-" << currVel.DebugString() << std::endl; TODO : Remove after testing } ///////////////////////////////////////////////// @@ -838,17 +828,12 @@ void OceanCurrentWorldPlugin::PublishStratifiedCurrentVelocity() { gz::msgs::Set(stratcurrentVel.add_velocity(), gz::math::Vector3d(it->X(), it->Y(), it->Z())); stratcurrentVel.add_depth(it->W()); - // gzmsg << "X: " << it->X() << std::endl; - // gzmsg << "Y: " << it->Y() << std::endl; - // gzmsg << "Z: " << it->Z() << std::endl; - // gzmsg << "Depth: " << it->W() << std::endl; } if (stratcurrentVel.velocity_size() == 0) { return; } this->dataPtr->gz_node_scvel_world_pub.Publish(stratcurrentVel); - // gzmsg << stratcurrentVel.DebugString() << std::endl; } ///////////////////////////////////////////////// diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index c2ccba2c..0b02852e 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -240,14 +240,14 @@ void OceanCurrentPlugin::Configure( &OceanCurrentPlugin::UpdateStratCurrentVelocity, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current horizontal angle //TODO + // Advertise the service to update the current horizontal angle this->dataPtr->set_current_horz_angle = this->dataPtr->rosNode->create_service( "set_current_horz_angle", std::bind( &OceanCurrentPlugin::UpdateHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the current horizontal angle //TODO + // Advertise the service to update the current horizontal angle this->dataPtr->set_current_vert_angle = this->dataPtr->rosNode->create_service( "set_current_vert_angle", @@ -604,8 +604,6 @@ void OceanCurrentPlugin::PostUpdate( if (!_info.paused) { rclcpp::spin_some(this->dataPtr->rosNode); - gzmsg << "ros 2 is running" << std::endl; - if (_info.iterations % 1000 == 0) { gzmsg << "dave_ros_gz_plugins::OceanCurrentPlugin::PostUpdate" << std::endl; @@ -640,8 +638,6 @@ void OceanCurrentPlugin::PostUpdate( this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish stratified current velocity - // dave_interfaces::msg::StratifiedCurrentVelocity stratCurrentVelocityMsg; - auto stratCurrentVelocityMsg = dave_interfaces::msg::StratifiedCurrentVelocity(); stratCurrentVelocityMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); @@ -650,8 +646,7 @@ void OceanCurrentPlugin::PostUpdate( // Updating for stratified behaviour of Ocean Currents // What is the .size value over here, to be (checked) - for (size_t i = 0; i < sharedPtr->currentStratifiedVelocity.size(); - i++) // need to check if the values are in sync with ocean_cureent_world_plugin.cc(TODO) + for (size_t i = 0; i < sharedPtr->currentStratifiedVelocity.size(); i++) { geometry_msgs::msg::Vector3 velocity; velocity.x = sharedPtr->currentStratifiedVelocity[i].X(); From 8a46c2e714f3b3db3d7dc96f36f1cfa4caaf72ce Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Thu, 23 Jan 2025 06:05:30 +0530 Subject: [PATCH 72/79] ready to review code - OCP, OCWP --- .../src/OceanCurrentModelPlugin.cc | 12 ++---------- gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc | 10 ++++++---- .../dave_robot_models/description/rexrov/model.sdf | 2 +- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index fe55f7f4..4d5c131b 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -222,13 +222,11 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( if (currentVelocityParams->HasElement("topic_stratified")) { this->dataPtr->transientCurrentVelocityTopic = - "/hydrodynamics/" + currentVelocityParams->Get("topic_stratified") + - "_database"; + currentVelocityParams->Get("topic_stratified"); } else { - this->dataPtr->transientCurrentVelocityTopic = - "/hydrodynamics/stratified_current_velocity_database"; + this->dataPtr->transientCurrentVelocityTopic = "stratified_current_velocity_topic_database"; } // Read Gauss-Markov parameters @@ -648,12 +646,6 @@ void OceanCurrentModelPlugin::CalculateOceanCurrent(double vehicleDepth) void OceanCurrentModelPlugin::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - // Publish the Current Velocity. TODO find if we really need this - // if (!_info.paused && _info.simTime > this->dataPtr->lastUpdate) - // { - // this->dataPtr->lastUpdate = _info.simTime; - // PostPublishCurrentVelocity(); - // } this->dataPtr->lastUpdate = _info.simTime; PublishCurrentVelocity(_info); if (!_info.paused) diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index 0b02852e..9a11802e 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -143,7 +143,8 @@ void OceanCurrentPlugin::Configure( // Access the "real" currentHorzAngleModel auto & horzModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; // Set the topic for the stratified current velocity database - this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = "stratifiedCurrentVelocityTopic_database"; + this->dataPtr->stratifiedCurrentVelocityDatabaseTopic = + "stratified_current_velocity_topic_database"; if (_sdf->HasElement("namespace")) { @@ -155,9 +156,10 @@ void OceanCurrentPlugin::Configure( } // Reinitialize the ROS 2 node with the model namespace // TODO: Do we need this confirm with - // woen-sug : ) this->dataPtr->rosNode = - // std::make_shared("underwater_current_ros_plugin", - // this->dataPtr->model_namespace); + // woen-sug : ) + + this->dataPtr->rosNode = + std::make_shared("underwater_current_ros_plugin", this->dataPtr->model_namespace); // Create and advertise Messages // Advertise the flow velocity as a stamped twist message diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 97b09f9f..e328fc1f 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -896,7 +896,7 @@ base_link $(arg namespace)/base_link - + 0.0 0.0 From 393575256416843a73ff80434fa873d3fc92a125 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Wed, 22 Jan 2025 17:21:49 -0800 Subject: [PATCH 73/79] minor changes --- .../OceanCurrentModelPlugin.hh | 4 ++-- .../src/OceanCurrentModelPlugin.cc | 4 ++-- .../dave_ros_gz_plugins/OceanCurrentPlugin.hh | 18 +++++++++--------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh index a15a9786..d9b707d9 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh @@ -3,8 +3,8 @@ #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" +#include "dave_interfaces/msg/stratified_current_database.hpp" +#include "dave_interfaces/msg/stratified_current_velocity.hpp" // OceanCurrentModelPlugin diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index 4d5c131b..08a52f29 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -25,8 +25,8 @@ #include // Message interfaces includes -#include "dave_interfaces/msg/Stratified_Current_Database.hpp" -#include "dave_interfaces/msg/Stratified_Current_Velocity.hpp" +#include "dave_interfaces/msg/stratified_current_database.hpp" +#include "dave_interfaces/msg/stratified_current_velocity.hpp" // Standard library includes #include diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh index cb05e09f..d430ce32 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/OceanCurrentPlugin.hh @@ -21,14 +21,14 @@ #include "dave_interfaces/msg/stratified_current_velocity.hpp" // Dave Interfaces: Service Types -#include "dave_interfaces/srv/Get_Current_Model.hpp" -#include "dave_interfaces/srv/Get_Origin_Spherical_Coord.hpp" -#include "dave_interfaces/srv/Set_Current_Direction.hpp" -#include "dave_interfaces/srv/Set_Current_Model.hpp" -#include "dave_interfaces/srv/Set_Current_Velocity.hpp" -#include "dave_interfaces/srv/Set_Origin_Spherical_Coord.hpp" -#include "dave_interfaces/srv/Set_Stratified_Current_Direction.hpp" -#include "dave_interfaces/srv/Set_Stratified_Current_Velocity.hpp" +#include "dave_interfaces/srv/get_current_model.hpp" +#include "dave_interfaces/srv/get_origin_spherical_coord.hpp" +#include "dave_interfaces/srv/set_current_direction.hpp" +#include "dave_interfaces/srv/set_current_model.hpp" +#include "dave_interfaces/srv/set_current_velocity.hpp" +#include "dave_interfaces/srv/set_origin_spherical_coord.hpp" +#include "dave_interfaces/srv/set_stratified_current_direction.hpp" +#include "dave_interfaces/srv/set_stratified_current_velocity.hpp" // #include "dave_interfaces/srv/Stratified_Current_Database.hpp" // #include "dave_interfaces/srv/Stratified_Current_Velocity.hpp" @@ -111,4 +111,4 @@ private: }; } // namespace dave_ros_gz_plugins -#endif // DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ \ No newline at end of file +#endif // DAVE_ROS_GZ_PLUGINS__OCEAN_CURRENT_PLUGIN_HH_ From 8cfeada3d724c330d6084c7268b3b798ca6da2e8 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 25 Jan 2025 03:41:02 +0530 Subject: [PATCH 74/79] Evything's working ! including model plugin --- .../OceanCurrentModelPlugin.hh | 9 +++++++ .../src/OceanCurrentModelPlugin.cc | 27 ++++++++++++++----- .../description/rexrov/model.sdf | 24 ++++++++++++----- 3 files changed, 46 insertions(+), 14 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh index d9b707d9..5bd24c4b 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh @@ -56,6 +56,15 @@ public: void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); + // // Callback to receive incoming stratified or global current + // void OnIncomingCurrent(const gz::msgs::StratifiedCurrentVelocity & _msg); + + // // Publish final current to hydrodynamics plugin + // void PublishToHydrodynamics(const gz::math::Vector3d & _current); + + // // Helper to find the vehicle depth from ECM, etc. + // double ComputeVehicleDepth(gz::sim::EntityComponentManager & _ecm); + void CalculateOceanCurrent(double vehicleDepth); // ---------------------------------------------- diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index 08a52f29..ca0e6916 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -167,18 +167,25 @@ void OceanCurrentModelPlugin::Configure( this->dataPtr->modelName = _sdf->Get("namespace"); // Set the flow velocity topic from SDF or use the default topic name + + /// For Hydrodynamics Plugin - - This allows the robot to have an individual + /// namespace for current. This is useful when you have multiple vehicles in different + /// locations and you wish to set the currents of each vehicle separately. If no namespace is + /// given then the plugin listens on the `/ocean_current` topic for a `Vector3d` message. + /// Otherwise it listens on `/model/{namespace name}/ocean_current`.[String, Optional] + if (_sdf->HasElement("flow_velocity_topic")) { this->dataPtr->currentVelocityTopic = - _sdf->Get("flow_velocity_topic") + "/" + this->dataPtr->modelName; + "model/" + this->dataPtr->modelName + "/" + _sdf->Get("flow_velocity_topic"); + gzwarn << "Setting flow_velocity_topic to anything other than 'ocean_current' is not " + "recommended, as it might not be compatible with the hydrodynamics plugin." + << std::endl; } else { - this->dataPtr->currentVelocityTopic = - "hydrodynamics/current_velocity/" + this->dataPtr->modelName; - gzwarn << "Empty flow_velocity_topic for transient_current model plugin. Default topicName " - "definition is used." - << std::endl; + this->dataPtr->currentVelocityTopic = this->dataPtr->currentVelocityTopic = + "model/" + this->dataPtr->modelName + "/ocean_current"; } gzmsg << "Transient velocity topic name for " << this->dataPtr->modelName << " : " << this->dataPtr->currentVelocityTopic << std::endl; @@ -226,7 +233,8 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( } else { - this->dataPtr->transientCurrentVelocityTopic = "stratified_current_velocity_topic_database"; + this->dataPtr->transientCurrentVelocityTopic = + "hydrodynamics/stratified_current_velocity_topic_database"; } // Read Gauss-Markov parameters @@ -669,6 +677,11 @@ void OceanCurrentModelPlugin::PublishCurrentVelocity(const gz::sim::UpdateInfo & flowVelMsg.twist.linear.x = this->dataPtr->currentVelocity.X(); flowVelMsg.twist.linear.y = this->dataPtr->currentVelocity.Y(); flowVelMsg.twist.linear.z = this->dataPtr->currentVelocity.Z(); + + // For Testing with higher current velocities - + // flowVelMsg.twist.linear.x = 25; + // flowVelMsg.twist.linear.y = 25; + // flowVelMsg.twist.linear.z = 0; this->dataPtr->flowVelocityPub->publish(flowVelMsg); // Generate and publish Gazebo topic according to the vehicle depth diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index e328fc1f..c76750f2 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -212,9 +212,9 @@ 0.0151357 - + 1 - + + + + rexrov + + false + false + + 1028 + + base_link @@ -892,32 +902,32 @@ filename="OceanCurrentModelPlugin" name="dave_gz_model_plugins::OceanCurrentModelPlugin"> rexrov - hydrodynamics/current_velocity + ocean_current base_link $(arg namespace)/base_link - 0.0 + 1.0 0.0 0.3 0.0 - 0.0 + 1.0 0.0 0.3 0.0 - 0.0 + 1.0 0.0 0.3 0.0 false - + \ No newline at end of file From 5a1f21597a37907eb0b1a2bc5233b56cfb2044ee Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sat, 25 Jan 2025 05:15:26 +0530 Subject: [PATCH 75/79] code clean-up --- .../OceanCurrentModelPlugin.hh | 9 --------- .../src/OceanCurrentModelPlugin.cc | 19 +++---------------- .../description/rexrov/model.sdf | 2 +- 3 files changed, 4 insertions(+), 26 deletions(-) diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh index 5bd24c4b..d9b707d9 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh @@ -56,15 +56,6 @@ public: void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm); - // // Callback to receive incoming stratified or global current - // void OnIncomingCurrent(const gz::msgs::StratifiedCurrentVelocity & _msg); - - // // Publish final current to hydrodynamics plugin - // void PublishToHydrodynamics(const gz::math::Vector3d & _current); - - // // Helper to find the vehicle depth from ECM, etc. - // double ComputeVehicleDepth(gz::sim::EntityComponentManager & _ecm); - void CalculateOceanCurrent(double vehicleDepth); // ---------------------------------------------- diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index ca0e6916..8b5eee2b 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -1,11 +1,9 @@ -// Plugin includes #include "dave_gz_model_plugins/OceanCurrentModelPlugin.hh" #include "dave_gz_world_plugins/gauss_markov_process.hh" #include "dave_gz_world_plugins/tidal_oscillation.hh" // Gazebo includes #include -#include #include #include #include @@ -13,36 +11,25 @@ #include #include #include -#include #include #include #include -// ROS2 includes +// ROS 2 includes #include -#include #include -#include -// Message interfaces includes +// Custom message interfaces #include "dave_interfaces/msg/stratified_current_database.hpp" -#include "dave_interfaces/msg/stratified_current_velocity.hpp" // Standard library includes -#include -#include #include #include -#include #include +#include #include -#include #include -// Common utilities -#include "gz/common/StringUtils.hh" -#include "gz/plugin/Register.hh" - GZ_ADD_PLUGIN( dave_gz_model_plugins::OceanCurrentModelPlugin, gz::sim::System, dave_gz_model_plugins::OceanCurrentModelPlugin::ISystemConfigure, diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index c76750f2..4ebe854f 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -906,7 +906,7 @@ base_link $(arg namespace)/base_link - + 1.0 0.0 From 68cc23c89bcc8c39714c2f48d3a73318e880c8ad Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 26 Jan 2025 06:39:22 +0530 Subject: [PATCH 76/79] code cleanup --- .../msg/StratifiedCurrentVelocity.msg | 3 +-- dave_interfaces/package.xml | 1 + gazebo/dave_gz_model_plugins/CMakeLists.txt | 4 ++-- .../OceanCurrentModelPlugin.hh | 3 +-- .../src/OceanCurrentModelPlugin.cc | 6 ------ .../src/OceanCurrentWorldPlugin.cc | 13 ------------ .../src/OceanCurrentPlugin.cc | 21 ------------------- models/dave_worlds/package.xml | 1 + .../{ocp.world => ocean_current_plugin.world} | 0 9 files changed, 6 insertions(+), 46 deletions(-) rename models/dave_worlds/worlds/{ocp.world => ocean_current_plugin.world} (100%) diff --git a/dave_interfaces/msg/StratifiedCurrentVelocity.msg b/dave_interfaces/msg/StratifiedCurrentVelocity.msg index 997787bd..b59cbce7 100644 --- a/dave_interfaces/msg/StratifiedCurrentVelocity.msg +++ b/dave_interfaces/msg/StratifiedCurrentVelocity.msg @@ -6,5 +6,4 @@ std_msgs/Header header float32[] depths # Velocities -geometry_msgs/Vector3[] velocities - +geometry_msgs/Vector3[] velocities \ No newline at end of file diff --git a/dave_interfaces/package.xml b/dave_interfaces/package.xml index 0e24716b..a8653a72 100644 --- a/dave_interfaces/package.xml +++ b/dave_interfaces/package.xml @@ -4,6 +4,7 @@ 0.0.0 TODO: Package description lena + Gaurav Kumar TODO: License declaration ament_cmake rclcpp diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt index e1c6d471..fd3e02ac 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -12,8 +12,8 @@ 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) #r -find_package(gz-msgs10 REQUIRED) #r +find_package(Protobuf REQUIRED) +find_package(gz-msgs10 REQUIRED) find_package(dave_gz_world_plugins REQUIRED) # Set version variables diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh index d9b707d9..650eb237 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/OceanCurrentModelPlugin.hh @@ -8,9 +8,8 @@ // OceanCurrentModelPlugin -#include -// #include #include +#include #include #include diff --git a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc index 8b5eee2b..05a34732 100644 --- a/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc +++ b/gazebo/dave_gz_model_plugins/src/OceanCurrentModelPlugin.cc @@ -183,9 +183,6 @@ void OceanCurrentModelPlugin::Configure( // Read the stratified ocean current topic name from the SDF LoadCurrentVelocityParams(sdfClone, _ecm); - // Initialize the Gauss-Markov process - // Gauss_Markov_process_initialize(_sdf); - // Advertise the ROS flow velocity as a stamped twist message this->dataPtr->flowVelocityPub = this->ros_node_->create_publisher( @@ -224,9 +221,6 @@ void OceanCurrentModelPlugin::LoadCurrentVelocityParams( "hydrodynamics/stratified_current_velocity_topic_database"; } - // Read Gauss-Markov parameters - // sdf::ElementPtr currentVelocityDirection; - // initialize velocity_north_model parameters if (currentVelocityParams->HasElement("velocity_north")) { diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 8e343ed8..2eafbd86 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -97,7 +97,6 @@ OceanCurrentWorldPlugin::OceanCurrentWorldPlugin() } ///////////////////////////////////////////////// -// OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() = default; OceanCurrentWorldPlugin::~OceanCurrentWorldPlugin() { @@ -131,10 +130,6 @@ void OceanCurrentWorldPlugin::Configure( this->dataPtr->ns = _sdf->Get("namespace"); gzmsg << "Loading Ocean current world plugin" << std::endl; - // Initializing the transport node - // this->dataPtr->gz_node = std::make_shared(); - - // this->dataPtr->gz_node->Init(this->dataPtr->world.Name(_ecm)); // check if correct LoadGlobalCurrentConfig(); LoadStratifiedCurrentDatabase(); @@ -540,11 +535,6 @@ void OceanCurrentWorldPlugin::LoadStratifiedCurrentDatabase() ///////////////////////////////////////////////// void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() { - // NOTE: The plugin currently requires stratified current, so the - // global current set up in this method is potentially - // inconsistent or redundant. - // Consider setting it up as one or the other, but not both? - // Retrieve the velocity configuration, if existent sdf::ElementPtr currentVelocityParams; @@ -576,12 +566,9 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() if (currentVelocityParams->HasElement("velocity")) { sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity"); - // this->sharedDataPtr = std::make_shared(); if (elem->HasElement("mean")) { this->sharedDataPtr->currentVelModel.mean = elem->Get("mean"); - // this->sharedDataPtr->currentVelModel.mean = - // std::make_shared(elem->Get("mean")); } if (elem->HasElement("min")) { diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index 9a11802e..864828b2 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -176,15 +176,6 @@ void OceanCurrentPlugin::Configure( this->dataPtr->stratifiedCurrentDatabasePub = this->dataPtr->rosNode->create_publisher( this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, 1); - // // Advertise the stratified ocean current message - // this->dataPtr->stratifiedCurrentVelocityPub = - // this->dataPtr->rosNode->create_publisher( - // data.stratifiedCurrentVelocityTopic, rclcpp::QoS(10)); - - // // Advertise the stratified ocean current database message - // this->dataPtr->stratifiedCurrentDatabasePub = - // this->dataPtr->rosNode->create_publisher( - // this->dataPtr->stratifiedCurrentVelocityDatabaseTopic, rclcpp::QoS(10)); // Advertise the service to get the current velocity model this->dataPtr->get_current_velocity_model = @@ -263,13 +254,6 @@ void OceanCurrentPlugin::Configure( &OceanCurrentPlugin::UpdateStratHorzAngle, this, std::placeholders::_1, std::placeholders::_2)); - // // Advertise the service to update the current vertical angle - // this->dataPtr->set_current_vert_angle_model = - // this->dataPtr->rosNode->create_service( - // "set_current_vert_angle_model", std::bind( - // &OceanCurrentPlugin::UpdateVertAngleModel, this, - // std::placeholders::_1, std::placeholders::_2)); - // Advertise the service to update the stratified current vertical angle this->dataPtr->set_stratified_current_vert_angle = this->dataPtr->rosNode->create_service( @@ -292,8 +276,6 @@ bool OceanCurrentPlugin::UpdateHorzAngle( auto & horzModel = worldPlugin->sharedDataPtr->currentHorzAngleModel; _res->success = horzModel.SetMean(_req->angle); return true; - // _res->success = data.currentHorzAngleModel.SetMean(_req->angle); - // return true; } ///////////////////////////////////////////////// @@ -646,8 +628,6 @@ void OceanCurrentPlugin::PostUpdate( stratCurrentVelocityMsg.header.frame_id = "world"; // Updating for stratified behaviour of Ocean Currents - // What is the .size value over here, to be (checked) - for (size_t i = 0; i < sharedPtr->currentStratifiedVelocity.size(); i++) { geometry_msgs::msg::Vector3 velocity; @@ -661,7 +641,6 @@ void OceanCurrentPlugin::PostUpdate( this->dataPtr->stratifiedCurrentVelocityPub->publish(stratCurrentVelocityMsg); // Generate and publish stratified current database - // dave_interfaces::msg::StratifiedCurrentDatabase currentDatabaseMsg; auto currentDatabaseMsg = dave_interfaces::msg::StratifiedCurrentDatabase(); for (int i = 0; i < sharedPtr->stratifiedDatabase.size(); i++) { diff --git a/models/dave_worlds/package.xml b/models/dave_worlds/package.xml index c9aeb527..daf6f6e7 100644 --- a/models/dave_worlds/package.xml +++ b/models/dave_worlds/package.xml @@ -4,6 +4,7 @@ 0.0.0 TODO: Package description lena + Gaurav Kumar Apache-2.0 ament_cmake ament_lint_auto diff --git a/models/dave_worlds/worlds/ocp.world b/models/dave_worlds/worlds/ocean_current_plugin.world similarity index 100% rename from models/dave_worlds/worlds/ocp.world rename to models/dave_worlds/worlds/ocean_current_plugin.world From 0f0c5fe5eaa18b277f5ce62379540776c23b450c Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Sun, 26 Jan 2025 06:53:53 +0530 Subject: [PATCH 77/79] timemachine --- .docker/jazzy.arm64v8.dockerfile | 2 +- examples/dave_demos/launch/dave_robot.launch.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.docker/jazzy.arm64v8.dockerfile b/.docker/jazzy.arm64v8.dockerfile index dc2243d5..ab6235a2 100644 --- a/.docker/jazzy.arm64v8.dockerfile +++ b/.docker/jazzy.arm64v8.dockerfile @@ -78,7 +78,7 @@ # Using the pre-built image for above commented out dockerfile code lines # hadolint ignore=DL3007 -FROM --platform=linux/arm64 woensugchoi/ubuntu-arm-rdp-base:latest +FROM woensugchoi/ubuntu-arm-rdp-base:latest ARG USER=docker # ROS-Gazebo arg diff --git a/examples/dave_demos/launch/dave_robot.launch.py b/examples/dave_demos/launch/dave_robot.launch.py index 17f0f6e5..26c21afc 100644 --- a/examples/dave_demos/launch/dave_robot.launch.py +++ b/examples/dave_demos/launch/dave_robot.launch.py @@ -39,8 +39,7 @@ def launch_setup(context, *args, **kwargs): gz_args.append(" -r") if debug.perform(context) == "true": gz_args.append(" -v ") - if verbose.perform(context) == "true": - gz_args.append(" -v ") + gz_args.append(verbose.perform(context)) # Include the first launch file gz_sim_launch = IncludeLaunchDescription( From e2c54ed572d6e89112c873dd7fc36b0ded1fb662 Mon Sep 17 00:00:00 2001 From: Gaurav Kumar <84905312+GauravKumar9920@users.noreply.github.com> Date: Tue, 28 Jan 2025 07:06:13 +0530 Subject: [PATCH 78/79] added the flag to switch between the constant and stratified ocean currents --- .../OceanCurrentWorldPlugin.hh | 1 + .../src/OceanCurrentWorldPlugin.cc | 32 +++++++++++++------ .../src/OceanCurrentPlugin.cc | 32 +++++++++++-------- .../description/rexrov/model.sdf | 9 +++--- .../worlds/ocean_current_plugin.world | 3 +- 5 files changed, 48 insertions(+), 29 deletions(-) diff --git a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh index d0a43bf3..1d110683 100644 --- a/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh +++ b/gazebo/dave_gz_world_plugins/include/dave_gz_world_plugins/OceanCurrentWorldPlugin.hh @@ -78,6 +78,7 @@ public: gz::math::Vector3d currentVelocity; // Current linear velocity vector std::vector currentStratifiedVelocity; // Depth-specific linear velocity vectors for stratified current + bool use_constant_current; // Flag to check if constant current is to be used // Tidal harmonic data (M2, S2, N2 constituents) double M2_amp, M2_phase, M2_speed; // M2 tidal constituent diff --git a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc index 2eafbd86..e1084472 100644 --- a/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc +++ b/gazebo/dave_gz_world_plugins/src/OceanCurrentWorldPlugin.cc @@ -547,15 +547,24 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() gzerr << "Current configuration not available" << std::endl; } + if (currentVelocityParams->HasElement("use_constant_current")) + { + this->sharedDataPtr->use_constant_current = + currentVelocityParams->Get("use_constant_current"); + } + else + { + this->sharedDataPtr->use_constant_current = false; + } + // Read the topic names from the SDF file if (currentVelocityParams->HasElement("topic")) { - this->sharedDataPtr->currentVelocityTopic = - this->dataPtr->ns + "/" + currentVelocityParams->Get("topic"); + this->sharedDataPtr->currentVelocityTopic = currentVelocityParams->Get("topic"); } else { - this->sharedDataPtr->currentVelocityTopic = "current_velocity"; + this->sharedDataPtr->currentVelocityTopic = "ocean_current"; } if (this->sharedDataPtr->currentVelocityTopic.empty()) @@ -745,11 +754,13 @@ void OceanCurrentWorldPlugin::LoadGlobalCurrentConfig() this->sharedDataPtr->currentVertAngleModel.lastUpdate = std::chrono::duration(this->dataPtr->lastUpdate).count(); - // Advertise the current velocity & stratified current velocity topics - this->dataPtr->gz_node_cvel_world_pub = - this->dataPtr->gz_node.Advertise(this->sharedDataPtr->currentVelocityTopic); - gzmsg << "Current velocity topic name: " << this->sharedDataPtr->currentVelocityTopic - << std::endl; + if (this->sharedDataPtr->use_constant_current) + { // Advertise the current velocity & stratified current velocity topics + this->dataPtr->gz_node_cvel_world_pub = this->dataPtr->gz_node.Advertise( + this->sharedDataPtr->currentVelocityTopic); + gzmsg << "Current velocity topic name: " << this->sharedDataPtr->currentVelocityTopic + << std::endl; + } } // ---------------------------------------------- @@ -830,7 +841,10 @@ void OceanCurrentWorldPlugin::PostUpdate( // gzmsg << "Ocean current world plugin post update" << std::endl; // Update time stamp this->dataPtr->lastUpdate = _info.simTime; - PublishCurrentVelocity(); + if (this->sharedDataPtr->use_constant_current) + { + PublishCurrentVelocity(); + } PublishStratifiedCurrentVelocity(); } // ---------------------------------------------- diff --git a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc index 864828b2..59841b3b 100644 --- a/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/gazebo/dave_ros_gz_plugins/src/OceanCurrentPlugin.cc @@ -161,11 +161,13 @@ void OceanCurrentPlugin::Configure( this->dataPtr->rosNode = std::make_shared("underwater_current_ros_plugin", this->dataPtr->model_namespace); - // Create and advertise Messages - // Advertise the flow velocity as a stamped twist message - this->dataPtr->flowVelocityPub = - this->dataPtr->rosNode->create_publisher( - "currentVelocityTopic", 1); + if (sharedPtr->use_constant_current) + { // Create and advertise Messages + // Advertise the flow velocity as a stamped twist message + this->dataPtr->flowVelocityPub = + this->dataPtr->rosNode->create_publisher( + "currentVelocityTopic", 1); + } // Advertise the stratified ocean current message this->dataPtr->stratifiedCurrentVelocityPub = @@ -608,18 +610,20 @@ void OceanCurrentPlugin::PostUpdate( return; } - // Generate and publish current velocity according to the vehicle depth - // geometry_msgs::msg::TwistStamped flowVelMsg; - auto flowVelMsg = geometry_msgs::msg::TwistStamped(); + // Publish constant current velocity + if (sharedPtr->use_constant_current) + { + auto flowVelMsg = geometry_msgs::msg::TwistStamped(); - flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); - flowVelMsg.header.frame_id = "world"; + flowVelMsg.header.stamp = this->dataPtr->rosNode->get_clock()->now(); + flowVelMsg.header.frame_id = "world"; - flowVelMsg.twist.linear.x = sharedPtr->currentVelocity.X(); - flowVelMsg.twist.linear.y = sharedPtr->currentVelocity.Y(); - flowVelMsg.twist.linear.z = sharedPtr->currentVelocity.Z(); + flowVelMsg.twist.linear.x = sharedPtr->currentVelocity.X(); + flowVelMsg.twist.linear.y = sharedPtr->currentVelocity.Y(); + flowVelMsg.twist.linear.z = sharedPtr->currentVelocity.Z(); - this->dataPtr->flowVelocityPub->publish(flowVelMsg); + this->dataPtr->flowVelocityPub->publish(flowVelMsg); + } // Generate and publish stratified current velocity auto stratCurrentVelocityMsg = dave_interfaces::msg::StratifiedCurrentVelocity(); diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 4ebe854f..c6d87e4a 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -812,7 +812,7 @@ name="gz::sim::systems::Hydrodynamics"> - rexrov + false false @@ -897,8 +897,7 @@ - - rexrov @@ -906,7 +905,7 @@ base_link $(arg namespace)/base_link - + hydrodynamics/stratified_current_velocity_topic_database 1.0 0.0 @@ -927,7 +926,7 @@ false - + --> \ No newline at end of file diff --git a/models/dave_worlds/worlds/ocean_current_plugin.world b/models/dave_worlds/worlds/ocean_current_plugin.world index 80c45841..a47a92f3 100755 --- a/models/dave_worlds/worlds/ocean_current_plugin.world +++ b/models/dave_worlds/worlds/ocean_current_plugin.world @@ -119,7 +119,8 @@ hydrodynamics - current_velocity + true + ocean_current 2 0 From e75dd244824db553d94070b25fc74b2e241f2185 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Wed, 5 Feb 2025 11:18:40 +0900 Subject: [PATCH 79/79] for clean build outpout --- gazebo/dave_gz_world_plugins/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gazebo/dave_gz_world_plugins/CMakeLists.txt b/gazebo/dave_gz_world_plugins/CMakeLists.txt index 448eddf7..b5b1cfb0 100644 --- a/gazebo/dave_gz_world_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_world_plugins/CMakeLists.txt @@ -3,6 +3,12 @@ cmake_minimum_required(VERSION 3.8) #Define the project name project(dave_gz_world_plugins) +# Suppress the parameter passing ABI warning +add_compile_options(-Wno-psabi) + +# Define BOOST_BIND_GLOBAL_PLACEHOLDERS to suppress Boost deprecation warning +add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS) + find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(gz-cmake3 REQUIRED)