From 7a3e5ea40bc139cc0984d6f8fe8b046de806f12f Mon Sep 17 00:00:00 2001 From: John Stark Date: Fri, 30 Jun 2023 11:11:56 -0400 Subject: [PATCH] add matric definition --- .../lidar_localizer_ros2/src/ndt_matching.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp b/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp index 2223ede6e4e..f5ff14dc72e 100644 --- a/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp +++ b/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp @@ -262,6 +262,20 @@ carma_ros2_utils::CallbackReturn NDTMatching::handle_on_configure(const rclcpp_l } #endif + Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation + Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation + Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ()); + tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); + + // Updated in initialpose_callback or gnss_callback + initial_pose.x = 0.0; + initial_pose.y = 0.0; + initial_pose.z = 0.0; + initial_pose.roll = 0.0; + initial_pose.pitch = 0.0; + initial_pose.yaw = 0.0; + // Initialize publishers predict_pose_pub = create_publisher("predict_pose", 10); predict_pose_imu_pub = create_publisher("predict_pose_imu", 10);