fix(gnss_poser): add new autoware sensing msgs for covariance default value problem

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

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

sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp

@@ -19,11 +19,11 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 #include <tier4_debug_msgs/msg/bool_stamped.hpp>
-#include <ublox_msgs/msg/nav_pvt.hpp>
 
 #include <boost/circular_buffer.hpp>
 
@@ -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<sensor_msgs::msg::NavSatFix>::SharedPtr nav_sat_fix_sub_;
-  rclcpp::Subscription<ublox_msgs::msg::NavPVT>::SharedPtr nav_pvt_sub_;
+  rclcpp::Subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>::SharedPtr
+    autoware_orientation_sub_;
 
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::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<geometry_msgs::msg::Point> position_buffer_;
 
   int plane_zone_;
 
-  boost::circular_buffer<geometry_msgs::msg::Point> 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
 

sensing/gnss_poser/launch/gnss_poser.launch.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="input_topic_fix" default="/fix"/>
-  <arg name="input_topic_navpvt" default="/navpvt"/>
+  <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"/>
@@ -15,13 +15,12 @@
 
   <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
   <arg name="buff_epoch" default="1"/>
-  <arg name="use_ublox_receiver" default="false"/>
+  <arg name="use_gnss_ins_orientation" default="true"/>
   <arg name="plane_zone" default="9"/>
 
   <node pkg="gnss_poser" exec="gnss_poser" name="gnss_poser" output="screen">
     <remap from="fix" to="$(var input_topic_fix)"/>
-    <remap from="navpvt" to="$(var input_topic_navpvt)"/>
-
+    <remap from="autoware_orientation" to="$(var input_topic_orientation)"/>
     <remap from="gnss_pose" to="$(var output_topic_gnss_pose)"/>
     <remap from="gnss_pose_cov" to="$(var output_topic_gnss_pose_cov)"/>
     <remap from="gnss_fixed" to="$(var output_topic_gnss_fixed)"/>
@@ -33,7 +32,6 @@
 
     <param name="coordinate_system" value="$(var coordinate_system)"/>
     <param name="buff_epoch" value="$(var buff_epoch)"/>
-    <param name="use_ublox_receiver" value="$(var use_ublox_receiver)"/>
     <param name="plane_zone" value="$(var plane_zone)"/>
     <param from="$(var param_file)"/>
   </node>

sensing/gnss_poser/package.xml

@@ -11,6 +11,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_sensing_msgs</depend>
   <depend>geo_pos_conv</depend>
   <depend>geographiclib</depend>
   <depend>geometry_msgs</depend>
@@ -21,7 +22,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_debug_msgs</depend>
-  <depend>ublox_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

sensing/gnss_poser/src/gnss_poser_core.cpp

@@ -14,6 +14,8 @@
 
 #include "gnss_poser/gnss_poser_core.hpp"
 
+#include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
+
 #include <algorithm>
 #include <memory>
 #include <string>
@@ -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<int>("plane_zone", 9)),
+  msg_gnss_ins_orientation_stamped_(
+    std::make_shared<autoware_sensing_msgs::msg::GnssInsOrientationStamped>())
 {
   int coordinate_system =
     declare_parameter("coordinate_system", static_cast<int>(CoordinateSystem::MGRS));
@@ -45,8 +49,10 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
 
   nav_sat_fix_sub_ = create_subscription<sensor_msgs::msg::NavSatFix>(
     "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1));
-  nav_pvt_sub_ = create_subscription<ublox_msgs::msg::NavPVT>(
-    "navpvt", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavPVT, this, std::placeholders::_1));
+  autoware_orientation_sub_ =
+    create_subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>(
+      "autoware_orientation", rclcpp::QoS{1},
+      std::bind(&GNSSPoser::callbackGnssInsOrientationStamped, this, std::placeholders::_1));
 
   pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("gnss_pose", rclcpp::QoS{1});
   pose_cov_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -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)