fix(gnss_poser) new autoware_sensing_msgs added

Signed-off-by: melike tanrikulu <[email protected]> fix(gnss_poser): degree2radian deleted Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): change orientation calculation Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): add default values for covariance and change ptr as sharedptr Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): add autoware_msgs Signed-off-by: melike tanrikulu <[email protected]> fix(gnss_poser): remove unnecessary part in CMakeList.txt Signed-off-by: melike tanrikulu <[email protected]> change callback variable as ConstSharedPointer Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix read msg header Signed-off-by: melike tanrikulu <[email protected]> fix(gnss_poser) new autoware_sensing_msgs added Signed-off-by: melike tanrikulu <[email protected]> fix(gnss_poser): degree2radian deleted Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): change orientation calculation Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): add default values for covariance and change ptr as sharedptr Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix fix(gnss_poser): remove unnecessary part in CMakeList.txt Signed-off-by: melike tanrikulu <[email protected]> change callback variable as ConstSharedPointer Signed-off-by: melike tanrikulu <[email protected]> ci(pre-commit): autofix read msg header Signed-off-by: melike tanrikulu <[email protected]> fix(gnss_poser): add arg for orientation topic Signed-off-by: melike tanrikulu <[email protected]>
autowarefoundation · Jul 19, 2022 · 5b5d1df · 5b5d1df
1 parent 036b742
commit 5b5d1df
Show file tree

Hide file tree

Showing 7 changed files with 40 additions and 35 deletions.
diff --git 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
@@ -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
@@ -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 callbackAutowareOrientation(
+    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,13 @@ class GNSSPoser : public rclcpp::Node
   std::string map_frame_;
 
   sensor_msgs::msg::NavSatFix nav_sat_fix_origin_;
-  bool use_ublox_receiver_;
+  autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr orientation_msg_;
 
-  int plane_zone_;
+  bool use_gnss_heading_;
 
   boost::circular_buffer<geometry_msgs::msg::Point> position_buffer_;
-  ublox_msgs::msg::NavPVT::ConstSharedPtr nav_pvt_msg_ptr_;
+
+  int plane_zone_;
 };
 }  // namespace gnss_poser
 

diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/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_heading" 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>

diff --git a/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml b/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml
diff --git a/sensing/gnss_poser/package.xml b/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>

diff --git 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 <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
+
 #include <algorithm>
 #include <memory>
 #include <string>
@@ -29,7 +31,7 @@ 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)),
+  use_gnss_heading_(declare_parameter("use_gnss_heading", true)),
   plane_zone_(declare_parameter("plane_zone", 9))
 {
   int coordinate_system =
@@ -45,8 +47,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::callbackAutowareOrientation, 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 +93,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_heading_ && orientation_msg_ != nullptr) {
+    orientation = orientation_msg_->orientation.orientation;
   } else {
     static auto prev_position = median_position;
     orientation = getQuaternionByPositionDifference(median_position, prev_position);
@@ -135,18 +139,23 @@ void GNSSPoser::callbackNavSatFix(
     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] =
     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;
+  gnss_base_pose_cov_msg.pose.covariance[6 * 3 + 3] =
+    orientation_msg_ != nullptr ? std::pow(orientation_msg_->orientation.rmse_rotation_x, 2) : 0.1;
+  gnss_base_pose_cov_msg.pose.covariance[6 * 4 + 4] =
+    orientation_msg_ != nullptr ? std::pow(orientation_msg_->orientation.rmse_rotation_y, 2) : 0.1;
+  gnss_base_pose_cov_msg.pose.covariance[6 * 5 + 5] =
+    orientation_msg_ != nullptr ? std::pow(orientation_msg_->orientation.rmse_rotation_z, 2) : 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::callbackAutowareOrientation(
+  const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg)
 {
-  nav_pvt_msg_ptr_ = nav_pvt_msg_ptr;
+  orientation_msg_->header = msg->header;
+  orientation_msg_->orientation = msg->orientation;
 }
 
 bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg)