From 1743eb5d31b7bcdd7ef0b60a866cca3169f7e3d5 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Wed, 17 Jul 2024 13:14:44 -0300 Subject: [PATCH 01/23] new sc branch --- dave_interfaces/CMakeLists.txt | 30 +++++++ dave_interfaces/msg/Location.msg | 4 + dave_interfaces/msg/UsblCommand.msg | 3 + dave_interfaces/msg/UsblResponse.msg | 3 + dave_interfaces/package.xml | 19 +++++ gazebo/dave_ros_gz_plugins/CMakeLists.txt | 57 +++++++++++++ .../hooks/dave_ros_gz_plugins.dsv.in | 1 + .../dave_ros_gz_plugins/SphericalCoords.hh | 52 ++++++++++++ gazebo/dave_ros_gz_plugins/package.xml | 17 ++++ .../src/SphericalCoords.cc | 80 +++++++++++++++++++ 10 files changed, 266 insertions(+) create mode 100644 dave_interfaces/CMakeLists.txt create mode 100644 dave_interfaces/msg/Location.msg create mode 100644 dave_interfaces/msg/UsblCommand.msg create mode 100644 dave_interfaces/msg/UsblResponse.msg create mode 100644 dave_interfaces/package.xml create mode 100644 gazebo/dave_ros_gz_plugins/CMakeLists.txt create mode 100644 gazebo/dave_ros_gz_plugins/hooks/dave_ros_gz_plugins.dsv.in create mode 100644 gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh create mode 100644 gazebo/dave_ros_gz_plugins/package.xml create mode 100644 gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt new file mode 100644 index 00000000..acb0f468 --- /dev/null +++ b/dave_interfaces/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(dave_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/UsblCommand.msg" + "msg/UsblResponse.msg" + "msg/Location.msg" +) + +# Install message package.xml +install(FILES + package.xml + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies(rosidl_default_runtime) +# Install CMake package configuration +ament_export_include_directories(include) + +ament_package() diff --git a/dave_interfaces/msg/Location.msg b/dave_interfaces/msg/Location.msg new file mode 100644 index 00000000..1576b20f --- /dev/null +++ b/dave_interfaces/msg/Location.msg @@ -0,0 +1,4 @@ +int32 transponder_id +float64 x +float64 y +float64 z \ No newline at end of file diff --git a/dave_interfaces/msg/UsblCommand.msg b/dave_interfaces/msg/UsblCommand.msg new file mode 100644 index 00000000..c000b821 --- /dev/null +++ b/dave_interfaces/msg/UsblCommand.msg @@ -0,0 +1,3 @@ +int32 transponder_id +int32 command_id +string data \ No newline at end of file diff --git a/dave_interfaces/msg/UsblResponse.msg b/dave_interfaces/msg/UsblResponse.msg new file mode 100644 index 00000000..49ff7a39 --- /dev/null +++ b/dave_interfaces/msg/UsblResponse.msg @@ -0,0 +1,3 @@ +int32 transceiver_id +int32 response_id +string data \ No newline at end of file diff --git a/dave_interfaces/package.xml b/dave_interfaces/package.xml new file mode 100644 index 00000000..dc6dfa89 --- /dev/null +++ b/dave_interfaces/package.xml @@ -0,0 +1,19 @@ + + + dave_interfaces + 0.0.0 + TODO: Package description + lena + TODO: License declaration + ament_cmake + rclcpp + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_lint_auto + ament_lint_common + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/CMakeLists.txt b/gazebo/dave_ros_gz_plugins/CMakeLists.txt new file mode 100644 index 00000000..79ec7c04 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(dave_ros_gz_plugins) + +# Find required packages +find_package(ament_cmake REQUIRED) +find_package(gz-cmake3 REQUIRED) +find_package(gz-plugin2 REQUIRED COMPONENTS register) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(gz-common5 REQUIRED COMPONENTS profiler) +find_package(gz-sim8 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(dave_interfaces REQUIRED) + +# Set version variables +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) +set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + +message(STATUS "Compiling against Gazebo Harmonic") + +add_library(SphericalCoords SHARED src/SphericalCoords.cc) + +target_include_directories(SphericalCoords PRIVATE include) + +target_link_libraries(SphericalCoords + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) + +# Specify dependencies for FullSystem using ament_target_dependencies +ament_target_dependencies(SphericalCoords + dave_interfaces + rclcpp + geometry_msgs + std_msgs +) + +# Install targets +install(TARGETS SphericalCoords + 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_ros_gz_plugins/hooks/dave_ros_gz_plugins.dsv.in b/gazebo/dave_ros_gz_plugins/hooks/dave_ros_gz_plugins.dsv.in new file mode 100644 index 00000000..5e60e0e3 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/hooks/dave_ros_gz_plugins.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh new file mode 100644 index 00000000..bb3058eb --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh @@ -0,0 +1,52 @@ +/* + * 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_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_ +#define DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_ + +#include + +#include +#include + +namespace dave_ros_gz_plugins + +{ +class SphericalCoords : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + SphericalCoords(); + ~SphericalCoords() 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_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_ros_gz_plugins + +#endif // DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_ diff --git a/gazebo/dave_ros_gz_plugins/package.xml b/gazebo/dave_ros_gz_plugins/package.xml new file mode 100644 index 00000000..b240b498 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/package.xml @@ -0,0 +1,17 @@ + + + dave_ros_gz_plugins + 0.0.0 + TODO: Package description + lena + TODO: License declaration + ament_cmake + rclcpp + std_msgs + dave_interfaces + ament_lint_auto + ament_lint_common + + ament_cmake + + \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc new file mode 100644 index 00000000..5efd3229 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc @@ -0,0 +1,80 @@ +/* + * 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 "gz/common/StringUtils.hh" +#include "gz/plugin/Register.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/World.hh" + +#include "dave_ros_gz_plugins/SphericalCoords.hh" + +GZ_ADD_PLUGIN( + dave_ros_gz_plugins::SphericalCoords, gz::sim::System, + dave_ros_gz_plugins::SphericalCoords::ISystemConfigure, + dave_ros_gz_plugins::SphericalCoords::ISystemPostUpdate) + +namespace dave_ros_gz_plugins +{ + +struct SphericalCoords::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + std::string modelName; + std::string ns; + gz::sim::EntityComponentManager * ecm; +}; + +SphericalCoords::SphericalCoords() : dataPtr(std::make_unique()) {} + +void SphericalCoords::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_ros_gz_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("usbl_transceiver_node"); + + auto model = gz::sim::Model(_entity); + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + this->dataPtr->ecm = &_ecm; +} + +void SphericalCoords::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ +} + +} // namespace dave_ros_gz_plugins From ddb5f2623b3d813094f2823d5329d36e18d48efe Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 19 Jul 2024 11:49:42 -0300 Subject: [PATCH 02/23] add interfaces --- dave_interfaces/CMakeLists.txt | 7 + dave_interfaces/package.xml | 1 + .../srv/GetOriginSphericalCoord.srv | 22 +++ .../srv/SetOriginSphericalCoord.srv | 23 +++ .../srv/TransformFromSphericalCoord.srv | 23 +++ .../srv/TransformToSphericalCoord.srv | 23 +++ .../dave_ros_gz_plugins/SphericalCoords.hh | 22 +++ .../src/SphericalCoords.cc | 170 ++++++++++++++++-- .../worlds/dave_bimanual_example.world | 2 + 9 files changed, 276 insertions(+), 17 deletions(-) create mode 100644 dave_interfaces/srv/GetOriginSphericalCoord.srv create mode 100644 dave_interfaces/srv/SetOriginSphericalCoord.srv create mode 100644 dave_interfaces/srv/TransformFromSphericalCoord.srv create mode 100644 dave_interfaces/srv/TransformToSphericalCoord.srv diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index acb0f468..ca08cd7e 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -9,12 +9,19 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) + rosidl_generate_interfaces(${PROJECT_NAME} "msg/UsblCommand.msg" "msg/UsblResponse.msg" "msg/Location.msg" + "srv/SetOriginSphericalCoord.srv" + "srv/GetOriginSphericalCoord.srv" + "srv/TransformToSphericalCoord.srv" + "srv/TransformFromSphericalCoord.srv" + DEPENDENCIES geometry_msgs ) # Install message package.xml diff --git a/dave_interfaces/package.xml b/dave_interfaces/package.xml index dc6dfa89..0e24716b 100644 --- a/dave_interfaces/package.xml +++ b/dave_interfaces/package.xml @@ -8,6 +8,7 @@ ament_cmake rclcpp std_msgs + geometry_msgs rosidl_default_generators rosidl_default_runtime rosidl_interface_packages diff --git a/dave_interfaces/srv/GetOriginSphericalCoord.srv b/dave_interfaces/srv/GetOriginSphericalCoord.srv new file mode 100644 index 00000000..0056764c --- /dev/null +++ b/dave_interfaces/srv/GetOriginSphericalCoord.srv @@ -0,0 +1,22 @@ +# Copyright (c) 2016 The dave Simulator Authors. +# All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +--- +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude \ No newline at end of file diff --git a/dave_interfaces/srv/SetOriginSphericalCoord.srv b/dave_interfaces/srv/SetOriginSphericalCoord.srv new file mode 100644 index 00000000..268885ef --- /dev/null +++ b/dave_interfaces/srv/SetOriginSphericalCoord.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2016 The dave Simulator Authors. +# All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude +--- +bool success \ No newline at end of file diff --git a/dave_interfaces/srv/TransformFromSphericalCoord.srv b/dave_interfaces/srv/TransformFromSphericalCoord.srv new file mode 100644 index 00000000..63e5915e --- /dev/null +++ b/dave_interfaces/srv/TransformFromSphericalCoord.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2016 The dave Simulator Authors. +# All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude +--- +geometry_msgs/Vector3 output \ No newline at end of file diff --git a/dave_interfaces/srv/TransformToSphericalCoord.srv b/dave_interfaces/srv/TransformToSphericalCoord.srv new file mode 100644 index 00000000..29d3174b --- /dev/null +++ b/dave_interfaces/srv/TransformToSphericalCoord.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2016 The dave Simulator Authors. +# All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +geometry_msgs/Vector3 input +--- +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude_deg +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude_deg +# Altitude [m]. Positive is above the WGS 84 ellipsoid +float64 altitude \ No newline at end of file diff --git a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh index bb3058eb..2a696e16 100644 --- a/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh @@ -21,7 +21,13 @@ #include #include + #include +#include +#include "dave_interfaces/srv/get_origin_spherical_coord.hpp" +#include "dave_interfaces/srv/set_origin_spherical_coord.hpp" +#include "dave_interfaces/srv/transform_from_spherical_coord.hpp" +#include "dave_interfaces/srv/transform_to_spherical_coord.hpp" namespace dave_ros_gz_plugins @@ -41,6 +47,22 @@ public: void PostUpdate( const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; + bool GetOriginSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response); + + bool SetOriginSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response); + + bool TransformToSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response); + + bool TransformFromSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response); + private: std::shared_ptr ros_node_; diff --git a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc index 5efd3229..ab713f39 100644 --- a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc +++ b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc @@ -14,22 +14,13 @@ * limitations under the License. */ -#include -#include #include #include -#include -#include +#include #include -#include -#include -#include -#include #include -#include "gz/common/StringUtils.hh" #include "gz/plugin/Register.hh" -#include "gz/sim/components/Model.hh" #include "gz/sim/components/World.hh" #include "dave_ros_gz_plugins/SphericalCoords.hh" @@ -45,10 +36,13 @@ namespace dave_ros_gz_plugins struct SphericalCoords::PrivateData { // Add any private data members here. - gz::sim::Model model; - std::string modelName; std::string ns; gz::sim::EntityComponentManager * ecm; + gz::sim::World world; + rclcpp::Service::SharedPtr getOriginSrv; + rclcpp::Service::SharedPtr setOriginSrv; + rclcpp::Service::SharedPtr transformToSrv; + rclcpp::Service::SharedPtr transformFromSrv; }; SphericalCoords::SphericalCoords() : dataPtr(std::make_unique()) {} @@ -57,24 +51,166 @@ void SphericalCoords::Configure( const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) { - gzdbg << "dave_ros_gz_plugins::UsblTransceiver::Configure on entity: " << _entity << std::endl; + gzdbg << "dave_ros_gz_plugins::SphericalCoords::Configure on entity: " << _entity << std::endl; if (!rclcpp::ok()) { rclcpp::init(0, nullptr); } - this->ros_node_ = std::make_shared("usbl_transceiver_node"); + this->ros_node_ = std::make_shared("sc_node"); - auto model = gz::sim::Model(_entity); - this->dataPtr->model = model; - this->dataPtr->modelName = model.Name(_ecm); + // auto model = gz::sim::Model(_entity); + // this->dataPtr->model = model; + // this->dataPtr->modelName = model.Name(_ecm); this->dataPtr->ecm = &_ecm; + + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); + this->dataPtr->world = gz::sim::World(worldEntity); + + auto coordsOpt = this->dataPtr->world.SphericalCoordinates(_ecm); + + if (coordsOpt.has_value()) + { + auto coords = coordsOpt.value(); + gzmsg << "Spherical coordinates reference=" << std::endl + << "\t- Latitude [degrees]=" << coords.LatitudeReference().Degree() << std::endl + << "\t- Longitude [degrees]=" << coords.LongitudeReference().Degree() << std::endl + << "\t- Altitude [m]=" << coords.ElevationReference() << std::endl + << "\t- Heading [degrees] =" << coords.HeadingOffset().Degree() << std::endl; + } + else + { + gzmsg << "Spherical coordinates not available." << std::endl; + } + + this->dataPtr->getOriginSrv = + this->ros_node_->create_service( + "/gz/get_origin_spherical_coordinates", std::bind( + &SphericalCoords::GetOriginSphericalCoord, this, + std::placeholders::_1, std::placeholders::_2)); + + this->dataPtr->setOriginSrv = + this->ros_node_->create_service( + "/gz/set_origin_spherical_coordinates", std::bind( + &SphericalCoords::SetOriginSphericalCoord, this, + std::placeholders::_1, std::placeholders::_2)); + + this->dataPtr->transformToSrv = + this->ros_node_->create_service( + "/gz/transform_to_spherical_coordinates", std::bind( + &SphericalCoords::TransformToSphericalCoord, this, + std::placeholders::_1, std::placeholders::_2)); + + this->dataPtr->transformFromSrv = + this->ros_node_->create_service( + "/gz/transform_from_spherical_coordinates", + std::bind( + &SphericalCoords::TransformFromSphericalCoord, this, std::placeholders::_1, + std::placeholders::_2)); +} + +bool SphericalCoords::TransformFromSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response) +{ + gz::math::Vector3d scVec = + gz::math::Vector3d(request->latitude_deg, request->longitude_deg, request->altitude); + + gzmsg << "Called FROM and latitude: " << scVec.X() << std::endl; + + auto coords = this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm); + gz::math::Vector3d cartVec = coords->SphericalFromLocalPosition(scVec); + + response->output.x = cartVec.X(); + response->output.y = cartVec.Y(); + response->output.z = cartVec.Z(); + + return true; +} + +bool SphericalCoords::TransformToSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response) +{ + gz::math::Vector3d cartVec = + gz::math::Vector3d(request->input.x, request->input.y, request->input.z); + + gzmsg << "Called TO and X: " << cartVec.X() << std::endl; + + auto coords = this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm); + gz::math::Vector3d scVec = coords->SphericalFromLocalPosition(cartVec); + + response->latitude_deg = scVec.X(); + response->longitude_deg = scVec.Y(); + response->altitude = scVec.Z(); + + return true; +} + +bool SphericalCoords::GetOriginSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->latitude_deg = + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LatitudeReference().Degree(); + response->longitude_deg = + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LongitudeReference().Degree(); + response->altitude = + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->ElevationReference(); + + gzmsg << "Called Get" << std::endl; + return true; +} + +bool SphericalCoords::SetOriginSphericalCoord( + const std::shared_ptr request, + std::shared_ptr response) +{ + gz::math::Angle angle; + + angle.SetDegree(request->latitude_deg); + + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->SetLatitudeReference(angle); + + angle.SetDegree(request->longitude_deg); + + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->SetLongitudeReference(angle); + this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm) + ->SetElevationReference(request->altitude); + + response->success = true; + + // Debugging information + gzmsg << "SetOriginSphericalCoord called with: " + << "Latitude: " << request->latitude_deg << ", Longitude: " << request->longitude_deg + << ", Altitude: " << request->altitude << std::endl; + + gzmsg + << "New Spherical Coordinates: " + << "Latitude: " + << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LatitudeReference().Degree() + << ", Longitude: " + << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LongitudeReference().Degree() + << ", Altitude: " + << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->ElevationReference() + << std::endl; + + return true; } void SphericalCoords::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { + if (!_info.paused) + { + rclcpp::spin_some(this->ros_node_); + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_ros_gz_plugins::SphericalCoords::PostUpdate" << std::endl; + } + } } } // namespace dave_ros_gz_plugins diff --git a/models/dave_worlds/worlds/dave_bimanual_example.world b/models/dave_worlds/worlds/dave_bimanual_example.world index 9fe25b8f..461200b9 100644 --- a/models/dave_worlds/worlds/dave_bimanual_example.world +++ b/models/dave_worlds/worlds/dave_bimanual_example.world @@ -44,6 +44,8 @@ 3.515625 + + 50 0 150 0 0 0 From 954c965c2ec4df568f600fedff4bbad684ade8e0 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 19 Jul 2024 11:50:37 -0300 Subject: [PATCH 03/23] change launch to add verbose --- examples/dave_demos/launch/dave_world.launch.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 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), ] ) From 1cd7702f4f74bba6a337c0b8fee320b9da458ff6 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 19 Jul 2024 12:57:03 -0300 Subject: [PATCH 04/23] fix set origin sc --- .../src/SphericalCoords.cc | 54 ++++++++++++------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc index ab713f39..5c4e5383 100644 --- a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc +++ b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc @@ -167,34 +167,50 @@ bool SphericalCoords::SetOriginSphericalCoord( const std::shared_ptr request, std::shared_ptr response) { + auto optionalSphericalCoords = this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm); + + if (!optionalSphericalCoords.has_value()) + { + gzmsg << "Failed to get spherical coordinates" << std::endl; + response->success = false; + return false; + } + + auto sphericalCoords = optionalSphericalCoords.value(); + + // gzmsg << "Current Spherical Coordinates Before Setting: " + // << "Latitude: " << sphericalCoords.LatitudeReference().Degree() + // << ", Longitude: " << sphericalCoords.LongitudeReference().Degree() + // << ", Altitude: " << sphericalCoords.ElevationReference() << std::endl; + gz::math::Angle angle; angle.SetDegree(request->latitude_deg); - - this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->SetLatitudeReference(angle); + sphericalCoords.SetLatitudeReference(angle); angle.SetDegree(request->longitude_deg); + sphericalCoords.SetLongitudeReference(angle); - this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->SetLongitudeReference(angle); - this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm) - ->SetElevationReference(request->altitude); + sphericalCoords.SetElevationReference(request->altitude); + + // Apply the modified spherical coordinates back to the world + this->dataPtr->world.SetSphericalCoordinates(*this->dataPtr->ecm, sphericalCoords); response->success = true; - // Debugging information - gzmsg << "SetOriginSphericalCoord called with: " - << "Latitude: " << request->latitude_deg << ", Longitude: " << request->longitude_deg - << ", Altitude: " << request->altitude << std::endl; - - gzmsg - << "New Spherical Coordinates: " - << "Latitude: " - << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LatitudeReference().Degree() - << ", Longitude: " - << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->LongitudeReference().Degree() - << ", Altitude: " - << this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm)->ElevationReference() - << std::endl; + auto newSphericalCoords = this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm); + + if (newSphericalCoords.has_value()) + { + gzmsg << "Current Spherical Coordinates after Setting: " + << "Latitude: " << newSphericalCoords->LatitudeReference().Degree() + << ", Longitude: " << newSphericalCoords->LongitudeReference().Degree() + << ", Altitude: " << newSphericalCoords->ElevationReference() << std::endl; + } + else + { + gzmsg << "Failed to get new spherical coordinates" << std::endl; + } return true; } From 950969d0f948acfc96eb6093ae8e4023cadbabf3 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 19 Jul 2024 13:45:50 -0300 Subject: [PATCH 05/23] fix transform from sc to local --- gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc index 5c4e5383..2145a8d7 100644 --- a/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc +++ b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc @@ -120,7 +120,7 @@ bool SphericalCoords::TransformFromSphericalCoord( gzmsg << "Called FROM and latitude: " << scVec.X() << std::endl; auto coords = this->dataPtr->world.SphericalCoordinates(*this->dataPtr->ecm); - gz::math::Vector3d cartVec = coords->SphericalFromLocalPosition(scVec); + gz::math::Vector3d cartVec = coords->LocalFromSphericalPosition(scVec); response->output.x = cartVec.X(); response->output.y = cartVec.Y(); From b01c36fa429ccc1d057b19c6b868e84dfa90bca5 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 8 Jul 2024 01:59:43 -0300 Subject: [PATCH 06/23] 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 598ce3fc4e192fd3b8aedb8c0248d0e09d243858 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 8 Jul 2024 18:07:56 -0300 Subject: [PATCH 07/23] usbl transponder working --- 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 +- 5 files changed, 167 insertions(+), 13 deletions(-) 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 22a52588c3ec8b114b6529d4a44e245fcd156f1f Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 11:39:02 -0300 Subject: [PATCH 08/23] 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 bd73bfeff49ee91fb9920ab427f0988e8aafce45 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 11:41:13 -0300 Subject: [PATCH 09/23] 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 4ea1171313b7923a256406096c9cf79ae8564fc5 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:12:39 -0300 Subject: [PATCH 10/23] fix entity bug --- .../dave_model_systems/src/UsblTransceiver.cc | 43 +++++++++++-------- .../dave_model_systems/src/UsblTransponder.cc | 10 ++--- 2 files changed, 30 insertions(+), 23 deletions(-) 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 3db68e4896e7fb8a53a0fa496c7482fb9431e55a Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:49:28 -0300 Subject: [PATCH 11/23] 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 3a99656047baea03f68b1f8ce20830c59fc7e1db Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:53:52 -0300 Subject: [PATCH 12/23] 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 effd08907f72f1b9bc9131b4e3da74e5df6040dd Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 15:38:28 -0300 Subject: [PATCH 13/23] 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 7882becec7d2a90ca410a07040175cf2265465a5 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 20:05:10 -0300 Subject: [PATCH 14/23] 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 18e1f62e13f4b9504b48426083bdbbbdf1ba66f3 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Sat, 13 Jul 2024 18:54:58 -0300 Subject: [PATCH 15/23] 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 6c1ec089eb8970b08b4099ebeb31055b9aac7d29 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:25:41 +0900 Subject: [PATCH 16/23] 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 8b446c6c87797f16a00798a7650a590ee085e876 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:38:45 +0900 Subject: [PATCH 17/23] 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 e109afb33b48220ec76e93fad8b3b5c08e15ec75 Mon Sep 17 00:00:00 2001 From: Woen-Sug Choi Date: Mon, 15 Jul 2024 22:39:07 +0900 Subject: [PATCH 18/23] 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 227a97bdf76c62d1b8ac09fcaf54bbad6b4d72dc Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 15 Jul 2024 13:51:03 -0300 Subject: [PATCH 19/23] add custom location message and mode conditions --- .../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 +- 5 files changed, 197 insertions(+), 68 deletions(-) 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 + - From 010a7d2d20c622170ec51bc41cbdf85aac791648 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Mon, 8 Jul 2024 18:07:56 -0300 Subject: [PATCH 21/23] usbl transponder working --- .../dave_model_systems/src/UsblTransponder.cc | 356 ++++++++++++++++++ 1 file changed, 356 insertions(+) create mode 100644 gazebo/dave_model_systems/src/UsblTransponder.cc diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc new file mode 100644 index 00000000..8acc3224 --- /dev/null +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -0,0 +1,356 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/World.hh" + +#include +#include + +#include "dave_model_systems/UsblTransponder.hh" + +// Available interrogation modes +std::vector im = {"common", "individual"}; +using std::placeholders::_1; + +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; + 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; + std::string m_transponderDevice; + std::string m_transponderID; + double m_noiseSigma; + double m_noiseMu; + gz::transport::Node m_gzNode; + gz::transport::Node::Publisher m_globalPosPub; + gz::sim::Entity linkEntity; + rclcpp::Publisher::SharedPtr m_commandResponsePub; + rclcpp::Subscription::SharedPtr m_iisSub; + rclcpp::Subscription::SharedPtr m_cisSub; + rclcpp::Subscription::SharedPtr m_temperatureSub; + rclcpp::Subscription::SharedPtr m_commandSub; + double m_soundSpeed; + double m_temperature; +}; + +UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) +{ + dataPtr->m_temperature = 10.0; // Default initialization + dataPtr->m_noiseSigma = 0.0; // Default initialization + dataPtr->m_noiseMu = 0.0; // Default initialization +} + +void UsblTransponder::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_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); + + 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"); + } + + // 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() +{ + // randomly generate from normal distribution for noise + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> d(this->dataPtr->m_noiseMu, this->dataPtr->m_noiseSigma); + + gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); + gz::math::Vector3 position = pose.Pos(); + auto pub_msg = gz::msgs::Vector3d(); + // std::cout << position.X() << " " << position.Y() << " " + // << position.Z() << std::endl; + pub_msg.set_x(position.X() + d(gen)); + pub_msg.set_y(position.Y() + d(gen)); + pub_msg.set_z(position.Z() + d(gen)); + this->dataPtr->m_globalPosPub.Publish(pub_msg); +} + +void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) +{ + 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) +{ + 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); + rclcpp::spin(this->ros_node_); + } +} + +} // namespace dave_model_systems + From d287b953a6520306bc515bf286d05732bd50b96d Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 12 Jul 2024 14:12:39 -0300 Subject: [PATCH 22/23] fix entity bug --- gazebo/dave_model_systems/src/UsblTransponder.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc index 8acc3224..053e2956 100644 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ b/gazebo/dave_model_systems/src/UsblTransponder.cc @@ -273,11 +273,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 aab676d2023b88775cbf001c23d33c0de8f2af94 Mon Sep 17 00:00:00 2001 From: hmoyen Date: Fri, 19 Jul 2024 14:36:16 -0300 Subject: [PATCH 23/23] delete old package --- .../dave_model_systems/src/UsblTransponder.cc | 356 ------------------ 1 file changed, 356 deletions(-) delete mode 100644 gazebo/dave_model_systems/src/UsblTransponder.cc diff --git a/gazebo/dave_model_systems/src/UsblTransponder.cc b/gazebo/dave_model_systems/src/UsblTransponder.cc deleted file mode 100644 index 053e2956..00000000 --- a/gazebo/dave_model_systems/src/UsblTransponder.cc +++ /dev/null @@ -1,356 +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. - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "gz/sim/components/Model.hh" -#include "gz/sim/components/World.hh" - -#include -#include - -#include "dave_model_systems/UsblTransponder.hh" - -// Available interrogation modes -std::vector im = {"common", "individual"}; -using std::placeholders::_1; - -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; - 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; - std::string m_transponderDevice; - std::string m_transponderID; - double m_noiseSigma; - double m_noiseMu; - gz::transport::Node m_gzNode; - gz::transport::Node::Publisher m_globalPosPub; - gz::sim::Entity linkEntity; - rclcpp::Publisher::SharedPtr m_commandResponsePub; - rclcpp::Subscription::SharedPtr m_iisSub; - rclcpp::Subscription::SharedPtr m_cisSub; - rclcpp::Subscription::SharedPtr m_temperatureSub; - rclcpp::Subscription::SharedPtr m_commandSub; - double m_soundSpeed; - double m_temperature; -}; - -UsblTransponder::UsblTransponder() : dataPtr(std::make_unique()) -{ - dataPtr->m_temperature = 10.0; // Default initialization - dataPtr->m_noiseSigma = 0.0; // Default initialization - dataPtr->m_noiseMu = 0.0; // Default initialization -} - -void UsblTransponder::Configure( - const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) -{ - gzdbg << "dave_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); - - 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"); - } - - // 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() -{ - // randomly generate from normal distribution for noise - std::random_device rd{}; - std::mt19937 gen{rd()}; - std::normal_distribution<> d(this->dataPtr->m_noiseMu, this->dataPtr->m_noiseSigma); - - gz::math::Pose3d pose = worldPose(this->dataPtr->linkEntity, *this->dataPtr->ecm); - gz::math::Vector3 position = pose.Pos(); - auto pub_msg = gz::msgs::Vector3d(); - // std::cout << position.X() << " " << position.Y() << " " - // << position.Z() << std::endl; - pub_msg.set_x(position.X() + d(gen)); - pub_msg.set_y(position.Y() + d(gen)); - pub_msg.set_z(position.Z() + d(gen)); - this->dataPtr->m_globalPosPub.Publish(pub_msg); -} - -void UsblTransponder::iisRosCallback(const std_msgs::msg::String::SharedPtr msg) -{ - 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) -{ - 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); - rclcpp::spin(this->ros_node_); - } -} - -} // namespace dave_model_systems -