From fbea0f6e874cacd032df9c575220151e3d0f9a8e Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Tue, 24 Sep 2024 22:49:01 +0900 Subject: [PATCH 1/2] fix: awsim orientaion --- .../src/imu_gnss_poser_node.cpp | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) 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 index cdc327fc..e546ca03 100644 --- 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 @@ -26,16 +26,31 @@ class ImuGnssPoser : public rclcpp::Node private: void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - //msg->pose.pose.orientation.x = imu_msg_.orientation.x; - //msg->pose.pose.orientation.y = imu_msg_.orientation.y; - //msg->pose.pose.orientation.z = imu_msg_.orientation.z; - //msg->pose.pose.orientation.w = imu_msg_.orientation.w; 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] = 0.01; msg->pose.covariance[7*4] = 0.01; msg->pose.covariance[7*5] = 0.1; + // insert imu if orientation is nan or empty + if (std::isnan(msg->pose.pose.orientation.x) || + std::isnan(msg->pose.pose.orientation.y) || + std::isnan(msg->pose.pose.orientation.z) || + std::isnan(msg->pose.pose.orientation.w) || + (msg->pose.pose.orientation.x == 0 && + msg->pose.pose.orientation.y == 0 && + msg->pose.pose.orientation.z == 0 && + msg->pose.pose.orientation.w == 0)) + { + msg->pose.pose.orientation.x = imu_msg_.orientation.x; + msg->pose.pose.orientation.y = imu_msg_.orientation.y; + msg->pose.pose.orientation.z = imu_msg_.orientation.z; + msg->pose.pose.orientation.w = imu_msg_.orientation.w; + // this covariance means orientation is not reliable + msg->pose.covariance[7*3] = 1000.0; + msg->pose.covariance[7*4] = 1000.0; + msg->pose.covariance[7*5] = 1000.0; + } pub_pose_->publish(*msg); if (!is_ekf_initialized_) pub_initial_pose_3d_->publish(*msg); From d90cdc093b68c43166a9ee66a2a6ecc2cfd53881 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Wed, 25 Sep 2024 10:33:05 +0900 Subject: [PATCH 2/2] chore: cov --- .../imu_gnss_poser/src/imu_gnss_poser_node.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) 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 index e546ca03..2d216380 100644 --- 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 @@ -29,9 +29,9 @@ class ImuGnssPoser : public rclcpp::Node 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] = 0.01; - msg->pose.covariance[7*4] = 0.01; - msg->pose.covariance[7*5] = 0.1; + msg->pose.covariance[7*3] = 100000.0; + msg->pose.covariance[7*4] = 100000.0; + msg->pose.covariance[7*5] = 100000.0; // insert imu if orientation is nan or empty if (std::isnan(msg->pose.pose.orientation.x) || std::isnan(msg->pose.pose.orientation.y) || @@ -47,9 +47,6 @@ class ImuGnssPoser : public rclcpp::Node msg->pose.pose.orientation.z = imu_msg_.orientation.z; msg->pose.pose.orientation.w = imu_msg_.orientation.w; // this covariance means orientation is not reliable - msg->pose.covariance[7*3] = 1000.0; - msg->pose.covariance[7*4] = 1000.0; - msg->pose.covariance[7*5] = 1000.0; } pub_pose_->publish(*msg); if (!is_ekf_initialized_)