Stick to the global utm_zone_ when transforming gps to UTM (#627)

* Stick to the global utm_zone_ when transforming gps to UTM
cra-ros-pkg · Jun 3, 2021 · cecf5cc · cecf5cc
1 parent 2f238a6
commit cecf5cc
Show file tree

Hide file tree

Showing 6 changed files with 165 additions and 17 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -63,6 +63,7 @@ add_service_files(
     GetState.srv
     SetDatum.srv
     SetPose.srv
+    SetUTMZone.srv
     ToggleFilterProcessing.srv
     FromLL.srv
     ToLL.srv
@@ -78,7 +79,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS
     include
-  LIBRARIES
+  LIBRARIES 
     ekf
     ekf_localization_nodelet
     filter_base
@@ -332,4 +333,10 @@ if (CATKIN_ENABLE_TESTING)
   catkin_add_gtest(navsat_conversions-test test/test_navsat_conversions.cpp)
   target_link_libraries(navsat_conversions-test navsat_transform ${catkin_LIBRARIES})
 
+  #### NAVSAT TRANSFORM TESTS ####
+  add_rostest_gtest(test_navsat_transform
+                    test/test_navsat_transform.test
+                    test/test_navsat_transform.cpp)
+  target_link_libraries(test_navsat_transform ${catkin_LIBRARIES} ${rostest_LIBRARIES})
+
 endif()
diff --git a/include/robot_localization/navsat_transform.h b/include/robot_localization/navsat_transform.h
@@ -36,6 +36,7 @@
 #include <robot_localization/SetDatum.h>
 #include <robot_localization/ToLL.h>
 #include <robot_localization/FromLL.h>
+#include <robot_localization/SetUTMZone.h>
 
 #include <ros/ros.h>
 
@@ -52,6 +53,8 @@
 
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
 
 #include <string>
 
@@ -90,6 +93,11 @@ class NavSatTransform
     //!
     bool fromLLCallback(robot_localization::FromLL::Request& request, robot_localization::FromLL::Response& response);
 
+    //! @brief Callback for the UTM zone service
+    //!
+    bool setUTMZoneCallback(robot_localization::SetUTMZone::Request& request,
+                            robot_localization::SetUTMZone::Response& response);
+
     //! @brief Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's
     //! centroid and returns the cartesian-frame pose of said centroid.
     //!
@@ -241,9 +249,13 @@ class NavSatTransform
     //!
     std::string gps_frame_id_;
 
-    //! @brief UTM zone as determined after transforming GPS message
+    //! @brief the UTM zone (zero means UPS)
+    //!
+    int utm_zone_;
+
+    //! @brief hemisphere (true means north, false means south)
     //!
-    std::string utm_zone_;
+    bool northp_;
 
     //! @brief Frame ID of the GPS odometry output
     //!
@@ -339,6 +351,10 @@ class NavSatTransform
     //!
     ros::ServiceServer from_ll_srv_;
 
+    //! @brief Service for set UTM zone
+    //!
+    ros::ServiceServer set_utm_zone_srv_;
+
     //! @brief Transform buffer for managing coordinate transforms
     //!
     tf2_ros::Buffer tf_buffer_;

diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp
@@ -61,7 +61,7 @@ namespace RobotLocalization
     yaw_offset_(0.0),
     base_link_frame_id_("base_link"),
     gps_frame_id_(""),
-    utm_zone_(""),
+    utm_zone_(0),
     world_frame_id_("odom"),
     transform_timeout_(ros::Duration(0)),
     tf_listener_(tf_buffer_)
@@ -110,6 +110,7 @@ namespace RobotLocalization
 
     to_ll_srv_ = nh.advertiseService("toLL", &NavSatTransform::toLLCallback, this);
     from_ll_srv_ = nh.advertiseService("fromLL", &NavSatTransform::fromLLCallback, this);
+    set_utm_zone_srv_ = nh.advertiseService("setUTMZone", &NavSatTransform::setUTMZoneCallback, this);
 
     if (use_manual_datum_ && nh_priv.hasParam("datum"))
     {
@@ -383,6 +384,7 @@ namespace RobotLocalization
   {
     if (!transform_good_)
     {
+      ROS_ERROR("No transform available (yet)");
       return false;
     }
     tf2::Vector3 point;
@@ -411,8 +413,9 @@ namespace RobotLocalization
     }
     else
     {
-      std::string utm_zone_tmp;
-      NavsatConversions::LLtoUTM(latitude, longitude, cartesian_y, cartesian_x, utm_zone_tmp);
+      int zone_tmp;
+      bool nortp_tmp;
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone_tmp, nortp_tmp, cartesian_x, cartesian_y, utm_zone_);
     }
 
     cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude));
@@ -421,6 +424,7 @@ namespace RobotLocalization
 
     if (!transform_good_)
     {
+      ROS_ERROR("No transform available (yet)");
       return false;
     }
 
@@ -429,6 +433,17 @@ namespace RobotLocalization
     return true;
   }
 
+  bool NavSatTransform::setUTMZoneCallback(robot_localization::SetUTMZone::Request& request,
+                                           robot_localization::SetUTMZone::Response& response)
+  {
+    double x_unused;
+    double y_unused;
+    int prec_unused;
+    GeographicLib::MGRS::Reverse(request.utm_zone, utm_zone_, northp_, x_unused, y_unused, prec_unused, true);
+    ROS_INFO("UTM zone set to %d %s", utm_zone_, northp_ ? "north" : "south");
+    return true;
+  }
+
   nav_msgs::Odometry NavSatTransform::cartesianToMap(const tf2::Transform& cartesian_pose) const
   {
     nav_msgs::Odometry gps_odom{};
@@ -473,11 +488,12 @@ namespace RobotLocalization
     }
     else
     {
-      NavsatConversions::UTMtoLL(odom_as_cartesian.getOrigin().getY(),
-                                 odom_as_cartesian.getOrigin().getX(),
-                                 utm_zone_,
-                                 latitude,
-                                 longitude);
+      GeographicLib::UTMUPS::Reverse(utm_zone_,
+                                     northp_,
+                                     odom_as_cartesian.getOrigin().getX(),
+                                     odom_as_cartesian.getOrigin().getY(),
+                                     latitude,
+                                     longitude);
       altitude = odom_as_cartesian.getOrigin().getZ();
     }
   }
@@ -613,8 +629,11 @@ namespace RobotLocalization
       }
       else
       {
-        std::string utm_zone_tmp;
-        NavsatConversions::LLtoUTM(msg->latitude, msg->longitude, cartesian_y, cartesian_x, utm_zone_tmp);
+        // Transform to UTM using the fixed utm_zone_
+        int zone_tmp;
+        bool northp_tmp;
+        GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude,
+                                       zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_);
       }
       latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
       latest_cartesian_covariance_.setZero();
@@ -831,15 +850,17 @@ namespace RobotLocalization
     }
     else
     {
-      NavsatConversions::LLtoUTM(msg->latitude, msg->longitude, cartesian_y, cartesian_x, utm_zone_,
-                                 utm_meridian_convergence_);
-      utm_meridian_convergence_ *= NavsatConversions::RADIANS_PER_DEGREE;
+      double k_tmp;
+      double utm_meridian_convergence_degrees;
+      GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude, utm_zone_, northp_,
+                                     cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp);
+      utm_meridian_convergence_ = utm_meridian_convergence_degrees * NavsatConversions::RADIANS_PER_DEGREE;
     }
 
     ROS_INFO_STREAM("Datum (latitude, longitude, altitude) is (" << std::fixed << msg->latitude << ", " <<
                     msg->longitude << ", " << msg->altitude << ")");
     ROS_INFO_STREAM("Datum " << ((use_local_cartesian_)? "Local Cartesian" : "UTM") <<
-                    " coordinate is (" << std::fixed << cartesian_x << ", " << cartesian_y << ")");
+                    " coordinate is (" << std::fixed << cartesian_x << ", " << cartesian_y << ") zone " << utm_zone_);
 
     transform_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
     transform_cartesian_pose_.setRotation(tf2::Quaternion::getIdentity());

diff --git a/srv/SetUTMZone.srv b/srv/SetUTMZone.srv
@@ -0,0 +1,4 @@
+# Set an UTM zone navsat_transform should stick to.
+# Example: 31U
+string utm_zone
+---
diff --git a/test/test_navsat_transform.cpp b/test/test_navsat_transform.cpp
@@ -0,0 +1,91 @@
+/*
+ * 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 "robot_localization/navsat_transform.h"
+#include <robot_localization/SetDatum.h>
+#include <robot_localization/ToLL.h>
+#include <robot_localization/FromLL.h>
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+TEST(NavSatTransformUTMJumpTest, UtmTest)
+{
+  ros::NodeHandle nh;
+  ros::ServiceClient set_datum_client = nh.serviceClient<robot_localization::SetDatum>("/datum");
+  ros::ServiceClient from_ll_client = nh.serviceClient<robot_localization::FromLL>("/fromLL");
+
+  EXPECT_TRUE(set_datum_client.waitForExistence(ros::Duration(5)));
+
+  // Initialise the navsat_transform node to a UTM zone
+  robot_localization::SetDatum set_datum_srv;
+  set_datum_srv.request.geo_pose.position.latitude = 1;
+  set_datum_srv.request.geo_pose.position.longitude = 4;
+  set_datum_srv.request.geo_pose.orientation.w = 1;
+  EXPECT_TRUE(set_datum_client.call(set_datum_srv));
+
+  // Let the node figure out its transforms
+  ros::Duration(0.2).sleep();
+
+  // Request the GPS point of said point:
+  robot_localization::FromLL from_ll_srv;
+  from_ll_srv.request.ll_point.latitude = 10;
+  from_ll_srv.request.ll_point.longitude = 4.5;
+  EXPECT_TRUE(from_ll_client.call(from_ll_srv));
+  auto initial_response = from_ll_srv.response;
+
+  // Request GPS point also in that zone
+  from_ll_srv.request.ll_point.longitude = 5.5;
+  EXPECT_TRUE(from_ll_client.call(from_ll_srv));
+  auto same_zone_response = from_ll_srv.response;
+
+  // 1° of longitude is about 111 kilometers in length
+  EXPECT_NEAR(initial_response.map_point.x, same_zone_response.map_point.x, 120e3);
+  EXPECT_NEAR(initial_response.map_point.y, same_zone_response.map_point.y, 120e3);
+
+  // Request GPS point from neighboring zone (zone crossing is at 6 degrees)
+  from_ll_srv.request.ll_point.longitude = 6.5;
+  from_ll_client.call(from_ll_srv);
+  auto neighbour_zone_response = from_ll_srv.response;
+
+  EXPECT_NEAR(initial_response.map_point.x, neighbour_zone_response.map_point.x, 2*120e3);
+  EXPECT_NEAR(initial_response.map_point.y, neighbour_zone_response.map_point.y, 2*120e3);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "test_navsat_transform");
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/test_navsat_transform.test b/test/test_navsat_transform.test
@@ -0,0 +1,9 @@
+<!-- Launch file for navsat_transform test -->
+
+<launch>
+
+    <node name="navsat_transform_node" pkg="robot_localization" type="navsat_transform_node" output="screen" />
+
+    <test test-name="test_navsat_transform" pkg="robot_localization" type="test_navsat_transform" />
+
+</launch>