| 108 | } |
| 109 | |
| 110 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg) |
| 111 | { |
| 112 | // 将原始IMU数据通过外参变换转到雷达坐标系下 |
| 113 | sensor_msgs::Imu thisImu = *imuMsg; |
| 114 | |
| 115 | std::lock_guard<std::mutex> lock1(imuLock); |
| 116 | std::lock_guard<std::mutex> lock2(biasLock); |
| 117 | |
| 118 | thisImu.linear_acceleration.x -= accBias(0); |
| 119 | thisImu.linear_acceleration.y -= accBias(1); |
| 120 | thisImu.linear_acceleration.z -= accBias(2); |
| 121 | thisImu.angular_velocity.x -= gyrBias(0); |
| 122 | thisImu.angular_velocity.y -= gyrBias(1); |
| 123 | thisImu.angular_velocity.z -= gyrBias(2); |
| 124 | imuQueue.push_back(thisImu); |
| 125 | } |
| 126 | |
| 127 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg) |
| 128 | { |
nothing calls this directly
no outgoing calls
no test coverage detected