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

Feat/gnss poser/subscribe map projector info #24

Merged
merged 4 commits into from
Sep 1, 2023
Merged

Feat/gnss poser/subscribe map projector info #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a7360a2
fix map origin
kminoda Aug 31, 2023
13cf528
style(pre-commit): autofix
pre-commit-ci[bot] Aug 31, 2023
16768a9
remove config file
kminoda Aug 31, 2023
2315133
rfix readme
kminoda Aug 31, 2023
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
1 change: 0 additions & 1 deletion sensing/gnss_poser/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
1 change: 0 additions & 1 deletion sensing/gnss_poser/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
| `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; 1: MGRS, 4: UTM Local Coordinate System |
| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. |

## Assumptions / Known limits
Expand Down
11 changes: 0 additions & 11 deletions sensing/gnss_poser/config/set_local_origin.param.yaml
View file
Open in desktop

This file was deleted.

9 changes: 5 additions & 4 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 @@ -22,6 +22,7 @@
#include <geography_utils/height.hpp>
#include <rclcpp/logging.hpp>

#include <geographic_msgs/msg/geo_point.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>

#include <string>
Expand Down Expand Up @@ -64,20 +65,20 @@ GNSSStat NavSatFix2UTM(

GNSSStat NavSatFix2LocalCartesianUTM(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger,
const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger,
const std::string & target_vertical_datum)
{
GNSSStat utm_local;
try {
// origin of the local coordinate system in global frame
GNSSStat utm_origin;
GeographicLib::UTMUPS::Forward(
nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone,
utm_origin.east_north_up, utm_origin.x, utm_origin.y);

utm_origin.z = geography_utils::convert_height(
nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude,
"WGS84", target_vertical_datum);
geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84",
target_vertical_datum);

// individual coordinates of global coordinate system
double global_x = 0.0;
Expand Down
6 changes: 2 additions & 4 deletions sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ class GNSSPoser : public rclcpp::Node
bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg);
bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg);
GNSSStat convert(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & projector_type,
const std::string & vertical_datum);
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
const MapProjectorInfo::Message & projector_info);
geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat);
geometry_msgs::msg::Point getMedianPosition(
const boost::circular_buffer<geometry_msgs::msg::Point> & position_buffer);
Expand Down Expand Up @@ -101,8 +101,6 @@ class GNSSPoser : public rclcpp::Node
std::string gnss_base_frame_;
std::string map_frame_;
bool received_map_projector_info_ = false;

sensor_msgs::msg::NavSatFix nav_sat_fix_origin_;
bool use_gnss_ins_orientation_;

boost::circular_buffer<geometry_msgs::msg::Point> position_buffer_;
Expand Down
2 changes: 0 additions & 2 deletions 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 @@ -2,7 +2,6 @@
<launch>
<arg name="input_topic_fix" default="/fix"/>
<arg name="input_topic_orientation" default="/autoware_orientation"/>
<arg name="param_file" default="$(find-pkg-share gnss_poser)/config/set_local_origin.param.yaml"/>

<arg name="output_topic_gnss_pose" default="gnss_pose"/>
<arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
Expand Down Expand Up @@ -31,7 +30,6 @@

<param name="buff_epoch" value="$(var buff_epoch)"/>
<param name="use_gnss_ins_orientation" value="$(var use_gnss_ins_orientation)"/>
<param from="$(var param_file)"/>
<param name="gnss_pose_pub_method" value="$(var gnss_pose_pub_method)"/>
</node>
</launch>
1 change: 1 addition & 0 deletions sensing/gnss_poser/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
<depend>autoware_sensing_msgs</depend>
<depend>component_interface_specs</depend>
<depend>component_interface_utils</depend>
<depend>geographic_msgs</depend>
<depend>geographiclib</depend>
<depend>geography_utils</depend>
<depend>geometry_msgs</depend>
Expand Down
19 changes: 9 additions & 10 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 @@ -42,8 +42,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
sub_map_projector_info_,
[this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); });

nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0);

int buff_epoch = declare_parameter("buff_epoch", 1);
position_buffer_.set_capacity(buff_epoch);

Expand Down Expand Up @@ -95,8 +93,7 @@ void GNSSPoser::callbackNavSatFix(
}

// get position
const auto gnss_stat =
convert(*nav_sat_fix_msg_ptr, projector_info_.projector_type, projector_info_.vertical_datum);
const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_);
const auto position = getPosition(gnss_stat);

geometry_msgs::msg::Pose gnss_antenna_pose{};
Expand Down Expand Up @@ -201,16 +198,18 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix
}

GNSSStat GNSSPoser::convert(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const std::string & projector_type,
const std::string & vertical_datum)
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
const MapProjectorInfo::Message & map_projector_info)
{
GNSSStat gnss_stat;
if (projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) {
if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) {
gnss_stat = NavSatFix2LocalCartesianUTM(
nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), vertical_datum);
} else if (projector_type == MapProjectorInfo::Message::MGRS) {
nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(),
map_projector_info.vertical_datum);
} else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) {
gnss_stat = NavSatFix2MGRS(
nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), vertical_datum);
nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(),
map_projector_info.vertical_datum);
} else {
RCLCPP_ERROR_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
Expand Down
Loading