RosClient开源及核心代码解析

| ， | |

#简介

接上篇ROS与Android的窃窃私语,很多同学表示

就给几张图和APK的下载连接,然并卵,劳资要知其所以然!

这几天忙着整理代码,准备着自己的第一个开源项目

同学,代码拿走,记得点赞,查看代码请移步RosClient

功能及界面大家都熟悉了,接下来讲讲核心功能代码

#建立连接

这么简单我就不解释了吧

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
private void connect(String ip, String port) {
    client = new ROSBridgeClient("ws://" + ip + ":" + port);
    boolean conneSucc = client.connect(new ROSClient.ConnectionStatusListener() {
        @Override
        public void onConnect() {
            client.setDebug(true);
            ((RCApplication)getApplication()).setRosClient(client);
            Log.d(TAG,"Connect ROS success");
            startActivity(new Intent(MainActivity.this,NodesActivity.class));
        }
        @Override
        public void onDisconnect(boolean normal, String reason, int code) {
            Log.d(TAG,"ROS disconnect");
        }
        @Override
        public void onError(Exception ex) {
            ex.printStackTrace();
            Log.d(TAG,"ROS communication error");
        }
    });
}

ROSBridgeClient为通信的连接对象,后续的所有功能都依赖于它,所以为了能在所有Activity中使用,保存在Application中,另外断开连接放在了Application中的onTerminate()中,此方法在应用完全退出后,会被调用

#枚举信息

比连接代码还简单

1
2
3
4
5
6
7
8
9
10
11
client = ((RCApplication)getApplication()).getRosClient();
try {
    //Get list data
    nodeDataArray[0] = client.getNodes();
    nodeDataArray[1] = client.getServices();
    nodeDataArray[2] = client.getTopics();
} catch (InterruptedException e) {
    e.printStackTrace();
}
nodeListView.setAdapter(new NodeListAdapter());

通过ROSBridgeClient的三个方法分别获取到Node list,Service list,Topic list,然后绑到列表上去

#各种操作

  • 参数枚举

    大部分Service的调用和Topick的发布都是带参数的,所以我们需要知道他们的参数列表,对于不是基本参数类型(string,bool,number),我们还需要知道它是由哪些字段组成的

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    try {
        //获取参数列表,包含了该Topic或Service所有参数信息
        if(detailType.equalsIgnoreCase("topic")) {
            typeDef = client.getTopicMessageList(detailName);
        } else if(detailType.equalsIgnoreCase("service")) {
            ....
            typeDef = client.getServiceRequestList(detailName);
        }
        root = TreeNode.root();
        //生成参数列表树控件
        genParamTree(root, typeDef[0]);
        AndroidTreeView tView = new AndroidTreeView(this, root);
        paramContainer.addView(tView.getView());
    } catch (InterruptedException e) {
        e.printStackTrace();
    }
    private void genParamTree(TreeNode parent, TypeDef type) {
            for (int i = 0; i < type.fieldtypes.length; i++) {
                if(type.fieldtypes[i].contains("/")) {//Not basic type
                    for (TypeDef t : typeDef) {
                        //set child for all fields
                        if (t.type.equals(type.fieldtypes[i])) {
                            TreeNode node = new TreeNode(new TreeViewHolder.TreeItem(type.fieldnames[i],type.fieldtypes[i],false)).setViewHolder(new TreeViewHolder(this));
                            //非基本类型参数则递归调用
                            genParamTree(node, t);
                            parent.addChild(node);
                            break;
                        }
                    }
                } else {//Basic type
                    TreeNode node = new TreeNode(new TreeViewHolder.TreeItem(type.fieldnames[i],type.fieldtypes[i],true)).setViewHolder(new TreeViewHolder(this));
                    parent.addChild(node);
                }
            }
    }

  • 生成调用参数数据

    遍历上面的参数控件树,拼接调用所需的参数数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
private String getTreeData(TreeNode parent) {
    String json = "";
    List<TreeNode> nodeList = parent.getChildren();
    for (int i = 0; i < nodeList.size(); i++) {
        TreeNode node = nodeList.get(i);
        if(node.isLeaf()) {//leaf node is basic type
            String data = ((TreeViewHolder)node.getViewHolder()).jsonData;
            json += data + ",";
        } else {
            String data = getTreeData(node);
            json +="\"" + ((TreeViewHolder)node.getViewHolder()).jsonData +  "\":{" + data + "},";
        }
    }
    if(!TextUtils.isEmpty(json))
        json = json.substring(0,json.length() - 1);
    return json;
}

  • 运动控制

    针对Topic /cmd_vel,做一个手势滑动的处理,依然很easy

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    private void processMoveTopic() {
            timer = new Timer();
            TimerTask timerTask = new TimerTask() {
                @Override
                public void run() {
                    if (moving) {
                        client.send("{\"op\":\"publish\",\"topic\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + linearX + ",\"y\":0,\"z\":0},\"angular\":{\"x\":0,\"y\":0,\"z\":" + angularZ + "}}}");
                        Log.d(TAG,"send cmd_vel msg:x:" + linearX + " z:" + angularZ);
                    }
                }
            };
            timer.schedule(timerTask, 1000, 600);
            tvLog.setOnTouchListener(new View.OnTouchListener() {
                @Override
                public boolean onTouch(View view, MotionEvent motionEvent) {
                    switch (motionEvent.getAction()) {
                        case MotionEvent.ACTION_DOWN:
                            moving = true;
                            break;
                        case MotionEvent.ACTION_MOVE:
                            float x = motionEvent.getX();
                            float y = motionEvent.getY();
                            int width = view.getWidth();
                            int height = view.getHeight();
                            float mx = (x - (width / 2f));
                            float my = (height / 2f - y);
                            if(Math.abs(mx) > Math.abs(my)) {
                                angularZ = - mx / (width / 2f) / 0.5f;//Max 0.5
                            } else {
                                linearX = my / (height / 2f) / 0.5f;//Max 0.5
                            }
                            break;
                        case MotionEvent.ACTION_UP:
                            moving = false;
                            linearX = 0;
                            angularZ = 0;
                            break;
                    }
                    return true;
                }
            });
        }

  • 地图展示

    针对接收到来自于 /map Topic的数据,做展示处理,rviz您肯定用过

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    public void parseMapTopic(PublishEvent event) {
        try {
            JSONParser parser = new JSONParser();
            JSONObject jsonObj = (JSONObject) parser.parse(event.msg);
            JSONArray dataArray = (JSONArray)jsonObj.get("data");
            JSONObject jsonInfo = (JSONObject)jsonObj.get("info");
            int width = (int)(long)jsonInfo.get("width");
            int height = (int)(long)jsonInfo.get("height");
            //原理简单来说就是将data字段当作位图数据来生成图片,color请自便
            
            final Bitmap bitmap = Bitmap.createBitmap(width,height, Bitmap.Config.RGB_565);
            int len = dataArray.size();
            int x, y, d, p;
            for(int i = 0; i< len; i++) {
                x = i % width;
                y = i / width;
                d = (int)(long)dataArray.get(i);
                if(d == -1) {
                    bitmap.setPixel(x, y, Color.rgb(0x59, 0x59, 0x59));
                } else {
                    p = 0x59 + (int)((0xB1 - 0x59) * d / 100f);
                    bitmap.setPixel(x, y, Color.rgb(p,p,p));
                }
            }
            runOnUiThread(new Runnable() {
                @Override
                public void run() {
                    tvLog.setBackground(new BitmapDrawable(getResources(),bitmap));
                    tvLog.setText(tvLog.getText() + "Received map,set to background\n");
                }
            });
            return;
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

#后续功能

  • 通过蓝牙通信传递连接所需IP及端口信息,连接过程自动化
  • SLAM相关,包括不限与tf展示,发布目标点…
  • ROS与Android上层应用的交互,如接打电话,收发短信…