feat: use original gnss (#36)

* update pose transformer * update reference * fix launch error * fix(gps_module)pose to posestamped * update reference launch * use gnss original covariance * update plotjuggler * can change gnss cov * parameterized gnss module cov * update rviz * update gpss param * publish original pose when interface rosbag
AutomotiveAIChallenge · Oct 8, 2024 · e480d53 · e480d53
1 parent d575dbc
commit e480d53
Show file tree

Hide file tree

Showing 10 changed files with 214 additions and 176 deletions.
diff --git a/...rkspace/src/aichallenge_submit/aichallenge_submit_launch/config/localizer_plotjuggler.xml b/...rkspace/src/aichallenge_submit/aichallenge_submit_launch/config/localizer_plotjuggler.xml
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
@@ -69,8 +69,12 @@
       <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
     </include>
 
+    <!-- Imu Gnss Poser -->
     <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen">
+      <remap from="/sensing/gnss/pose_with_covariance" to="/sensing/gnss/gps_module/pose_with_covariance"/>
       <param from="$(find-pkg-share imu_gnss_poser)/config/imu_gnss_poser.param.yaml"/>
+       <!-- Use true when AWSIM(Simulator) is used -->
+      <param name="specific_covariance" value="$(var simulation)"/>
     </node>
 
     <!-- EKF_Localizer -->
@@ -262,7 +266,10 @@
   </group>
 
   <!-- Pose Cov Transformer Node -->
-  <node pkg="pose_transformer" exec="pose_transformer" name="pose_transformer" output="screen"/>
+  <node pkg="pose_transformer" exec="pose_transformer" name="pose_transformer" output="screen">
+    <!-- To-do, tomoki Only for interfacebag -->
+    <param name="publish_gnss_pose_original" value="$(var is_rosbag)"/>
+  </node>
   <!-- Acc  culculator Node -->
   <node pkg="vehicle_state_culculator" exec="vehicle_state_culculator_node" name="vehicle_state_culculator_node" output="screen"/>
 

diff --git a/aichallenge/workspace/src/aichallenge_submit/gps_module/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/gps_module/CMakeLists.txt
@@ -20,6 +20,7 @@ install(TARGETS
 
 install(DIRECTORY
   launch
+  config
   DESTINATION share/${PROJECT_NAME}
 )
 

diff --git a/aichallenge/workspace/src/aichallenge_submit/gps_module/config/gps_module.params.yaml b/aichallenge/workspace/src/aichallenge_submit/gps_module/config/gps_module.params.yaml
@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    debug: false
+
+    scale_position_covariance_x: 1.0
+    scale_position_covariance_y: 1.0
+    # Not used 
+    scale_position_covariance_z: 1.0
+
+    scale_orientation_covariance_0: 1.0
+    scale_orientation_covariance_1: 1.0
+    scale_orientation_covariance_2: 1.0
diff --git a/aichallenge/workspace/src/aichallenge_submit/gps_module/launch/gps_module.launch.xml b/aichallenge/workspace/src/aichallenge_submit/gps_module/launch/gps_module.launch.xml
@@ -1,7 +1,6 @@
 <launch>
   <node pkg="gps_module" exec="true_gps_module" name="true_gps_module" output="screen">
-
-    <param name="debug" value="false"/>
+    <param from="$(find-pkg-share gps_module)/config/gps_module.params.yaml" />
     <remap from="~/input/pose_with_covariance" to="/sensing/gnss/pose_with_covariance"/>
     <remap from="~/output/pose_with_covariance" to="/sensing/gnss/gps_module/pose_with_covariance"/>
     <remap from="~/output/pose" to="/sensing/gnss/gps_module/pose"/>

diff --git a/aichallenge/workspace/src/aichallenge_submit/gps_module/src/true_gps_module.cpp b/aichallenge/workspace/src/aichallenge_submit/gps_module/src/true_gps_module.cpp
@@ -1,6 +1,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include <deque>
 
 class PoseComparisonNode : public rclcpp::Node
@@ -12,7 +12,12 @@ class PoseComparisonNode : public rclcpp::Node
         // パラメータの宣言とデフォルト値
         this->declare_parameter<bool>("debug", false);
         debug_ = this->get_parameter("debug").as_bool();
-
+        scale_pos_cov_x = this->declare_parameter<float>("scale_position_covariance_x");
+        scale_pos_cov_y = this->declare_parameter<float>("scale_position_covariance_y");
+        scale_pos_cov_z = this->declare_parameter<float>("scale_position_covariance_z");
+        scale_ori_cov_0 = this->declare_parameter<float>("scale_orientation_covariance_0");
+        scale_ori_cov_1 = this->declare_parameter<float>("scale_orientation_covariance_1");
+        scale_ori_cov_2 = this->declare_parameter<float>("scale_orientation_covariance_2");
         // サブスクリプションの設定
         subscription_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
             "~/input/pose_with_covariance", 10, std::bind(&PoseComparisonNode::pose_callback, this, std::placeholders::_1));
@@ -21,16 +26,22 @@ class PoseComparisonNode : public rclcpp::Node
         publisher_pose_with_cov_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("~/output/pose_with_covariance", 10);
 
         // Poseメッセージ用のパブリッシャー
-        publisher_pose_ = this->create_publisher<geometry_msgs::msg::Pose>("~/output/pose", 10);
+        publisher_pose_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("~/output/pose", 10);
     }
 
 private:
     // 最新の2つのメッセージを保持するためのキュー
     std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
     bool debug_;  // デバッグフラグ
+    float scale_pos_cov_x;
+    float scale_pos_cov_y;
+    float scale_pos_cov_z;
+    float scale_ori_cov_0;
+    float scale_ori_cov_1;
+    float scale_ori_cov_2;
 
     rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_pose_with_cov_;
-    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_pose_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_pose_;
 
     void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
     {
@@ -70,11 +81,26 @@ class PoseComparisonNode : public rclcpp::Node
             }
 
             // PoseWithCovarianceStampedメッセージのパブリッシュ
-            publisher_pose_with_cov_->publish(*new_pose);
+            auto new_pose_modify_cov = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(*new_pose);
+            new_pose_modify_cov->header.stamp = this->now();
+            //Pose Cov
+            new_pose_modify_cov->pose.covariance[0] = new_pose->pose.covariance[0] * scale_pos_cov_x;            
+            new_pose_modify_cov->pose.covariance[7] = new_pose->pose.covariance[7] * scale_pos_cov_y;
+            // Keep Z 0.0
+            // new_pose_modify_cov->pose.covariance[14] = new_pose->pose.covariance[14] * scale_pos_cov_z;
+            // Orientation Cov, nature 0.1 0.1 1.0
+            new_pose_modify_cov->pose.covariance[22] = new_pose->pose.covariance[22] * scale_ori_cov_0;       
+            new_pose_modify_cov->pose.covariance[29] = new_pose->pose.covariance[29] * scale_ori_cov_1;
+            new_pose_modify_cov->pose.covariance[35] = new_pose->pose.covariance[35] * scale_ori_cov_2;
+
+
+            publisher_pose_with_cov_->publish(*new_pose_modify_cov);
 
             // Poseメッセージのパブリッシュ
-            geometry_msgs::msg::Pose pose_msg = new_pose->pose.pose;
-            publisher_pose_->publish(pose_msg);
+            auto pose_msg = std::make_shared<geometry_msgs::msg::PoseStamped>();
+            pose_msg->header = new_pose->header;
+            pose_msg->pose = new_pose->pose.pose;
+            publisher_pose_->publish(*pose_msg);
         } else {
             if (debug_) {
                 RCLCPP_INFO(this->get_logger(), "Pose not changed!");

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
@@ -1,5 +1,8 @@
 /**:
   ros__parameters:
+    # Use true when AWSIM(Simulator) is used
+    # specific_covariance: false
+    # ONLY FOR SPECIFIC COVARIANCE
     position_0_cov: 0.1
     position_1_cov: 0.1
     position_2_cov: 0.1

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
@@ -22,6 +22,7 @@ class ImuGnssPoser : public rclcpp::Node
             "/sensing/imu/imu_raw", qos,
             std::bind(&ImuGnssPoser::imu_callback, this, std::placeholders::_1));
 
+        specific_cov_ = this->declare_parameter<bool>("specific_covariance");
         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");
@@ -35,13 +36,14 @@ class ImuGnssPoser : public rclcpp::Node
     void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
 
         // this covariance means orientation is not reliable
-        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_;
-
+        if (specific_cov_){
+            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_;
+        }
         // Only Simulation
         // insert imu if orientation is nan or empty
         if (std::isnan(msg->pose.pose.orientation.x) ||
@@ -85,6 +87,7 @@ class ImuGnssPoser : public rclcpp::Node
     float ori_0_cov_;
     float ori_1_cov_;
     float ori_2_cov_;
+    bool specific_cov_ = false;
 };
 
 int main(int argc, char *argv[])

diff --git a/aichallenge/workspace/src/aichallenge_submit/pose_transformer/src/pose_transformer.cpp b/aichallenge/workspace/src/aichallenge_submit/pose_transformer/src/pose_transformer.cpp
@@ -12,10 +12,10 @@ tf_listener_(tf_buffer_, this, false)
     pub_gnss_pose_original_= this->declare_parameter<bool>("publish_gnss_pose_original", false);
 
     // pub, subの初期化
-    sub_gnss_pose_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/sensing/gnss/pose_with_covariance", 1, std::bind(&Pose_transformer::on_gnss_pose, this, _1));
-    pub_gnss_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/sensing/gnss/base_link/pose", 1);
+    sub_gnss_pose_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/sensing/gnss/gps_module/pose_with_covariance", 1, std::bind(&Pose_transformer::on_gnss_pose, this, _1));
+    pub_gnss_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/sensing/gnss/gnss_module/base_link/pose", 1);
     if (pub_gnss_pose_original_)
-        pub_orig_gnss_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/sensing/gnss/pose", 1);
+        pub_orig_gnss_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/sensing/gnss/gps_module/pose", 1);
     sub_kinematic_state_ = create_subscription<nav_msgs::msg::Odometry>("/localization/kinematic_state", 1, std::bind(&Pose_transformer::on_kinematic_state, this, _1));
     pub_kinematic_state_ = create_publisher<nav_msgs::msg::Odometry>("/localization/base_link/kinematic_state", 1);
     sub_imu_raw_ = create_subscription<sensor_msgs::msg::Imu>("/sensing/imu/imu_raw", 1, std::bind(&Pose_transformer::on_imu_data, this, _1));

diff --git a/...enge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz b/...enge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz
@@ -6,9 +6,9 @@ Panels:
       Expanded:
         - /Map1/Lanelet2VectorMap1
         - /Sensing1
-        - /Sensing1/ImuGnss1/Covariance1
         - /Sensing1/ImuGnss1/Covariance1/Position1
         - /Sensing1/Odometry1/Shape1
+        - /Sensing1/GNSSModule1
         - /EKF_Localizer1
         - /Planning1/ScenarioPlanning1
         - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1
@@ -297,7 +297,7 @@ Visualization Manager:
               Color: 204; 51; 204
               Scale: 1
               Value: true
-            Value: true
+            Value: false
           Enabled: true
           Head Length: 0.30000001192092896
           Head Radius: 0.10000000149011612
@@ -367,15 +367,15 @@ Visualization Manager:
               Scale: 1
               Value: false
             Position:
-              Alpha: 0.20000000298023224
+              Alpha: 0.699999988079071
               Color: 204; 51; 204
               Scale: 1
               Value: true
-            Value: false
+            Value: true
           Enabled: true
           Head Length: 0.10000000149011612
           Head Radius: 0.20000000298023224
-          Name: GNSS
+          Name: GNSSModule
           Shaft Length: 1.5
           Shaft Radius: 0.20000000298023224
           Shape: Arrow
@@ -385,7 +385,7 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /sensing/gnss/pose_with_covariance
+            Value: /sensing/gnss/gps_module/pose_with_covariance
           Value: true
       Enabled: true
       Name: Sensing
@@ -440,11 +440,7 @@ Visualization Manager:
               Enabled: true
               Name: RouteArea
               Namespaces:
-                goal_lanelets: true
-                lane_start_bound: true
-                left_lane_bound: true
-                right_lane_bound: true
-                route_lanelets: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Transient Local
@@ -571,8 +567,7 @@ Visualization Manager:
                       Enabled: true
                       Name: Bound
                       Namespaces:
-                        left_bound: true
-                        right_bound: true
+                        {}
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
@@ -862,7 +857,7 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: -39.03660583496094
+      Scale: -51.57180404663086
       Target Frame: base_link
       Value: TopDownOrtho (rviz_default_plugins)
       X: 2.4756786823272705
@@ -904,8 +899,6 @@ Visualization Manager:
         X: 0
         Y: 0
 Window Geometry:
-  AutowareScreenCapturePanel:
-    collapsed: false
   Displays:
     collapsed: false
   Height: 1016
@@ -915,7 +908,7 @@ Window Geometry:
     collapsed: false
   MultiDataMonitor:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd00000004000000000000017e000003a2fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e1000000c700fffffffc00000322000000bb000000bb0100001afa000000010100000003fb00000020004d0075006c007400690044006100740061004d006f006e00690074006f0072030000061f0000016e000001600000039ffb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008c00fffffffb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c01000002850000006e0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007100fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065000000038a0000010b0000000000000000fb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c01000002f90000006e0000000000000000fb00000034004100750074006f007700610072006500530063007200650065006e004300610070007400750072006500500061006e0065006c010000036d0000006e0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005b6000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
-Original file line number
+Diff line change
@@ Expand Up / @@ -20,6 +20,7 @@ install(TARGETS @@
     install(DIRECTORY
       launch
+      config
       DESTINATION share/${PROJECT_NAME}
     )
@@ Expand Down @@