TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix coordinate transformation of tf

This commit is contained in:
Yu Asabe 2022-02-25 18:38:23 +09:00
parent d23ad2dc96
commit 91962c7286
2 changed files with 31 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/cpm_application.cpp
View File

@ -93,13 +93,16 @@ namespace v2x {
std::string mgrs; std::string mgrs;
int zone; int zone;
int grid_num_x = 4;
int grid_num_y = 39;
int grid_size = 100000;
bool northp; bool northp;
double x, y; double x, y;
GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y);
GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs); // GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs);
int x_mgrs = std::stoi(mgrs.substr(5, 5)); double x_mgrs = x - grid_num_x * grid_size;
int y_mgrs = std::stoi(mgrs.substr(10, 5)); double y_mgrs = y - grid_num_y * grid_size;
// RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs); // RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs);
// Calculate orientation from Heading // Calculate orientation from Heading

32
src/v2x_app.cpp
View File

@ -74,15 +74,33 @@ namespace v2x
double roll, pitch, yaw; double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw); matrix.getRPY(roll, pitch, yaw);
char mgrs[20]; char mgrs[30];
int zone, prec; int zone = 54;
bool northp; int grid_num_x = 4;
int grid_num_y = 39;
int grid_size = 100000;
int prec;
bool northp = true;
double x_mgrs, y_mgrs; double x_mgrs, y_mgrs;
double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); double lat, lon;
GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); // sprintf(mgrs, "54SVE%.3f%.3f", x, y);
// RCLCPP_INFO(node_->get_logger(), "%s", mgrs);
// Convert MGRS coordinate to UTM coordinate.
// GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
// Reverse projection from UTM to geographic.
GeographicLib::UTMUPS::Reverse(
zone,
northp,
grid_num_x * grid_size + x,
grid_num_y * grid_size + y,
lat,
lon
);
RCLCPP_INFO(node_->get_logger(), "%f, %f", lat, lon);
if (cp && cp_started_) { if (cp && cp_started_) {
cp->updateMGRS(&x, &y); cp->updateMGRS(&x, &y);