基于ROS通信协议的ORB-SLAM与Android客户端交互-如何接收点云数据绘点

上文中提到如何通过ROS协议收发数据，但在ORBSLAM算法中每帧输出的点云数据量很大，如何对接收到的点云数据都转换成(x,y)坐标的形式，并在android客户端的camera preview中进行绘制呢？

本工程将点云数据以std_msg.String类型进行传。每次传送100个，点与点之间以 “;” 号间隔，点的格式为(x,y)：

rostopic pub /rmytopic std_msgs/String "100,100;150,150;200,200;250,250;300,300;350,350;400,400;450,450;500,500;550,550;200,100;250,150;300,200;350,250;400,300;450,350;500,400;550,450;600,500;750,550;110,100;160,150;210,200;260,250;310,300;360,350;410,400;460,450;510,500;560,550;120,100;170,150;220,200;270,250;320,300;370,350;420,400;470,450;520,500;570,550;130,100;180,150;230,200;280,250;330,300;380,350;430,400;480,450;530,500;580,550;140,100;190,150;240,200;290,250;340,300;390,350;440,400;490,450;540,500;590,550;100,110;150,160;200,210;250,260;310,300;350,360;400,410;450,460;500,510;550,560;100,120;150,170;200,220;250,270;300,320;350,370;400,420;450,470;500,520;550,570;100,111;150,150;200,211;250,250;311,300;350,355;411,400;450,455;511,500;550,555;122,122;154,159;222,222;256,256;333,333;399,399;420,430;458,459;544,544;659,650"

本工程用到的android_ROS实例工程为：[https://github.com/huaibovip/android_ros_sensors.git]
(https://github.com/huaibovip/android_ros_sensors.git)
修改了里面android_tutorial_camera_imu,参考上一篇博文，增加Listener.java工程可以实现android客户端向远程服务器发送数据，通过修改主函数，实现接收点云数据，并作整数类型转换。

定义一个整数类型数组，用来作为点云坐标存储容器，用for循环完成绘图。
代码如下：

    java.lang.String str;//用作std_msg.String转换string
    int a[] = new int[2];//接收x,y坐标
    DrawOnTop mDraw;//DrawOnTop完成基于坐标绘图
     @Override
     //设置接收ROS信息的node节点，需要监听的topic信息
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);

        requestWindowFeature(Window.FEATURE_NO_TITLE);
        getWindow().addFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN);
        setContentView(R.layout.activity_main);
        Display display=getWindowManager().getDefaultDisplay();
        Point size = new Point();
        display.getSize(size);
        rosCameraPreviewView = findViewById(R.id.ros_camera_preview_view);this.getSystemService(Context.LOCATION_SERVICE);
        mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
        show = (TextView)super.findViewById(R.id.show);
        rosTextView = (RosTextView)findViewById(R.id.text);
        rosTextView.setTopicName("rmytopic");
        rosTextView.setMessageType(std_msgs.String._TYPE);
        rosTextView.setMessageToStringCallable(new MessageCallable() {
            @Override
            public String call(std_msgs.String message) {
                str = message.getData();
                getNumber(str);
                Log.d("listener", "main heard: "" + str+ """);
                return message.getData();
            }
        });
    }   

    }
    public int[] getNumber(java.lang.String strInput)
    {

        java.lang.String[] parts = strInput.split(";");
        for(int i=0;i<100;i++)
        {
            try {
                java.lang.String[] sub = parts[i].split(",");
                a[0] = Integer.parseInt( sub[0].trim());
                a[1] = Integer.parseInt( sub[1].trim());
                mDraw = new DrawOnTop(this,a[0],a[1]);
                Log.d("listener","p"+i+": "+a[0]+" "+a[1]);
                addContentView(mDraw, new WindowManager.LayoutParams(LayoutParams.WRAP_CONTENT, WindowManager.LayoutParams.WRAP_CONTENT));
            } catch(NumberFormatException nfe) {
                System.out.println("Could not parse " + nfe);
            }
        }
        return a;
    }

最终可以在terminal里print出每个点的坐标信息，并且能在实时采集的camera图像上看到绘制的点：