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..f48f9707 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,10 +26,10 @@ 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.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;