diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e4c3df74..7afa14da 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -80,24 +80,25 @@ repos: - id: clang-format args: ["-fallback-style=none", "-i"] - - repo: local - hooks: - - id: ament_cppcheck - name: ament_cppcheck - description: Static code analysis of C/C++ files. - entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + # - repo: local + # hooks: + # - id: ament_cppcheck + # name: ament_cppcheck + # description: Static code analysis of C/C++ files. + # entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - - repo: local - hooks: - - id: ament_cpplint - name: ament_cpplint - description: Static code analysis of C/C++ files. - entry: ament_cpplint - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: ["--linelength=100", "--filter=-whitespace/newline"] + # - repo: https://github.com/cpplint/cpplint + # rev: 1.6.1 + # hooks: + # - id: cpplint + # name: cpplint + # description: Static code analysis of C/C++ files + # language: python + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + # entry: cpplint + # args: ["--linelength=100", "--filter=-whitespace/newline"] # Cmake hooks (can be installed with pip3 install ament-lint-cmake-py) - repo: local diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt new file mode 100644 index 00000000..acb0f468 --- /dev/null +++ b/dave_interfaces/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(dave_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/UsblCommand.msg" + "msg/UsblResponse.msg" + "msg/Location.msg" +) + +# Install message package.xml +install(FILES + package.xml + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies(rosidl_default_runtime) +# Install CMake package configuration +ament_export_include_directories(include) + +ament_package() diff --git a/dave_interfaces/msg/Location.msg b/dave_interfaces/msg/Location.msg new file mode 100644 index 00000000..1576b20f --- /dev/null +++ b/dave_interfaces/msg/Location.msg @@ -0,0 +1,4 @@ +int32 transponder_id +float64 x +float64 y +float64 z \ No newline at end of file diff --git a/dave_interfaces/msg/UsblCommand.msg b/dave_interfaces/msg/UsblCommand.msg new file mode 100644 index 00000000..c000b821 --- /dev/null +++ b/dave_interfaces/msg/UsblCommand.msg @@ -0,0 +1,3 @@ +int32 transponder_id +int32 command_id +string data \ No newline at end of file diff --git a/dave_interfaces/msg/UsblResponse.msg b/dave_interfaces/msg/UsblResponse.msg new file mode 100644 index 00000000..49ff7a39 --- /dev/null +++ b/dave_interfaces/msg/UsblResponse.msg @@ -0,0 +1,3 @@ +int32 transceiver_id +int32 response_id +string data \ No newline at end of file diff --git a/dave_interfaces/package.xml b/dave_interfaces/package.xml new file mode 100644 index 00000000..dc6dfa89 --- /dev/null +++ b/dave_interfaces/package.xml @@ -0,0 +1,19 @@ + + + dave_interfaces + 0.0.0 + TODO: Package description + lena + TODO: License declaration + ament_cmake + rclcpp + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_lint_auto + ament_lint_common + + ament_cmake + + \ No newline at end of file diff --git a/examples/dave_demos/launch/dave_world.launch.py b/examples/dave_demos/launch/dave_world.launch.py index 329a0864..32f360db 100644 --- a/examples/dave_demos/launch/dave_world.launch.py +++ b/examples/dave_demos/launch/dave_world.launch.py @@ -12,14 +12,19 @@ def launch_setup(context, *args, **kwargs): pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim") world_name = LaunchConfiguration("world_name").perform(context) + verbose_flag = LaunchConfiguration("verbose").perform(context) world_file_name = f"{world_name}.world" world_path = os.path.join(pkg_dave_worlds, "worlds", world_file_name) # Gazebo simulation launch + gz_args = f"-r {world_path}" + if verbose_flag.lower() == "true": + gz_args += " --verbose" + gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")), - launch_arguments={"gz_args": f"-r {world_path}"}.items(), + launch_arguments={"gz_args": gz_args}.items(), ) return [gz_sim] @@ -30,9 +35,14 @@ def generate_launch_description(): [ DeclareLaunchArgument( "world_name", - default_value="dave_bimanual_example", # Default world file name without extension + default_value="dave_bimanual_example", description="Name of the world file", ), + DeclareLaunchArgument( + "verbose", + default_value="false", + description="Enable verbose mode for Gazebo simulation", + ), OpaqueFunction(function=launch_setup), ] ) diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt new file mode 100644 index 00000000..837afe30 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) + +# Define the project name +project(dave_gz_sensor_plugins) + +# Find required packages +find_package(ament_cmake REQUIRED) +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 +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(UsblTransceiver SHARED src/UsblTransceiver.cc) +add_library(UsblTransponder SHARED src/UsblTransponder.cc) + +target_include_directories(UsblTransceiver PRIVATE include) +target_include_directories(UsblTransponder PRIVATE include) + +target_link_libraries(UsblTransceiver + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + +target_link_libraries(UsblTransponder +gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + + +# Specify dependencies for FullSystem using ament_target_dependencies +ament_target_dependencies(UsblTransceiver + dave_interfaces + rclcpp + geometry_msgs + std_msgs +) + +ament_target_dependencies(UsblTransponder + dave_interfaces + rclcpp + std_msgs +) + +# Install targets +install(TARGETS UsblTransceiver UsblTransponder + DESTINATION lib/${PROJECT_NAME} +) + +# Install headers +install(DIRECTORY include/ + DESTINATION include/ +) + +# Environment hooks +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") + +# Testing setup +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Configure ament +ament_package() diff --git a/gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in b/gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in new file mode 100644 index 00000000..3d270f30 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh new file mode 100644 index 00000000..24dac936 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + * + */ + +#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ +#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "dave_interfaces/msg/location.hpp" +#include "dave_interfaces/msg/usbl_command.hpp" +#include "dave_interfaces/msg/usbl_response.hpp" + +namespace dave_gz_sensor_plugins + +{ +class UsblTransceiver : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + UsblTransceiver(); + ~UsblTransceiver() override = default; + + void Configure( + const gz::sim::Entity & entity, const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override; + + void PostUpdate( + const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; + + void receiveGazeboCallback( + const std::string & transponder, const gz::msgs::Vector3d & transponder_position); + void temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg); + void interrogationModeRosCallback(const std_msgs::msg::String::SharedPtr msg); + void commandingResponseCallback(const dave_interfaces::msg::UsblResponse msg); + void channelSwitchCallback(const std_msgs::msg::String::SharedPtr msg); + void commandingResponseTestCallback(const std_msgs::msg::String::SharedPtr msg); + void sendCommand(int command_id, std::string & transponder_id); + void sendPing(); + void calculateRelativePose( + gz::math::Vector3 position, double & bearing, double & range, double & elevation); + void publishPosition(int & transponder_id, double & bearing, double & range, double & elevation); + +private: + std::shared_ptr ros_node_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_gz_sensor_plugins + +#endif // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh new file mode 100644 index 00000000..6cf6cdd1 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + * + */ + +#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ +#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ + +#include +#include + +#include + +#include +#include +#include +#include "dave_interfaces/msg/usbl_command.hpp" +#include "dave_interfaces/msg/usbl_response.hpp" + +namespace dave_gz_sensor_plugins + +{ +class UsblTransponder : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + UsblTransponder(); + ~UsblTransponder() override = default; + + void Configure( + const gz::sim::Entity & entity, const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override; + + void PostUpdate( + const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; + + void sendLocation(); + void iisRosCallback(const std_msgs::msg::String::SharedPtr msg); + void temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg); + void cisRosCallback(const std_msgs::msg::String::SharedPtr msg); + void commandRosCallback(const dave_interfaces::msg::UsblCommand msg); + void sendPing(); + void interrogationModeCallback(const std_msgs::msg::String::SharedPtr msg); + +private: + std::shared_ptr ros_node_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_gz_sensor_plugins + +#endif // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml new file mode 100644 index 00000000..b1c657d6 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -0,0 +1,15 @@ + + + dave_gz_sensor_plugins + 0.0.0 + DAVE sensor plugins + Helena Moyen + Apache 2.0 + ament_cmake + ament_lint_auto + dave_interfaces + dave_interfaces + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc new file mode 100644 index 00000000..05b9c52a --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc @@ -0,0 +1,631 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + */ + +#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 + +#include "dave_gz_sensor_plugins/UsblTransceiver.hh" + +// available interrogation modes +std::vector im = {"common", "individual"}; +using std::placeholders::_1; + +GZ_ADD_PLUGIN( + dave_gz_sensor_plugins::UsblTransceiver, gz::sim::System, + dave_gz_sensor_plugins::UsblTransceiver::ISystemConfigure, + dave_gz_sensor_plugins::UsblTransceiver::ISystemPostUpdate) + +namespace dave_gz_sensor_plugins +{ + +struct UsblTransceiver::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + std::string modelName; + std::string ns; + std::string m_transceiverDevice; + gz::sim::EntityComponentManager * ecm; + std::string m_transceiverID; + std::vector m_transponderEntities; + std::string m_transponderDevice; + std::vector m_deployedTransponders; + std::vector m_transponderAttachedObjects; + bool m_enablePingerScheduler; + std::string m_transponderAttachedObject; + std::string m_interrogationMode; + gz::sim::Entity linkEntity; + gz::transport::Node m_gzNode; + rclcpp::Publisher::SharedPtr m_publishTransponderRelPos; + rclcpp::Publisher::SharedPtr m_cisPinger; + rclcpp::Publisher::SharedPtr m_interrogationModePub; + std::unordered_map::SharedPtr> m_iisPinger; + std::unordered_map::SharedPtr> + m_commandPubs; + rclcpp::Publisher::SharedPtr m_publishTransponderRelPosCartesian; + rclcpp::Subscription::SharedPtr m_temperatureSub; + rclcpp::Subscription::SharedPtr m_interrogationModeSub; + rclcpp::Subscription::SharedPtr m_commandResponseSub; + rclcpp::Subscription::SharedPtr m_channelSwitchSub; + rclcpp::Subscription::SharedPtr m_commandResponseTestSub; + std::unordered_map> + transponderCallbacks; + std::string m_channel; + double m_soundSpeed; + double m_temperature; + double m_pingFrequency; + bool setGlobalMode; +}; + +// // Define constants for command IDs +const int BATTERY_LEVEL = 1; +const int GO_TO = 2; + +UsblTransceiver::UsblTransceiver() : dataPtr(std::make_unique()) +{ + dataPtr->m_channel = "1"; + dataPtr->m_temperature = 10.0; + dataPtr->setGlobalMode = false; +} + +void UsblTransceiver::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_gz_sensor_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("usbl_transceiver_node"); + + auto model = gz::sim::Model(_entity); + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + this->dataPtr->ecm = &_ecm; + + auto links = this->dataPtr->model.Links(_ecm); + + if (!links.empty()) + { + auto linkEntity = links.front(); + auto linkName = _ecm.Component(linkEntity); + std::string linkNameStr = linkName->Data(); + gzmsg << "Principal link name: " << linkNameStr << std::endl; + this->dataPtr->linkEntity = model.LinkByName(_ecm, linkNameStr); + std::cout << worldPose(this->dataPtr->linkEntity, _ecm) << std::endl; + } + else + { + gzmsg << "No links found in the model." << std::endl; + } + + if (!model.Valid(_ecm)) + { + gzerr << "UsblTransceiver plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + // Grab namespace from SDF + if (!_sdf->HasElement("namespace")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->ns = _sdf->Get("namespace"); + + // Grab sound speed from SDF + if (!_sdf->HasElement("sound_speed")) + { + this->dataPtr->m_soundSpeed = 1540.0; + gzmsg << "Sound speed default to " << this->dataPtr->m_soundSpeed << std::endl; + } + else + { + this->dataPtr->m_soundSpeed = _sdf->Get("sound_speed"); + gzmsg << "Sound speed: " << this->dataPtr->m_soundSpeed << std::endl; + } + + // Obtain transceiver device name from SDF + if (!_sdf->HasElement("transceiver_device")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + this->dataPtr->m_transceiverDevice = _sdf->Get("transceiver_device"); + gzmsg << "Entity: " << this->dataPtr->m_transceiverDevice << std::endl; + + // Get transceiver device id + if (!_sdf->HasElement("transceiver_ID")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + this->dataPtr->m_transceiverID = _sdf->Get("transceiver_ID"); + + // get transponder device name + if (!_sdf->HasElement("transponder_device")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->m_transponderDevice = _sdf->Get("transponder_device"); + gzmsg << "Transponder device: " << this->dataPtr->m_transponderDevice << std::endl; + + // get commanding transponders + if (!_sdf->HasElement("transponder_ID")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + auto transponders = gz::common::Split(_sdf->Get("transponder_ID"), ','); + gzmsg << "Current deployed transponders are: \n"; + + for (auto & transponder : transponders) + { + gzmsg << transponder << std::endl; + this->dataPtr->m_deployedTransponders.push_back(transponder); + } + + // Enable automation of sending pings to transponder + if (!_sdf->HasElement("enable_ping_scheduler")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + this->dataPtr->m_enablePingerScheduler = _sdf->Get("enable_ping_scheduler"); + + if (this->dataPtr->m_enablePingerScheduler) + { + if (!_sdf->HasElement("ping_frequency")) + { + gzmsg << "Ping frequency not specified, default to 1 Hz" << std::endl; + this->dataPtr->m_pingFrequency = 1; + } + else + { + this->dataPtr->m_pingFrequency = _sdf->Get("ping_frequency"); + } + } + gzmsg << "Pinger enable? " << this->dataPtr->m_enablePingerScheduler << std::endl; + + // Get object that transponder attached to + if (!_sdf->HasElement("transponder_attached_object")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + auto transpondersObjects = + gz::common::Split(_sdf->Get("transponder_attached_object"), ','); + gzmsg << "Current deployed transponders objects are: \n"; + + for (auto & object : transpondersObjects) + { + this->dataPtr->m_transponderAttachedObjects.push_back(object); + gzmsg << "Added " << object << " to list of transponders objects" << std::endl; + } + + /* interrogation mode - 2 options + * II (individual interrogation) <-----> CRS (common response signal) + * CI (common interrogation) <-----> IRS (individual response + * signal) from transponder_01 + * ͱ-> IRS from transponder_02 + * ͱ-> IRS from transponder_03 + * ⋮ + */ + + if (_sdf->HasElement("interrogation_mode")) + { + std::string interrogation_mode = _sdf->Get("interrogation_mode"); + gzmsg << interrogation_mode << " interrogation mode is requested" << std::endl; + if (std::find(im.begin(), im.end(), interrogation_mode) != im.end()) + { + gzmsg << interrogation_mode << " interrogation mode is used" << std::endl; + this->dataPtr->m_interrogationMode = interrogation_mode; + } + else + { + gzmsg << "Specified interrogation mode is unavailable, " + << "Common mode is used" << std::endl; + this->dataPtr->m_interrogationMode = "common"; + } + } + else + { + gzmsg << "Interrogation mode is not specified, Common mode is used" << std::endl; + this->dataPtr->m_interrogationMode = "common"; + } + + // Gazebo subscriber + + for (auto & transponder : this->dataPtr->m_deployedTransponders) + { + std::string transponder_position = "/" + this->dataPtr->ns + "/" + + this->dataPtr->m_transceiverDevice + "_" + transponder + + "/global_position"; + + gzmsg << "Transponder topic" << transponder_position << std::endl; + + // Created lambda function to get the transponder ID in the created callback + + std::function callback = + [this, transponder](const gz::msgs::Vector3d & msg) + { this->receiveGazeboCallback(transponder, msg); }; + + this->dataPtr->transponderCallbacks[transponder] = callback; + this->dataPtr->m_gzNode.Subscribe(transponder_position, callback); + } + + // ROS2 Publishers + + std::string transponder_location_topic = "/" + this->dataPtr->ns + "/" + + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/transponder_location"; + + std::string interrogation_mode_topic = "/" + this->dataPtr->ns + "/" + + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/interrogation_mode"; + + std::string cis_pinger_topic = "/" + this->dataPtr->ns + "/common_interrogation_ping"; + + this->dataPtr->m_publishTransponderRelPos = + this->ros_node_->create_publisher( + transponder_location_topic, 1); + + this->dataPtr->m_cisPinger = + this->ros_node_->create_publisher(cis_pinger_topic, 1); + + this->dataPtr->m_interrogationModePub = + this->ros_node_->create_publisher(interrogation_mode_topic, 1); + + for (auto & transponder : this->dataPtr->m_deployedTransponders) + { + std::string ping_topic( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transponderDevice + "_" + transponder + + "/individual_interrogation_ping"); + std::string command_topic( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transponderDevice + "_" + transponder + + "/command_request"); + this->dataPtr->m_iisPinger[transponder] = + this->ros_node_->create_publisher(ping_topic, 1); + this->dataPtr->m_commandPubs[transponder] = + this->ros_node_->create_publisher(command_topic, 1); + } + std::string transponder_location_cartesian_topic = + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/transponder_location_cartesian"; + + this->dataPtr->m_publishTransponderRelPosCartesian = + this->ros_node_->create_publisher( + transponder_location_cartesian_topic, 1); + + // ROS2 Subscribers + + this->dataPtr->m_temperatureSub = this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/temperature", + 1, std::bind(&UsblTransceiver::temperatureRosCallback, this, _1)); + + this->dataPtr->m_interrogationModeSub = + this->ros_node_->create_subscription( + interrogation_mode_topic, 1, + std::bind(&UsblTransceiver::interrogationModeRosCallback, this, _1)); + + this->dataPtr->m_commandResponseSub = + this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/command_response", + 1, std::bind(&UsblTransceiver::commandingResponseCallback, this, _1)); + + this->dataPtr->m_channelSwitchSub = this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/channel_switch", + 1, std::bind(&UsblTransceiver::channelSwitchCallback, this, _1)); + + this->dataPtr->m_commandResponseTestSub = + this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/command_response_test", + 1, std::bind(&UsblTransceiver::commandingResponseTestCallback, this, _1)); +} + +void UsblTransceiver::commandingResponseTestCallback(const std_msgs::msg::String::SharedPtr msg) +{ + std::string transponder_id = msg->data; + sendCommand(BATTERY_LEVEL, transponder_id); +} + +void UsblTransceiver::sendCommand(int command_id, std::string & transponder_id) +{ + dave_interfaces::msg::UsblCommand command; + command.command_id = command_id; + command.transponder_id = std::stoi(transponder_id); + + if (command_id == BATTERY_LEVEL) + { + command.data = "Report battery level"; + } + else if (command_id == GO_TO) + { + command.data = "Go to this location"; + } + else + { + command.data = "This is dummy message"; + } + + this->dataPtr->m_commandPubs[transponder_id]->publish(command); +} + +void UsblTransceiver::sendPing() +{ + std_msgs::msg::String ping_msg; + ping_msg.data = "ping"; + + for (auto & object : this->dataPtr->m_transponderAttachedObjects) + { + auto transponder = this->dataPtr->ecm->EntityByComponents( + gz::sim::components::Name(object), gz::sim::components::Model()); + + gz::math::Pose3d pose_transponder = worldPose(transponder, *this->dataPtr->ecm); + gzmsg << "Transponder pose" << pose_transponder << std::endl; + + gz::math::Pose3d pose_transceiver = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gzmsg << "Transceiver pose" << pose_transceiver << std::endl; + + double dist = (pose_transponder.Pos() - pose_transceiver.Pos()).Length(); + this->dataPtr->m_soundSpeed = 1540.4 + pose_transceiver.Z() / 1000 * 17; + + sleep(dist / this->dataPtr->m_soundSpeed); + + if (this->dataPtr->m_interrogationMode.compare("common") == 0) + { + this->dataPtr->m_cisPinger->publish(ping_msg); + } + else if (this->dataPtr->m_interrogationMode.compare("individual") == 0) + { + this->dataPtr->m_iisPinger[this->dataPtr->m_channel]->publish(ping_msg); + } + else + { + gzmsg << "Interrogation mode not recognized\n"; + } + } +} + +void UsblTransceiver::channelSwitchCallback(const std_msgs::msg::String::SharedPtr msg) +{ + gzmsg << "Switching to transponder_" << msg->data << " channel\n"; + this->dataPtr->m_channel = msg->data; + + this->dataPtr->m_interrogationMode = "individual"; + + std_msgs::msg::String mode; + mode.data = this->dataPtr->m_interrogationMode; + + this->dataPtr->m_interrogationModePub->publish(mode); + + gzmsg << "Interrogation mode set to individual, because channel switch was requested" + << std::endl; +} + +void UsblTransceiver::commandingResponseCallback(const dave_interfaces::msg::UsblResponse msg) +{ + gzmsg << "Response_id: " << msg.response_id << ", transponder id: " << msg.transceiver_id + << ", data: " << msg.data << std::endl; +} + +void UsblTransceiver::interrogationModeRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + std::string mode = msg->data; + if (std::find(im.begin(), im.end(), mode) != im.end()) + { + this->dataPtr->m_interrogationMode = mode; + gzmsg << "Interrogation mode set to " << mode << std::endl; + } + else + { + gzmsg << "The input mode is not available\n"; + } +} + +void UsblTransceiver::temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg) +{ + gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Vector3 position = pose.Pos(); + + this->dataPtr->m_temperature = msg->data; + + // Base on https://dosits.org/tutorials/science/tutorial-speed/ + this->dataPtr->m_soundSpeed = + 1540.4 + position.Z() / 1000 * 17 + (this->dataPtr->m_temperature - 10) * 4; + gzmsg << "Detected change of temperature, transceiver sound speed is now: " + << this->dataPtr->m_soundSpeed << " m/s\n"; +} + +void UsblTransceiver::receiveGazeboCallback( + const std::string & transponder, const gz::msgs::Vector3d & transponder_position) +{ + bool publish; + + if (this->dataPtr->m_interrogationMode.compare("common") == 0) + { + gzmsg << "In common mode, publishing position..." << std::endl; + + publish = true; + } + else if ( + this->dataPtr->m_interrogationMode.compare("individual") == 0 && + transponder.compare(this->dataPtr->m_channel) == 0) + { + publish = true; + + gzmsg << "In individual mode, publishing position for transponder_" << transponder << std::endl; + } + else + { + publish = false; + + gzmsg << "Interrogation mode is not set to common and wrong channel is being pinged" + << std::endl; + } + + if (publish) + { + gzmsg << "Transceiver acquires transponder_" << transponder + << " position: " << transponder_position.x() << " " << transponder_position.y() << " " + << transponder_position.z() << std::endl; + + int transponder_id; + + try + { + transponder_id = std::stoi(transponder); + } + catch (const std::invalid_argument & e) + { + gzmsg << "Invalid transponder ID: " << transponder_id << std::endl; + + return; + } + catch (const std::out_of_range & e) + { + gzmsg << "Transponder ID out of range: " << transponder_id << std::endl; + + return; + } + + gz::math::Vector3 transponder_position_ign = gz::math::Vector3( + transponder_position.x(), transponder_position.y(), transponder_position.z()); + + double bearing = 0, range = 0, elevation = 0; + calculateRelativePose(transponder_position_ign, bearing, range, elevation); + + publishPosition(transponder_id, bearing, range, elevation); + } +} + +void UsblTransceiver::publishPosition( + int & transponder_id, double & bearing, double & range, double & elevation) +{ + dave_interfaces::msg::Location location; + + location.x = bearing; + location.y = range; + location.z = elevation; + + dave_interfaces::msg::Location location_cartesian; + location_cartesian.x = range * cos(elevation * M_PI / 180) * cos(bearing * M_PI / 180); + location_cartesian.y = range * cos(elevation * M_PI / 180) * sin(bearing * M_PI / 180); + location_cartesian.z = range * sin(elevation * M_PI / 180); + + location_cartesian.transponder_id = transponder_id; + location.transponder_id = transponder_id; + + // gzmsg << "Spherical Coordinate: \n\tBearing: " << location.x + // << " degree(s)\n\tRange: " << location.y << " m\n\tElevation: " << location.z + // << " degree(s)\n"; + // gzmsg << "Cartesian Coordinate: \n\tX: " << location_cartesian.x + // << " m\n\tY: " << location_cartesian.y << " m\n\tZ: " << location_cartesian.z << " + // m\n\n"; + + this->dataPtr->m_publishTransponderRelPos->publish(location); + this->dataPtr->m_publishTransponderRelPosCartesian->publish(location_cartesian); +} + +void UsblTransceiver::calculateRelativePose( + gz::math::Vector3 position, double & bearing, double & range, double & elevation) +{ + gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Vector3 my_pos = pose.Pos(); + auto direction = position - my_pos; + + bearing = atan2(direction.Y(), direction.X()) * 180 / M_PI; + range = sqrt( + direction.X() * direction.X() + direction.Y() * direction.Y() + direction.Z() * direction.Z()); + elevation = asin(direction.Z() / direction.Length()) * 180 / M_PI; + + // gzmsg << "bearing: " << bearing << "\n"; + // gzmsg << "range: " << range << "\n"; + // gzmsg << "elevation: " << elevation << "\n\n"; +} + +void UsblTransceiver::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused) + { + rclcpp::spin_some(this->ros_node_); + + if (!this->dataPtr->setGlobalMode) + { + std_msgs::msg::String mode; + mode.data = this->dataPtr->m_interrogationMode; + + this->dataPtr->m_interrogationModePub->publish(mode); + gzmsg << "Published mode" << std::endl; + this->dataPtr->setGlobalMode = true; + } + + if (this->dataPtr->m_enablePingerScheduler) + { + // Calculate the time interval between pings based on frequency + double pingInterval = 1.0 / this->dataPtr->m_pingFrequency; + + // Check if it's time to send a ping + if (_info.iterations % static_cast(pingInterval * 1000) == 0) + { + sendPing(); + } + } + } +} + +} // namespace dave_gz_sensor_plugins diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc new file mode 100644 index 00000000..c1a94cca --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc @@ -0,0 +1,389 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/World.hh" + +#include +#include + +#include "dave_gz_sensor_plugins/UsblTransponder.hh" + +using std::placeholders::_1; + +GZ_ADD_PLUGIN( + dave_gz_sensor_plugins::UsblTransponder, gz::sim::System, + dave_gz_sensor_plugins::UsblTransponder::ISystemConfigure, + dave_gz_sensor_plugins::UsblTransponder::ISystemPostUpdate) + +namespace dave_gz_sensor_plugins +{ + +struct UsblTransponder::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + gz::sim::Entity transceiverEntity; + gz::sim::EntityComponentManager * ecm; + std::string modelName; + std::string m_interrogationMode; + std::string m_transceiverModelName; + std::string ns; // 'namespace' is a reserved word in C++, using 'ns' instead. + std::string m_transceiverDevice; + std::string m_transceiverID; + std::string m_transponderDevice; + std::string m_transponderID; + double m_noiseSigma; + double m_noiseMu; + gz::transport::Node m_gzNode; + gz::transport::Node::Publisher m_globalPosPub; + gz::sim::Entity linkEntity; + rclcpp::Publisher::SharedPtr m_commandResponsePub; + rclcpp::Subscription::SharedPtr m_iisSub; + rclcpp::Subscription::SharedPtr m_cisSub; + rclcpp::Subscription::SharedPtr m_interrogationModeSub; + rclcpp::Subscription::SharedPtr m_temperatureSub; + rclcpp::Subscription::SharedPtr m_commandSub; + double m_soundSpeed; + double m_temperature; +}; + +UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) +{ + dataPtr->m_temperature = 10.0; // Default initialization + dataPtr->m_noiseSigma = 1.0; // Default initialization + dataPtr->m_noiseMu = 0.0; // Default initialization +} + +void UsblTransponder::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_gz_sensor_plugins::UsblTransponder::Configure on entity: " << _entity << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->dataPtr->ecm = &_ecm; + + auto model = gz::sim::Model(_entity); + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + auto links = this->dataPtr->model.Links(_ecm); + + if (!links.empty()) + { + auto linkEntity = links.front(); + auto linkName = _ecm.Component(linkEntity); + std::string linkNameStr = linkName->Data(); + gzmsg << "Principal link name: " << linkNameStr << std::endl; + this->dataPtr->linkEntity = model.LinkByName(_ecm, linkNameStr); + std::cout << worldPose(this->dataPtr->linkEntity, _ecm) << std::endl; + } + else + { + gzmsg << "No links found in the model." << std::endl; + } + + if (!model.Valid(_ecm)) + { + gzerr << "UsblTransponder plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + // Grab namespace from SDF + if (!_sdf->HasElement("namespace")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->ns = _sdf->Get("namespace"); + + // Grab sound speed from SDF + if (!_sdf->HasElement("sound_speed")) + { + this->dataPtr->m_soundSpeed = 1540.0; + gzmsg << "Sound speed default to " << this->dataPtr->m_soundSpeed << std::endl; + } + else + { + this->dataPtr->m_soundSpeed = _sdf->Get("sound_speed"); + gzmsg << "Sound speed: " << this->dataPtr->m_soundSpeed << std::endl; + } + + // Obtain transceiver device name from SDF + if (!_sdf->HasElement("transceiver_device")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + this->dataPtr->m_transceiverDevice = _sdf->Get("transceiver_device"); + gzmsg << "Entity: " << this->dataPtr->m_transceiverDevice << std::endl; + + // Get transceiver device id + if (!_sdf->HasElement("transceiver_ID")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + + this->dataPtr->m_transceiverID = _sdf->Get("transceiver_ID"); + + // Get transponder device name + if (!_sdf->HasElement("transponder_device")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->m_transponderDevice = _sdf->Get("transponder_device"); + gzmsg << "Transponder device: " << this->dataPtr->m_transponderDevice << std::endl; + + // Get commanding transponders + if (!_sdf->HasElement("transponder_ID")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->m_transponderID = _sdf->Get("transponder_ID"); + this->ros_node_ = + std::make_shared("usbl_transponder_" + this->dataPtr->m_transponderID + "_node"); + + // Get the mean of normal distribution for the noise model + if (_sdf->HasElement("mu")) + { + this->dataPtr->m_noiseMu = _sdf->Get("mu"); + } + + // Get the standard deviation of normal distribution for the noise model + if (_sdf->HasElement("sigma")) + { + this->dataPtr->m_noiseSigma = _sdf->Get("sigma"); + } + + // Get transceiver model name + if (!_sdf->HasElement("transceiver_model")) + { + gzerr << "Missing required parameter , " + << "plugin will not be initialized." << std::endl; + return; + } + this->dataPtr->m_transceiverModelName = _sdf->Get("transceiver_model"); + gzmsg << "Transceiver model: " << this->dataPtr->m_transceiverModelName << std::endl; + + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); + this->dataPtr->transceiverEntity = _ecm.EntityByComponents( + gz::sim::components::Name(this->dataPtr->m_transceiverModelName), gz::sim::components::Model()); + // this->dataPtr->transceiverModel = worldEntity.ModelByName(_ecm, + // this->dataPtr->transceiverModelName); + + // Define Gazebo publisher for entity's global position + this->dataPtr->m_globalPosPub = this->dataPtr->m_gzNode.Advertise( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transponderID + "/global_position"); + + std::string commandResponseTopic( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/command_response"); + + this->dataPtr->m_commandResponsePub = + this->ros_node_->create_publisher(commandResponseTopic, 1); + + this->dataPtr->m_iisSub = this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transponderDevice + "_" + + this->dataPtr->m_transponderID + "/individual_interrogation_ping", + 1, std::bind(&UsblTransponder::iisRosCallback, this, _1)); + + this->dataPtr->m_cisSub = this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/common_interrogation_ping", 1, + std::bind(&UsblTransponder::cisRosCallback, this, _1)); + + this->dataPtr->m_temperatureSub = this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transponderDevice + "_" + + this->dataPtr->m_transponderID + "/temperature", + 1, std::bind(&UsblTransponder::temperatureRosCallback, this, _1)); + + this->dataPtr->m_commandSub = + this->ros_node_->create_subscription( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transponderDevice + "_" + + this->dataPtr->m_transponderID + "/command_request", + 1, std::bind(&UsblTransponder::commandRosCallback, this, _1)); + + std::string interrogation_mode_topic = "/" + this->dataPtr->ns + "/" + + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/interrogation_mode"; + + this->dataPtr->m_interrogationModeSub = + this->ros_node_->create_subscription( + interrogation_mode_topic, 1, + std::bind(&UsblTransponder::interrogationModeCallback, this, _1)); +} + +void UsblTransponder::sendLocation() +{ + // randomly generate from normal distribution for noise + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> d(this->dataPtr->m_noiseMu, this->dataPtr->m_noiseSigma); + + gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Vector3 position = pose.Pos(); + auto pub_msg = gz::msgs::Vector3d(); + // std::cout << position.X() << " " << position.Y() << " " + // << position.Z() << std::endl; + pub_msg.set_x(position.X() + d(gen)); + pub_msg.set_y(position.Y() + d(gen)); + pub_msg.set_z(position.Z() + d(gen)); + this->dataPtr->m_globalPosPub.Publish(pub_msg); +} + +void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + if (this->dataPtr->m_interrogationMode.compare("individual") == 0) + { + gz::math::Pose3d pose_transponder = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Pose3d pose_transceiver = + worldPose(this->dataPtr->transceiverEntity, *this->dataPtr->ecm); + gz::math::Vector3 position_transponder = pose_transponder.Pos(); + gz::math::Vector3 position_transceiver = pose_transceiver.Pos(); + + // For each kilometer increase in depth (pressure), the sound speed increases by 17 m/s + // Base on https://dosits.org/tutorials/science/tutorial-speed/ + this->dataPtr->m_soundSpeed = 1540.4 + position_transponder.Z() / 1000 * 17; + double dist = (position_transponder - position_transceiver).Length(); + std::string command = msg->data; + + if (!command.compare("ping")) + { + // gzmsg << this->dataPtr->m_transponderDevice + "_" + this->dataPtr->m_transponderID + + // ": Received iis_ping, responding\n"; + // gzmsg << "Distance " << dist << std::endl; + // gzmsg << "Pose transponder " << position_transponder << std::endl; + // gzmsg << "Pose transceiver " << position_transceiver << std::endl; + sleep(dist / this->dataPtr->m_soundSpeed); + sendLocation(); + } + else + { + gzmsg << "Unknown command, ignore\n"; + } + } + else + { + gzmsg << "Not in individual mode: cannot send location" << std::endl; + } +} + +void UsblTransponder::cisRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + if (this->dataPtr->m_interrogationMode.compare("common") == 0) + { + gz::math::Pose3d pose_transponder = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Pose3d pose_transceiver = + worldPose(this->dataPtr->transceiverEntity, *this->dataPtr->ecm); + gz::math::Vector3 position_transponder = pose_transponder.Pos(); + gz::math::Vector3 position_transceiver = pose_transceiver.Pos(); + + // For each kilometer increase in depth (pressure), the sound speed increases by 17 m/s + // Base on https://dosits.org/tutorials/science/tutorial-speed/ + this->dataPtr->m_soundSpeed = 1540.4 + position_transponder.Z() / 1000 * 17; + double dist = (position_transponder - position_transceiver).Length(); + std::string command = msg->data; + + if (!command.compare("ping")) + { + gzmsg << this->dataPtr->m_transponderDevice + "_" + this->dataPtr->m_transponderID + + ": Received cis_ping, responding\n"; + sleep(dist / this->dataPtr->m_soundSpeed); + sendLocation(); + } + else + { + gzmsg << "Unknown command, ignore\n"; + } + } + else + { + gzmsg << "Not in common mode: cannot send location"; + } +} + +void UsblTransponder::interrogationModeCallback(const std_msgs::msg::String::SharedPtr msg) +{ + this->dataPtr->m_interrogationMode = msg->data; + + gzmsg << "Transponders changed interrogation mode to " << this->dataPtr->m_interrogationMode + << std::endl; +} + +void UsblTransponder::temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg) +{ + gz::math::Pose3d pose_transponder = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Vector3 position_transponder = pose_transponder.Pos(); + + this->dataPtr->m_temperature = msg->data; + + // Base on https://dosits.org/tutorials/science/tutorial-speed/ + this->dataPtr->m_soundSpeed = + 1540.4 + position_transponder.Z() / 1000 * 17 + (this->dataPtr->m_temperature - 10) * 4; + gzmsg << "Detected change of temperature, transponder sound speed is now: " + << this->dataPtr->m_soundSpeed << " m/s\n"; +} + +void UsblTransponder::commandRosCallback(const dave_interfaces::msg::UsblCommand msg) +{ + dave_interfaces::msg::UsblResponse response_msg; + response_msg.data = "Hi from transponder_" + this->dataPtr->m_transponderID; + response_msg.response_id = 1; + + response_msg.transceiver_id = this->dataPtr->m_transceiverID.back() - '0'; + this->dataPtr->m_commandResponsePub->publish(response_msg); +} + +void UsblTransponder::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused) + { + // gzdbg << "dave_gz_sensor_plugins::UsblTransponder::PostUpdate" << std::endl; + rclcpp::spin_some(this->ros_node_); + } +} + +} // namespace dave_gz_sensor_plugins diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world new file mode 100644 index 00000000..1b4878dd --- /dev/null +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -0,0 +1,134 @@ + + + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + 0 0 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun + + + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + + + + + + USBL + transceiver_manufacturer + transponder_manufacturer + 1,2 + 168 + true + common + 0.5 + sphere,sphere2 + 1540 + + + + + 3 3 1 0 0 0 + + + + + 1 + + + + + + + + 1 + + + + 0 1 0 1 + 0 1 0 1 + + + + + USBL + transponder_manufacturer + 1 + transceiver_manufacturer + 168 + box + 0 + 0.0 + + + + + 6 6 1 0 0 0 + + + + + 1 + + + + + + + + 1 + + + + 0 0 1 1 + 0 0 1 1 + + + + + USBL + transponder_manufacturer + 2 + transceiver_manufacturer + 168 + box + 1540 + 0 + 0.0 + + + +