ROS環境下Android客戶端與ORBSLAM2

ROS環境下編譯ORBSLAM-2
- ROS安裝(Ubuntu18.04)
- ORB-SLAM2 演算法環境搭建
跑通Android客戶端
- Android客戶端開源代碼
- RVIZ 如何接收IMU 和影像資料
- Android攝像頭的相機標定
- 基于ROS協議的收發資料
如何Reszie接收影像的大小

ROS環境下編譯ORBSLAM-2

ROS安裝(Ubuntu18.04)

ROS環境準備安裝

ORB-SLAM2 演算法環境搭建

創建ROS workspace:

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

添加環境變數:

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

安裝依賴庫：

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

編譯安裝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>

在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 和影像資料

接收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”!

接收影像資料
首先安裝對應版本的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接收影像的大小

添加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 ->

添加opencv域:using namespace cv;
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'

轉載請註明出處，本文鏈接：https://www.uj5u.com/yidong/304351.html

標籤：其他

上一篇：擁有社交牛逼癥的我，還是覺得位元組面試好難呀

下一篇：關于Unity ShareSDK 打包遇到的問題和官方檔案部分細節講解