feat: parameterized cov (#9)

AutomotiveAIChallenge · Oct 1, 2024 · aab7118 · aab7118
1 parent b43ccb7
commit aab7118
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 9 deletions.
diff --git a/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -63,7 +63,9 @@
       <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
     </include>
 
-    <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen"/>
+    <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen">
+      <param from="$(find-pkg-share imu_gnss_poser)/config/imu_gnss_poser.param.yaml"/>
+    </node>
 
     <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
       <arg name="enable_yaw_bias_estimation" value="false"/>

diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt
@@ -14,6 +14,6 @@ ament_auto_add_executable(imu_gnss_poser_node
   src/imu_gnss_poser_node.cpp
 )
 
-ament_auto_package(
-  INSTALL_TO_SHARE
+ament_auto_package(INSTALL_TO_SHARE
+  config
 )
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/config/imu_gnss_poser.param.yaml b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/config/imu_gnss_poser.param.yaml
@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    position_0_cov: 0.1
+    position_1_cov: 0.1
+    position_2_cov: 0.1
+    orientation_0_cov: 100000.0 
+    orientation_1_cov: 100000.0 
+    orientation_2_cov: 100000.0 
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp
@@ -21,19 +21,26 @@ class ImuGnssPoser : public rclcpp::Node
         sub_imu_ = this->create_subscription<sensor_msgs::msg::Imu>(
             "/sensing/imu/imu_raw", qos,
             std::bind(&ImuGnssPoser::imu_callback, this, std::placeholders::_1));
+
+        posi_0_cov_ = this->declare_parameter<float>("position_0_cov");
+        posi_1_cov_ = this->declare_parameter<float>("position_1_cov");
+        posi_2_cov_ = this->declare_parameter<float>("position_2_cov");
+        ori_0_cov_ = this->declare_parameter<float>("orientation_0_cov");
+        ori_1_cov_ = this->declare_parameter<float>("orientation_1_cov");
+        ori_2_cov_ = this->declare_parameter<float>("orientation_2_cov");
     }
 
 private:
 
     void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
 
         // this covariance means orientation is not reliable
-        msg->pose.covariance[7*0] = 0.1;
-        msg->pose.covariance[7*1] = 0.1;
-        msg->pose.covariance[7*2] = 0.1;
-        msg->pose.covariance[7*3] = 100000.0;
-        msg->pose.covariance[7*4] = 100000.0;
-        msg->pose.covariance[7*5] = 100000.0;
+        msg->pose.covariance[7*0] = posi_0_cov_;
+        msg->pose.covariance[7*1] = posi_1_cov_;
+        msg->pose.covariance[7*2] = posi_2_cov_;
+        msg->pose.covariance[7*3] = ori_0_cov_;
+        msg->pose.covariance[7*4] = ori_1_cov_;
+        msg->pose.covariance[7*5] = ori_2_cov_;
 
         // insert imu if orientation is nan or empty
         if (std::isnan(msg->pose.pose.orientation.x) ||
@@ -71,6 +78,12 @@ class ImuGnssPoser : public rclcpp::Node
     rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
     sensor_msgs::msg::Imu imu_msg_;
     bool is_ekf_initialized_ = {false};
+    float posi_0_cov_;
+    float posi_1_cov_;
+    float posi_2_cov_;
+    float ori_0_cov_;
+    float ori_1_cov_;
+    float ori_2_cov_;
 };
 
 int main(int argc, char *argv[])