Skip to content

Commit

Permalink
Fix UTM Utils and Get Tests to Build in ROS 2 (#729)
Browse files Browse the repository at this point in the history 
* Fix north and south projections

* Enable tests and fix coordinate types

* Updating test condition to match ROS 1 condition

* Format build files like on other in-progress branch

* Got a couple of the swri_transform_util unit tests working.

* Got a few more transform manager tests to work. Loss of precision somewhere causing existing test to fail.

* Uncommented all swri_transform_util::TransformManager tests.

* Update tests for ROS 2 except pytests

* Port python tests

* Update navsatfix test to match gpsfix test

---------

Co-authored-by: robert.brothers <[email protected]>
  • Loading branch information
@vknisley-swri @rjb0026
vknisley-swri and rjb0026 authored Mar 15, 2024
1 parent db2506b commit 982c9f6
Show file tree
Hide file tree
Showing 17 changed files with 1,105 additions and 349 deletions.
76 changes: 50 additions & 26 deletions swri_transform_util/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,32 +150,56 @@ target_include_directories(lat_lon_tf_echo

target_link_libraries(lat_lon_tf_echo ${PROJECT_NAME})

# TODO pjr Convert unit tests
# if(BUILD_TESTING)
# find_package(ament_cmake_gtest REQUIRED)
#
# add_rostest_gtest(test_local_xy_util launch/local_xy_util.test test/test_local_xy_util.cpp)
# target_link_libraries(test_local_xy_util ${PROJECT_NAME})
#
# add_rostest_gtest(test_utm_util launch/utm_util.test test/test_utm_util.cpp)
# target_link_libraries(test_utm_util ${PROJECT_NAME})
#
# add_rostest_gtest(test_transform_manager launch/transform_manager.test test/test_transform_manager.cpp)
# target_link_libraries(test_transform_manager ${PROJECT_NAME})
#
# add_rostest_gtest(test_georeference launch/georeference.test test/test_georeference.cpp)
# target_link_libraries(test_georeference ${PROJECT_NAME})
#
# add_rostest_gtest(test_transform_util launch/transform_util.test test/test_transform_util.cpp)
# target_link_libraries(test_transform_util ${PROJECT_NAME})
#
# add_rostest(test/initialize_invalid_gps.test)
# add_rostest(test/initialize_invalid_navsat.test)
# add_rostest(test/initialize_origin_auto_custom.test)
# add_rostest(test/initialize_origin_auto_gps.test)
# add_rostest(test/initialize_origin_auto_navsat.test)
# add_rostest(test/initialize_origin_manual.test)
# endif()
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(launch_testing_ament_cmake)

ament_add_gtest_executable(transform_manager_test test/test_transform_manager.cpp)
target_include_directories(transform_manager_test PRIVATE
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
)
target_link_libraries(transform_manager_test ${PROJECT_NAME})
ament_target_dependencies(transform_manager_test rclcpp tf2 tf2_ros)

ament_add_gtest(local_xy_util_test test/test_local_xy_util.cpp)
target_link_libraries(local_xy_util_test ${PROJECT_NAME})
ament_target_dependencies(local_xy_util_test rclcpp)

ament_add_gtest(utm_util_test test/test_utm_util.cpp)
target_link_libraries(utm_util_test ${PROJECT_NAME})
ament_target_dependencies(utm_util_test rclcpp)

ament_add_gtest(georeference_test test/test_georeference.cpp)
target_link_libraries(georeference_test ${PROJECT_NAME})
ament_target_dependencies(georeference_test rclcpp tf2 ament_index_cpp)

ament_add_gtest(transform_util_test test/test_transform_util.cpp)
target_link_libraries(transform_util_test ${PROJECT_NAME})
ament_target_dependencies(transform_util_test rclcpp tf2)

add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/launch/transform_manager.test.py)
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/launch/local_xy_util.test.py)
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_invalid_gps.test.py)
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_invalid_navsat.test.py)
# The "custom" functionality appears to be gone in ROS 2.
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_auto_gps.test.py)
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_auto_navsat.test.py)
add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_manual.test.py)

install(TARGETS
transform_manager_test
utm_util_test
georeference_test
transform_util_test
local_xy_util_test
DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY test
DESTINATION share/${PROJECT_NAME}
)

endif()

install(DIRECTORY include/
DESTINATION include
Expand Down
84 changes: 84 additions & 0 deletions swri_transform_util/launch/local_xy_util.test.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
#!/usr/bin/env python
# *****************************************************************************
#
# Copyright (c) 2024, Southwest Research Institute® (SwRI®)
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Southwest Research Institute® (SwRI®) nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# *****************************************************************************

import os
import unittest
import pytest

import launch
import launch_ros
import launch_testing
import ament_index_python
from launch_ros.actions import Node

def get_tests(*, args=[]):
test_path = os.path.join(
ament_index_python.get_package_prefix("swri_transform_util"),
"lib/swri_transform_util",
"local_xy_util_test",
)

return launch_testing.actions.GTest(
path=[test_path],
name="local_xy_util_test",
additional_env={"PYTHONBUFFERED": "1"},
output="screen",
)

@pytest.mark.launch_test
def generate_test_description():
init_origin = Node(
package="swri_transform_util",
name="initialize_origin",
executable="initialize_origin.py",
parameters=[{
"local_xy_frame": "/far_field",
"local_xy_origin": "swri",
"local_xy_origins": [29.45196669, -98.61370577, 233.719, 0.0],
}]
)

return launch.LaunchDescription(
[
init_origin,
launch_testing.util.KeepAliveProc(),
launch_testing.actions.ReadyToTest(),
]
)

class LocalXyUtilTest(unittest.TestCase):
def test_local_xy_util(self, launch_service, proc_info, proc_output):
tests = get_tests();
with launch_testing.tools.launch_process(
launch_service, tests, proc_info, proc_output
):
proc_info.assertWaitForStartup(process=tests, timeout=30)
proc_info.assertWaitForShutdown(process=tests, timeout=600)
launch_testing.asserts.assertExitCodes(proc_info, process=tests)
130 changes: 130 additions & 0 deletions swri_transform_util/launch/transform_manager.test.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,130 @@
#!/usr/bin/env python
# *****************************************************************************
#
# Copyright (c) 2024, Southwest Research Institute® (SwRI®)
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Southwest Research Institute® (SwRI®) nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# *****************************************************************************

import os
import unittest
import pytest

import launch
import launch_ros
import launch_testing
import ament_index_python
from launch_ros.actions import Node

def get_tests(*, args=[]):
test_path = os.path.join(
ament_index_python.get_package_prefix("swri_transform_util"),
"lib/swri_transform_util",
"transform_manager_test",
)

return launch_testing.actions.GTest(
path=[test_path],
name="transform_manager_test",
additional_env={"PYTHONBUFFERED": "1"},
output="screen",
)

@pytest.mark.launch_test
def generate_test_description():
init_origin = Node(
package="swri_transform_util",
name="initialize_origin",
executable="initialize_origin.py",
parameters=[{
"local_xy_frame": "/far_field",
"local_xy_origin": "swri",
"local_xy_origins": [29.45196669, -98.61370577, 233.719, 0.0],
}]
)

tf1 = Node(
package="tf2_ros",
name="tf1",
executable="static_transform_publisher",
arguments=[
"--x", "500.0",
"--y", "500.0",
"--z", "0",
"--roll", "0",
"--pitch", "0",
"--yaw", "0",
"--frame-id", "far_field",
"--child-frame-id", "near_field"],
)
tf2 = Node(
package="tf2_ros",
name="tf2",
executable="static_transform_publisher",
arguments=[
"--x", "1000.0",
"--y", "0.0",
"--z", "0.0",
"--roll", "0.0",
"--pitch", "0.0",
"--yaw", "0.0",
"--frame-id", "far_field",
"--child-frame-id", "veh_far_field"],
)
tf3 = Node(
package="tf2_ros",
name="tf3",
executable="static_transform_publisher",
arguments=[
"--x", "0.0",
"--y", "0.0",
"--z", "0.0",
"--roll", "0.0",
"--pitch", "0.0",
"--yaw", "0.0",
"--frame-id", "veh_far_field",
"--child-frame-id", "veh_near_field"],
)

return launch.LaunchDescription(
[
init_origin,
tf1,
tf2,
tf3,
launch_testing.util.KeepAliveProc(),
launch_testing.actions.ReadyToTest(),
]
)

class TransformManagerTest(unittest.TestCase):
def test_transform_manager(self, launch_service, proc_info, proc_output):
tests = get_tests();
with launch_testing.tools.launch_process(
launch_service, tests, proc_info, proc_output
):
proc_info.assertWaitForStartup(process=tests, timeout=30)
proc_info.assertWaitForShutdown(process=tests, timeout=600)
launch_testing.asserts.assertExitCodes(proc_info, process=tests)
8 changes: 8 additions & 0 deletions swri_transform_util/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,14 @@

<exec_depend>python3-numpy</exec_depend>

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_index_cpp</test_depend>
<test_depend>launch_ros</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>launch_testing_ament_cmake</test_depend>
<test_depend>tf_transformations</test_depend>
<test_depend>python-transforms3d-pip</test_depend>

<export>
<build_type>ament_cmake</build_type>
<rosdoc config="rosdoc.yaml" />
Expand Down
3 changes: 2 additions & 1 deletion swri_transform_util/src/local_xy_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include <functional>

#include <tf2/utils.h>
#include <rcutils/logging_macros.h>

#include <swri_math_util/constants.h>
#include <swri_math_util/trig_util.h>
Expand Down Expand Up @@ -209,7 +210,7 @@ namespace swri_transform_util
}

frame_ = frame;

RCUTILS_LOG_WARN("LocalXyWgs84Util initializing origin to lat: %f, lon: %f, alt: %f", latitude, longitude, reference_altitude_);
Initialize();
pose_sub_.reset();
return;
Expand Down
22 changes: 9 additions & 13 deletions swri_transform_util/src/utm_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,12 +86,12 @@ namespace swri_transform_util
for (int i = 0; i < 60; i++)
{
snprintf(args, sizeof(args), "+proj=utm +ellps=WGS84 +zone=%d", i + 1);
snprintf(args, sizeof(args), "+proj=utm +ellps=WGS84 +zone=%d +south", i + 1);

P_ll_north_[i] = proj_create_crs_to_crs(PJ_DEFAULT_CTX,
"+proj=latlong +ellps=WGS84",
args,
NULL);

snprintf(args, sizeof(args), "+proj=utm +ellps=WGS84 +zone=%d +south", i + 1);
P_ll_south_[i] = proj_create_crs_to_crs(PJ_DEFAULT_CTX,
"+proj=latlong +ellps=WGS84",
args,
Expand Down Expand Up @@ -121,12 +121,10 @@ namespace swri_transform_util
zone = GetZone(longitude);
band = GetBand(latitude);

double x = longitude * swri_math_util::_deg_2_rad;
double y = latitude * swri_math_util::_deg_2_rad;

// Get easting and northing values.
PJ_COORD c, c_out;
c = proj_coord(x, y, 0, 0);
c.lp.lam = longitude;
c.lp.phi = latitude;

// Get easting and northing values.
if (band <= 'N')
Expand Down Expand Up @@ -164,23 +162,21 @@ namespace swri_transform_util
{
boost::unique_lock<boost::mutex> lock(mutex_);

double x = easting;
double y = northing;

PJ_COORD c, c_out;
c = proj_coord(easting, northing, 0, 0);
c.enu.e = easting;
c.enu.n = northing;

if (band <= 'N')
{
c_out = proj_trans(P_ll_south_[zone - 1], PJ_INV, c);
}
else
{
c_out = proj_trans(P_ll_south_[zone - 1], PJ_INV, c);
c_out = proj_trans(P_ll_north_[zone - 1], PJ_INV, c);
}

longitude = c_out.xyz.x * swri_math_util::_rad_2_deg;
latitude = c_out.xyz.y * swri_math_util::_rad_2_deg;
longitude = c_out.lp.lam;
latitude = c_out.lp.phi;
}

UtmUtil::UtmUtil() :
Expand Down
Loading

0 comments on commit 982c9f6

Please sign in to comment.