| 28 | namespace VIO { |
| 29 | |
| 30 | RosBaseDataProvider::RosBaseDataProvider() |
| 31 | : DataProvider(), |
| 32 | it_(nullptr), |
| 33 | stereo_calib_(), |
| 34 | vio_output_(), |
| 35 | nh_(), |
| 36 | nh_private_("~"), |
| 37 | vio_output_queue_("VIO output") { |
| 38 | ROS_INFO(">>>>>>> Initializing Spark-VIO-ROS <<<<<<<"); |
| 39 | |
| 40 | // Parse calibration info for camera and IMU |
| 41 | // Calibration info on parameter server (Parsed from yaml) |
| 42 | parseCameraData(&stereo_calib_); |
| 43 | parseImuData(&imu_data_, &pipeline_params_.imu_params_); |
| 44 | // parse backend/frontend parameters |
| 45 | // Mind that parseBackendParams modifies the imu_params_ if using the default! |
| 46 | // TODO(TONI) the parseImuData is completely flawed, since parse backend |
| 47 | // is actually parsing the imu params!! |
| 48 | |
| 49 | parseBackendParams(); |
| 50 | |
| 51 | CHECK(pipeline_params_.backend_params_); |
| 52 | parseFrontendParams(); |
| 53 | |
| 54 | // Print parameters to check. |
| 55 | printParsedParams(); |
| 56 | |
| 57 | it_ = VIO::make_unique<image_transport::ImageTransport>(nh_); |
| 58 | |
| 59 | // Get ROS params |
| 60 | CHECK(nh_private_.getParam("base_link_frame_id", base_link_frame_id_)); |
| 61 | CHECK(!base_link_frame_id_.empty()); |
| 62 | CHECK(nh_private_.getParam("world_frame_id", world_frame_id_)); |
| 63 | CHECK(!world_frame_id_.empty()); |
| 64 | CHECK(nh_private_.getParam("left_cam_frame_id", left_cam_frame_id_)); |
| 65 | CHECK(!left_cam_frame_id_.empty()); |
| 66 | CHECK(nh_private_.getParam("right_cam_frame_id", right_cam_frame_id_)); |
| 67 | CHECK(!right_cam_frame_id_.empty()); |
| 68 | |
| 69 | // Publishers |
| 70 | odometry_pub_ = nh_.advertise<nav_msgs::Odometry>("odometry", 10, true); |
| 71 | frontend_stats_pub_ = |
| 72 | nh_.advertise<std_msgs::Float64MultiArray>("frontend_stats", 10); |
| 73 | resiliency_pub_ = |
| 74 | nh_.advertise<std_msgs::Float64MultiArray>("resiliency", 10); |
| 75 | imu_bias_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("imu_bias", 10); |
| 76 | pointcloud_pub_ = |
| 77 | nh_.advertise<PointCloudXYZRGB>("time_horizon_pointcloud", 10, true); |
| 78 | mesh_3d_frame_pub_ = nh_.advertise<pcl_msgs::PolygonMesh>("mesh", 5, true); |
| 79 | debug_img_pub_ = it_->advertise("debug_mesh_img", 10, true); |
| 80 | |
| 81 | // Static TFs for left/right cameras |
| 82 | const gtsam::Pose3& body_Pose_left_cam = |
| 83 | stereo_calib_.left_camera_info_.body_Pose_cam_; |
| 84 | const gtsam::Pose3& body_Pose_right_cam = |
| 85 | stereo_calib_.right_camera_info_.body_Pose_cam_; |
| 86 | publishStaticTf(body_Pose_left_cam, |
| 87 | base_link_frame_id_, |
nothing calls this directly
no outgoing calls
no test coverage detected