Added FromLLArray service

CMakeLists.txt

@@ -40,6 +40,7 @@ set(library_name rl_lib)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/FromLL.srv"
+  "srv/FromLLArray.srv"
   "srv/GetState.srv"
   "srv/SetDatum.srv"
   "srv/SetPose.srv"

include/robot_localization/navsat_transform.hpp

@@ -36,6 +36,7 @@
 #include <robot_localization/srv/set_datum.hpp>
 #include <robot_localization/srv/to_ll.hpp>
 #include <robot_localization/srv/from_ll.hpp>
+#include <robot_localization/srv/from_ll_array.hpp>
 
 #include <Eigen/Dense>
 #include <GeographicLib/Geocentric.hpp>
@@ -99,6 +100,12 @@ class NavSatTransform : public rclcpp::Node
     const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
     std::shared_ptr<robot_localization::srv::FromLL::Response> response);
 
+  //! @brief Callback for the from Lat Long Array service
+  //!
+  bool fromLLArrayCallback(
+    const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
+    std::shared_ptr<robot_localization::srv::FromLLArray::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
@@ -211,6 +218,11 @@ class NavSatTransform : public rclcpp::Node
    */
   rclcpp::Service<robot_localization::srv::FromLL>::SharedPtr from_ll_srv_;
 
+  /**
+   * @brief Service for from Lat Long Array
+   */
+  rclcpp::Service<robot_localization::srv::FromLLArray>::SharedPtr from_ll_array_srv_;
+
   /**
    * @brief Navsatfix publisher
    */

src/navsat_transform.cpp

@@ -142,7 +142,8 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
     "toLL", std::bind(&NavSatTransform::toLLCallback, this, _1, _2));
   from_ll_srv_ = this->create_service<robot_localization::srv::FromLL>(
     "fromLL", std::bind(&NavSatTransform::fromLLCallback, this, _1, _2));
-
+  from_ll_array_srv_ = this->create_service<robot_localization::srv::FromLLArray>(
+    "fromLLArray", std::bind(&NavSatTransform::fromLLArrayCallback, this, _1, _2));
   std::vector<double> datum_vals;
   if (use_manual_datum_) {
     datum_vals = this->declare_parameter("datum", datum_vals);
@@ -448,6 +449,58 @@ bool NavSatTransform::fromLLCallback(
   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;
+}
+
 nav_msgs::msg::Odometry NavSatTransform::cartesianToMap(
   const tf2::Transform & cartesian_pose) const
 {

srv/FromLLArray.srv

@@ -0,0 +1,3 @@
+geographic_msgs/GeoPoint[] ll_points
+---
+geometry_msgs/Point[] map_points