Added FromLLArray service

CMakeLists.txt

@@ -43,6 +43,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

@@ -32,15 +32,18 @@
 #ifndef ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
 #define ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
 
-#include <memory>
+#include <stdexcept>
 #include <string>
+#include <memory>
 
 #include "Eigen/Dense"
+#include "GeographicLib/Geocentric.hpp"
 #include "GeographicLib/LocalCartesian.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/timer.hpp"
 #include "robot_localization/srv/from_ll.hpp"
+#include "robot_localization/srv/from_ll_array.hpp"
 #include "robot_localization/srv/set_datum.hpp"
 #include "robot_localization/srv/set_utm_zone.hpp"
 #include "robot_localization/srv/to_ll.hpp"
@@ -56,8 +59,7 @@
 namespace robot_localization
 {
 
-class NavSatTransform : public rclcpp::Node
-{
+class NavSatTransform : public rclcpp::Node {
 public:
   /**
    * @brief Constructor
@@ -84,8 +86,7 @@ class NavSatTransform : public rclcpp::Node
    * @brief Callback for the datum service
    */
   bool datumCallback(
-    const std::shared_ptr<robot_localization::srv::SetDatum::Request>
-    request,
+    const std::shared_ptr<robot_localization::srv::SetDatum::Request> request,
     std::shared_ptr<robot_localization::srv::SetDatum::Response>);
 
   //! @brief Callback for the to Lat Long service
@@ -100,17 +101,28 @@ 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 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 Callback for the UTM zone service
-  */
+   */
   bool setUTMZoneCallback(
     const std::shared_ptr<robot_localization::srv::SetUTMZone::Request> request,
     std::shared_ptr<robot_localization::srv::SetUTMZone::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.
+   * @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.
    */
   void getRobotOriginCartesianPose(
     const tf2::Transform & gps_cartesian_pose,
@@ -149,13 +161,13 @@ class NavSatTransform : public rclcpp::Node
    * @brief Converts the odometry data back to GPS and broadcasts it
    * @param[out] filtered_gps The NavSatFix message to prepare
    */
-  bool prepareFilteredGps(sensor_msgs::msg::NavSatFix * filtered_gps);
+  bool prepareFilteredGps(sensor_msgs::msg::NavSatFix *filtered_gps);
 
   /**
    * @brief Prepares the GPS odometry message before sending
    * @param[out] gps_odom The odometry message to prepare
    */
-  bool prepareGpsOdometry(nav_msgs::msg::Odometry * gps_odom);
+  bool prepareGpsOdometry(nav_msgs::msg::Odometry *gps_odom);
 
   /**
    * @brief Used for setting the GPS data that will be used to compute the
@@ -175,7 +187,8 @@ class NavSatTransform : public rclcpp::Node
    * @brief Transforms the passed in pose from Cartesian to map frame
    *  @param[in] cartesian_pose the pose in Cartesian frame to use to transform
    */
-  nav_msgs::msg::Odometry cartesianToMap(const tf2::Transform & cartesian_pose) const;
+  nav_msgs::msg::Odometry
+  cartesianToMap(const tf2::Transform & cartesian_pose) const;
 
   /**
    * @brief Transforms the passed in point from map frame to lat/long
@@ -204,8 +217,8 @@ class NavSatTransform : public rclcpp::Node
   bool broadcast_cartesian_transform_;
 
   /**
-   * @brief Whether to broadcast the Cartesian transform as parent frame, default as
-   * child
+   * @brief Whether to broadcast the Cartesian transform as parent frame,
+   * default as child
    */
   bool broadcast_cartesian_transform_as_parent_frame_;
 
@@ -224,10 +237,17 @@ 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 Service for set UTM zone
-  */
-  rclcpp::Service<robot_localization::srv::SetUTMZone>::SharedPtr set_utm_zone_srv_;
+   */
+  rclcpp::Service<robot_localization::srv::SetUTMZone>::SharedPtr
+    set_utm_zone_srv_;
 
   /**
    * @brief Navsatfix publisher
@@ -369,7 +389,8 @@ class NavSatTransform : public rclcpp::Node
   tf2::Duration transform_timeout_;
 
   /**
-   * @brief Holds the Cartesian (UTM or local ENU) pose that is used to compute the transform
+   * @brief Holds the Cartesian (UTM or local ENU) pose that is used to compute
+   * the transform
    */
   tf2::Transform transform_cartesian_pose_;
 
@@ -379,12 +400,14 @@ class NavSatTransform : public rclcpp::Node
   tf2::Transform transform_world_pose_;
 
   /**
-   * @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian
+   * @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM
+   * coordinates as our cartesian
    */
   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
+   * @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_;
 
@@ -412,8 +435,8 @@ class NavSatTransform : public rclcpp::Node
    * @brief UTM's meridian convergence
    *
    * Angle between projected meridian (True North) and Cartesian's grid Y-axis.
-   * For Cartesian projection (Ellipsoidal Transverse Mercator) it is zero on the
-   * equator and non-zero everywhere else. It increases as the poles are
+   * For Cartesian projection (Ellipsoidal Transverse Mercator) it is zero on
+   * the equator and non-zero everywhere else. It increases as the poles are
    * approached or as we're getting farther from central meridian.
    */
   double utm_meridian_convergence_;
@@ -435,7 +458,7 @@ class NavSatTransform : public rclcpp::Node
 
   /**
    * @brief hemisphere (true means north, false means south)
-  */
+   */
   bool northp_;
 
   /**
@@ -465,13 +488,14 @@ class NavSatTransform : public rclcpp::Node
   /**
    * @brief Manual datum pose to be used by the transform computation
    *
-   * Then manual datum requested by a service request (or configuration) is stored
-   * here until the odom message is received, and the manual datum pose can be
-   * set.
+   * Then manual datum requested by a service request (or configuration) is
+   * stored here until the odom message is received, and the manual datum pose
+   * can be set.
    */
   geographic_msgs::msg::GeoPose manual_datum_geopose_;
 };
 
-}  // namespace robot_localization
 
-#endif  // ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
+} // namespace robot_localization
+
+#endif // ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_

src/navsat_transform.cpp

@@ -150,6 +150,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));
 
   set_utm_zone_srv_ = this->create_service<robot_localization::srv::SetUTMZone>(
     "setUTMZone", std::bind(&NavSatTransform::setUTMZoneCallback, this, _1, _2));
@@ -437,9 +439,40 @@ 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)
+{
+  decltype(response->map_points) converted_points;
+  converted_points.reserve(request->ll_points.size());
+
+  try {
+    std::transform(request->ll_points.begin(), request->ll_points.end(),
+                   std::back_inserter(converted_points),
+                   [this] (const auto& point) { return fromLL(point); });
+  } catch(const std::runtime_error & e) {
+    return false;
+  }
+
+  response->map_points = std::move(converted_points);
+  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;
 
@@ -466,21 +499,17 @@ bool NavSatTransform::fromLLCallback(
         zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_);
     } catch (GeographicLib::GeographicErr const & e) {
       RCLCPP_ERROR_STREAM(this->get_logger(), e.what());
-      return false;
+      throw;
     }
   }
 
   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;
+  return cartesianToMap(cartesian_pose).pose.pose.position;
 }
 
 bool NavSatTransform::setUTMZoneCallback(
@@ -502,6 +531,7 @@ bool NavSatTransform::setUTMZoneCallback(
   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