Fix/set utm map frame change (#830)

* Fix utm service properly resetting transforms and fix test for this

---------

Co-authored-by: Ferry Schoenmakers <ferry.schoenmakers@nobleo.nl>

include/robot_localization/navsat_transform.h

@@ -210,6 +210,10 @@ class NavSatTransform
     //!
     bool use_local_cartesian_;
 
+    //! @brief Whether we want to force the user's UTM zone and not rely on current GPS data for determining it
+    //!
+    bool force_user_utm_;
+
     //! @brief When true, do not print warnings for tf lookup failures.
     //!
     bool tf_silent_failure_;

src/navsat_transform.cpp

@@ -56,6 +56,7 @@ namespace RobotLocalization
     use_manual_datum_(false),
     use_odometry_yaw_(false),
     use_local_cartesian_(false),
+    force_user_utm_(false),
     zero_altitude_(false),
     magnetic_declination_(0.0),
     yaw_offset_(0.0),
@@ -451,7 +452,13 @@ namespace RobotLocalization
     double y_unused;
     int prec_unused;
     GeographicLib::MGRS::Reverse(request.utm_zone, utm_zone_, northp_, x_unused, y_unused, prec_unused, true);
+    // Toggle flags such that transforms get updated to user utm zone
+    force_user_utm_ = true;
+    use_manual_datum_ = false;
+    transform_good_ = false;
+    has_transform_gps_ = false;
     ROS_INFO("UTM zone set to %d %s", utm_zone_, northp_ ? "north" : "south");
+
     return true;
   }
 
@@ -875,8 +882,18 @@ namespace RobotLocalization
     {
       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);
+      try
+      {
+        // If we're using a fixed UTM zone, then we want to use the zone that the user gave us.
+        int set_zone = force_user_utm_ ? utm_zone_ : -1;
+        GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude, utm_zone_, northp_,
+                                     cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp, set_zone);
+      }
+      catch (const GeographicLib::GeographicErr& e)
+      {
+        ROS_ERROR_STREAM_THROTTLE(1.0, e.what());
+        return;
+      }
       utm_meridian_convergence_ = utm_meridian_convergence_degrees * NavsatConversions::RADIANS_PER_DEGREE;
     }
 

test/test_navsat_transform.cpp

@@ -35,6 +35,9 @@
 #include <robot_localization/ToLL.h>
 #include <robot_localization/FromLL.h>
 
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/NavSatFix.h>
+
 #include <gtest/gtest.h>
 
 #include <string>
@@ -82,6 +85,86 @@ TEST(NavSatTransformUTMJumpTest, UtmTest)
   EXPECT_NEAR(initial_response.map_point.y, neighbour_zone_response.map_point.y, 2*120e3);
 }
 
+TEST(NavSatTransformUTMJumpTest, UtmServiceTest)
+{
+  ros::NodeHandle nh;
+  ros::ServiceClient set_zone_client = nh.serviceClient<robot_localization::SetUTMZone>("/setUTMZone");
+  tf2_ros::Buffer tf_buffer;
+  tf2_ros::TransformListener tf_listener(tf_buffer);
+
+  EXPECT_TRUE(set_zone_client.waitForExistence(ros::Duration(5)));
+
+  // Publish input topics
+  ros::Publisher gps_pub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1, true);
+  ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odometry/filtered", 1, true);
+
+  sensor_msgs::NavSatFix gps_msg;
+  gps_msg.header.frame_id = "base_link";
+  gps_msg.header.stamp = ros::Time::now();
+  gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
+  gps_msg.latitude = 36.0;
+  gps_msg.longitude = 0.0;
+
+  nav_msgs::Odometry odom_msg;
+  odom_msg.header.frame_id = "odom";
+  odom_msg.header.stamp = ros::Time::now();
+  odom_msg.child_frame_id = "base_link";
+  odom_msg.pose.pose.orientation.w = 1;
+
+  // Initialise the navsat_transform node to a UTM zone
+  robot_localization::SetUTMZone set_zone_srv;
+  set_zone_srv.request.utm_zone = "30U";
+  EXPECT_TRUE(set_zone_client.call(set_zone_srv));
+  gps_msg.header.stamp = ros::Time::now();
+  gps_pub.publish(gps_msg);
+  odom_msg.header.stamp = ros::Time::now();
+  odom_pub.publish(odom_msg);
+  // Let the node figure out its transforms
+  ros::Duration(0.3).sleep();
+
+  // Record the initial map transform
+  geometry_msgs::TransformStamped initial_transform;
+  try
+  {
+    EXPECT_TRUE(tf_buffer.canTransform("utm", "odom", ros::Time::now(), ros::Duration(3.0)));
+    initial_transform = tf_buffer.lookupTransform("utm", "odom", ros::Time::now());
+  }
+  catch (tf2::TransformException &ex)
+  {
+    ROS_ERROR("%s", ex.what());
+    FAIL();
+  }
+
+  // Set the zone to a neighboring zone
+  set_zone_srv.request.utm_zone = "31U";
+  EXPECT_TRUE(set_zone_client.call(set_zone_srv));
+  tf_buffer.clear();
+  gps_msg.header.stamp = ros::Time::now();
+  gps_pub.publish(gps_msg);
+  odom_msg.header.stamp = ros::Time::now();
+  odom_pub.publish(odom_msg);
+  // Let the node figure out its transforms
+  ros::Duration(0.3).sleep();
+
+  // Check if map transform has changed
+  geometry_msgs::TransformStamped new_transform;
+  try
+  {
+    EXPECT_TRUE(tf_buffer.canTransform("utm", "odom", ros::Time::now(), ros::Duration(3.0)));
+    new_transform = tf_buffer.lookupTransform("utm", "odom", ros::Time::now());
+  }
+  catch (tf2::TransformException &ex)
+  {
+    ROS_ERROR("%s", ex.what());
+    FAIL();
+  }
+
+  // Check difference between initial and new transform
+  EXPECT_GT(std::abs(initial_transform.transform.translation.x - new_transform.transform.translation.x), 1.0);
+  EXPECT_GT(std::abs(initial_transform.transform.translation.x - new_transform.transform.translation.x), 1.0);
+  EXPECT_GT(std::abs(initial_transform.transform.rotation.z - new_transform.transform.rotation.z), 0.02);
+}
+
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "test_navsat_transform");

test/test_navsat_transform.test

@@ -2,7 +2,13 @@
 
 <launch>
 
-    <node name="navsat_transform_node" pkg="robot_localization" type="navsat_transform_node" output="screen" />
+    <node name="navsat_transform_node" pkg="robot_localization" type="navsat_transform_node" output="screen" >
+        <param name="use_odometry_yaw" value="true" />
+        <param name="broadcast_cartesian_transform" value="true" />
+        <param name="broadcast_cartesian_transform_as_parent_frame" value="true" />
+    </node>
+
+    <node name="static_pub" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 map base_link" />
 
     <test test-name="test_navsat_transform" pkg="robot_localization" type="test_navsat_transform" />