Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(gnss_poser): UTM coordinate publish in local coordinate system with gnss_poser #1225

Closed
wants to merge 16 commits into from
Closed

fix(gnss_poser): UTM coordinate publish in local coordinate system with gnss_poser #1225

Changes from 2 commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
b51dad8
added NavSatFix2UTMLocal function to gnss_poser
ataparlar Jun 29, 2022
6f8e70f
works
ataparlar Jul 3, 2022
8c13c76
Merge branch 'autowarefoundation:main' into main
ataparlar Jul 4, 2022
4489425
Merge branch 'main' of https://github.com/ataparlar/autoware.universe…
ataparlar Jul 4, 2022
f58b307
x y is correct, z is not local
ataparlar Jul 4, 2022
3f58d40
utm_local is ready to pr
ataparlar Jul 4, 2022
738a0f7
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 4, 2022
82ccbc3
fix(gnss_poser): made a better naming for params. package works with …
ataparlar Jul 4, 2022
23559b8
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 4, 2022
b2c27fa
fix(gnss_poser): Merge remote-tracking branch 'origin/local_utm_gnss'…
ataparlar Jul 4, 2022
3adfcd6
Merge remote-tracking branch 'origin/local_utm_gnss' into local_utm_gnss
ataparlar Jul 4, 2022
b982619
utm_local is ready to pr
ataparlar Jul 4, 2022
c7113c2
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 4, 2022
71080b3
fix(gnss_poser): made a better naming for params. package works with …
ataparlar Jul 4, 2022
9c75487
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 4, 2022
b5da3ef
Merge remote-tracking branch 'origin/local_utm_gnss' into local_utm_gnss
ataparlar Jul 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 28 additions & 28 deletions sensing/gnss_poser/include/gnss_poser/convert.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,36 +103,36 @@ bool is_utm_origin_initialized = false;
GNSSStat utm_origin;
GNSSStat utm_local;
GNSSStat NavSatFix2UTMLocal(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_,
const rclcpp::Logger & logger)
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger)
{
utm_origin.coordinate_system = CoordinateSystem::UTM;
utm_local.coordinate_system = CoordinateSystem::UTM;
double global_x;
double global_y;
try {
if (is_utm_origin_initialized==0) {
GeographicLib::UTMUPS::Forward(
nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, utm_origin.zone, utm_origin.northup, utm_origin.x, utm_origin.y);
utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger);
is_utm_origin_initialized = true;
}
else {
GeographicLib::UTMUPS::Forward(
nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup, global_x, global_y);
utm_local.latitude = nav_sat_fix_msg.latitude;
utm_local.longitude = nav_sat_fix_msg.longitude;
utm_local.altitude = nav_sat_fix_msg.altitude;
utm_local.x = global_x - utm_origin.x;
utm_local.y = global_y - utm_origin.y;
utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z;
}
}
catch (const GeographicLib::GeographicErr & err) {
RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM in local coordinates" << err.what());
utm_origin.coordinate_system = CoordinateSystem::UTM;
utm_local.coordinate_system = CoordinateSystem::UTM;
double global_x;
double global_y;
try {
if (is_utm_origin_initialized == 0) {
GeographicLib::UTMUPS::Forward(
nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, utm_origin.zone,
utm_origin.northup, utm_origin.x, utm_origin.y);
utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger);
is_utm_origin_initialized = true;
} else {
GeographicLib::UTMUPS::Forward(
nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup,
global_x, global_y);
utm_local.latitude = nav_sat_fix_msg.latitude;
utm_local.longitude = nav_sat_fix_msg.longitude;
utm_local.altitude = nav_sat_fix_msg.altitude;
utm_local.x = global_x - utm_origin.x;
utm_local.y = global_y - utm_origin.y;
utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z;
}
return utm_local;
} catch (const GeographicLib::GeographicErr & err) {
RCLCPP_ERROR_STREAM(
logger, "Failed to convert from LLH to UTM in local coordinates" << err.what());
}
return utm_local;
}

GNSSStat UTM2MGRS(
Expand Down