[移动开发]ROS环境下Android客户端与ORBSLAM2

ROS环境下编译ORBSLAM-2

ROS安装(Ubuntu18.04)

ROS环境准备安装

ORB-SLAM2 算法环境搭建

1. 创建ROS workspace:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
mkdir ORB-SLAM2

2. 添加环境变量:

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

3. 安装依赖库：

sudo apt-get install libblas-dev liblapack-dev

编译Pangolin:

sudo apt-get install libglew-dev
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install libpython2.7-dev
sudo apt-get install build-essential
cd ~/catkin_ws/ORB-SLAM2
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake -DCPP11_NO_BOOST = 1..
make

下载Eigen 注意是3.2.10版本:

mkdir build
cd build
cmake ..
make
sudo make install

安装OpenCV:

sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev

mkdir release
cd release
cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local ..
make
sudo make install

5. 编译安装ORB_SLAM2:

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/(user)/catkin_ws/ORB-SLAM2/Examples/ROS

cd ORB_SLAM2
chmod +x build_ros.sh
./build_ros.sh

error:uspleep()函数未定义的错误->解决方法，在报错代码文件加:

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

6. 在ROS中使用usb摄像头:

cd catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
cd ..
catkin_make
mkdir build
cd build
cmake ..
make

查找替换摄像头：
ls /dev/video*
cd /home/pan/catkin_ws/src/usb_cam
cd launch
gedit usb_cam-test.launch

修改/dev/videoX 为支持摄像头
8. 运行指令:

roscore
roslaunch usb_cam usb_cam-test.launch
rosrun ORB_SLAM2 Mono /home/(user)/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/(user)/catkin_ws/src/ORB_SLAM2/Examples/Monocular/MyCam.yaml

跑通Android客户端

Android客户端开源代码

Android 客户端源代码：Github
修改build的地址

PC接收侧代码：Github
保证客户端和PC在同一局域网下，修改客户端连接ip地址

RVIZ 如何接收IMU 和图像数据

1. 接收IMU 数据：
   ubutun安装和ROS版本一致的IMU-TOOLS

sudo apt-get install ros-melodic-imu-tools
或者
sudo apt-get install ros-kinetic-imu-tools

指令打开RIVZ后选择接收Add - By topic - 添加 imu
将topic选为/android/imu
且在 Global Options -> Fix Frame 中 将 map 改为 imu。

参考操作：

调试rviz，并解决问题“For frame [laser]: Fixed Frame [map] does not exist”!

2. 接收图像数据
   首先安装对应版本的ROS配件：

sudo apt-get install ros-indigo-image-view ros-indigo-rqt-image-view ros-indigo-image-transport-plugins 
或者
sudo apt-get install ros-melodic-image-view ros-melodic-rqt-image-view ros-melodic-image-transport-plugins

在Terminal 先启动roscore 在通过rostopic list 查看已有的订阅主题
默认的image传输为compressed，因此需要做将image转换为raw格式
cd到Android_Camera-IMU-master文件夹下找android_cam-imu.launch文件，修改其中的订阅节点如下
Terminal中输入指令：roslaunch android_cam-imu.launch
Add Topic -> image -> Image Topic 设置为/camera/image_raw

Android摄像头的相机标定

github源码
相机标定模块使用说明
1. 在DASH中输入cheese打开相机，从各个角度拍摄标定棋盘10~20张，一定要变换相机姿态，减少误差。
2. 将ccheese拍摄到的图片拷贝到camera_calibration目录下，注意不要出现（.jpg命名，其中为自然数字，否则图片有可能被覆盖）。
3. 打开终端，执行./rename.sh讲图片名统一编号为自然数序列。
4. 在终端执行./camera_calibration boardWidth boardHeight squareSize frameNumber
boardWidth :棋盘横向角点数目 如：9
boardHeight :棋盘纵向角点数目 如：6
squareSize ：棋盘中每个格子（要求是正方形）的实际边长，单位：mm 如：25
frameNumber：要计算的图片数量 如：17

基于ROS协议的收发数据

参考文章：知乎
在android_tutorial_camera_imu 工程下增加Talker和Listener Class:
Listener.java监听rmytopic

package org.ros.android.android_tutorial_camera_imu;

//import org.apache.commons.logging.Log;
import android.util.Log;

import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.topic.Subscriber;

public class Listener extends AbstractNodeMain {

    @Override
    public GraphName getDefaultNodeName() {
        return GraphName.of("rosjava_tutorial_pubsub/listener");
    }

    public void onStart(ConnectedNode connectedNode)
    {
        //final Log log = connectedNode.getLog();
        Log.d("listener", "Node Log "+ connectedNode.getLog() + " ");
        Subscriber<std_msgs.String> subscriber = connectedNode.newSubscriber("rmytopic", std_msgs.String._TYPE);
        subscriber.addMessageListener(new MessageListener<std_msgs.String>() {
            @Override
            public void onNewMessage(std_msgs.String message) {
                Log.d("listener", "I heard: \"" + message.getData() + "\"");

            }
        });
    }
}

Talker.java发送至chatter

package org.ros.android.android_tutorial_camera_imu;


import org.ros.concurrent.CancellableLoop;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;

/**
 * A simple {@link Publisher} {@link NodeMain}.
 *
 * @author damonkohler@google.com (Damon Kohler)
 */
public class Talker extends AbstractNodeMain {


    @Override
    public GraphName getDefaultNodeName() {
        return GraphName.of("rosjava_tutorial_pubsub/talker");
    }

    @Override
    public void onStart(final ConnectedNode connectedNode) {
        final Publisher<std_msgs.String> publisher =
                connectedNode.newPublisher("chatter", std_msgs.String._TYPE);
        // This CancellableLoop will be canceled automatically when the node shuts
        // down.
        connectedNode.executeCancellableLoop(new CancellableLoop() {

            @Override
            protected void setup() {
            }

            @Override
            public void loop() throws InterruptedException {
                std_msgs.String str = publisher.newMessage();
                str.setData("Hello ROS from client");
                publisher.publish(str);
                Thread.sleep(25000);
            }
        });
    }
}

修改MainActivity.java

/*
 * Copyright (C) 2011 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
package org.ros.android.android_tutorial_camera_imu;

import android.Manifest;
import android.annotation.TargetApi;
import android.content.Context;
import android.content.pm.PackageManager;
import android.hardware.Camera;
import android.hardware.SensorManager;
import android.location.LocationManager;
import android.os.Build;
import android.os.Bundle;
import android.support.v4.app.ActivityCompat;
import android.util.Log;
import android.view.MotionEvent;
import android.view.Window;
import android.view.WindowManager;
import android.widget.Toast;
import org.ros.address.InetAddressFactory;
import org.ros.android.RosActivity;
import org.ros.android.view.camera.RosCameraPreviewView;
import org.ros.node.NodeConfiguration;
import org.ros.node.NodeMainExecutor;
import java.util.List;

import org.ros.android.MessageCallable;
import org.ros.android.view.RosTextView;
import org.ros.android.android_tutorial_camera_imu.Talker;

/**
 * @author ethan.rublee@gmail.com (Ethan Rublee)
 * @author damonkohler@google.com (Damon Kohler)
 * @author huaibovip@gmail.com (Charles)
 */

public class MainActivity extends RosActivity {

    private int cameraId = 0;
    private RosCameraPreviewView rosCameraPreviewView;
    private NavSatFixPublisher fix_pub;
    private ImuPublisher imu_pub;

    private NodeMainExecutor nodeMainExecutor;
    private LocationManager mLocationManager;
    private SensorManager mSensorManager;

    private RosTextView<std_msgs.String> rosTextView;
    private Talker talker;
    private Listener listener;
    public MainActivity() {
        super("ROS", "Camera & Imu");
    }

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);

        requestWindowFeature(Window.FEATURE_NO_TITLE);
        getWindow().addFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN);
        setContentView(R.layout.activity_main);

        rosCameraPreviewView = findViewById(R.id.ros_camera_preview_view);
        mLocationManager = (LocationManager) this.getSystemService(Context.LOCATION_SERVICE);
        mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
        rosTextView = (RosTextView<std_msgs.String>)findViewById(R.id.text);
        rosTextView.setTopicName("rmytopic");
        rosTextView.setMessageType(std_msgs.String._TYPE);

        rosTextView.setMessageToStringCallable(new MessageCallable<String, std_msgs.String>() {
            @Override
            public String call(std_msgs.String message) {
                return message.getData();
            }
        });
    }

    @Override
    public boolean onTouchEvent(MotionEvent event) {
        if (Build.VERSION.SDK_INT < Build.VERSION_CODES.P) {
            if (event.getAction() == MotionEvent.ACTION_UP) {
                int numberOfCameras = Camera.getNumberOfCameras();
                final Toast toast;
                if (numberOfCameras > 1) {
                    cameraId = (cameraId + 1) % numberOfCameras;
                    rosCameraPreviewView.releaseCamera();
                    rosCameraPreviewView.setCamera(getCamera());

                    toast = Toast.makeText(this, "Switching cameras.", Toast.LENGTH_SHORT);
                } else {
                    toast = Toast.makeText(this, "No alternative cameras to switch to.", Toast.LENGTH_SHORT);
                }
                runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                        toast.show();
                    }
                });
            }
        }
        return true;
    }

    @Override @TargetApi(Build.VERSION_CODES.ICE_CREAM_SANDWICH_MR1) //API = 15
    protected void init(NodeMainExecutor nodeMainExecutor) {
        this.nodeMainExecutor = nodeMainExecutor;

        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) {
            String[] PERMISSIONS = {"", "", "", ""};
            PERMISSIONS[0] = Manifest.permission.ACCESS_FINE_LOCATION;
            PERMISSIONS[1] = Manifest.permission.CAMERA;
            PERMISSIONS[2] = Manifest.permission.READ_EXTERNAL_STORAGE;
            PERMISSIONS[3] = Manifest.permission.WRITE_EXTERNAL_STORAGE;
            ActivityCompat.requestPermissions(this, PERMISSIONS, 0);
        }else {
            NodeConfiguration nodeConfiguration1 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
            nodeConfiguration1.setMasterUri(getMasterUri());
            nodeConfiguration1.setNodeName("android_sensors_driver_nav_sat_fix");
            this.fix_pub = new NavSatFixPublisher(mLocationManager);
            nodeMainExecutor.execute(this.fix_pub, nodeConfiguration1);

            rosCameraPreviewView.setCamera(getCamera());
            NodeConfiguration nodeConfiguration2 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
            nodeConfiguration2.setMasterUri(getMasterUri());
            nodeConfiguration2.setNodeName("android_sensors_driver_camera");
            nodeMainExecutor.execute(this.rosCameraPreviewView, nodeConfiguration2);
        }

        NodeConfiguration nodeConfiguration3 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration3.setMasterUri(getMasterUri());
        nodeConfiguration3.setNodeName("android_sensors_driver_imu");
        this.imu_pub = new ImuPublisher(mSensorManager);
        nodeMainExecutor.execute(this.imu_pub, nodeConfiguration3);
        talker = new Talker();
        listener = new Listener();
        NodeConfiguration nodeConfiguration4 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration4.setMasterUri(getMasterUri());
        NodeConfiguration nodeConfiguration5 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration5.setMasterUri(getMasterUri());
        nodeMainExecutor.execute(talker, nodeConfiguration4);
        nodeMainExecutor.execute(listener,nodeConfiguration5);
        nodeMainExecutor.execute(rosTextView,nodeConfiguration4);

    }

    private void executeGPS() {
        NodeConfiguration nodeConfiguration1 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration1.setMasterUri(getMasterUri());
        nodeConfiguration1.setNodeName("android_sensors_driver_nav_sat_fix");
        this.fix_pub = new NavSatFixPublisher(mLocationManager);
        nodeMainExecutor.execute(this.fix_pub, nodeConfiguration1);
    }

    private void executeCamera() {
        rosCameraPreviewView.setCamera(getCamera());
        NodeConfiguration nodeConfiguration2 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration2.setMasterUri(getMasterUri());
        nodeConfiguration2.setNodeName("android_sensors_driver_camera");
        nodeMainExecutor.execute(this.rosCameraPreviewView, nodeConfiguration2);
    }

    private Camera getCamera() {
        Camera cam = Camera.open(cameraId);
        cam.stopPreview();
        Camera.Parameters camParams = cam.getParameters();
        camParams.setPreviewSize(720,480);
        camParams.setPictureSize(720,480);
        List<Camera.Size> supportedPreviewSizes = cam.getParameters().getSupportedPreviewSizes();
        List<Camera.Size> pictureSizes = cam.getParameters().getSupportedPictureSizes();
        int length = pictureSizes.size();
        for (int i = 0; i < length; i++) {
            Log.d("SupportedSizes","SupportedPictureSizes : " + pictureSizes.get(i).width + "x" + pictureSizes.get(i).height);
        }
        length = supportedPreviewSizes.size();
        for (int i = 0; i < length; i++) {
            Log.d("SupportedSizes","SupportedPreviewSizes : " + supportedPreviewSizes.get(i).width + "x" + supportedPreviewSizes.get(i).height);
        }

        //设置像素
        //camParams.setPreviewSize(1280,720);
        //camParams.setPictureSize(1280,720);

        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.ICE_CREAM_SANDWICH) {
            if (camParams.getSupportedFocusModes().contains(
                    Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO)) {
                camParams.setFocusMode(Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO);
            } else {
                camParams.setFocusMode(Camera.Parameters.FOCUS_MODE_CONTINUOUS_PICTURE);
            }
        }
        cam.setParameters(camParams);
        cam.startPreview();
        return cam;
    }

    @Override
    public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] grantResults) {
        // If request is cancelled, the result arrays are empty.
        if (requestCode == 0) {
            if (grantResults.length > 0 && grantResults[0] == PackageManager.PERMISSION_GRANTED) {
                // permission was granted, yay! Do the
                executeGPS();
            }
            if (grantResults.length > 1 && grantResults[1] == PackageManager.PERMISSION_GRANTED) {
                // permission was granted, yay! Do the
                executeCamera();
            }

            if (grantResults.length > 2 && grantResults[2] == PackageManager.PERMISSION_GRANTED &&
                    grantResults[3] == PackageManager.PERMISSION_GRANTED) {
                // permission was granted, yay! Do the
            }
        }
    }



}

PC终端侧
指令查阅：ROSwiki

查信息：
rostopic echo /chatter[topicName]
查发送速率：
rostopic hz /topic_name
发送数据：
rostopic pub my_topic std_msgs/String "hello there"

遇到问题：
pc端查看rosgraph图，一切连接正常，查看android ros node 发布的话题，也有消息输出，但是当通过pc端使用rostopic pub 指令向android ros node 订阅的话题发布消息时，调试来看，android 端接收不到这个消息，问题在于没有在pc端设置ROS_IP环境变量

即需要在pc端每一个运行node的terminal中事先要设置ROS_IP环境变量为pc的IP地址

export ROS_IP=[your pc ip]

如何Reszie接收图像的大小

1. 添加node节点至ORB_SLAM2中
   将工作空间中的ORB_SLAM2/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src中的ros_mono.cc

ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);

这里调用的为相机原尺寸的图像，需要对图像大小resize(),

重新编译ORBSLAM2, 并将Android相机内参修改yaml文件，运行指令：
进入ros_mono.cc ->

1. 添加opencv域:using namespace cv;
2. void ImageGrabber::GrabImage()函数,先将ROS图像转为CV格式，再利用opencv Resize()函数重新修改大小：

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptr;
    cv::Mat img;
    cv::Mat img_resize;
    try
    {
        cv_ptr = cv_bridge::toCvShare(msg);
        img = cv_ptr->image;
        resize(img, img_resize, cv::Size(720, 480));
        //circle(img,cvPoint(150,100),10,CV_RGB(0,255,255),2,8,0); 
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    //mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
    mpSLAM->TrackMonocular(img_resize,cv_ptr->header.stamp.toSec());
}

最后，重新编译文件，再次运行指令：

rosrun ORB_SLAM2 Mono '/home/xujiayi/Desktop/ORBvoc.bin' '/home/xujiayi/catkin_ws/ORB_SLAM2/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2/Asus.yaml'