From 89ca9bf18eaa8e0825be6245c55b25def573ccdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20S=C5=82omiany?= Date: Thu, 22 Apr 2021 14:59:44 +0200 Subject: [PATCH 1/3] Added FromLLArray service --- CMakeLists.txt | 1 + .../robot_localization/navsat_transform.hpp | 12 ++++ src/navsat_transform.cpp | 55 ++++++++++++++++++- srv/FromLLArray.srv | 3 + 4 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 srv/FromLLArray.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 2246b4955..49e0467f5 100644 --- a/CMakeLists.txt +++ b/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" diff --git a/include/robot_localization/navsat_transform.hpp b/include/robot_localization/navsat_transform.hpp index c53cd92c7..d3ccd2e57 100644 --- a/include/robot_localization/navsat_transform.hpp +++ b/include/robot_localization/navsat_transform.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -99,6 +100,12 @@ class NavSatTransform : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + //! @brief Callback for the from Lat Long Array service + //! + bool fromLLArrayCallback( + const std::shared_ptr request, + std::shared_ptr 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::SharedPtr from_ll_srv_; + /** + * @brief Service for from Lat Long Array + */ + rclcpp::Service::SharedPtr from_ll_array_srv_; + /** * @brief Navsatfix publisher */ diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index d9bdef80b..c401b785c 100644 --- a/src/navsat_transform.cpp +++ b/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( "fromLL", std::bind(&NavSatTransform::fromLLCallback, this, _1, _2)); - + from_ll_array_srv_ = this->create_service( + "fromLLArray", std::bind(&NavSatTransform::fromLLArrayCallback, this, _1, _2)); std::vector 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 request, + std::shared_ptr 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 { diff --git a/srv/FromLLArray.srv b/srv/FromLLArray.srv new file mode 100644 index 000000000..552162234 --- /dev/null +++ b/srv/FromLLArray.srv @@ -0,0 +1,3 @@ +geographic_msgs/GeoPoint[] ll_points +--- +geometry_msgs/Point[] map_points \ No newline at end of file From cbcd769bbf82169ed3aea2dc3cb914c736950352 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mateusz=20Macia=C5=9B?= Date: Wed, 15 Dec 2021 10:45:33 +0100 Subject: [PATCH 2/3] User local coordinates also in toll services. --- src/navsat_transform.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index c401b785c..de062e43e 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -538,14 +538,24 @@ void NavSatTransform::mapToLL( odom_as_cartesian.mult(cartesian_world_trans_inverse_, pose); odom_as_cartesian.setRotation(tf2::Quaternion::getIdentity()); - - // Now convert the data back to lat/long and place into the message - navsat_conversions::UTMtoLL( - odom_as_cartesian.getOrigin().getY(), - odom_as_cartesian.getOrigin().getX(), - utm_zone_, - latitude, - longitude); + if (use_local_cartesian_) { + gps_local_cartesian_.Reverse( + odom_as_cartesian.getOrigin().getX(), + odom_as_cartesian.getOrigin().getY(), + odom_as_cartesian.getOrigin().getZ(), + latitude, + longitude, + altitude + ); + } else { + // Now convert the data back to lat/long and place into the message + navsat_conversions::UTMtoLL( + odom_as_cartesian.getOrigin().getY(), + odom_as_cartesian.getOrigin().getX(), + utm_zone_, + latitude, + longitude); + } altitude = odom_as_cartesian.getOrigin().getZ(); } From dc74221e0596b7a91b27c42b764d1037d8152d20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20S=C5=82omiany?= Date: Sun, 8 May 2022 18:21:36 +0200 Subject: [PATCH 3/3] FromLLArray - update after review --- .../robot_localization/navsat_transform.hpp | 7 ++ src/navsat_transform.cpp | 95 +++++++------------ srv/FromLLArray.srv | 2 +- 3 files changed, 42 insertions(+), 62 deletions(-) diff --git a/include/robot_localization/navsat_transform.hpp b/include/robot_localization/navsat_transform.hpp index d3ccd2e57..9074fbefa 100644 --- a/include/robot_localization/navsat_transform.hpp +++ b/include/robot_localization/navsat_transform.hpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include #include #include @@ -51,6 +53,7 @@ #include #include +#include #include namespace robot_localization @@ -106,6 +109,10 @@ class NavSatTransform : public rclcpp::Node const std::shared_ptr request, std::shared_ptr 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 diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index de062e43e..945144c2f 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -408,9 +408,38 @@ bool NavSatTransform::fromLLCallback( const std::shared_ptr request, std::shared_ptr 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 request, + std::shared_ptr 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 request, - std::shared_ptr 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( diff --git a/srv/FromLLArray.srv b/srv/FromLLArray.srv index 552162234..4a727e0b2 100644 --- a/srv/FromLLArray.srv +++ b/srv/FromLLArray.srv @@ -1,3 +1,3 @@ geographic_msgs/GeoPoint[] ll_points --- -geometry_msgs/Point[] map_points \ No newline at end of file +geometry_msgs/Point[] map_points