Skip to content

Commit

Permalink
feat(autoware_gnss_poser): porting from universe to core, autoware_gn…
Browse files Browse the repository at this point in the history
…ss_poser, remove package dependency to <autoware_component_interface_specs> which is under universe repo: v0.2

Signed-off-by: liuXinGangChina <[email protected]>
  • Loading branch information
liuXinGangChina committed Jan 15, 2025
1 parent aef1abf commit 35de0a3
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 12 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#ifndef AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_
#define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_

#include <autoware/component_interface_specs/map.hpp>
#include <autoware_map_msgs/msg/map_projector_info.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
Expand Down Expand Up @@ -46,9 +46,7 @@ class GNSSPoser : public rclcpp::Node
explicit GNSSPoser(const rclcpp::NodeOptions & node_options);

private:
using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo;

void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);
void callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg);
void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr);
void callback_gnss_ins_orientation_stamped(
const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg);
Expand Down Expand Up @@ -78,7 +76,7 @@ class GNSSPoser : public rclcpp::Node
tf2_ros::TransformListener tf2_listener_;
tf2_ros::TransformBroadcaster tf2_broadcaster_;

rclcpp::Subscription<MapProjectorInfo::Message>::SharedPtr sub_map_projector_info_;
rclcpp::Subscription<autoware_map_msgs::msg::MapProjectorInfo>::SharedPtr sub_map_projector_info_;
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr nav_sat_fix_sub_;
rclcpp::Subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>::SharedPtr
autoware_orientation_sub_;
Expand All @@ -87,7 +85,7 @@ class GNSSPoser : public rclcpp::Node
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_cov_pub_;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::BoolStamped>::SharedPtr fixed_pub_;

MapProjectorInfo::Message projector_info_;
autoware_map_msgs::msg::MapProjectorInfo projector_info_;
const std::string base_frame_;
const std::string gnss_base_frame_;
const std::string map_frame_;
Expand Down
2 changes: 1 addition & 1 deletion sensing/autoware_gnss_poser/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

<build_depend>libboost-dev</build_depend>

<depend>autoware_component_interface_specs</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_geography_utils</depend>
<depend>autoware_sensing_msgs</depend>
<depend>geographic_msgs</depend>
Expand Down
10 changes: 5 additions & 5 deletions sensing/autoware_gnss_poser/src/gnss_poser_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,8 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
gnss_pose_pub_method_(static_cast<int>(declare_parameter<int>("gnss_pose_pub_method")))
{
// Subscribe to map_projector_info topic
sub_map_projector_info_ = create_subscription<MapProjectorInfo::Message>(
MapProjectorInfo::name, rclcpp::QoS{1},
sub_map_projector_info_ = create_subscription<autoware_map_msgs::msg::MapProjectorInfo>(
"/map/map_projector_info", rclcpp::QoS{1},
std::bind(&GNSSPoser::callback_map_projector_info, this, std::placeholders::_1));

// Set up position buffer
Expand Down Expand Up @@ -68,7 +68,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0;
}

void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg)
void GNSSPoser::callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg)
{
projector_info_ = *msg;
received_map_projector_info_ = true;
Expand All @@ -86,7 +86,7 @@ void GNSSPoser::callback_nav_sat_fix(
return;
}

if (projector_info_.projector_type == MapProjectorInfo::Message::LOCAL) {
if (projector_info_.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) {
RCLCPP_ERROR_THROTTLE(
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(),
"map_projector_info is local projector type. Unable to convert GNSS pose.");
Expand Down Expand Up @@ -117,7 +117,7 @@ void GNSSPoser::callback_nav_sat_fix(
geometry_msgs::msg::Point position =
autoware::geography_utils::project_forward(gps_point, projector_info_);
position.z = autoware::geography_utils::convert_height(
position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84,
position.z, gps_point.latitude, gps_point.longitude, autoware_map_msgs::msg::MapProjectorInfo::WGS84,
projector_info_.vertical_datum);

geometry_msgs::msg::Pose gnss_antenna_pose{};
Expand Down

0 comments on commit 35de0a3

Please sign in to comment.