| 579 | } |
| 580 | |
| 581 | void transformUpdate() |
| 582 | { |
| 583 | mtx.lock(); |
| 584 | affine_odom_to_map = affine_imu_to_map*affine_imu_to_odom.inverse(); |
| 585 | |
| 586 | // cout<<"affine_imu_to_map "<<affine_imu_to_map.matrix()<<endl; |
| 587 | // cout<<"affine_odom_to_map "<<affine_odom_to_map.matrix()<<endl; |
| 588 | // // Publish TF |
| 589 | // static tf::TransformBroadcaster br; |
| 590 | // tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]), |
| 591 | // tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5])); |
| 592 | // // child frame 'lidar_link' expressed in parent_frame 'mapFrame' |
| 593 | // tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLidarInfoStamp, mapFrame, lidarFrame); |
| 594 | // br.sendTransform(trans_odom_to_lidar); |
| 595 | mtx.unlock(); |
| 596 | } |
| 597 | void run() |
| 598 | { |
| 599 | ros::Rate matchingRate(globalMatchingRate); |
nothing calls this directly
no outgoing calls
no test coverage detected