From a9f2309c4b5f8ad8be0b6122934bf8add80e99c1 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 8 Jul 2024 01:59:43 -0300 Subject: [PATCH 01/16] add usbl plugins --- gazebo/dave_model_systems/CMakeLists.txt | 90 ++++++++ .../hooks/dave_model_systems.dsv.in | 1 + .../include/dave_model_systems/BasicSystem.hh | 44 ++++ .../include/dave_model_systems/FullSystem.hh | 61 +++++ .../dave_model_systems/UsblTransceiver.hh | 55 +++++ .../dave_model_systems/UsblTransponder.hh | 58 +++++ gazebo/dave_model_systems/package.xml | 13 ++ gazebo/dave_model_systems/src/BasicSystem.cc | 47 ++++ gazebo/dave_model_systems/src/FullSystem.cc | 102 +++++++++ .../dave_model_systems/src/UsblTransceiver.cc | 216 ++++++++++++++++++ .../dave_model_systems/src/UsblTransponder.cc | 213 +++++++++++++++++ models/dave_worlds/worlds/usbl_tutorial.world | 84 +++++++ 12 files changed, 984 insertions(+) create mode 100644 gazebo/dave_model_systems/CMakeLists.txt create mode 100644 gazebo/dave_model_systems/hooks/dave_model_systems.dsv.in create mode 100644 gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh create mode 100644 gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh create mode 100644 gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh create mode 100644 gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh create mode 100644 gazebo/dave_model_systems/package.xml create mode 100644 gazebo/dave_model_systems/src/BasicSystem.cc create mode 100644 gazebo/dave_model_systems/src/FullSystem.cc create mode 100644 gazebo/dave_model_systems/src/UsblTransceiver.cc create mode 100644 gazebo/dave_model_systems/src/UsblTransponder.cc create mode 100644 models/dave_worlds/worlds/usbl_tutorial.world diff --git a/gazebo/dave_model_systems/CMakeLists.txt b/gazebo/dave_model_systems/CMakeLists.txt new file mode 100644 index 00000000..b019ac56 --- /dev/null +++ b/gazebo/dave_model_systems/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.5) + +# Define the project name +project(dave_model_systems) + +# 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) + +# 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 BasicSystem library +add_library(BasicSystem SHARED src/BasicSystem.cc) +add_library(UsblTransceiver SHARED src/UsblTransceiver.cc) +add_library(UsblTransponder SHARED src/UsblTransponder.cc) + +# Include directories for BasicSystem +target_include_directories(BasicSystem PRIVATE include) +target_include_directories(UsblTransceiver PRIVATE include) +target_include_directories(UsblTransponder PRIVATE include) + +# Link libraries for BasicSystem +target_link_libraries(BasicSystem + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + +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}) + +# Add FullSystem library +add_library(FullSystem SHARED src/FullSystem.cc) + +# Include directories for FullSystem +target_include_directories(FullSystem PRIVATE include) + +# Link libraries for FullSystem +target_link_libraries(FullSystem + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + +# Specify dependencies for FullSystem using ament_target_dependencies +ament_target_dependencies(FullSystem + rclcpp + std_msgs +) + +# Specify dependencies for FullSystem using ament_target_dependencies +ament_target_dependencies(UsblTransceiver + rclcpp + std_msgs +) + +ament_target_dependencies(UsblTransponder + rclcpp + std_msgs +) + + +# Install targets +install(TARGETS BasicSystem FullSystem 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_model_systems/hooks/dave_model_systems.dsv.in b/gazebo/dave_model_systems/hooks/dave_model_systems.dsv.in new file mode 100644 index 00000000..3d270f30 --- /dev/null +++ b/gazebo/dave_model_systems/hooks/dave_model_systems.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh b/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh new file mode 100644 index 00000000..58db3d8e --- /dev/null +++ b/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh @@ -0,0 +1,44 @@ +/* + * 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_MODEL_SYSTEMS__BASICSYSTEM_HH_ +#define DAVE_MODEL_SYSTEMS__BASICSYSTEM_HH_ + +// The only required include in the header is this one. +// All others will depend on what your plugin does. +#include + +namespace dave_model_systems +{ +// This is the main plugin's class. It must inherit from System and at least +// one other interface. +// Here we use `ISystemPostUpdate`, which is used to get results after +// physics runs. The opposite of that, `ISystemPreUpdate`, would be used by +// plugins that want to send commands. +class BasicSystem : public gz::sim::System, public gz::sim::ISystemPostUpdate +{ + // Plugins inheriting ISystemPostUpdate must implement the PostUpdate + // callback. This is called at every simulation iteration after the physics + // updates the world. The _info variable provides information such as time, + // while the _ecm provides an interface to all entities and components in + // simulation. +public: + void PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; +}; +} // namespace dave_model_systems +#endif // DAVE_MODEL_SYSTEMS__BASICSYSTEM_HH_ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh b/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh new file mode 100644 index 00000000..71cde54f --- /dev/null +++ b/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh @@ -0,0 +1,61 @@ +/* + * 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_MODEL_SYSTEMS__FULLSYSTEM_HH_ +#define DAVE_MODEL_SYSTEMS__FULLSYSTEM_HH_ + +#include + +#include +#include +#include +#include + +namespace dave_model_systems +{ +class FullSystem : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemUpdate, + public gz::sim::ISystemPostUpdate, + public gz::sim::ISystemReset +{ +public: + void Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _element, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) override; + +public: + void PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; + +public: + void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; + +public: + void PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; + +public: + void Reset(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; + +private: + rclcpp::Node::SharedPtr ros_node_; + rclcpp::Publisher::SharedPtr log_pub_; +}; +} // namespace dave_model_systems +#endif // DAVE_MODEL_SYSTEMS__FULLSYSTEM_HH_ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh new file mode 100644 index 00000000..d95646be --- /dev/null +++ b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh @@ -0,0 +1,55 @@ +/* + * 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_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ +#define DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ + +#include +#include + +#include +#include +#include + +namespace dave_model_systems + +{ +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; + +private: + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr log_pub_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_model_systems + +#endif // DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh new file mode 100644 index 00000000..9d1be4c3 --- /dev/null +++ b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh @@ -0,0 +1,58 @@ +/* + * 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_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ +#define DAVE_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ + +#include +#include + +#include + +#include +#include + +namespace dave_model_systems + +{ +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(const gz::sim::EntityComponentManager & _ecm); + +private: + std::shared_ptr ros_node_; + rclcpp::Publisher::SharedPtr log_pub_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_model_systems + +#endif // DAVE_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ diff --git a/gazebo/dave_model_systems/package.xml b/gazebo/dave_model_systems/package.xml new file mode 100644 index 00000000..9e7067cf --- /dev/null +++ b/gazebo/dave_model_systems/package.xml @@ -0,0 +1,13 @@ + + + dave_model_systems + 0.0.0 + DAVE model systems + Helena Moyen + Apache 2.0 + ament_cmake + ament_lint_auto + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_model_systems/src/BasicSystem.cc b/gazebo/dave_model_systems/src/BasicSystem.cc new file mode 100644 index 00000000..89e12e59 --- /dev/null +++ b/gazebo/dave_model_systems/src/BasicSystem.cc @@ -0,0 +1,47 @@ +/* + * 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. + * + */ + +// We'll use a string and the gzmsg command below for a brief example. +// Remove these includes if your plugin doesn't need them. +#include + +// This header is required to register plugins. It's good practice to place it +// in the cc file, like it's done here. +#include +#include + +#include "dave_model_systems/BasicSystem.hh" + +// This is required to register the plugin. Make sure the interfaces match +// what's in the header. +GZ_ADD_PLUGIN( + dave_model_systems::BasicSystem, gz::sim::System, + dave_model_systems::BasicSystem::ISystemPostUpdate) + +namespace dave_model_systems +{ + +void BasicSystem::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::BasicSystem::PostUpdate" << std::endl; + } +} + +} // namespace dave_model_systems diff --git a/gazebo/dave_model_systems/src/FullSystem.cc b/gazebo/dave_model_systems/src/FullSystem.cc new file mode 100644 index 00000000..758c7a7f --- /dev/null +++ b/gazebo/dave_model_systems/src/FullSystem.cc @@ -0,0 +1,102 @@ +/* + * 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. + * + */ + +// We'll use a string and the gzmsg command below for a brief example. +// Remove these includes if your plugin doesn't need them. +// Don't forget to include the plugin's header. +#include + +#include +#include + +#include "dave_model_systems/FullSystem.hh" +// This is required to register the plugin. Make sure the interfaces match +// what's in the header. +GZ_ADD_PLUGIN( + dave_model_systems::FullSystem, gz::sim::System, dave_model_systems::FullSystem::ISystemConfigure, + dave_model_systems::FullSystem::ISystemPreUpdate, dave_model_systems::FullSystem::ISystemUpdate, + dave_model_systems::FullSystem::ISystemPostUpdate, dave_model_systems::FullSystem::ISystemReset) + +namespace dave_model_systems +{ + +void FullSystem::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _element, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_model_systems::FullSystem::Configure on entity: " << _entity << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("full_system_node"); + this->log_pub_ = this->ros_node_->create_publisher("/gazebo_logs", 10); + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::FullSystem::Configure on entity: " + std::to_string(_entity); + this->log_pub_->publish(msg); +} + +void FullSystem::PreUpdate( + const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::FullSystem::PreUpdate" << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::FullSystem::PreUpdate"; + this->log_pub_->publish(msg); + } +} + +void FullSystem::Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::FullSystem::Update" << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::FullSystem::Update"; + this->log_pub_->publish(msg); + } +} + +void FullSystem::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::FullSystem::PostUpdate" << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::FullSystem::PostUpdate"; + this->log_pub_->publish(msg); + } +} + +void FullSystem::Reset(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) +{ + gzdbg << "dave_model_systems::FullSystem::Reset" << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::FullSystem::Reset"; + this->log_pub_->publish(msg); +} +} // namespace dave_model_systems diff --git a/gazebo/dave_model_systems/src/UsblTransceiver.cc b/gazebo/dave_model_systems/src/UsblTransceiver.cc new file mode 100644 index 00000000..84c58e5f --- /dev/null +++ b/gazebo/dave_model_systems/src/UsblTransceiver.cc @@ -0,0 +1,216 @@ +/* + * 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. + */ +// System headers +#include +#include +#include + +// External headers (Gazebo, ROS, etc.) +#include +#include +#include +#include +#include +#include "gz/common/StringUtils.hh" +#include "gz/plugin/Register.hh" + +// Internal headers (headers from your project) +#include "dave_model_systems/UsblTransceiver.hh" + +// available interrogation modes +std::vector im = {"common", "individual"}; + +GZ_ADD_PLUGIN( + dave_model_systems::UsblTransceiver, gz::sim::System, + dave_model_systems::UsblTransceiver::ISystemConfigure, + dave_model_systems::UsblTransceiver::ISystemPostUpdate) + +namespace dave_model_systems +{ + +struct UsblTransceiver::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + std::string modelName; + 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::vector m_deployedTransponders; + bool m_enablePingerScheduler; + std::string m_transponderAttachedObject; + std::string m_interrogationMode; + gz::transport::Node node; +}; + +UsblTransceiver::UsblTransceiver() : dataPtr(std::make_unique()) +{ + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("usbl_transceiver_node"); + this->log_pub_ = this->ros_node_->create_publisher("/gazebo_logs", 10); +} + +void UsblTransceiver::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_model_systems::UsblTransceiver::Configure on entity: " << _entity << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::UsblTransceiver::Configure on entity: " + std::to_string(_entity); + this->log_pub_->publish(msg); + + auto model = gz::sim::Model(_entity); + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + 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"); + + // 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"); + 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; + } + + this->dataPtr->m_transponderAttachedObject = + _sdf->Get("transponder_attached_object"); + gzmsg << "Transponder attached object: " << this->dataPtr->m_transponderAttachedObject + << 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"); + 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"; + } +} + +void UsblTransceiver::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::UsblTransceiver::PostUpdate" << std::endl; + std_msgs::msg::String msg; + msg.data = "dave_model_systems::UsblTransceiver::PostUpdate: namespace = " + this->dataPtr->ns + + ", model name = " + this->dataPtr->modelName; + this->log_pub_->publish(msg); + } +} + +} // namespace dave_model_systems diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc new file mode 100644 index 00000000..199ef00d --- /dev/null +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -0,0 +1,213 @@ +/* + * 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 +#include + +#include "dave_model_systems/UsblTransponder.hh" + +// Available interrogation modes +std::vector im = {"common", "individual"}; + +GZ_ADD_PLUGIN( + dave_model_systems::UsblTransponder, gz::sim::System, + dave_model_systems::UsblTransponder::ISystemConfigure, + dave_model_systems::UsblTransponder::ISystemPostUpdate) + +namespace dave_model_systems +{ + +struct UsblTransponder::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + std::string modelName; + 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; +}; + +UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) +{ + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("usbl_transponder_node"); + this->log_pub_ = this->ros_node_->create_publisher("/transponder", 10); +} + +void UsblTransponder::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_model_systems::UsblTransponder::Configure on entity: " << _entity << std::endl; + + std_msgs::msg::String msg; + msg.data = "dave_model_systems::UsblTransponder::Configure on entity: " + std::to_string(_entity); + this->log_pub_->publish(msg); + + 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"); + + // 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"); + + // 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"); + } + + // 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"); +} + +void UsblTransponder::sendLocation(const gz::sim::EntityComponentManager & _ecm) +{ + // 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, _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::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused && _info.iterations % 1000 == 0) + { + gzdbg << "dave_model_systems::UsblTransponder::PostUpdate" << std::endl; + std_msgs::msg::String msg; + msg.data = "dave_model_systems::UsblTransponder::PostUpdate: namespace = " + this->dataPtr->ns + + ", model name = " + this->dataPtr->modelName; + this->log_pub_->publish(msg); + sendLocation(_ecm); + } +} + +} // namespace dave_model_systems diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world new file mode 100644 index 00000000..e8583e43 --- /dev/null +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -0,0 +1,84 @@ + + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun + + + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + + + + USBL + transceiver_manufacturer + transponder_manufacturer + 1,2 + 168 + false + sphere + + + + + 3 3 0.5 0 0 0 + + + + + 1 + + + + + + + + 1 + + + + + + USBL + transponder_manufacturer + 1 + transceiver_manufacturer + 168 + 0 + 0.0 + + + + + \ No newline at end of file From 5b015fd76ba865e15b22c9c26539afbf16f63cd1 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 8 Jul 2024 18:07:56 -0300 Subject: [PATCH 02/16] usbl transponder working --- dave_interfaces/CMakeLists.txt | 35 ++++ dave_interfaces/msg/UsblCommand.msg | 3 + dave_interfaces/msg/UsblResponse.msg | 3 + dave_interfaces/package.xml | 19 ++ gazebo/dave_model_systems/CMakeLists.txt | 4 +- .../dave_model_systems/UsblTransponder.hh | 9 +- gazebo/dave_model_systems/package.xml | 2 + .../dave_model_systems/src/UsblTransponder.cc | 162 ++++++++++++++++-- models/dave_worlds/worlds/usbl_tutorial.world | 3 +- 9 files changed, 227 insertions(+), 13 deletions(-) create mode 100644 dave_interfaces/CMakeLists.txt create mode 100644 dave_interfaces/msg/UsblCommand.msg create mode 100644 dave_interfaces/msg/UsblResponse.msg create mode 100644 dave_interfaces/package.xml diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt new file mode 100644 index 00000000..60ab8ef7 --- /dev/null +++ b/dave_interfaces/CMakeLists.txt @@ -0,0 +1,35 @@ +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" +) + +# Install generated messages +install(DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# 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/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/gazebo/dave_model_systems/CMakeLists.txt b/gazebo/dave_model_systems/CMakeLists.txt index b019ac56..f2dde6c3 100644 --- a/gazebo/dave_model_systems/CMakeLists.txt +++ b/gazebo/dave_model_systems/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(gz-common5 REQUIRED COMPONENTS profiler) find_package(gz-sim8 REQUIRED) +find_package(dave_interfaces REQUIRED) # Set version variables set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) @@ -57,16 +58,17 @@ ament_target_dependencies(FullSystem # Specify dependencies for FullSystem using ament_target_dependencies ament_target_dependencies(UsblTransceiver + dave_interfaces rclcpp std_msgs ) ament_target_dependencies(UsblTransponder + dave_interfaces rclcpp std_msgs ) - # Install targets install(TARGETS BasicSystem FullSystem UsblTransceiver UsblTransponder DESTINATION lib/${PROJECT_NAME} diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh index 9d1be4c3..c5743473 100644 --- a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh +++ b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh @@ -24,7 +24,10 @@ #include #include +#include #include +#include "dave_interfaces/msg/usbl_command.hpp" +#include "dave_interfaces/msg/usbl_response.hpp" namespace dave_model_systems @@ -44,7 +47,11 @@ public: void PostUpdate( const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; - void sendLocation(const gz::sim::EntityComponentManager & _ecm); + 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); private: std::shared_ptr ros_node_; diff --git a/gazebo/dave_model_systems/package.xml b/gazebo/dave_model_systems/package.xml index 9e7067cf..ab11c06d 100644 --- a/gazebo/dave_model_systems/package.xml +++ b/gazebo/dave_model_systems/package.xml @@ -7,6 +7,8 @@ Apache 2.0 ament_cmake ament_lint_auto + dave_interfaces + dave_interfaces ament_cmake diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc index 199ef00d..f5815c3b 100644 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -30,6 +30,8 @@ #include #include #include +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/World.hh" #include #include @@ -38,6 +40,7 @@ // Available interrogation modes std::vector im = {"common", "individual"}; +using std::placeholders::_1; GZ_ADD_PLUGIN( dave_model_systems::UsblTransponder, gz::sim::System, @@ -51,7 +54,10 @@ 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_transceiverModelName; std::string ns; // 'namespace' is a reserved word in C++, using 'ns' instead. std::string m_transceiverDevice; std::string m_transceiverID; @@ -62,17 +68,20 @@ struct UsblTransponder::PrivateData 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_temperatureSub; + rclcpp::Subscription::SharedPtr m_commandSub; + double m_soundSpeed; + double m_temperature; }; UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) { - if (!rclcpp::ok()) - { - rclcpp::init(0, nullptr); - } - - this->ros_node_ = std::make_shared("usbl_transponder_node"); - this->log_pub_ = this->ros_node_->create_publisher("/transponder", 10); + dataPtr->m_temperature = 10.0; // Default initialization + dataPtr->m_noiseSigma = 0.0; // Default initialization + dataPtr->m_noiseMu = 0.0; // Default initialization } void UsblTransponder::Configure( @@ -81,6 +90,15 @@ void UsblTransponder::Configure( { gzdbg << "dave_model_systems::UsblTransponder::Configure on entity: " << _entity << std::endl; + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("usbl_transponder_node"); + this->dataPtr->ecm = &_ecm; + this->log_pub_ = this->ros_node_->create_publisher("/transponder", 10); + std_msgs::msg::String msg; msg.data = "dave_model_systems::UsblTransponder::Configure on entity: " + std::to_string(_entity); this->log_pub_->publish(msg); @@ -172,20 +190,63 @@ void UsblTransponder::Configure( 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)); } -void UsblTransponder::sendLocation(const gz::sim::EntityComponentManager & _ecm) +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, _ecm); + 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() << " " @@ -196,6 +257,87 @@ void UsblTransponder::sendLocation(const gz::sim::EntityComponentManager & _ecm) this->dataPtr->m_globalPosPub.Publish(pub_msg); } +void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + 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"; + } +} + +void UsblTransponder::cisRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + 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"; + } +} + +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) { @@ -206,7 +348,7 @@ void UsblTransponder::PostUpdate( msg.data = "dave_model_systems::UsblTransponder::PostUpdate: namespace = " + this->dataPtr->ns + ", model name = " + this->dataPtr->modelName; this->log_pub_->publish(msg); - sendLocation(_ecm); + rclcpp::spin(this->ros_node_); } } diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index e8583e43..1af2d3d0 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -49,7 +49,7 @@ - 3 3 0.5 0 0 0 + 3 3 0 0 0 0 @@ -75,6 +75,7 @@ 1 transceiver_manufacturer 168 + box 0 0.0 From 35cacfcde2c8c64fd5c353590b015f5fc052236e Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 11:39:02 -0300 Subject: [PATCH 03/16] usbl transceiver working --- gazebo/dave_model_systems/CMakeLists.txt | 2 + .../dave_model_systems/UsblTransceiver.hh | 18 +- .../dave_model_systems/UsblTransponder.hh | 2 +- .../dave_model_systems/src/UsblTransceiver.cc | 334 ++++++++++++++++-- .../dave_model_systems/src/UsblTransponder.cc | 21 +- models/dave_worlds/worlds/usbl_tutorial.world | 40 ++- 6 files changed, 372 insertions(+), 45 deletions(-) diff --git a/gazebo/dave_model_systems/CMakeLists.txt b/gazebo/dave_model_systems/CMakeLists.txt index f2dde6c3..f4ae7c91 100644 --- a/gazebo/dave_model_systems/CMakeLists.txt +++ b/gazebo/dave_model_systems/CMakeLists.txt @@ -11,6 +11,7 @@ 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 @@ -60,6 +61,7 @@ ament_target_dependencies(FullSystem ament_target_dependencies(UsblTransceiver dave_interfaces rclcpp + geometry_msgs std_msgs ) diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh index d95646be..e1c275c0 100644 --- a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh +++ b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh @@ -18,12 +18,17 @@ #ifndef DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ #define DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ +#include +#include #include #include #include #include +#include #include +#include "dave_interfaces/msg/usbl_command.hpp" +#include "dave_interfaces/msg/usbl_response.hpp" namespace dave_model_systems @@ -43,9 +48,20 @@ public: void PostUpdate( const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; + void receiveGazeboCallback(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 calcuateRelativePose( + gz::math::Vector3 position, double & bearing, double & range, double & elevation); + void publishPosition(double & bearing, double & range, double & elevation); + private: std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr log_pub_; struct PrivateData; std::unique_ptr dataPtr; diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh index c5743473..bb56facc 100644 --- a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh +++ b/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh @@ -52,10 +52,10 @@ public: 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(); private: std::shared_ptr ros_node_; - rclcpp::Publisher::SharedPtr log_pub_; struct PrivateData; std::unique_ptr dataPtr; diff --git a/gazebo/dave_model_systems/src/UsblTransceiver.cc b/gazebo/dave_model_systems/src/UsblTransceiver.cc index 84c58e5f..8c34cb56 100644 --- a/gazebo/dave_model_systems/src/UsblTransceiver.cc +++ b/gazebo/dave_model_systems/src/UsblTransceiver.cc @@ -13,25 +13,33 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -// System headers + +#include #include #include +#include #include -// External headers (Gazebo, ROS, etc.) #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 -// Internal headers (headers from your project) #include "dave_model_systems/UsblTransceiver.hh" // available interrogation modes std::vector im = {"common", "individual"}; +using std::placeholders::_1; GZ_ADD_PLUGIN( dave_model_systems::UsblTransceiver, gz::sim::System, @@ -46,26 +54,43 @@ struct UsblTransceiver::PrivateData // Add any private data members here. gz::sim::Model model; std::string modelName; - std::string ns; // 'namespace' is a reserved word in C++, using 'ns' instead. + 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::transport::Node node; + gz::sim::Entity linkEntity; + gz::transport::Node m_gzNode; + rclcpp::Publisher::SharedPtr m_publishTransponderRelPos; + rclcpp::Publisher::SharedPtr m_cisPinger; + 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::string m_channel; + double m_soundSpeed; + double m_temperature; }; +// // Define constants for command IDs +const int BATTERY_LEVEL = 1; +const int GO_TO = 2; + UsblTransceiver::UsblTransceiver() : dataPtr(std::make_unique()) { - if (!rclcpp::ok()) - { - rclcpp::init(0, nullptr); - } - - this->ros_node_ = std::make_shared("usbl_transceiver_node"); - this->log_pub_ = this->ros_node_->create_publisher("/gazebo_logs", 10); + dataPtr->m_channel = "1"; + dataPtr->m_temperature = 10.0; } void UsblTransceiver::Configure( @@ -74,13 +99,33 @@ void UsblTransceiver::Configure( { gzdbg << "dave_model_systems::UsblTransceiver::Configure on entity: " << _entity << std::endl; - std_msgs::msg::String msg; - msg.data = "dave_model_systems::UsblTransceiver::Configure on entity: " + std::to_string(_entity); - this->log_pub_->publish(msg); + 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)) { @@ -164,10 +209,17 @@ void UsblTransceiver::Configure( return; } - this->dataPtr->m_transponderAttachedObject = - _sdf->Get("transponder_attached_object"); - gzmsg << "Transponder attached object: " << this->dataPtr->m_transponderAttachedObject - << std::endl; + auto transpondersObjects = + gz::common::Split(_sdf->Get("transponder_attached_object"), ','); + gzmsg << "Current deployed transponders objects are: \n"; + + for (auto & object : transpondersObjects) + { + gzmsg << object << std::endl; + this->dataPtr->m_transponderAttachedObjects.push_back(object); + this->dataPtr->m_transponderEntities.push_back( + _ecm.EntityByComponents(gz::sim::components::Name(object), gz::sim::components::Model())); + } /* interrogation mode - 2 options * II (individual interrogation) <-----> CRS (common response signal) @@ -198,18 +250,248 @@ void UsblTransceiver::Configure( 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; + + this->dataPtr->m_gzNode.Subscribe( + transponder_position, &UsblTransceiver::receiveGazeboCallback, this); + } + + // ROS2 Publishers + + std::string transponder_location_topic = "/" + this->dataPtr->ns + "/" + + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/transponder_location"; + + this->dataPtr->m_publishTransponderRelPos = + this->ros_node_->create_publisher(transponder_location_topic, 1); + + std::string cis_pinger_topic = "/" + this->dataPtr->ns + "/common_interrogation_ping"; + this->dataPtr->m_cisPinger = + this->ros_node_->create_publisher(cis_pinger_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( + "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + + this->dataPtr->m_transceiverID + "/interrogation_mode", + 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 & transponder : this->dataPtr->m_transponderEntities) + { + gz::math::Pose3d pose_transponder = worldPose(transponder, *this->dataPtr->ecm); + + gz::math::Pose3d pose_transceiver = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + + double dist = (pose_transponder.Pos() - pose_transceiver.Pos()).Length(); + this->dataPtr->m_soundSpeed = + 1540.4 + pose_transceiver.Z() / 1000 * 17; // Defined here because there was no def before + + 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; +} + +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; + } + 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 gz::msgs::Vector3d & transponder_position) +{ + gzmsg << "Transceiver acquires transponders position: " << transponder_position.x() << " " + << transponder_position.y() << " " << transponder_position.z() << std::endl; + + 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; + calcuateRelativePose(transponder_position_ign, bearing, range, elevation); + + publishPosition(bearing, range, elevation); +} + +void UsblTransceiver::publishPosition(double & bearing, double & range, double & elevation) +{ + geometry_msgs::msg::Vector3 location; + location.x = bearing; + location.y = range; + location.z = elevation; + + geometry_msgs::msg::Vector3 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); + + 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::calcuateRelativePose( + 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 && _info.iterations % 1000 == 0) + if (!_info.paused) { - gzdbg << "dave_model_systems::UsblTransceiver::PostUpdate" << std::endl; - std_msgs::msg::String msg; - msg.data = "dave_model_systems::UsblTransceiver::PostUpdate: namespace = " + this->dataPtr->ns + - ", model name = " + this->dataPtr->modelName; - this->log_pub_->publish(msg); + // gzdbg << "dave_model_systems::UsblTransceiver::PostUpdate" << std::endl; + + sendPing(); + + rclcpp::spin_some(this->ros_node_); } } diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc index f5815c3b..d3693929 100644 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -14,7 +14,6 @@ * limitations under the License. */ -#include #include #include #include @@ -38,8 +37,6 @@ #include "dave_model_systems/UsblTransponder.hh" -// Available interrogation modes -std::vector im = {"common", "individual"}; using std::placeholders::_1; GZ_ADD_PLUGIN( @@ -95,13 +92,7 @@ void UsblTransponder::Configure( rclcpp::init(0, nullptr); } - this->ros_node_ = std::make_shared("usbl_transponder_node"); this->dataPtr->ecm = &_ecm; - this->log_pub_ = this->ros_node_->create_publisher("/transponder", 10); - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::UsblTransponder::Configure on entity: " + std::to_string(_entity); - this->log_pub_->publish(msg); auto model = gz::sim::Model(_entity); this->dataPtr->model = model; @@ -177,6 +168,8 @@ void UsblTransponder::Configure( 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")) @@ -341,14 +334,10 @@ void UsblTransponder::commandRosCallback(const dave_interfaces::msg::UsblCommand void UsblTransponder::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - if (!_info.paused && _info.iterations % 1000 == 0) + if (!_info.paused) { - gzdbg << "dave_model_systems::UsblTransponder::PostUpdate" << std::endl; - std_msgs::msg::String msg; - msg.data = "dave_model_systems::UsblTransponder::PostUpdate: namespace = " + this->dataPtr->ns + - ", model name = " + this->dataPtr->modelName; - this->log_pub_->publish(msg); - rclcpp::spin(this->ros_node_); + // gzdbg << "dave_model_systems::UsblTransponder::PostUpdate" << std::endl; + rclcpp::spin_some(this->ros_node_); } } diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index 1af2d3d0..b705f232 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -44,7 +44,7 @@ 1,2 168 false - sphere + sphere,sphere2 @@ -81,5 +81,43 @@ + + + + 6 6 0 0 0 0 + + + + + 1 + + + + + + + + 1 + + + + + + USBL + transponder_manufacturer + 2 + transceiver_manufacturer + 168 + box + 0 + 0.0 + + + + + + \ No newline at end of file From 38610661f13c4335fa027b0b1e0312ba8e828660 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 11:41:13 -0300 Subject: [PATCH 04/16] usbl transceiver working --- gazebo/dave_model_systems/CMakeLists.txt | 25 +---- .../include/dave_model_systems/BasicSystem.hh | 44 -------- .../include/dave_model_systems/FullSystem.hh | 61 ----------- gazebo/dave_model_systems/src/BasicSystem.cc | 47 -------- gazebo/dave_model_systems/src/FullSystem.cc | 102 ------------------ 5 files changed, 1 insertion(+), 278 deletions(-) delete mode 100644 gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh delete mode 100644 gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh delete mode 100644 gazebo/dave_model_systems/src/BasicSystem.cc delete mode 100644 gazebo/dave_model_systems/src/FullSystem.cc diff --git a/gazebo/dave_model_systems/CMakeLists.txt b/gazebo/dave_model_systems/CMakeLists.txt index f4ae7c91..df133644 100644 --- a/gazebo/dave_model_systems/CMakeLists.txt +++ b/gazebo/dave_model_systems/CMakeLists.txt @@ -21,41 +21,18 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) message(STATUS "Compiling against Gazebo Harmonic") -# Add BasicSystem library -add_library(BasicSystem SHARED src/BasicSystem.cc) add_library(UsblTransceiver SHARED src/UsblTransceiver.cc) add_library(UsblTransponder SHARED src/UsblTransponder.cc) -# Include directories for BasicSystem -target_include_directories(BasicSystem PRIVATE include) target_include_directories(UsblTransceiver PRIVATE include) target_include_directories(UsblTransponder PRIVATE include) -# Link libraries for BasicSystem -target_link_libraries(BasicSystem - gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) - 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}) -# Add FullSystem library -add_library(FullSystem SHARED src/FullSystem.cc) - -# Include directories for FullSystem -target_include_directories(FullSystem PRIVATE include) - -# Link libraries for FullSystem -target_link_libraries(FullSystem - gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) - -# Specify dependencies for FullSystem using ament_target_dependencies -ament_target_dependencies(FullSystem - rclcpp - std_msgs -) # Specify dependencies for FullSystem using ament_target_dependencies ament_target_dependencies(UsblTransceiver @@ -72,7 +49,7 @@ ament_target_dependencies(UsblTransponder ) # Install targets -install(TARGETS BasicSystem FullSystem UsblTransceiver UsblTransponder +install(TARGETS UsblTransceiver UsblTransponder DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh b/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh deleted file mode 100644 index 58db3d8e..00000000 --- a/gazebo/dave_model_systems/include/dave_model_systems/BasicSystem.hh +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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_MODEL_SYSTEMS__BASICSYSTEM_HH_ -#define DAVE_MODEL_SYSTEMS__BASICSYSTEM_HH_ - -// The only required include in the header is this one. -// All others will depend on what your plugin does. -#include - -namespace dave_model_systems -{ -// This is the main plugin's class. It must inherit from System and at least -// one other interface. -// Here we use `ISystemPostUpdate`, which is used to get results after -// physics runs. The opposite of that, `ISystemPreUpdate`, would be used by -// plugins that want to send commands. -class BasicSystem : public gz::sim::System, public gz::sim::ISystemPostUpdate -{ - // Plugins inheriting ISystemPostUpdate must implement the PostUpdate - // callback. This is called at every simulation iteration after the physics - // updates the world. The _info variable provides information such as time, - // while the _ecm provides an interface to all entities and components in - // simulation. -public: - void PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; -}; -} // namespace dave_model_systems -#endif // DAVE_MODEL_SYSTEMS__BASICSYSTEM_HH_ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh b/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh deleted file mode 100644 index 71cde54f..00000000 --- a/gazebo/dave_model_systems/include/dave_model_systems/FullSystem.hh +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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_MODEL_SYSTEMS__FULLSYSTEM_HH_ -#define DAVE_MODEL_SYSTEMS__FULLSYSTEM_HH_ - -#include - -#include -#include -#include -#include - -namespace dave_model_systems -{ -class FullSystem : public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemUpdate, - public gz::sim::ISystemPostUpdate, - public gz::sim::ISystemReset -{ -public: - void Configure( - const gz::sim::Entity & _entity, const std::shared_ptr & _element, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) override; - -public: - void PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; - -public: - void Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; - -public: - void PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) override; - -public: - void Reset(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) override; - -private: - rclcpp::Node::SharedPtr ros_node_; - rclcpp::Publisher::SharedPtr log_pub_; -}; -} // namespace dave_model_systems -#endif // DAVE_MODEL_SYSTEMS__FULLSYSTEM_HH_ diff --git a/gazebo/dave_model_systems/src/BasicSystem.cc b/gazebo/dave_model_systems/src/BasicSystem.cc deleted file mode 100644 index 89e12e59..00000000 --- a/gazebo/dave_model_systems/src/BasicSystem.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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. - * - */ - -// We'll use a string and the gzmsg command below for a brief example. -// Remove these includes if your plugin doesn't need them. -#include - -// This header is required to register plugins. It's good practice to place it -// in the cc file, like it's done here. -#include -#include - -#include "dave_model_systems/BasicSystem.hh" - -// This is required to register the plugin. Make sure the interfaces match -// what's in the header. -GZ_ADD_PLUGIN( - dave_model_systems::BasicSystem, gz::sim::System, - dave_model_systems::BasicSystem::ISystemPostUpdate) - -namespace dave_model_systems -{ - -void BasicSystem::PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -{ - if (!_info.paused && _info.iterations % 1000 == 0) - { - gzdbg << "dave_model_systems::BasicSystem::PostUpdate" << std::endl; - } -} - -} // namespace dave_model_systems diff --git a/gazebo/dave_model_systems/src/FullSystem.cc b/gazebo/dave_model_systems/src/FullSystem.cc deleted file mode 100644 index 758c7a7f..00000000 --- a/gazebo/dave_model_systems/src/FullSystem.cc +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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. - * - */ - -// We'll use a string and the gzmsg command below for a brief example. -// Remove these includes if your plugin doesn't need them. -// Don't forget to include the plugin's header. -#include - -#include -#include - -#include "dave_model_systems/FullSystem.hh" -// This is required to register the plugin. Make sure the interfaces match -// what's in the header. -GZ_ADD_PLUGIN( - dave_model_systems::FullSystem, gz::sim::System, dave_model_systems::FullSystem::ISystemConfigure, - dave_model_systems::FullSystem::ISystemPreUpdate, dave_model_systems::FullSystem::ISystemUpdate, - dave_model_systems::FullSystem::ISystemPostUpdate, dave_model_systems::FullSystem::ISystemReset) - -namespace dave_model_systems -{ - -void FullSystem::Configure( - const gz::sim::Entity & _entity, const std::shared_ptr & _element, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) -{ - gzdbg << "dave_model_systems::FullSystem::Configure on entity: " << _entity << std::endl; - - if (!rclcpp::ok()) - { - rclcpp::init(0, nullptr); - } - - this->ros_node_ = std::make_shared("full_system_node"); - this->log_pub_ = this->ros_node_->create_publisher("/gazebo_logs", 10); - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::FullSystem::Configure on entity: " + std::to_string(_entity); - this->log_pub_->publish(msg); -} - -void FullSystem::PreUpdate( - const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ - if (!_info.paused && _info.iterations % 1000 == 0) - { - gzdbg << "dave_model_systems::FullSystem::PreUpdate" << std::endl; - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::FullSystem::PreUpdate"; - this->log_pub_->publish(msg); - } -} - -void FullSystem::Update(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ - if (!_info.paused && _info.iterations % 1000 == 0) - { - gzdbg << "dave_model_systems::FullSystem::Update" << std::endl; - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::FullSystem::Update"; - this->log_pub_->publish(msg); - } -} - -void FullSystem::PostUpdate( - const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) -{ - if (!_info.paused && _info.iterations % 1000 == 0) - { - gzdbg << "dave_model_systems::FullSystem::PostUpdate" << std::endl; - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::FullSystem::PostUpdate"; - this->log_pub_->publish(msg); - } -} - -void FullSystem::Reset(const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) -{ - gzdbg << "dave_model_systems::FullSystem::Reset" << std::endl; - - std_msgs::msg::String msg; - msg.data = "dave_model_systems::FullSystem::Reset"; - this->log_pub_->publish(msg); -} -} // namespace dave_model_systems From fc92e364e266c3c1c9d3376f03f4fa32cdc6956b Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:12:39 -0300 Subject: [PATCH 05/16] fix entity bug --- .../dave_demos/launch/dave_world.launch.py | 14 +++++- .../dave_model_systems/src/UsblTransceiver.cc | 43 +++++++++++-------- .../dave_model_systems/src/UsblTransponder.cc | 10 ++--- 3 files changed, 42 insertions(+), 25 deletions(-) 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_model_systems/src/UsblTransceiver.cc b/gazebo/dave_model_systems/src/UsblTransceiver.cc index 8c34cb56..2b03e5f9 100644 --- a/gazebo/dave_model_systems/src/UsblTransceiver.cc +++ b/gazebo/dave_model_systems/src/UsblTransceiver.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -23,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -215,10 +217,8 @@ void UsblTransceiver::Configure( for (auto & object : transpondersObjects) { - gzmsg << object << std::endl; this->dataPtr->m_transponderAttachedObjects.push_back(object); - this->dataPtr->m_transponderEntities.push_back( - _ecm.EntityByComponents(gz::sim::components::Name(object), gz::sim::components::Model())); + gzmsg << "Added " << object << "to list of transponders objects" << std::endl; } /* interrogation mode - 2 options @@ -363,11 +363,16 @@ void UsblTransceiver::sendPing() std_msgs::msg::String ping_msg; ping_msg.data = "ping"; - for (auto & transponder : this->dataPtr->m_transponderEntities) + 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 = @@ -431,8 +436,8 @@ void UsblTransceiver::temperatureRosCallback(const std_msgs::msg::Float64::Share void UsblTransceiver::receiveGazeboCallback(const gz::msgs::Vector3d & transponder_position) { - gzmsg << "Transceiver acquires transponders position: " << transponder_position.x() << " " - << transponder_position.y() << " " << transponder_position.z() << std::endl; + // gzmsg << "Transceiver acquires transponders position: " << transponder_position.x() << " " + // << transponder_position.y() << " " << transponder_position.z() << std::endl; gz::math::Vector3 transponder_position_ign = gz::math::Vector3( transponder_position.x(), transponder_position.y(), transponder_position.z()); @@ -455,11 +460,12 @@ void UsblTransceiver::publishPosition(double & bearing, double & range, double & location_cartesian.y = range * cos(elevation * M_PI / 180) * sin(bearing * M_PI / 180); location_cartesian.z = range * sin(elevation * M_PI / 180); - 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"; + // 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); @@ -477,9 +483,9 @@ void UsblTransceiver::calcuateRelativePose( 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"; + // gzmsg << "bearing: " << bearing << "\n"; + // gzmsg << "range: " << range << "\n"; + // gzmsg << "elevation: " << elevation << "\n\n"; } void UsblTransceiver::PostUpdate( @@ -487,11 +493,12 @@ void UsblTransceiver::PostUpdate( { if (!_info.paused) { - // gzdbg << "dave_model_systems::UsblTransceiver::PostUpdate" << std::endl; - - sendPing(); - rclcpp::spin_some(this->ros_node_); + + if (_info.iterations % 1000 == 0) + { + sendPing(); + } } } diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc index d3693929..b5da21ad 100644 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -266,11 +266,11 @@ void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) 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; + // 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(); } From a23edad60706808c709ed1855222c2c2567d97a3 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:49:28 -0300 Subject: [PATCH 06/16] corrections --- .../CMakeLists.txt | 2 +- .../hooks/dave_model_systems.dsv.in | 0 .../dave_gz_model_plugins}/UsblTransceiver.hh | 10 +++++----- .../dave_gz_model_plugins}/UsblTransponder.hh | 10 +++++----- .../package.xml | 0 .../src/UsblTransceiver.cc | 14 +++++++------- .../src/UsblTransponder.cc | 16 ++++++++-------- 7 files changed, 26 insertions(+), 26 deletions(-) rename gazebo/{dave_model_systems => dave_gz_model_plugins}/CMakeLists.txt (98%) rename gazebo/{dave_model_systems => dave_gz_model_plugins}/hooks/dave_model_systems.dsv.in (100%) rename gazebo/{dave_model_systems/include/dave_model_systems => dave_gz_model_plugins/include/dave_gz_model_plugins}/UsblTransceiver.hh (91%) rename gazebo/{dave_model_systems/include/dave_model_systems => dave_gz_model_plugins/include/dave_gz_model_plugins}/UsblTransponder.hh (89%) rename gazebo/{dave_model_systems => dave_gz_model_plugins}/package.xml (100%) rename gazebo/{dave_model_systems => dave_gz_model_plugins}/src/UsblTransceiver.cc (97%) rename gazebo/{dave_model_systems => dave_gz_model_plugins}/src/UsblTransponder.cc (96%) diff --git a/gazebo/dave_model_systems/CMakeLists.txt b/gazebo/dave_gz_model_plugins/CMakeLists.txt similarity index 98% rename from gazebo/dave_model_systems/CMakeLists.txt rename to gazebo/dave_gz_model_plugins/CMakeLists.txt index df133644..06fc3283 100644 --- a/gazebo/dave_model_systems/CMakeLists.txt +++ b/gazebo/dave_gz_model_plugins/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) # Define the project name -project(dave_model_systems) +project(dave_gz_model_plugins) # Find required packages find_package(ament_cmake REQUIRED) diff --git a/gazebo/dave_model_systems/hooks/dave_model_systems.dsv.in b/gazebo/dave_gz_model_plugins/hooks/dave_model_systems.dsv.in similarity index 100% rename from gazebo/dave_model_systems/hooks/dave_model_systems.dsv.in rename to gazebo/dave_gz_model_plugins/hooks/dave_model_systems.dsv.in diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh similarity index 91% rename from gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh rename to gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh index e1c275c0..5e1dec30 100644 --- a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransceiver.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh @@ -15,8 +15,8 @@ * */ -#ifndef DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ -#define DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ +#ifndef DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ +#define DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ #include #include @@ -30,7 +30,7 @@ #include "dave_interfaces/msg/usbl_command.hpp" #include "dave_interfaces/msg/usbl_response.hpp" -namespace dave_model_systems +namespace dave_gz_model_plugins { class UsblTransceiver : public gz::sim::System, @@ -66,6 +66,6 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; -} // namespace dave_model_systems +} // namespace dave_gz_model_plugins -#endif // DAVE_MODEL_SYSTEMS__USBLTRANSCEIVER_HH_ +#endif // DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ diff --git a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh similarity index 89% rename from gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh rename to gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh index bb56facc..e3679328 100644 --- a/gazebo/dave_model_systems/include/dave_model_systems/UsblTransponder.hh +++ b/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh @@ -15,8 +15,8 @@ * */ -#ifndef DAVE_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ -#define DAVE_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ +#ifndef DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ +#define DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ #include #include @@ -29,7 +29,7 @@ #include "dave_interfaces/msg/usbl_command.hpp" #include "dave_interfaces/msg/usbl_response.hpp" -namespace dave_model_systems +namespace dave_gz_model_plugins { class UsblTransponder : public gz::sim::System, @@ -60,6 +60,6 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; -} // namespace dave_model_systems +} // namespace dave_gz_model_plugins -#endif // DAVE_MODEL_SYSTEMS__USBLTRANSPONDER_HH_ +#endif // DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ diff --git a/gazebo/dave_model_systems/package.xml b/gazebo/dave_gz_model_plugins/package.xml similarity index 100% rename from gazebo/dave_model_systems/package.xml rename to gazebo/dave_gz_model_plugins/package.xml diff --git a/gazebo/dave_model_systems/src/UsblTransceiver.cc b/gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc similarity index 97% rename from gazebo/dave_model_systems/src/UsblTransceiver.cc rename to gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc index 2b03e5f9..41261922 100644 --- a/gazebo/dave_model_systems/src/UsblTransceiver.cc +++ b/gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc @@ -37,18 +37,18 @@ #include -#include "dave_model_systems/UsblTransceiver.hh" +#include "dave_gz_model_plugins/UsblTransceiver.hh" // available interrogation modes std::vector im = {"common", "individual"}; using std::placeholders::_1; GZ_ADD_PLUGIN( - dave_model_systems::UsblTransceiver, gz::sim::System, - dave_model_systems::UsblTransceiver::ISystemConfigure, - dave_model_systems::UsblTransceiver::ISystemPostUpdate) + dave_gz_model_plugins::UsblTransceiver, gz::sim::System, + dave_gz_model_plugins::UsblTransceiver::ISystemConfigure, + dave_gz_model_plugins::UsblTransceiver::ISystemPostUpdate) -namespace dave_model_systems +namespace dave_gz_model_plugins { struct UsblTransceiver::PrivateData @@ -99,7 +99,7 @@ void UsblTransceiver::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - gzdbg << "dave_model_systems::UsblTransceiver::Configure on entity: " << _entity << std::endl; + gzdbg << "dave_gz_model_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; if (!rclcpp::ok()) { @@ -502,4 +502,4 @@ void UsblTransceiver::PostUpdate( } } -} // namespace dave_model_systems +} // namespace dave_gz_model_plugins diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_gz_model_plugins/src/UsblTransponder.cc similarity index 96% rename from gazebo/dave_model_systems/src/UsblTransponder.cc rename to gazebo/dave_gz_model_plugins/src/UsblTransponder.cc index b5da21ad..d27f9c0b 100644 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ b/gazebo/dave_gz_model_plugins/src/UsblTransponder.cc @@ -35,16 +35,16 @@ #include #include -#include "dave_model_systems/UsblTransponder.hh" +#include "dave_gz_model_plugins/UsblTransponder.hh" using std::placeholders::_1; GZ_ADD_PLUGIN( - dave_model_systems::UsblTransponder, gz::sim::System, - dave_model_systems::UsblTransponder::ISystemConfigure, - dave_model_systems::UsblTransponder::ISystemPostUpdate) + dave_gz_model_plugins::UsblTransponder, gz::sim::System, + dave_gz_model_plugins::UsblTransponder::ISystemConfigure, + dave_gz_model_plugins::UsblTransponder::ISystemPostUpdate) -namespace dave_model_systems +namespace dave_gz_model_plugins { struct UsblTransponder::PrivateData @@ -85,7 +85,7 @@ void UsblTransponder::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - gzdbg << "dave_model_systems::UsblTransponder::Configure on entity: " << _entity << std::endl; + gzdbg << "dave_gz_model_plugins::UsblTransponder::Configure on entity: " << _entity << std::endl; if (!rclcpp::ok()) { @@ -336,9 +336,9 @@ void UsblTransponder::PostUpdate( { if (!_info.paused) { - // gzdbg << "dave_model_systems::UsblTransponder::PostUpdate" << std::endl; + // gzdbg << "dave_gz_model_plugins::UsblTransponder::PostUpdate" << std::endl; rclcpp::spin_some(this->ros_node_); } } -} // namespace dave_model_systems +} // namespace dave_gz_model_plugins From 04f10ee6917ff79df031b9df1e654ec697bddbc1 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:53:52 -0300 Subject: [PATCH 07/16] change package name --- ...ve_model_systems.dsv.in => dave_gz_model_plugins.dsv.in} | 0 gazebo/dave_gz_model_plugins/package.xml | 4 ++-- models/dave_worlds/worlds/usbl_tutorial.world | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) rename gazebo/dave_gz_model_plugins/hooks/{dave_model_systems.dsv.in => dave_gz_model_plugins.dsv.in} (100%) diff --git a/gazebo/dave_gz_model_plugins/hooks/dave_model_systems.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_model_systems.dsv.in rename to gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_model_plugins/package.xml index ab11c06d..fc6e9788 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_model_plugins/package.xml @@ -1,8 +1,8 @@ - dave_model_systems + dave_gz_model_plugins 0.0.0 - DAVE model systems + DAVE model plugins Helena Moyen Apache 2.0 ament_cmake diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index b705f232..fce8fae5 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -37,7 +37,7 @@ + name="dave_gz_model_plugins::UsblTransceiver"> USBL transceiver_manufacturer transponder_manufacturer @@ -69,7 +69,7 @@ + name="dave_gz_model_plugins::UsblTransponder"> USBL transponder_manufacturer 1 @@ -104,7 +104,7 @@ + name="dave_gz_model_plugins::UsblTransponder"> USBL transponder_manufacturer 2 From 1e14e8830690ea8530b241efe079518daa9b32b8 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 15:38:28 -0300 Subject: [PATCH 08/16] change package name to sensors --- .../CMakeLists.txt | 2 +- .../hooks/dave_gz_sensor_plugins.dsv.in} | 0 .../dave_gz_sensor_plugins}/UsblTransceiver.hh | 10 +++++----- .../dave_gz_sensor_plugins}/UsblTransponder.hh | 10 +++++----- .../package.xml | 4 ++-- .../src/UsblTransceiver.cc | 14 +++++++------- .../src/UsblTransponder.cc | 16 ++++++++-------- models/dave_worlds/worlds/usbl_tutorial.world | 6 +++--- 8 files changed, 31 insertions(+), 31 deletions(-) rename gazebo/{dave_gz_model_plugins => dave_gz_sensor_plugins}/CMakeLists.txt (98%) rename gazebo/{dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in => dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in} (100%) rename gazebo/{dave_gz_model_plugins/include/dave_gz_model_plugins => dave_gz_sensor_plugins/include/dave_gz_sensor_plugins}/UsblTransceiver.hh (91%) rename gazebo/{dave_gz_model_plugins/include/dave_gz_model_plugins => dave_gz_sensor_plugins/include/dave_gz_sensor_plugins}/UsblTransponder.hh (89%) rename gazebo/{dave_gz_model_plugins => dave_gz_sensor_plugins}/package.xml (86%) rename gazebo/{dave_gz_model_plugins => dave_gz_sensor_plugins}/src/UsblTransceiver.cc (97%) rename gazebo/{dave_gz_model_plugins => dave_gz_sensor_plugins}/src/UsblTransponder.cc (96%) diff --git a/gazebo/dave_gz_model_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt similarity index 98% rename from gazebo/dave_gz_model_plugins/CMakeLists.txt rename to gazebo/dave_gz_sensor_plugins/CMakeLists.txt index 06fc3283..92d161a1 100644 --- a/gazebo/dave_gz_model_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) # Define the project name -project(dave_gz_model_plugins) +project(dave_gz_sensor_plugins) # Find required packages find_package(ament_cmake REQUIRED) diff --git a/gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in b/gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in similarity index 100% rename from gazebo/dave_gz_model_plugins/hooks/dave_gz_model_plugins.dsv.in rename to gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh similarity index 91% rename from gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh rename to gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh index 5e1dec30..2282e065 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransceiver.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh @@ -15,8 +15,8 @@ * */ -#ifndef DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ -#define DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ +#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ +#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ #include #include @@ -30,7 +30,7 @@ #include "dave_interfaces/msg/usbl_command.hpp" #include "dave_interfaces/msg/usbl_response.hpp" -namespace dave_gz_model_plugins +namespace dave_gz_sensor_plugins { class UsblTransceiver : public gz::sim::System, @@ -66,6 +66,6 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; -} // namespace dave_gz_model_plugins +} // namespace dave_gz_sensor_plugins -#endif // DAVE_GZ_MODEL_PLUGINS__USBLTRANSCEIVER_HH_ +#endif // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_ diff --git a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh similarity index 89% rename from gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh rename to gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh index e3679328..365ca24d 100644 --- a/gazebo/dave_gz_model_plugins/include/dave_gz_model_plugins/UsblTransponder.hh +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh @@ -15,8 +15,8 @@ * */ -#ifndef DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ -#define DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ +#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ +#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ #include #include @@ -29,7 +29,7 @@ #include "dave_interfaces/msg/usbl_command.hpp" #include "dave_interfaces/msg/usbl_response.hpp" -namespace dave_gz_model_plugins +namespace dave_gz_sensor_plugins { class UsblTransponder : public gz::sim::System, @@ -60,6 +60,6 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; -} // namespace dave_gz_model_plugins +} // namespace dave_gz_sensor_plugins -#endif // DAVE_GZ_MODEL_PLUGINS__USBLTRANSPONDER_HH_ +#endif // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_ diff --git a/gazebo/dave_gz_model_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml similarity index 86% rename from gazebo/dave_gz_model_plugins/package.xml rename to gazebo/dave_gz_sensor_plugins/package.xml index fc6e9788..b1c657d6 100644 --- a/gazebo/dave_gz_model_plugins/package.xml +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -1,8 +1,8 @@ - dave_gz_model_plugins + dave_gz_sensor_plugins 0.0.0 - DAVE model plugins + DAVE sensor plugins Helena Moyen Apache 2.0 ament_cmake diff --git a/gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc similarity index 97% rename from gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc rename to gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc index 41261922..3d3114f2 100644 --- a/gazebo/dave_gz_model_plugins/src/UsblTransceiver.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc @@ -37,18 +37,18 @@ #include -#include "dave_gz_model_plugins/UsblTransceiver.hh" +#include "dave_gz_sensor_plugins/UsblTransceiver.hh" // available interrogation modes std::vector im = {"common", "individual"}; using std::placeholders::_1; GZ_ADD_PLUGIN( - dave_gz_model_plugins::UsblTransceiver, gz::sim::System, - dave_gz_model_plugins::UsblTransceiver::ISystemConfigure, - dave_gz_model_plugins::UsblTransceiver::ISystemPostUpdate) + dave_gz_sensor_plugins::UsblTransceiver, gz::sim::System, + dave_gz_sensor_plugins::UsblTransceiver::ISystemConfigure, + dave_gz_sensor_plugins::UsblTransceiver::ISystemPostUpdate) -namespace dave_gz_model_plugins +namespace dave_gz_sensor_plugins { struct UsblTransceiver::PrivateData @@ -99,7 +99,7 @@ void UsblTransceiver::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - gzdbg << "dave_gz_model_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; + gzdbg << "dave_gz_sensor_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; if (!rclcpp::ok()) { @@ -502,4 +502,4 @@ void UsblTransceiver::PostUpdate( } } -} // namespace dave_gz_model_plugins +} // namespace dave_gz_sensor_plugins diff --git a/gazebo/dave_gz_model_plugins/src/UsblTransponder.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc similarity index 96% rename from gazebo/dave_gz_model_plugins/src/UsblTransponder.cc rename to gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc index d27f9c0b..5e541424 100644 --- a/gazebo/dave_gz_model_plugins/src/UsblTransponder.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc @@ -35,16 +35,16 @@ #include #include -#include "dave_gz_model_plugins/UsblTransponder.hh" +#include "dave_gz_sensor_plugins/UsblTransponder.hh" using std::placeholders::_1; GZ_ADD_PLUGIN( - dave_gz_model_plugins::UsblTransponder, gz::sim::System, - dave_gz_model_plugins::UsblTransponder::ISystemConfigure, - dave_gz_model_plugins::UsblTransponder::ISystemPostUpdate) + dave_gz_sensor_plugins::UsblTransponder, gz::sim::System, + dave_gz_sensor_plugins::UsblTransponder::ISystemConfigure, + dave_gz_sensor_plugins::UsblTransponder::ISystemPostUpdate) -namespace dave_gz_model_plugins +namespace dave_gz_sensor_plugins { struct UsblTransponder::PrivateData @@ -85,7 +85,7 @@ void UsblTransponder::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - gzdbg << "dave_gz_model_plugins::UsblTransponder::Configure on entity: " << _entity << std::endl; + gzdbg << "dave_gz_sensor_plugins::UsblTransponder::Configure on entity: " << _entity << std::endl; if (!rclcpp::ok()) { @@ -336,9 +336,9 @@ void UsblTransponder::PostUpdate( { if (!_info.paused) { - // gzdbg << "dave_gz_model_plugins::UsblTransponder::PostUpdate" << std::endl; + // gzdbg << "dave_gz_sensor_plugins::UsblTransponder::PostUpdate" << std::endl; rclcpp::spin_some(this->ros_node_); } } -} // namespace dave_gz_model_plugins +} // namespace dave_gz_sensor_plugins diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index fce8fae5..a2b0f14b 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -37,7 +37,7 @@ + name="dave_gz_sensor_plugins::UsblTransceiver"> USBL transceiver_manufacturer transponder_manufacturer @@ -69,7 +69,7 @@ + name="dave_gz_sensor_plugins::UsblTransponder"> USBL transponder_manufacturer 1 @@ -104,7 +104,7 @@ + name="dave_gz_sensor_plugins::UsblTransponder"> USBL transponder_manufacturer 2 From 99dd3ab3f53a7080cebd32ab9f1fa2561c43516e Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 20:05:10 -0300 Subject: [PATCH 09/16] change cmake minimum --- gazebo/dave_gz_sensor_plugins/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index 92d161a1..837afe30 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) # Define the project name project(dave_gz_sensor_plugins) From 9748c5a3e264fbcda3cf5e04e8fc4f34d2c8151b Mon Sep 17 00:00:00 2001 From: hmoyen Date: Sat, 13 Jul 2024 18:54:58 -0300 Subject: [PATCH 10/16] add parameters --- .../src/UsblTransceiver.cc | 45 ++++++++++++++++--- .../src/UsblTransponder.cc | 14 +++++- models/dave_worlds/worlds/usbl_tutorial.world | 26 ++++++++--- 3 files changed, 73 insertions(+), 12 deletions(-) diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc index 3d3114f2..c5307c84 100644 --- a/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc @@ -83,6 +83,7 @@ struct UsblTransceiver::PrivateData std::string m_channel; double m_soundSpeed; double m_temperature; + double m_pingFrequency; }; // // Define constants for command IDs @@ -144,6 +145,18 @@ void UsblTransceiver::Configure( } 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")) { @@ -201,7 +214,20 @@ void UsblTransceiver::Configure( } this->dataPtr->m_enablePingerScheduler = _sdf->Get("enable_ping_scheduler"); - gzmsg << "pinger enable? " << this->dataPtr->m_enablePingerScheduler << std::endl; + + 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")) @@ -218,7 +244,7 @@ void UsblTransceiver::Configure( for (auto & object : transpondersObjects) { this->dataPtr->m_transponderAttachedObjects.push_back(object); - gzmsg << "Added " << object << "to list of transponders objects" << std::endl; + gzmsg << "Added " << object << " to list of transponders objects" << std::endl; } /* interrogation mode - 2 options @@ -375,8 +401,7 @@ void UsblTransceiver::sendPing() 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; // Defined here because there was no def before + this->dataPtr->m_soundSpeed = 1540.4 + pose_transceiver.Z() / 1000 * 17; sleep(dist / this->dataPtr->m_soundSpeed); @@ -413,6 +438,7 @@ void UsblTransceiver::interrogationModeRosCallback(const std_msgs::msg::String:: if (std::find(im.begin(), im.end(), mode) != im.end()) { this->dataPtr->m_interrogationMode = mode; + gzmsg << "Interrogation mode set to " << mode << std::endl; } else { @@ -495,9 +521,16 @@ void UsblTransceiver::PostUpdate( { rclcpp::spin_some(this->ros_node_); - if (_info.iterations % 1000 == 0) + if (this->dataPtr->m_enablePingerScheduler) { - sendPing(); + // 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(); + } } } } diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc index 5e541424..2e827adb 100644 --- a/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc @@ -77,7 +77,7 @@ struct UsblTransponder::PrivateData UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) { dataPtr->m_temperature = 10.0; // Default initialization - dataPtr->m_noiseSigma = 0.0; // Default initialization + dataPtr->m_noiseSigma = 1.0; // Default initialization dataPtr->m_noiseMu = 0.0; // Default initialization } @@ -129,6 +129,18 @@ void UsblTransponder::Configure( } 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")) { diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index a2b0f14b..8c773e2e 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -7,6 +7,7 @@ https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + 0 0 0 0 0 0 @@ -32,6 +33,10 @@ 1 1 1 + + 1 0 0 1 + 1 0 0 1 + @@ -43,13 +48,15 @@ transponder_manufacturer 1,2 168 - false + true + 0.5 sphere,sphere2 + 1540 - 3 3 0 0 0 0 + 3 3 1 0 0 0 @@ -65,6 +72,10 @@ 1 + + 0 1 0 1 + 0 1 0 1 + - 6 6 0 0 0 0 + 6 6 1 0 0 0 @@ -100,6 +111,10 @@ 1 + + 0 0 1 1 + 0 0 1 1 + transceiver_manufacturer 168 box + 1540 0 0.0 - + - \ No newline at end of file + From 424203052fa069050a9d7d8e44e8bad7b3384071 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Sun, 14 Jul 2024 21:57:47 -0300 Subject: [PATCH 11/16] delete install in cmake --- dave_interfaces/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index 60ab8ef7..d1f05819 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -16,12 +16,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblResponse.msg" ) -# Install generated messages -install(DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - # Install message package.xml install(FILES package.xml From f4d2a75124e0f97a125d1e4c5fbbf5243d7dcf69 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:25:41 +0900 Subject: [PATCH 12/16] testing --- test | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 test diff --git a/test b/test new file mode 100644 index 00000000..e69de29b From 3d9fc5854461fcb191eb0c5a9de280cf6cb9c42d Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:38:45 +0900 Subject: [PATCH 13/16] ament precommit fix attempt --- .pre-commit-config.yaml | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) 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 From 40b471f80ff7e5b14a4bd99de1a0abacceed2b22 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:39:07 +0900 Subject: [PATCH 14/16] remove test file --- test | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 test diff --git a/test b/test deleted file mode 100644 index e69de29b..00000000 From 65987176bdad5fde71a47f0bf194c7c6b4e2ebe7 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 15 Jul 2024 13:51:03 -0300 Subject: [PATCH 15/16] add custom location message and mode conditions --- dave_interfaces/CMakeLists.txt | 1 + dave_interfaces/msg/Location.msg | 4 + .../dave_gz_sensor_plugins/UsblTransceiver.hh | 8 +- .../dave_gz_sensor_plugins/UsblTransponder.hh | 1 + .../src/UsblTransceiver.cc | 137 +++++++++++++++--- .../src/UsblTransponder.cc | 111 +++++++++----- models/dave_worlds/worlds/usbl_tutorial.world | 8 +- 7 files changed, 202 insertions(+), 68 deletions(-) create mode 100644 dave_interfaces/msg/Location.msg diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index d1f05819..acb0f468 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" "msg/UsblResponse.msg" + "msg/Location.msg" ) # Install message package.xml 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/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 index 2282e065..24dac936 100644 --- 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 @@ -27,6 +27,7 @@ #include #include #include +#include "dave_interfaces/msg/location.hpp" #include "dave_interfaces/msg/usbl_command.hpp" #include "dave_interfaces/msg/usbl_response.hpp" @@ -48,7 +49,8 @@ public: void PostUpdate( const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; - void receiveGazeboCallback(const gz::msgs::Vector3d & transponder_position); + 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); @@ -56,9 +58,9 @@ public: void commandingResponseTestCallback(const std_msgs::msg::String::SharedPtr msg); void sendCommand(int command_id, std::string & transponder_id); void sendPing(); - void calcuateRelativePose( + void calculateRelativePose( gz::math::Vector3 position, double & bearing, double & range, double & elevation); - void publishPosition(double & bearing, double & range, double & elevation); + void publishPosition(int & transponder_id, double & bearing, double & range, double & elevation); private: std::shared_ptr ros_node_; 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 index 365ca24d..6cf6cdd1 100644 --- 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 @@ -53,6 +53,7 @@ public: 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_; diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc index c5307c84..05b9c52a 100644 --- a/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransceiver.cc @@ -69,21 +69,25 @@ struct UsblTransceiver::PrivateData std::string m_interrogationMode; gz::sim::Entity linkEntity; gz::transport::Node m_gzNode; - rclcpp::Publisher::SharedPtr m_publishTransponderRelPos; + 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::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 @@ -94,6 +98,7 @@ UsblTransceiver::UsblTransceiver() : dataPtr(std::make_unique()) { dataPtr->m_channel = "1"; dataPtr->m_temperature = 10.0; + dataPtr->setGlobalMode = false; } void UsblTransceiver::Configure( @@ -259,6 +264,7 @@ void UsblTransceiver::Configure( 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; @@ -287,8 +293,14 @@ void UsblTransceiver::Configure( gzmsg << "Transponder topic" << transponder_position << std::endl; - this->dataPtr->m_gzNode.Subscribe( - transponder_position, &UsblTransceiver::receiveGazeboCallback, this); + // 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 @@ -297,13 +309,22 @@ void UsblTransceiver::Configure( this->dataPtr->m_transceiverDevice + "_" + this->dataPtr->m_transceiverID + "/transponder_location"; - this->dataPtr->m_publishTransponderRelPos = - this->ros_node_->create_publisher(transponder_location_topic, 1); + 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( @@ -322,7 +343,7 @@ void UsblTransceiver::Configure( this->dataPtr->m_transceiverID + "/transponder_location_cartesian"; this->dataPtr->m_publishTransponderRelPosCartesian = - this->ros_node_->create_publisher( + this->ros_node_->create_publisher( transponder_location_cartesian_topic, 1); // ROS2 Subscribers @@ -334,9 +355,8 @@ void UsblTransceiver::Configure( this->dataPtr->m_interrogationModeSub = this->ros_node_->create_subscription( - "/" + this->dataPtr->ns + "/" + this->dataPtr->m_transceiverDevice + "_" + - this->dataPtr->m_transceiverID + "/interrogation_mode", - 1, std::bind(&UsblTransceiver::interrogationModeRosCallback, this, _1)); + interrogation_mode_topic, 1, + std::bind(&UsblTransceiver::interrogationModeRosCallback, this, _1)); this->dataPtr->m_commandResponseSub = this->ros_node_->create_subscription( @@ -424,6 +444,16 @@ void UsblTransceiver::channelSwitchCallback(const std_msgs::msg::String::SharedP { 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) @@ -460,32 +490,85 @@ void UsblTransceiver::temperatureRosCallback(const std_msgs::msg::Float64::Share << this->dataPtr->m_soundSpeed << " m/s\n"; } -void UsblTransceiver::receiveGazeboCallback(const gz::msgs::Vector3d & transponder_position) +void UsblTransceiver::receiveGazeboCallback( + const std::string & transponder, const gz::msgs::Vector3d & transponder_position) { - // gzmsg << "Transceiver acquires transponders position: " << transponder_position.x() << " " - // << transponder_position.y() << " " << transponder_position.z() << std::endl; + 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; - gz::math::Vector3 transponder_position_ign = gz::math::Vector3( - transponder_position.x(), transponder_position.y(), transponder_position.z()); + 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; - double bearing = 0, range = 0, elevation = 0; - calcuateRelativePose(transponder_position_ign, bearing, range, elevation); + int transponder_id; - publishPosition(bearing, range, elevation); + 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(double & bearing, double & range, double & elevation) +void UsblTransceiver::publishPosition( + int & transponder_id, double & bearing, double & range, double & elevation) { - geometry_msgs::msg::Vector3 location; + dave_interfaces::msg::Location location; + location.x = bearing; location.y = range; location.z = elevation; - geometry_msgs::msg::Vector3 location_cartesian; + 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"; @@ -497,7 +580,7 @@ void UsblTransceiver::publishPosition(double & bearing, double & range, double & this->dataPtr->m_publishTransponderRelPosCartesian->publish(location_cartesian); } -void UsblTransceiver::calcuateRelativePose( +void UsblTransceiver::calculateRelativePose( gz::math::Vector3 position, double & bearing, double & range, double & elevation) { gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); @@ -521,6 +604,16 @@ void UsblTransceiver::PostUpdate( { 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 diff --git a/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc index 2e827adb..c1a94cca 100644 --- a/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc +++ b/gazebo/dave_gz_sensor_plugins/src/UsblTransponder.cc @@ -54,6 +54,7 @@ struct UsblTransponder::PrivateData 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; @@ -68,6 +69,7 @@ struct UsblTransponder::PrivateData 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; @@ -242,6 +244,15 @@ void UsblTransponder::Configure( "/" + 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() @@ -264,61 +275,83 @@ void UsblTransponder::sendLocation() void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) { - 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")) + if (this->dataPtr->m_interrogationMode.compare("individual") == 0) { - // 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(); + 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 << "Unknown command, ignore\n"; + gzmsg << "Not in individual mode: cannot send location" << std::endl; } } void UsblTransponder::cisRosCallback(const std_msgs::msg::String::SharedPtr msg) { - 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")) + if (this->dataPtr->m_interrogationMode.compare("common") == 0) { - gzmsg << this->dataPtr->m_transponderDevice + "_" + this->dataPtr->m_transponderID + - ": Received cis_ping, responding\n"; - sleep(dist / this->dataPtr->m_soundSpeed); - sendLocation(); + 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 << "Unknown command, ignore\n"; + 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); diff --git a/models/dave_worlds/worlds/usbl_tutorial.world b/models/dave_worlds/worlds/usbl_tutorial.world index 8c773e2e..492a4b0d 100644 --- a/models/dave_worlds/worlds/usbl_tutorial.world +++ b/models/dave_worlds/worlds/usbl_tutorial.world @@ -1,4 +1,5 @@ + + - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane - - 0 0 0 0 0 0 + + 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 - + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun + @@ -41,20 +40,20 @@ - - USBL - transceiver_manufacturer - transponder_manufacturer - 1,2 - 168 - false - common - 0.5 - sphere,sphere2 - 1540 - + + USBL + transceiver_manufacturer + transponder_manufacturer + 1,2 + 168 + true + common + 0.5 + sphere,sphere2 + 1540 + @@ -80,23 +79,21 @@ - - USBL - transponder_manufacturer - 1 - transceiver_manufacturer - 168 - box - 0 - 0.0 - - + + USBL + transponder_manufacturer + 1 + transceiver_manufacturer + 168 + box + 0 + 0.0 + - - + 6 6 1 0 0 0 @@ -119,21 +116,19 @@ - - USBL - transponder_manufacturer - 2 - transceiver_manufacturer - 168 - box - 1540 - 0 - 0.0 - - + + USBL + transponder_manufacturer + 2 + transceiver_manufacturer + 168 + box + 1540 + 0 + 0.0 + -