基于ROS和安卓手机的IMU参数的通信软件

版权声明：本文为博主原创文章，未经博主允许不得转载。

标签（空格分隔）： rosjava android手机 IMU

---

前提是成功安装好ROS和android studio，并且能够成功运行android_core中实例。

本文采用的是在别人建好的app中新建一个ros android app,简单快速度的一种方法。

1、下载相关的app
注意要和你安装的ROS版本对应，我采用的是indigo版本。
https://github.com/rosjava/android_apps/tree/indigo

2、导入android_apps
在android studio 中导入下载好的android_apps,在弹出的是否更新build.gradle窗口选择【cancle】，选择其中一个moudle进行编译运行，成功后就可以新建自己需要的app

3、新建app
在android_core中新建moudle,最小api设置为10，选择empty Activity 的moudle。
(1) 将MainActivity继承RosActivity,点击RosActivity后使用【alt+enter】添加RosActivity的依赖库，接着导入包import org.ros.android.RosActivity;
(2) 重写init方法和一个结构体。先编写 rosjava发布者ImuPublisher,新建一个ImuPublisher.class类，具体代码如下：

public class ImuPublisher implements NodeMain {
    private ImuThread imuThread;
    private SensorListener sensorListener;
    private SensorManager sensorManager;
    private Publisher publisher;
    public ImuPublisher(SensorManager msensorManager) {
        this.sensorManager = msensorManager;
    }
    @Override
    public GraphName getDefaultNodeName() {
        return GraphName.of("android_sensors_driver/imuPublisher");
    }
    public interface NodeMain extends NodeListener {
        GraphName getDefaultNodeName();
    }
    @Override
    public void onStart(ConnectedNode connectedNode) {
        try
        {
            //主题为"android/imu"    消息类型为 "sensor_msgs/Imu" ,是标准类型的消息
            this.publisher = connectedNode.newPublisher("android/imu","sensor_msgs/Imu");
            int i = this.sensorManager.getSensorList(1).size();
            boolean hasAccel = false;
            if (i > 0)
                hasAccel = true;
            int j = this.sensorManager.getSensorList(4).size();
            boolean hasGyro = false;
            if (j > 0)
                hasGyro = true;
            int k = this.sensorManager.getSensorList(11).size();
            boolean hasQuat = false;
            if (k > 0)
                hasQuat = true;
            this.sensorListener = new SensorListener(publisher, hasAccel, hasGyro, hasQuat);
            this.imuThread = new ImuThread(this.sensorManager, sensorListener);
            this.imuThread.start();
        }catch (Exception e){
            if(connectedNode != null)
            {
                connectedNode.getLog().fatal(e);
            }else
            {
                e.printStackTrace();
            }
        }
    }
    @Override
    public void onShutdown(Node node) {
        this.imuThread.shutdown();
        try
        {
            this.imuThread.join();
            return;
        }
        catch (InterruptedException localInterruptedException)
        {
            localInterruptedException.printStackTrace();
        }
    }
    @Override
    public void onShutdownComplete(Node node) {
    }

    @Override
    public void onError(Node node, Throwable throwable) {
    }
    /**
     * run(){}注册传感器监听事件
     * shutdown(){}注销传感器监听事件
     */
    private class ImuThread extends Thread{
        private final SensorManager sensorManager;
        private ImuPublisher.SensorListener sensorListener;
        private Looper threadLooper;
        private final Sensor accelSensor;
        private final Sensor gyroSensor;
        private final Sensor quatSensor;
        private ImuThread(SensorManager sensorManager,ImuPublisher.SensorListener sensorListener){
            this.sensorManager = sensorManager;
            this.sensorListener = sensorListener;
            this.accelSensor =this.sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);//加速度(重力)传感器：
            this.gyroSensor =this.sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
            this.quatSensor =this.sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        }
        @Override
        public void run() {
            Looper.prepare();
            this.threadLooper =Looper.myLooper();
            this.sensorManager.registerListener(this.sensorListener,this.accelSensor,SensorManager.SENSOR_DELAY_UI);
            this.sensorManager.registerListener(this.sensorListener,this.gyroSensor,SensorManager.SENSOR_DELAY_UI);
            this.sensorManager.registerListener(this.sensorListener,this.quatSensor,SensorManager.SENSOR_DELAY_UI);
            Looper.loop();
        }
        public void shutdown()
        {
            this.sensorManager.unregisterListener(this.sensorListener);
            if(this.threadLooper !=null)
            {
                this.threadLooper.quit();
            }
        }
    }
    /**
     * 传感器事件监听
     * onAccuracyChanged(){}传感器精度发生变化进行监听
     * onSensorChanged(){}传感器数据发生变化进行监听
     */

    static class SensorListener implements SensorEventListener {
        private  long count = 0;
        private  Publisher publisher;
        private long accelTime;
        private long gyroTime;
        private long quatTime;
        private boolean hasAccel;
        private boolean hasGyro;
        private boolean hasQuat;
        private Imu imu;
        private SensorListener(Publisher publisher,boolean hasAccel,boolean hasGyro,boolean hasQuat){
            this.publisher = publisher;
            this.hasAccel = hasAccel;
            this.hasGyro = hasGyro;
            this.hasQuat = hasQuat;
            this.imu =this.publisher.newMessage();
        }
        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {
        }
        @Override
        public void onSensorChanged(SensorEvent event) {
            count ++;
            float[] values = event.values;
            StringBuilder sb = new StringBuilder();
            sb.append("\n传感器取值频率为:\n");
            sb.append(1000 / ((System.currentTimeMillis() - MainActivity.start_time) / count));
            Bundle b = new Bundle();
            b.putString("result", sb.toString());
            Message msg = new Message();
            msg.setData(b);
            MainActivity.handler.sendMessage(msg);
            //加速度
            this.imu.getLinearAcceleration().setX(event.values[0]);
            this.imu.getLinearAcceleration().setY(event.values[1]);
            this.imu.getLinearAcceleration().setZ(event.values[2]);
            double[] tmpCov1 = { 0.01D, 0.0D, 0.0D, 0.0D, 0.01D, 0.0D, 0.0D, 0.0D, 0.01D };
            this.imu.setLinearAccelerationCovariance(tmpCov1);
            this.accelTime = event.timestamp;
            //角速度
            this.imu.getAngularVelocity().setX(event.values[0]);
            this.imu.getAngularVelocity().setY(event.values[1]);
            this.imu.getAngularVelocity().setZ(event.values[2]);
            double[] tmpCov2 ={0.0025,0,0,0,0.0025,0,0,0,0.0025};// TODO Make Parameter
            this.imu.setAngularVelocityCovariance(tmpCov2);
            this.gyroTime = event.timestamp;
            //方向
            float[] quaternion = new float[4];
            SensorManager.getQuaternionFromVector(quaternion, event.values);
            this.imu.getOrientation().setW(quaternion[0]);
            this.imu.getOrientation().setX(quaternion[1]);
            this.imu.getOrientation().setY(quaternion[2]);
            this.imu.getOrientation().setZ(quaternion[3]);
            double[] tmpCov3 ={0.001,0,0,0,0.001,0,0,0,0.001};// TODO Make Parameter
            this.imu.setOrientationCovariance(tmpCov3);
            this.quatTime = event.timestamp;
            //求取获取传感器参数的频率
            long time_delta_millis =System.currentTimeMillis()- SystemClock.uptimeMillis();
            this.imu.getHeader().setStamp(Time.fromMillis(time_delta_millis + event.timestamp/1000000));
            this.imu.getHeader().setFrameId("/imu");// TODO Make parameter
            //前面组装消息    后面发布消息
            publisher.publish(this.imu);
            // Create a new message ，清空了this.imu
            this.imu =this.publisher.newMessage();
            this.accelTime =0L;
            this.gyroTime =0L;
            this.quatTime =0L;
        }
    }
}

(3)编写MainActivity.class,见如下代码：

public class MainActivity extends RosActivity {
    private ImuPublisher imu_pub;
    private SensorManager msensorManager;
    private EditText et;
    public static Handler handler;
    public static long start_time;
    public MainActivity() {
        super("android_sensors_driver", "android_sensors_driver");
    }
    @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);
        start_time = System.currentTimeMillis();
        et = (EditText)findViewById(R.id.et);
        //获取系统的传感器管理服务
        msensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
        handler = new Handler(){
            @Override
            public void handleMessage(Message msg) {
                Bundle bundle = msg.getData();
                txt1.setText(bundle.get("result").toString());
                super.handleMessage(msg);
            }
        };
    }
    @Override
    protected void onResume() {
        super.onResume();
    }
    //是从父类继承过来的，父类再启动中会startServic
    @Override
    protected void init(final NodeMainExecutor nodeMainExecutor) {
        NodeConfiguration nodeConfiguration1 = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
        nodeConfiguration1.setMasterUri(getMasterUri());
        nodeConfiguration1.setNodeName("android_sensors_driver");
        this.imu_pub = new ImuPublisher(msensorManager);
        nodeMainExecutor.execute(this.imu_pub,nodeConfiguration1);
    }
    @Override
    public boolean onTouchEvent(MotionEvent event) {
        return true;
    }
}

(4)android studio 的布局文件如下:

<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:tools="http://schemas.android.com/tools" android:id="@+id/activity_main"
    android:layout_width="match_parent" android:layout_height="match_parent"
    android:paddingBottom="@dimen/activity_vertical_margin"
    android:paddingLeft="@dimen/activity_horizontal_margin"
    android:paddingRight="@dimen/activity_horizontal_margin"
    android:paddingTop="@dimen/activity_vertical_margin"
    tools:context="com.example.betty.app.MainActivity">
    <EditText
        android:id="@+id/et"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:layout_centerHorizontal="true"
        android:layout_centerVertical="true"
        android:hint="test"
        android:text="hello_world"
        tools:context=".MainActivity" />
RelativeLayout>

(5) 功能清单文件如下：AndroidManifest.xml
“`

rostopic echo [ /android/imu]

在终端会显示加速度、角速度、方向的三个传感器参数。
至此，ROS和安卓手机的IMU参数的通信完成