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

Jump to bottom

fix(gnss_poser): added local coordinate system in UTM to gnss_poser #1227

Merged
merged 14 commits into from
Jul 20, 2022
Merged

fix(gnss_poser): added local coordinate system in UTM to gnss_poser #1227

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
4f9822d
fix(gnss_poser): UTM local coordinate system is added. #1227
ataparlar Jul 4, 2022
af56663
fix(gnss_poser): UTM local coordinate system is added. UTM default #1227
ataparlar Jul 4, 2022
698c3d2
fix(gnss_poser): Changed the function names to correct ones.
ataparlar Jul 4, 2022
4d8bcea
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 4, 2022
1dc5f8b
removed unnecessary -else- and changed readme for coordinate systems
ataparlar Jul 7, 2022
467c028
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 7, 2022
cf94c1a
update readme
xmfcx Jul 19, 2022
0322694
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 19, 2022
be31b37
fix(gnss_poser): better conversation function for local_cartesian_utm
ataparlar Jul 19, 2022
fe88429
fix(gnss_poser): Update readme
ataparlar Jul 20, 2022
4657137
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 20, 2022
0f14059
fix id
xmfcx Jul 20, 2022
f1b1e83
move variables closer to the scope they were used
xmfcx Jul 20, 2022
064f1e7
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 20, 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
29 changes: 13 additions & 16 deletions sensing/gnss_poser/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates

### Input

| Name | Type | Description |
| ---------------- | ----------------------------- | ----------------------------------------------------------------------------------------------- |
| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message |
| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution (You can see detail description in reference document [1]) |
| Name | Type | Description |
| ---------------- | ----------------------------- | --------------------------------------------------------------------------------------------------------------- |
| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message |
| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution. [click here for more details](https://github.com/KumarRobotics/ublox.git) |

### Output

Expand All @@ -27,14 +27,15 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates

### Core Parameters

| Name | Type | Default Value | Description |
| -------------------- | ------ | ---------------- | ----------------------------------------------------------------------------------------------- |
| `base_frame` | string | "base_link" | frame d |
| `gnss_frame` | string | "gnss" | frame id |
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
| `map_frame` | string | "map" | frame id |
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems (See, reference document [2]) |
| Name | Type | Default Value | Description |
| -------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------------------------------------------ |
| `base_frame` | string | "base_link" | frame id |
| `gnss_frame` | string | "gnss" | frame id |
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
| `map_frame` | string | "map" | frame id |
| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System |
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. [click here for more details](https://www.gsi.go.jp/LAW/heimencho.html) |

## Assumptions / Known limits

Expand All @@ -44,8 +45,4 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates

## (Optional) References/External links

[1] <https://github.com/KumarRobotics/ublox.git>

[2] <https://www.gsi.go.jp/LAW/heimencho.html>

## (Optional) Future extensions / Unimplemented parts
6 changes: 3 additions & 3 deletions sensing/gnss_poser/config/set_local_origin.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
ros__parameters:

# Latitude of local origin
latitude: 40.816617984672746
latitude: 40.81187906

# Longitude of local origin
longitude: 29.360491808334285
longitude: 29.35810110

# Altitude of local origin
altitude: 52.251157145314075
altitude: 47.62
38 changes: 35 additions & 3 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 @@ -57,12 +57,12 @@ double EllipsoidHeight2OrthometricHeight(
}
return OrthometricHeight;
}
GNSSStat NavSatFix2LocalCartesian(
GNSSStat NavSatFix2LocalCartesianWGS84(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger)
{
GNSSStat local_cartesian;
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN;
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84;

try {
GeographicLib::LocalCartesian localCartesian_origin(
Expand Down Expand Up @@ -99,7 +99,39 @@ GNSSStat NavSatFix2UTM(
}
return utm;
}

GNSSStat NavSatFix2LocalCartesianUTM(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
{
GNSSStat utm_local;
utm_local.coordinate_system = CoordinateSystem::UTM;
try {
// origin of the local coordinate system in global frame
GNSSStat utm_origin;
utm_origin.coordinate_system = CoordinateSystem::UTM;
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_origin, logger);
// individual coordinates of global coordinate system
double global_x = 0.0;
double global_y = 0.0;
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;
// individual coordinates of local coordinate system
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());
}
return utm_local;
}
GNSSStat UTM2MGRS(
const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger)
{
Expand Down
3 changes: 2 additions & 1 deletion sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@ enum class CoordinateSystem {
UTM = 0,
MGRS = 1,
PLANE = 2,
LOCAL_CARTESIAN = 3,
LOCAL_CARTESIAN_WGS84 = 3,
LOCAL_CARTESIAN_UTM = 4
};

struct GNSSStat
Expand Down
2 changes: 1 addition & 1 deletion sensing/gnss_poser/launch/gnss_poser.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<arg name="gnss_frame" default="gnss"/>
<arg name="map_frame" default="map"/>

<arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
<arg name="coordinate_system" default="4" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesianWGS84, 4:LocalCartesianUTM"/>
<arg name="buff_epoch" default="1"/>
<arg name="use_gnss_ins_orientation" default="true"/>
<arg name="plane_zone" default="9"/>
Expand Down
8 changes: 6 additions & 2 deletions sensing/gnss_poser/src/gnss_poser_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,12 +184,16 @@ GNSSStat GNSSPoser::convert(
GNSSStat gnss_stat;
if (coordinate_system == CoordinateSystem::UTM) {
gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) {
gnss_stat =
NavSatFix2LocalCartesianUTM(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::MGRS) {
gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger());
} else if (coordinate_system == CoordinateSystem::PLANE) {
gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN) {
gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) {
gnss_stat =
NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else {
RCLCPP_ERROR_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
Expand Down