Merge pull request #12 from IOES-Lab/sc-plugin

[GSOC-72] Migration of Spherical Coordinates Plugin

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

dave_interfaces/package.xml

@@ -8,6 +8,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
   <build_depend>rosidl_default_generators</build_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

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

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

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

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

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()

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@/

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 <memory>
+
+#include <gz/sim/System.hh>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/service.hpp>
+#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<const sdf::Element> & 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<dave_interfaces::srv::GetOriginSphericalCoord::Request> request,
+    std::shared_ptr<dave_interfaces::srv::GetOriginSphericalCoord::Response> response);
+
+  bool SetOriginSphericalCoord(
+    const std::shared_ptr<dave_interfaces::srv::SetOriginSphericalCoord::Request> request,
+    std::shared_ptr<dave_interfaces::srv::SetOriginSphericalCoord::Response> response);
+
+  bool TransformToSphericalCoord(
+    const std::shared_ptr<dave_interfaces::srv::TransformToSphericalCoord::Request> request,
+    std::shared_ptr<dave_interfaces::srv::TransformToSphericalCoord::Response> response);
+
+  bool TransformFromSphericalCoord(
+    const std::shared_ptr<dave_interfaces::srv::TransformFromSphericalCoord::Request> request,
+    std::shared_ptr<dave_interfaces::srv::TransformFromSphericalCoord::Response> response);
+
+private:
+  std::shared_ptr<rclcpp::Node> ros_node_;
+
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_ros_gz_plugins
+
+#endif  // DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_

gazebo/dave_ros_gz_plugins/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
+  <name>dave_ros_gz_plugins</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">lena</maintainer>
+  <license>TODO: License declaration</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>dave_interfaces</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>