使用Qt绘制cartographer地图(使用grpc上传)

因为Qt绘图的坐标系与rviz不同,所以获取submap的全局坐标后,绘制和rviz不同,要做一些调整,测试几种地图,基本是准确的,上传核心部分代码

void MainWindow::displaySubmaps()
{

     std::map<SubmapId,SubmapGlobalPose>  all_submap_data =
             rpc_handler.GetAllSubmap();

     for(auto submap_data : all_submap_data){

         int trajectory_id = submap_data.first.trajectory_id;
         int submap_index = submap_data.first.submap_index;

         if(trajectory_id != 0)  continue;

         QGraphicsItem *submap = new Submap();
         qDebug() << "--------------------------------------------------------------------------------------------------";
         qDebug() << "submap id " << submap_index;
         qDebug() << "submap_data.second.global_x " << submap_data.second.global_x;
         qDebug() << "submap_data.second.global_y " << submap_data.second.global_y;
         //qDebug() << "submap_data.second.global_yaw " << submap_data.second.global_yaw;
         int row =    submap_data.second.global_x /0.05;       //-10.05/0.05;
         int col =       submap_data.second.global_y/0.05;       //6.109/0.05;

         submap->setPos(row, -1.0*col);
         double rotation_angle =  -1.0*submap_data.second.global_yaw + 90;

        qDebug() << "rotation_angle " << rotation_angle;

         submap->setRotation(rotation_angle);    //-36.3315 //

         //submap->setOpacity(0.3);
         Slicedata slice_data = rpc_handler.GetSubmap(trajectory_id,submap_index);

         QGraphicsItem *slice_0 = new Slice(slice_data.image);

        qDebug() << "slice_data.slice_x " << slice_data.slice_x;
        qDebug() << "slice_data.slice_y " << slice_data.slice_y;
        
slice_0->setPos(-1.0*slice_data.slice_y/0.05,-1.0*slice_data.slice_x/0.05);
        slice_0->setParentItem(submap);
         //slice_0->setOpacity(0.5);

         scene->addItem(submap); //组合添加到场景中

     }

     QColor color(128,126,121); //经典灰色
     //QColor color(255,255,255);
     scene->setBackgroundBrush(QBrush(color));

    mTime.stop();
}

你可能感兴趣的:(C++/Qt自动化框架,2d_slam,qt,开发语言)