ego_planner使用时需要提供定位数据和点云。点云可以是使用视觉salm时，相机转过来的。也或者用激光雷达slam时，订阅点云数据和里程计信息。注意点云是要在世界坐标系下。

        <!-- 用于建图，直接使用odom不太行，这里需要相机的pose -->
        <arg name="camera_pose_topic" value="nouse"/>
        <arg name="depth_topic" value="/camera1/depth/image_rect_raw_nouse"/>

        <!-- <arg name="depth_topic" value="/d435i/depth/image_rect_raw"/> -->
        <!-- topic of point cloud measurement, such as from LIDAR  -->
        <!-- don't set camera pose and depth, if you already set this one! -->

        <!--  -->
        <arg name="cloud_topic" value="/cloud_registered"/>

使用gps定位时，当时直接把gps数据转为局部坐标，然后订阅雷达在世界坐标系下的点云信息。

void Gps_Local_Trans::GpsToLocalCallBack(const gps_to_local::waypoints::ConstPtr &msg) {

但是在运行时终端出现报错为：no odom！（带了个感叹号）。看代码如图

void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img) {

    pcl::PointCloud<pcl::PointXYZ> latest_cloud;
    pcl::fromROSMsg(*img, latest_cloud);

    md_.has_cloud_ = true;

    if (!md_.has_odom_) {
        std::cout << "no odom!" << std::endl;
        return;
    }
......
}

在点云的回调函数中找到了。读代码可以知道，应该是数据对齐的问题。

在grid_map.cpp中

if (mp_.pose_type_ == POSE_STAMPED) {
        pose_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(
            node_, "grid_map/pose", 25));

        sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(
            SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
        sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
    } else if (mp_.pose_type_ == ODOMETRY) {
        odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(
            node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));

        sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(
            SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));
        sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
    }

    //use odometry and point cloud
    indep_cloud_sub_ = node_.subscribe<sensor_msgs::PointCloud2>("grid_map/cloud", 10,
                                                                 &GridMap::cloudCallback, this);
    indep_odom_sub_ =
        node_.subscribe<nav_msgs::Odometry>("grid_map/odom", 10, &GridMap::odomCallback, this);

可以看到当平时使用slam的里程计和点云时，在slam中是进行了对齐的，所以ego里面没有再对齐。仿照上面，把gps数据的时间戳直接赋值为点云时间戳来对齐

ros::Publisher cloud_pub;
ros::Publisher odom_pub;
void cloudOdomCallback(const sensor_msgs::PointCloud2ConstPtr &cloud,
                       const nav_msgs::OdometryConstPtr &odom) {
    // ROS_INFO("cloud timestamp: %f, Odom timestamp: %f", cloud->header.stamp.toSec(),
    //          odom->header.stamp.toSec());
    ROS_INFO("aligned!!!");
    cloud_pub.publish(cloud);
    odom_pub.publish(odom);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "time_align");
    ros::NodeHandle nh;
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_cloud", 10);
    odom_pub = nh.advertise<nav_msgs::Odometry>("/aligned_odom", 10);
    message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, "/cloud_registered_",
                                                                    100); //点云信息
    message_filters::Subscriber<nav_msgs::Odometry> odom_sub(nh, "/Odometry_1",
                                                             100); //GPS里程计信息
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
                                                            nav_msgs::Odometry>
        SyncPolicyCloudOdom;
    std::shared_ptr<message_filters::Synchronizer<SyncPolicyCloudOdom>> sync_cloud_odom;
    sync_cloud_odom.reset(new message_filters::Synchronizer<SyncPolicyCloudOdom>(
        SyncPolicyCloudOdom(100), cloud_sub, odom_sub));
    sync_cloud_odom->registerCallback(boost::bind(&cloudOdomCallback, _1, _2));
    ros::spin();
    return 0;
}