diff --git a/dave_interfaces/CMakeLists.txt b/dave_interfaces/CMakeLists.txt index acb0f468..117faaff 100644 --- a/dave_interfaces/CMakeLists.txt +++ b/dave_interfaces/CMakeLists.txt @@ -9,12 +9,18 @@ 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/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..2a696e16 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh @@ -0,0 +1,74 @@ +/* + * 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 +#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 + +{ +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; + + 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_; + + 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..2145a8d7 --- /dev/null +++ b/gazebo/dave_ros_gz_plugins/src/SphericalCoords.cc @@ -0,0 +1,232 @@ +/* + * 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 "gz/plugin/Register.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. + 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()) {} + +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::SphericalCoords::Configure on entity: " << _entity << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("sc_node"); + + // 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->LocalFromSphericalPosition(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) +{ + 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); + sphericalCoords.SetLatitudeReference(angle); + + angle.SetDegree(request->longitude_deg); + sphericalCoords.SetLongitudeReference(angle); + + sphericalCoords.SetElevationReference(request->altitude); + + // Apply the modified spherical coordinates back to the world + this->dataPtr->world.SetSphericalCoordinates(*this->dataPtr->ecm, sphericalCoords); + + response->success = true; + + 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; +} + +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