FromLLArray - update after review

Author: MarcinSlomiany

include/robot_localization/navsat_transform.hpp

@@ -41,6 +41,8 @@
 #include <Eigen/Dense>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
+#include <geographic_msgs/msg/geo_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
@@ -51,6 +53,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <stdexcept>
 #include <string>
 
 namespace robot_localization
@@ -106,6 +109,10 @@ class NavSatTransform : public rclcpp::Node
     const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
     std::shared_ptr<robot_localization::srv::FromLLArray::Response> response);
 
+  //! @brief Method for convert point from Lat Lon to the map coordinates system
+  //!
+  geometry_msgs::msg::Point fromLL(const geographic_msgs::msg::GeoPoint & geo_point);
+
   /**
    * @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

src/navsat_transform.cpp

@@ -408,9 +408,38 @@ bool NavSatTransform::fromLLCallback(
   const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
   std::shared_ptr<robot_localization::srv::FromLL::Response> response)
 {
-  double altitude = request->ll_point.altitude;
-  double longitude = request->ll_point.longitude;
-  double latitude = request->ll_point.latitude;
+  try {
+    response->map_point = fromLL(request->ll_point);
+  }
+  catch(const std::runtime_error& e) {
+    return false;
+  }
+
+  return true;
+}
+
+bool NavSatTransform::fromLLArrayCallback(
+  const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
+  std::shared_ptr<robot_localization::srv::FromLLArray::Response> response)
+{
+  for (auto it = request->ll_points.begin(); it != request->ll_points.end(); ++it) {
+    try {
+      response->map_points.push_back(fromLL(*it));
+    }
+    catch(const std::runtime_error& e) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+geometry_msgs::msg::Point NavSatTransform::fromLL(
+  const geographic_msgs::msg::GeoPoint& geo_point)
+{
+  double altitude = geo_point.altitude;
+  double longitude = geo_point.longitude;
+  double latitude = geo_point.latitude;
 
   tf2::Transform cartesian_pose;
 
@@ -438,67 +467,11 @@ bool NavSatTransform::fromLLCallback(
 
   cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude));
 
-  nav_msgs::msg::Odometry gps_odom;
-
   if (!transform_good_) {
-    return false;
+    throw std::runtime_error("Invalid transform.");
   }
 
-  response->map_point = cartesianToMap(cartesian_pose).pose.pose.position;
-
-  return true;
-}
-
-bool NavSatTransform::fromLLArrayCallback(
-  const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
-  std::shared_ptr<robot_localization::srv::FromLLArray::Response> response)
-{
-  double altitude, longitude, latitude;
-  double cartesian_x {};
-  double cartesian_y {};
-  double cartesian_z {};
-  tf2::Transform cartesian_pose;
-  std::string utm_zone_tmp;
-  for (auto it = request->ll_points.begin(); it != request->ll_points.end(); ++it) {
-    //
-    altitude = it->altitude;
-    longitude = it->longitude;
-    latitude = it->latitude;
-    cartesian_x = 0.0;
-    cartesian_y = 0.0;
-    cartesian_z = 0.0;
-    //
-    if (use_local_cartesian_) {
-      gps_local_cartesian_.Forward(
-        latitude,
-        longitude,
-        altitude,
-        cartesian_x,
-        cartesian_y,
-        cartesian_z);
-    } else {
-
-      navsat_conversions::LLtoUTM(
-        latitude,
-        longitude,
-        cartesian_y,
-        cartesian_x,
-        utm_zone_tmp);
-    }
-
-    cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude));
-
-    // nav_msgs::msg::Odometry gps_odom;  // it's unused?
-
-    if (!transform_good_) {
-      return false;
-    }
-
-    response->map_points.push_back(cartesianToMap(cartesian_pose).pose.pose.position);
-
-  }
-
-  return true;
+  return cartesianToMap(cartesian_pose).pose.pose.position;
 }
 
 nav_msgs::msg::Odometry NavSatTransform::cartesianToMap(