ros nav_msgs/OccupancyGrid 显示错位,混乱

在项目想采用rosjava android_core 进行二次开发。初步分析代码,认为如果在上面开发,能极大减少我们的开发工作量。运行demo 看看地图展示的效果。

https://github.com/rosjava/android_core

branch:kinetic

运行应用 android_tutorial_map_viewer

在android 平板上是这样的效果:


2D地图出现错位,轮廓错乱,混乱的情况。没法用于导航了。

+ OccupancyGridLayer.java

-- 控制地图数据块显示排列顺序的是. 控制地图层缩放,平移,旋转的是

+ CameraControlLayer.java

-- 实现触摸事件行为

-- 传播平移,旋转,缩放事件

所以出现的问题需要详细分析OccupancyGridLayer.java,可能会找到答案。

1. 监听接受 nav_msgs.OccupancyGrid 消息

@Override

public void onStart(VisualizationView view, ConnectedNode connectedNode) {

super.onStart(view, connectedNode);

  previousGl =null;

  getSubscriber().addMessageListener(new MessageListener() {

@Override

    public void onNewMessage(nav_msgs.OccupancyGrid message) {

update(message);

    }

});

}

2. 为了绘制尺寸超出纹理最大尺寸的地图,我们将地图分割成多个块,并为每个块绘制一个纹理(直接翻译注释)

-- 块的相关属性有分辨率(决定物理转换关系,比如单位像素对应的0.05米),块的宽度,

-- 相对地图坐标系块的原点坐标

3. 把地图分辨率颜色值分为三类,分别表示可行走区域,不可行走区域,未知区域。

/**

* Color of occupied cells in the map.

*/

private static final int COLOR_OCCUPIED =0xff111111;

/**

* Color of free cells in the map.

*/

private static final int COLOR_FREE =0xffffffff;

/**

* Color of unknown cells in the map.

*/

private static final int COLOR_UNKNOWN =0xffdddddd;

4. private void update(nav_msgs.OccupancyGrid message) 方法会更新每一个图片块,初步怀疑是图片块原点坐标计算有问题

-- 根据图片的宽度,高度,计算可以划分多少图片块

-- 每个图片块初始化为相同的物理分辨率

-- 逐个计算基于地图坐标系下的图片块坐标原点,对比maven仓库中的 android_core:0.4 版本发现增加如下修正,地图显示就没有问题了。

for (int y =0; y < numTilesHigh; ++y) {

for (int x =0; x < numTilesWide; ++x) {

final int tileIndex = y * numTilesWide + x;

        tiles.get(tileIndex).setOrigin(origin.multiply(new Transform(new Vector3(x *

resolution * TextureBitmap.STRIDE,

            y * resolution * TextureBitmap.HEIGHT, 0.), Quaternion.identity())));

//        tiles.get(tileIndex).setStride(TextureBitmap.STRIDE);

//solution for tile has gap in bitmap!!! be carefull.

        if (x < numTilesWide -1) {

tiles.get(tileIndex).setStride(TextureBitmap.STRIDE);

        }else {

tiles.get(tileIndex).setStride(width % TextureBitmap.STRIDE);

        }

}

}

关键的地方在最后一个图片快的宽度处理,如果是最后一个图片块宽度更新为(width % TextureBitmap.STRIDE)

这样就没有问题了



总而言之在方法 private void update(nav_msgs.OccupancyGrid message)中根据第4小点说明添加如下代码

//solution for tile has gap in bitmap!!! be carefull.

if (x < numTilesWide -1) {

tiles.get(tileIndex).setStride(TextureBitmap.STRIDE);

}else {

tiles.get(tileIndex).setStride(width % TextureBitmap.STRIDE);

}

你可能感兴趣的:(ros nav_msgs/OccupancyGrid 显示错位,混乱)