Fix/navsat transform (#652)

* Add test for simple interface check on navsat_transform

Author: Timple

CMakeLists.txt

@@ -260,6 +260,18 @@ if(BUILD_TESTING)
   ament_add_gtest(test_navsat_conversions test/test_navsat_conversions.cpp)
   target_link_libraries(test_navsat_conversions ${library_name})
 
+  #### NAVSAT TRANSFORM TESTS ####
+  ament_add_gtest_executable(test_navsat_transform test/test_navsat_transform.cpp)
+  target_link_libraries(test_navsat_transform ${library_name})
+  rosidl_target_interfaces(test_navsat_transform
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  ament_add_test(test_navsat_transform
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 100
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_navsat_transform.launch.py"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
+
   ament_add_gtest_executable(test_ros_robot_localization_listener
                   test/test_ros_robot_localization_listener.cpp)
   target_link_libraries(test_ros_robot_localization_listener ${library_name})
@@ -295,6 +307,7 @@ if(BUILD_TESTING)
     #test_ekf_localization_node_bag3
     test_robot_localization_estimator
     test_navsat_conversions
+    test_navsat_transform
     test_ros_robot_localization_listener
     test_ros_robot_localization_listener_publisher
     #test_ukf_localization_node_bag1

src/navsat_transform.cpp

@@ -116,7 +116,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
       "Please use 'broadcast_cartesian_transform' instead.");
   } else {
     broadcast_cartesian_transform_ =
-      this->declare_parameter("broadcast_utm_transform", broadcast_cartesian_transform_);
+      this->declare_parameter("broadcast_cartesian_transform", broadcast_cartesian_transform_);
   }
 
   broadcast_cartesian_transform_as_parent_frame_ =

test/test_navsat_transform.cpp

@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2021, Charles River Analytics, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ * 3. Neither the name of the copyright holder 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 THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+
+#include <gtest/gtest.h>
+
+#include <robot_localization/navsat_conversions.hpp>
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "robot_localization/srv/set_datum.hpp"
+#include "robot_localization/srv/from_ll.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+using namespace std::chrono_literals;
+
+TEST(NavSatTransformUTMJumpTest, UtmTest)
+{
+  auto node_ = rclcpp::Node::make_shared("test_navsat_transform");
+  auto setDatumClient = node_->create_client<robot_localization::srv::SetDatum>("/datum");
+
+  EXPECT_TRUE(setDatumClient->wait_for_service(5s));
+
+  // Initialise the navsat_transform node to a UTM zone
+  auto setDatumRequest = std::make_shared<robot_localization::srv::SetDatum::Request>();
+  setDatumRequest->geo_pose.position.latitude = 1;
+  setDatumRequest->geo_pose.position.longitude = 4;
+  setDatumRequest->geo_pose.orientation.w = 1;
+
+  auto setDatumResponse = setDatumClient->async_send_request(setDatumRequest);
+  auto ret = rclcpp::spin_until_future_complete(node_, setDatumResponse, 5s);
+  EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+
+  return ret;
+}

test/test_navsat_transform.launch.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import launch
+from launch import LaunchDescription
+import launch_ros.actions
+import yaml
+import sys
+from launch.substitutions import EnvironmentVariable
+import launch.actions
+from launch.actions import DeclareLaunchArgument
+from launch_testing.legacy import LaunchTestService
+from launch.actions import ExecuteProcess
+from launch import LaunchService
+from ament_index_python.packages import get_package_prefix
+
+def generate_launch_description():
+    #*****test_navsat_transform.test*****
+    navsat_transform = launch_ros.actions.Node(
+        package='robot_localization',
+        executable='navsat_transform_node',
+        name='test_navsat_transform',
+        output='screen',
+    )
+
+    return LaunchDescription([
+        navsat_transform,
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_navsat_transform'],
+        output='screen',
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+if __name__ == '__main__':
+    sys.exit(main())