Skip to content

Commit 79749bd

Browse files
hmoyenhmoyen
authored
Merge pull request #12 from IOES-Lab/sc-plugin
[GSOC-72] Migration of Spherical Coordinates Plugin
2 parents 8604b2f + 9f1680f commit 79749bd

File tree

12 files changed

+481
-0
lines changed

12 files changed

+481
-0
lines changed

‎dave_interfaces/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,18 @@ endif()
99
find_package(ament_cmake REQUIRED)
1010
find_package(rclcpp REQUIRED)
1111
find_package(std_msgs REQUIRED)
12+
find_package(geometry_msgs REQUIRED)
1213
find_package(rosidl_default_generators REQUIRED)
1314

1415
rosidl_generate_interfaces(${PROJECT_NAME}
1516
"msg/UsblCommand.msg"
1617
"msg/UsblResponse.msg"
1718
"msg/Location.msg"
19+
"srv/SetOriginSphericalCoord.srv"
20+
"srv/GetOriginSphericalCoord.srv"
21+
"srv/TransformToSphericalCoord.srv"
22+
"srv/TransformFromSphericalCoord.srv"
23+
DEPENDENCIES geometry_msgs
1824
)
1925

2026
# Install message package.xml

‎dave_interfaces/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
<buildtool_depend>ament_cmake</buildtool_depend>
99
<depend>rclcpp</depend>
1010
<depend>std_msgs</depend>
11+
<depend>geometry_msgs</depend>
1112
<build_depend>rosidl_default_generators</build_depend>
1213
<exec_depend>rosidl_default_runtime</exec_depend>
1314
<member_of_group>rosidl_interface_packages</member_of_group>

‎dave_interfaces/srv/GetOriginSphericalCoord.srv

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
# Copyright (c) 2016 The dave Simulator Authors.
2+
# All rights reserved.
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
---
17+
# Latitude [degrees]. Positive is north of equator; negative is south.
18+
float64 latitude_deg
19+
# Longitude [degrees]. Positive is east of prime meridian; negative is west.
20+
float64 longitude_deg
21+
# Altitude [m]. Positive is above the WGS 84 ellipsoid
22+
float64 altitude

‎dave_interfaces/srv/SetOriginSphericalCoord.srv

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Copyright (c) 2016 The dave Simulator Authors.
2+
# All rights reserved.
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
# Latitude [degrees]. Positive is north of equator; negative is south.
17+
float64 latitude_deg
18+
# Longitude [degrees]. Positive is east of prime meridian; negative is west.
19+
float64 longitude_deg
20+
# Altitude [m]. Positive is above the WGS 84 ellipsoid
21+
float64 altitude
22+
---
23+
bool success

‎dave_interfaces/srv/TransformFromSphericalCoord.srv

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Copyright (c) 2016 The dave Simulator Authors.
2+
# All rights reserved.
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
# Latitude [degrees]. Positive is north of equator; negative is south.
17+
float64 latitude_deg
18+
# Longitude [degrees]. Positive is east of prime meridian; negative is west.
19+
float64 longitude_deg
20+
# Altitude [m]. Positive is above the WGS 84 ellipsoid
21+
float64 altitude
22+
---
23+
geometry_msgs/Vector3 output

‎dave_interfaces/srv/TransformToSphericalCoord.srv

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Copyright (c) 2016 The dave Simulator Authors.
2+
# All rights reserved.
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
geometry_msgs/Vector3 input
17+
---
18+
# Latitude [degrees]. Positive is north of equator; negative is south.
19+
float64 latitude_deg
20+
# Longitude [degrees]. Positive is east of prime meridian; negative is west.
21+
float64 longitude_deg
22+
# Altitude [m]. Positive is above the WGS 84 ellipsoid
23+
float64 altitude

‎gazebo/dave_ros_gz_plugins/CMakeLists.txt

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(dave_ros_gz_plugins)
3+
4+
# Find required packages
5+
find_package(ament_cmake REQUIRED)
6+
find_package(gz-cmake3 REQUIRED)
7+
find_package(gz-plugin2 REQUIRED COMPONENTS register)
8+
find_package(rclcpp REQUIRED)
9+
find_package(std_msgs REQUIRED)
10+
find_package(gz-common5 REQUIRED COMPONENTS profiler)
11+
find_package(gz-sim8 REQUIRED)
12+
find_package(geometry_msgs REQUIRED)
13+
find_package(dave_interfaces REQUIRED)
14+
15+
# Set version variables
16+
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
17+
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
18+
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
19+
20+
message(STATUS "Compiling against Gazebo Harmonic")
21+
22+
add_library(SphericalCoords SHARED src/SphericalCoords.cc)
23+
24+
target_include_directories(SphericalCoords PRIVATE include)
25+
26+
target_link_libraries(SphericalCoords
27+
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
28+
29+
# Specify dependencies for FullSystem using ament_target_dependencies
30+
ament_target_dependencies(SphericalCoords
31+
dave_interfaces
32+
rclcpp
33+
geometry_msgs
34+
std_msgs
35+
)
36+
37+
# Install targets
38+
install(TARGETS SphericalCoords
39+
DESTINATION lib/${PROJECT_NAME}
40+
)
41+
42+
# Install headers
43+
install(DIRECTORY include/
44+
DESTINATION include/
45+
)
46+
47+
# Environment hooks
48+
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
49+
50+
# Testing setup
51+
if(BUILD_TESTING)
52+
find_package(ament_lint_auto REQUIRED)
53+
ament_lint_auto_find_test_dependencies()
54+
endif()
55+
56+
# Configure ament
57+
ament_package()

‎gazebo/dave_ros_gz_plugins/hooks/dave_ros_gz_plugins.dsv.in

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/

‎gazebo/dave_ros_gz_plugins/include/dave_ros_gz_plugins/SphericalCoords.hh

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_
19+
#define DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_
20+
21+
#include <memory>
22+
23+
#include <gz/sim/System.hh>
24+
25+
#include <rclcpp/rclcpp.hpp>
26+
#include <rclcpp/service.hpp>
27+
#include "dave_interfaces/srv/get_origin_spherical_coord.hpp"
28+
#include "dave_interfaces/srv/set_origin_spherical_coord.hpp"
29+
#include "dave_interfaces/srv/transform_from_spherical_coord.hpp"
30+
#include "dave_interfaces/srv/transform_to_spherical_coord.hpp"
31+
32+
namespace dave_ros_gz_plugins
33+
34+
{
35+
class SphericalCoords : public gz::sim::System,
36+
public gz::sim::ISystemConfigure,
37+
public gz::sim::ISystemPostUpdate
38+
{
39+
public:
40+
SphericalCoords();
41+
~SphericalCoords() override = default;
42+
43+
void Configure(
44+
const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
45+
gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override;
46+
47+
void PostUpdate(
48+
const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override;
49+
50+
bool GetOriginSphericalCoord(
51+
const std::shared_ptr<dave_interfaces::srv::GetOriginSphericalCoord::Request> request,
52+
std::shared_ptr<dave_interfaces::srv::GetOriginSphericalCoord::Response> response);
53+
54+
bool SetOriginSphericalCoord(
55+
const std::shared_ptr<dave_interfaces::srv::SetOriginSphericalCoord::Request> request,
56+
std::shared_ptr<dave_interfaces::srv::SetOriginSphericalCoord::Response> response);
57+
58+
bool TransformToSphericalCoord(
59+
const std::shared_ptr<dave_interfaces::srv::TransformToSphericalCoord::Request> request,
60+
std::shared_ptr<dave_interfaces::srv::TransformToSphericalCoord::Response> response);
61+
62+
bool TransformFromSphericalCoord(
63+
const std::shared_ptr<dave_interfaces::srv::TransformFromSphericalCoord::Request> request,
64+
std::shared_ptr<dave_interfaces::srv::TransformFromSphericalCoord::Response> response);
65+
66+
private:
67+
std::shared_ptr<rclcpp::Node> ros_node_;
68+
69+
struct PrivateData;
70+
std::unique_ptr<PrivateData> dataPtr;
71+
};
72+
} // namespace dave_ros_gz_plugins
73+
74+
#endif // DAVE_ROS_GZ_PLUGINS__SPHERICALCOORDS_HH_

‎gazebo/dave_ros_gz_plugins/package.xml

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
3+
<name>dave_ros_gz_plugins</name>
4+
<version>0.0.0</version>
5+
<description>TODO: Package description</description>
6+
<maintainer email="[email protected]">lena</maintainer>
7+
<license>TODO: License declaration</license>
8+
<buildtool_depend>ament_cmake</buildtool_depend>
9+
<depend>rclcpp</depend>
10+
<depend>std_msgs</depend>
11+
<depend>dave_interfaces</depend>
12+
<test_depend>ament_lint_auto</test_depend>
13+
<test_depend>ament_lint_common</test_depend>
14+
<export>
15+
<build_type>ament_cmake</build_type>
16+
</export>
17+
</package>

0 commit comments

Comments
 (0)