diff --git a/build_depends.humble.repos b/build_depends.humble.repos index be4bfc8399abd..7c8f1df6c523c 100644 --- a/build_depends.humble.repos +++ b/build_depends.humble.repos @@ -8,6 +8,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware.core.git version: main + core/autoware_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_msgs.git + version: main core/external/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/build_depends.repos b/build_depends.repos index 6363f6b173985..b510096aef881 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -8,6 +8,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware.core.git version: main + core/autoware_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_msgs.git + version: main core/external/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 872019d8e0353..981f57f3eb7fc 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -19,11 +19,11 @@ #include +#include #include #include #include #include -#include #include @@ -49,7 +49,8 @@ class GNSSPoser : public rclcpp::Node private: void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); - void callbackNavPVT(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg); + void callbackGnssInsOrientationStamped( + const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); @@ -78,7 +79,8 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformBroadcaster tf2_broadcaster_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; - rclcpp::Subscription::SharedPtr nav_pvt_sub_; + rclcpp::Subscription::SharedPtr + autoware_orientation_sub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr pose_cov_pub_; @@ -91,12 +93,14 @@ class GNSSPoser : public rclcpp::Node std::string map_frame_; sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; - bool use_ublox_receiver_; + bool use_gnss_ins_orientation_; + + boost::circular_buffer position_buffer_; int plane_zone_; - boost::circular_buffer position_buffer_; - ublox_msgs::msg::NavPVT::ConstSharedPtr nav_pvt_msg_ptr_; + autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr + msg_gnss_ins_orientation_stamped_; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 8b48d7801c85f..1e2f56597020e 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -1,7 +1,7 @@ - + @@ -15,13 +15,12 @@ - + - - + @@ -33,7 +32,6 @@ - diff --git a/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml b/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml deleted file mode 100644 index f6827e5872215..0000000000000 --- a/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index e5fbaf575d28e..42b5d95ffd4a2 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -11,6 +11,7 @@ autoware_cmake + autoware_sensing_msgs geo_pos_conv geographiclib geometry_msgs @@ -21,7 +22,6 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - ublox_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index c812e9579af16..b4c1590e264a7 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,6 +14,8 @@ #include "gnss_poser/gnss_poser_core.hpp" +#include + #include #include #include @@ -29,8 +31,10 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) gnss_frame_(declare_parameter("gnss_frame", "gnss")), gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), map_frame_(declare_parameter("map_frame", "map")), - use_ublox_receiver_(declare_parameter("use_ublox_receiver", false)), - plane_zone_(declare_parameter("plane_zone", 9)) + use_gnss_ins_orientation_(declare_parameter("use_gnss_heading", true)), + plane_zone_(declare_parameter("plane_zone", 9)), + msg_gnss_ins_orientation_stamped_( + std::make_shared()) { int coordinate_system = declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); @@ -45,8 +49,10 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) nav_sat_fix_sub_ = create_subscription( "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1)); - nav_pvt_sub_ = create_subscription( - "navpvt", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavPVT, this, std::placeholders::_1)); + autoware_orientation_sub_ = + create_subscription( + "autoware_orientation", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callbackGnssInsOrientationStamped, this, std::placeholders::_1)); pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); pose_cov_pub_ = create_publisher( @@ -89,8 +95,8 @@ void GNSSPoser::callbackNavSatFix( // calc gnss antenna orientation geometry_msgs::msg::Quaternion orientation; - if (use_ublox_receiver_ && nav_pvt_msg_ptr_ != nullptr) { - orientation = getQuaternionByHeading(nav_pvt_msg_ptr_->heading); + if (use_gnss_ins_orientation_) { + orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; } else { static auto prev_position = median_position; orientation = getQuaternionByPositionDifference(median_position, prev_position); @@ -129,24 +135,36 @@ void GNSSPoser::callbackNavSatFix( geometry_msgs::msg::PoseWithCovarianceStamped gnss_base_pose_cov_msg; gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; - gnss_base_pose_cov_msg.pose.covariance[6 * 0 + 0] = + gnss_base_pose_cov_msg.pose.covariance[7 * 0] = canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; - gnss_base_pose_cov_msg.pose.covariance[6 * 1 + 1] = + gnss_base_pose_cov_msg.pose.covariance[7 * 1] = canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; - gnss_base_pose_cov_msg.pose.covariance[6 * 2 + 2] = + gnss_base_pose_cov_msg.pose.covariance[7 * 2] = canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; - gnss_base_pose_cov_msg.pose.covariance[6 * 3 + 3] = 0.1; - gnss_base_pose_cov_msg.pose.covariance[6 * 4 + 4] = 0.1; - gnss_base_pose_cov_msg.pose.covariance[6 * 5 + 5] = 1.0; + + if (use_gnss_ins_orientation_) { + gnss_base_pose_cov_msg.pose.covariance[7 * 3] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x, 2); + gnss_base_pose_cov_msg.pose.covariance[7 * 4] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y, 2); + gnss_base_pose_cov_msg.pose.covariance[7 * 5] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z, 2); + } else { + gnss_base_pose_cov_msg.pose.covariance[7 * 3] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[7 * 4] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[7 * 5] = 1.0; + } + pose_cov_pub_->publish(gnss_base_pose_cov_msg); // broadcast map to gnss_base_link publishTF(map_frame_, gnss_base_frame_, gnss_base_pose_msg); } -void GNSSPoser::callbackNavPVT(const ublox_msgs::msg::NavPVT::ConstSharedPtr nav_pvt_msg_ptr) +void GNSSPoser::callbackGnssInsOrientationStamped( + const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) { - nav_pvt_msg_ptr_ = nav_pvt_msg_ptr; + *msg_gnss_ins_orientation_stamped_ = *msg; } bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg)