Skip to content

Commit 1628175

Browse files
kminodapre-commit-ci[bot]
kminoda
and
pre-commit-ci[bot]
authored
feat(gnss_poser): subscribe map projector info (autowarefoundation#4791)
* feat(gnss_poser): Subscribe map_projector_info Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * small fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * update commetn Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add local cartesian Signed-off-by: kminoda <koji.minoda@tier4.jp> * update launch file Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * create new function for conversion of height Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor update Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * rename Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove unnecessary include Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix map origin Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * remove config file Signed-off-by: kminoda <koji.minoda@tier4.jp> * rfix readme Signed-off-by: kminoda <koji.minoda@tier4.jp> --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 9eb5b14 commit 1628175

9 files changed

+57
-66
lines changed

‎sensing/gnss_poser/CMakeLists.txt

-1
Original file line numberDiff line numberDiff line change
@@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node
3737

3838
ament_auto_package(INSTALL_TO_SHARE
3939
launch
40-
config
4140
)

‎sensing/gnss_poser/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
1212

1313
| Name | Type | Description |
1414
| ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ |
15+
| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info |
1516
| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message |
1617
| `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) |
1718

@@ -33,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
3334
| `gnss_frame` | string | "gnss" | frame id |
3435
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
3536
| `map_frame` | string | "map" | frame id |
36-
| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System |
3737
| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. |
3838

3939
## Assumptions / Known limits

‎sensing/gnss_poser/config/set_local_origin.param.yaml

-11
This file was deleted.

‎sensing/gnss_poser/include/gnss_poser/convert.hpp

+12-22
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <geography_utils/height.hpp>
2323
#include <rclcpp/logging.hpp>
2424

25+
#include <geographic_msgs/msg/geo_point.hpp>
2526
#include <sensor_msgs/msg/nav_sat_fix.hpp>
2627

2728
#include <string>
@@ -42,24 +43,17 @@ enum class MGRSPrecision {
4243

4344
GNSSStat NavSatFix2UTM(
4445
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger,
45-
int height_system)
46+
const std::string & target_vertical_datum)
4647
{
4748
GNSSStat utm;
4849

4950
try {
5051
GeographicLib::UTMUPS::Forward(
5152
nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x,
5253
utm.y);
53-
54-
std::string target_height_system;
55-
if (height_system == 0) {
56-
target_height_system = "EGM2008";
57-
} else {
58-
target_height_system = "WGS84";
59-
}
6054
utm.z = geography_utils::convert_height(
6155
nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84",
62-
target_height_system);
56+
target_vertical_datum);
6357
utm.latitude = nav_sat_fix_msg.latitude;
6458
utm.longitude = nav_sat_fix_msg.longitude;
6559
utm.altitude = nav_sat_fix_msg.altitude;
@@ -71,24 +65,20 @@ GNSSStat NavSatFix2UTM(
7165

7266
GNSSStat NavSatFix2LocalCartesianUTM(
7367
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
74-
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system)
68+
const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger,
69+
const std::string & target_vertical_datum)
7570
{
7671
GNSSStat utm_local;
7772
try {
7873
// origin of the local coordinate system in global frame
7974
GNSSStat utm_origin;
8075
GeographicLib::UTMUPS::Forward(
81-
nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
76+
geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone,
8277
utm_origin.east_north_up, utm_origin.x, utm_origin.y);
83-
std::string target_height_system;
84-
if (height_system == 0) {
85-
target_height_system = "EGM2008";
86-
} else {
87-
target_height_system = "WGS84";
88-
}
78+
8979
utm_origin.z = geography_utils::convert_height(
90-
nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude,
91-
"WGS84", target_height_system);
80+
geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84",
81+
target_vertical_datum);
9282

9383
// individual coordinates of global coordinate system
9484
double global_x = 0.0;
@@ -104,7 +94,7 @@ GNSSStat NavSatFix2LocalCartesianUTM(
10494
utm_local.y = global_y - utm_origin.y;
10595
utm_local.z = geography_utils::convert_height(
10696
nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude,
107-
"WGS84", target_height_system) -
97+
"WGS84", target_vertical_datum) -
10898
utm_origin.z;
10999
} catch (const GeographicLib::GeographicErr & err) {
110100
RCLCPP_ERROR_STREAM(
@@ -143,9 +133,9 @@ GNSSStat UTM2MGRS(
143133

144134
GNSSStat NavSatFix2MGRS(
145135
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision,
146-
const rclcpp::Logger & logger, int height_system)
136+
const rclcpp::Logger & logger, const std::string & vertical_datum)
147137
{
148-
const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system);
138+
const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum);
149139
const auto mgrs = UTM2MGRS(utm, precision, logger);
150140
return mgrs;
151141
}

‎sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp

+10-6
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717
#include "gnss_poser/convert.hpp"
1818
#include "gnss_poser/gnss_stat.hpp"
1919

20+
#include <component_interface_specs/map.hpp>
21+
#include <component_interface_utils/rclcpp.hpp>
2022
#include <rclcpp/rclcpp.hpp>
2123

2224
#include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
@@ -48,15 +50,18 @@ class GNSSPoser : public rclcpp::Node
4850
explicit GNSSPoser(const rclcpp::NodeOptions & node_options);
4951

5052
private:
53+
using MapProjectorInfo = map_interface::MapProjectorInfo;
54+
55+
void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg);
5156
void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr);
5257
void callbackGnssInsOrientationStamped(
5358
const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg);
5459

5560
bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg);
5661
bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg);
5762
GNSSStat convert(
58-
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system,
59-
int height_system);
63+
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
64+
const MapProjectorInfo::Message & projector_info);
6065
geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat);
6166
geometry_msgs::msg::Point getMedianPosition(
6267
const boost::circular_buffer<geometry_msgs::msg::Point> & position_buffer);
@@ -81,6 +86,7 @@ class GNSSPoser : public rclcpp::Node
8186
tf2_ros::TransformListener tf2_listener_;
8287
tf2_ros::TransformBroadcaster tf2_broadcaster_;
8388

89+
component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
8490
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr nav_sat_fix_sub_;
8591
rclcpp::Subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>::SharedPtr
8692
autoware_orientation_sub_;
@@ -89,20 +95,18 @@ class GNSSPoser : public rclcpp::Node
8995
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_cov_pub_;
9096
rclcpp::Publisher<tier4_debug_msgs::msg::BoolStamped>::SharedPtr fixed_pub_;
9197

92-
CoordinateSystem coordinate_system_;
98+
MapProjectorInfo::Message projector_info_;
9399
std::string base_frame_;
94100
std::string gnss_frame_;
95101
std::string gnss_base_frame_;
96102
std::string map_frame_;
97-
98-
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_;
103+
bool received_map_projector_info_ = false;
99104
bool use_gnss_ins_orientation_;
100105

101106
boost::circular_buffer<geometry_msgs::msg::Point> position_buffer_;
102107

103108
autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr
104109
msg_gnss_ins_orientation_stamped_;
105-
int height_system_;
106110
int gnss_pose_pub_method;
107111
};
108112
} // namespace gnss_poser

‎sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,6 @@
1818

1919
namespace gnss_poser
2020
{
21-
enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 };
22-
2321
struct GNSSStat
2422
{
2523
GNSSStat()

‎sensing/gnss_poser/launch/gnss_poser.launch.xml

-6
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22
<launch>
33
<arg name="input_topic_fix" default="/fix"/>
44
<arg name="input_topic_orientation" default="/autoware_orientation"/>
5-
<arg name="param_file" default="$(find-pkg-share gnss_poser)/config/set_local_origin.param.yaml"/>
65

76
<arg name="output_topic_gnss_pose" default="gnss_pose"/>
87
<arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
@@ -13,10 +12,8 @@
1312
<arg name="gnss_frame" default="gnss"/>
1413
<arg name="map_frame" default="map"/>
1514

16-
<arg name="coordinate_system" default="4" description="1:MGRS, 4:LocalCartesianUTM"/>
1715
<arg name="buff_epoch" default="1"/>
1816
<arg name="use_gnss_ins_orientation" default="true"/>
19-
<arg name="height_system" default="1" description="0:Orthometric Height 1:Ellipsoid Height"/>
2017
<arg name="gnss_pose_pub_method" default="0" description="0: Instant Value 1: Average Value 2: Median Value"/>
2118

2219
<node pkg="gnss_poser" exec="gnss_poser" name="gnss_poser" output="screen">
@@ -31,11 +28,8 @@
3128
<param name="gnss_frame" value="$(var gnss_frame)"/>
3229
<param name="map_frame" value="$(var map_frame)"/>
3330

34-
<param name="coordinate_system" value="$(var coordinate_system)"/>
3531
<param name="buff_epoch" value="$(var buff_epoch)"/>
3632
<param name="use_gnss_ins_orientation" value="$(var use_gnss_ins_orientation)"/>
37-
<param from="$(var param_file)"/>
38-
<param name="height_system" value="$(var height_system)"/>
3933
<param name="gnss_pose_pub_method" value="$(var gnss_pose_pub_method)"/>
4034
</node>
4135
</launch>

‎sensing/gnss_poser/package.xml

+3
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,9 @@
1414
<build_depend>libboost-dev</build_depend>
1515

1616
<depend>autoware_sensing_msgs</depend>
17+
<depend>component_interface_specs</depend>
18+
<depend>component_interface_utils</depend>
19+
<depend>geographic_msgs</depend>
1720
<depend>geographiclib</depend>
1821
<depend>geography_utils</depend>
1922
<depend>geometry_msgs</depend>

‎sensing/gnss_poser/src/gnss_poser_core.cpp

+31-17
Original file line numberDiff line numberDiff line change
@@ -34,16 +34,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
3434
use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)),
3535
msg_gnss_ins_orientation_stamped_(
3636
std::make_shared<autoware_sensing_msgs::msg::GnssInsOrientationStamped>()),
37-
height_system_(declare_parameter<int>("height_system", 1)),
3837
gnss_pose_pub_method(declare_parameter<int>("gnss_pose_pub_method", 0))
3938
{
40-
int coordinate_system =
41-
declare_parameter("coordinate_system", static_cast<int>(CoordinateSystem::MGRS));
42-
coordinate_system_ = static_cast<CoordinateSystem>(coordinate_system);
43-
44-
nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0);
45-
nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0);
46-
nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0);
39+
// Subscribe to map_projector_info topic
40+
const auto adaptor = component_interface_utils::NodeAdaptor(this);
41+
adaptor.init_sub(
42+
sub_map_projector_info_,
43+
[this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); });
4744

4845
int buff_epoch = declare_parameter("buff_epoch", 1);
4946
position_buffer_.set_capacity(buff_epoch);
@@ -61,9 +58,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
6158
fixed_pub_ = create_publisher<tier4_debug_msgs::msg::BoolStamped>("gnss_fixed", rclcpp::QoS{1});
6259
}
6360

61+
void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg)
62+
{
63+
projector_info_ = *msg;
64+
received_map_projector_info_ = true;
65+
}
66+
6467
void GNSSPoser::callbackNavSatFix(
6568
const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr)
6669
{
70+
// Return immediately if map_projector_info has not been received yet.
71+
if (!received_map_projector_info_) {
72+
RCLCPP_WARN_THROTTLE(
73+
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
74+
"map_projector_info has not been received yet. Check if the map_projection_loader is "
75+
"successfully launched.");
76+
return;
77+
}
78+
6779
// check fixed topic
6880
const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status);
6981

@@ -80,8 +92,8 @@ void GNSSPoser::callbackNavSatFix(
8092
return;
8193
}
8294

83-
// get position in coordinate_system
84-
const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_);
95+
// get position
96+
const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_);
8597
const auto position = getPosition(gnss_stat);
8698

8799
geometry_msgs::msg::Pose gnss_antenna_pose{};
@@ -186,20 +198,22 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix
186198
}
187199

188200
GNSSStat GNSSPoser::convert(
189-
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system,
190-
int height_system)
201+
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
202+
const MapProjectorInfo::Message & map_projector_info)
191203
{
192204
GNSSStat gnss_stat;
193-
if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) {
205+
if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) {
194206
gnss_stat = NavSatFix2LocalCartesianUTM(
195-
nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system);
196-
} else if (coordinate_system == CoordinateSystem::MGRS) {
207+
nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(),
208+
map_projector_info.vertical_datum);
209+
} else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) {
197210
gnss_stat = NavSatFix2MGRS(
198-
nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system);
211+
nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(),
212+
map_projector_info.vertical_datum);
199213
} else {
200214
RCLCPP_ERROR_STREAM_THROTTLE(
201215
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
202-
"Unknown Coordinate System");
216+
"Unknown Projector type");
203217
}
204218
return gnss_stat;
205219
}

0 commit comments

Comments
 (0)