Android client and ORBSLAM2 in ROS environment

Compiling ORBSLAM-2 in ROS environment

ROS installation (Ubuntu 18.04)

ROS environment ready for installation

Construction of ORB-SLAM2 algorithm environment

  1. Create ROS workspace:
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
mkdir ORB-SLAM2
  1. Add environment variable:
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
  1. Install dependent libraries:
sudo apt-get install libblas-dev liblapack-dev

Compiling 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

Download Eigen. Note that version 3.2.10:

mkdir build
cd build
cmake ..
make
sudo make install

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
  1. Compile and 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: for the undefined error of uspleep() function - > solution, add:

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
  1. Using usb camera in ROS:
cd catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
cd ..
catkin_make
mkdir build
cd build
cmake ..
make

Find a replacement camera:
ls /dev/video*
cd /home/pan/catkin_ws/src/usb_cam
cd launch
gedit usb_cam-test.launch

Modify / dev/videoX to support cameras
8. Operation instruction:

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

Runtong Android client

Android client open source code

Android client source code: Github
Modify the address of build

PC receiving side code: Github
Ensure that the client and PC are in the same LAN, and modify the client connection ip address

How does RVIZ receive IMU and image data

  1. Receive IMU data:
    Ubuntu installs IMU-TOOLS with the same version as ROS
sudo apt-get install ros-melodic-imu-tools
 perhaps
sudo apt-get install ros-kinetic-imu-tools
Command on RIVZ Select receive after Add - By topic - add to imu
 take topic be elected/android/imu
 And in Global Options -> Fix Frame Lieutenant general map Change to imu. 

Reference operation:

Debug rviz and solve the problem "For frame [laser]: Fixed Frame [map] does not exist"!

  1. Receive image data
    First install the corresponding version of ROS accessories:
sudo apt-get install ros-indigo-image-view ros-indigo-rqt-image-view ros-indigo-image-transport-plugins 
perhaps
sudo apt-get install ros-melodic-image-view ros-melodic-rqt-image-view ros-melodic-image-transport-plugins

Start roscore at Terminal and view the existing subscription topics through the rostopic list
The default image transmission is compressed, so you need to convert the image to raw format
cd to Android_ Find Android in the camera IMU master folder_ Cam-imu.launch file, modify the subscription node as follows
Input instruction in Terminal: roslaunch android_cam-imu.launch
Add topic - > Image - > Image topic is set to / camera/image_raw

Camera calibration of Android camera

github source code
Instructions for camera calibration module
1. Input cheese in DASH, turn on the camera, shoot 10 ~ 20 calibrated chessboards from all angles, and be sure to change the camera attitude to reduce errors.
2. Copy the pictures taken by ccheese to camera_ In the calibration directory, be careful not to appear (. jpg named, which is a natural number, otherwise the picture may be overwritten).
3. Open the terminal and execute. / rename.sh to uniformly number the picture names into natural number sequences.
4. Execute. / camera at the terminal_ calibration boardWidth boardHeight squareSize frameNumber
boardWidth: the number of horizontal corners of the chessboard, such as 9
boardHeight: the number of vertical corner points of chessboard, such as 6
squareSize: the actual side length of each grid (required to be square) in the chessboard, unit: mm, such as 25
frameNumber: the number of pictures to be calculated, for example: 17

Sending and receiving data based on ROS protocol

Reference article: Know
On Android_ tutorial_ camera_ Talker and Listener Class are added under IMU project:
Listener.java listens for 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 to 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);
            }
        });
    }
}

Modify 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);
        }

        //Set pixel
        //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 terminal side
Instruction review: ROSwiki

Check information:
rostopic echo /chatter[topicName]
Check the sending rate:
rostopic hz /topic_name
 Send data:
rostopic pub my_topic std_msgs/String "hello there"

Problems encountered:
When viewing the rosgraph on the pc side, all connections are normal. When viewing the topics published by android ros node, messages are also output. However, when publishing messages to the topics subscribed by android ros node through the rostopic pub command on the pc side, debugging shows that the android side cannot receive this message. The problem is that ROS is not set on the pc side_ IP environment variable

That is, ROS needs to be set in advance in each terminal running node on the pc side_ The IP environment variable is the IP address of the pc

export ROS_IP=[your pc ip]

How to Reszie receive image size

  1. Add node to orb_ In slam2
    The orb in the workspace_ SLAM2/src/ORB_ SLAM2/Examples/ROS/ORB_ ROS in slam2 / SRC_ mono.cc
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);

The image called here is the original size of the camera, and the image size needs to be resized(),

Recompile ORBSLAM2, modify the internal parameters of Android camera to yaml file, and run the command:
Enter ROS_ mono.cc ->

  1. Add opencv domain: using namespace cv;
  2. void ImageGrabber::GrabImage() function, first convert the ROS image to CV format, and then use opencv Resize() function to modify the size again:
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());
}

Finally, recompile the file and run the instruction again:

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

Keywords: Android slam

Added by suma237 on Tue, 28 Sep 2021 11:00:03 +0300