DiegoAPP–機器人控制

機器人的控制主要涉及到如下幾個文件

ControlApp，在連接到Robot後的主界面，執行控制任務，及執行各種功能的Fragment
RobotController，主要的機器人控制代碼，在此文件中定義了要接收發送的ROS Topic，以及接收後的處理方式
Fragement，執行各主要功能的的Fragment，通過ControlApp進行切換

1. RobotController主要功能說明

   進入RobotController後，表示APP已經和機器人連接，可以通過APP對機器人進行控制，對機器人狀態進行監控，主要功能如下：

ODOM信息的監控，機器的線速度，和角速度的監控
GPS信息的監控，顯示當前GPS定位信息
監控機器人的電池電量
接收顯示，激光雷達，雙攝像頭，地圖的信息
發送cmd_vel信息，通過遙控杆，手機重力感應來控制
高德地圖，通過接收機器人發送來的GPS信息，在高德地圖上實時顯示

2. ControllApp代碼主要邏輯說明
ControllApp繼承自RosActivity，並實現了ListView.OnItemClickListener接口，聲明瞭Fragment，Robotinfo，RobotController等變量，變量聲明部分代碼如下：

public class ControlApp extends RosActivity implements ListView.OnItemClickListener,
        IWaypointProvider, AdapterView.OnItemSelectedListener {

    /** Notification ticker for the App */
    public static final String NOTIFICATION_TICKER = "Diego Robot";
    /** Notification title for the App */
    public static final String NOTIFICATION_TITLE = "Diego Robot";

    /** The RobotInfo of the connected Robot */
    public static RobotInfo ROBOT_INFO;

    // Variables for managing the DrawerLayout
    private String[] mFeatureTitles;
    private DrawerLayout mDrawerLayout;
    private ListView mDrawerList;
    private ActionBarDrawerToggle mDrawerToggle;

    // NodeMainExecutor encapsulating the Robot's connection
    private NodeMainExecutor nodeMainExecutor;
    // The NodeConfiguration for the connection
    private NodeConfiguration nodeConfiguration;

    // Fragment for the Joystick
    private JoystickFragment joystickFragment;
    // Fragment for the HUD
    private HUDFragment hudFragment;


    // The RobotController for managing the connection to the Robot
    private RobotController controller;
    // The WarningSystem used for detecting imminent collisions
    private WarningSystem warningSystem;

    // Stuff for managing the current fragment
    private Fragment fragment = null;
    FragmentManager fragmentManager;
    int fragmentsCreatedCounter = 0;

    private int currentfragment=0;

    // For enabling/disabling the action menu
//    private boolean actionMenuEnabled = true;
    // The ActionBar spinner menu
    private Spinner actionMenuSpinner;

    // The index of the currently visible drawer
    private int drawerIndex = 1;

    // Log tag String
    private static final String TAG = "ControlApp";

    // List of waypoints
    private final LinkedList waypoints;
    // Specifies how close waypoints need to be to be considered touching
    private static final double MINIMUM_WAYPOINT_DISTANCE = 1.0;

    // Laser scan map // static so that it doesn't need to be saved/loaded every time the screen rotates
    private static LaserScanMap laserScanMap;



    // Bundle keys
    private static final String WAYPOINT_BUNDLE_ID = "com.diegorobot.app.waypoints";
    private static final String SELECTED_VIEW_NUMBER_BUNDLE_ID = "com.diegorobot.app.drawerIndex";
    private static final String CONTROL_MODE_BUNDLE_ID = "com.diegorobot.app.Views.Fragments.JoystickFragment.controlMode";

    // The saved instance state
    private Bundle savedInstanceState;

在ControlApp變量聲明部分，主要的變量有：

ROBOT_INFO, 連接的機器人信息類，從保存的機器人數據中讀取，保存修改的機器人信息
mFeatureTitles加載左側滑動菜單的內容，詳細的定義在資源文件strings.xml中描述

nodeMainExecutor，連接ROSMaster的Main node，這個是RosJava連接RosMaster必須的變量，通過此變量連接Master
joystickFragment，遙控杆Fragement
hudFragment，擡頭顯示的Fragment，顯示ODOM, GPS, BATTERY,已經急停按鈕
controller，機器人的控制變量， 通過接收和發送RosTopic來控制機器
fragment，當前界面顯示Fragment

onCreate()函數初始化了相關的變量，和Android APP一般的onCreate函數沒有什麼太大的區別，讀者如有疑惑，請學習Android App的開放教程，這裏不在說明

這裏主要說明一下selectItem()函數，這個函數在單擊事件中被調用，用於切換Fragement，代碼如下：

    private void selectItem(int position) {

        Bundle args = new Bundle();

        if (joystickFragment != null && getControlMode().ordinal() <= ControlMode.Tilt.ordinal()) {
            joystickFragment.show();
        }

        if (hudFragment != null) {
            hudFragment.show();
        }

        if (controller != null) {
            controller.initialize();
        }
        fragmentManager = getFragmentManager();

        setActionMenuEnabled(true);

        switch (position) {
            case 0:
                Log.d(TAG, "Drawer item 0 selected, finishing");

                fragmentsCreatedCounter = 0;
                currentfragment=0;

                int count = fragmentManager.getBackStackEntryCount();
                for (int i = 0; i < count; ++i) {
                    fragmentManager.popBackStackImmediate();
                }

                if (controller != null) {
                    controller.shutdownTopics();

                    new AsyncTask<Void, Void, Void>() {
                        @Override
                        protected Void doInBackground(Void... params) {
                            nodeMainExecutor.shutdownNodeMain(controller);
                            return null;
                        }
                    }.execute();
                }

                finish();

                return;

            case 1:
                fragment = new OverviewFragment();
                fragmentsCreatedCounter = 0;
                currentfragment=1;
                break;

            case 2:
                //fragment = new CameraViewFragment();
                fragment=new CameraTopViewFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=2;
                break;

            case 3:
                //fragment = new CameraViewFragment();
                fragment=new Camera1TopViewFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=3;
                break;

            case 4:
                //fragment = new CameraViewFragment();
                fragment=new CameraDoubleViewFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=4;
                break;

            case 5:
                fragment = new LaserScanFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=5;
                break;
            case 6:
                fragment = new LaserScanMapFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=6;
                break;
            case 7:
                fragment = new MapFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
               currentfragment=7;
                break;

            case 8://modify by william
                if (joystickFragment != null)
                    joystickFragment.hide();
                if (hudFragment != null) {
                    hudFragment.hide();

                    boolean stop = controller.getMotionPlan() == null || !controller.getMotionPlan().isResumable();
                    stop &= !controller.hasPausedPlan();
                    hudFragment.toggleEmergencyStopUI(stop);
                }

                setActionMenuEnabled(false);
                stopRobot(false);

                fragment = new PreferencesFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=8;
                break;

            case 9:
                if (joystickFragment != null)
                    joystickFragment.hide();
                if (hudFragment != null) {
                    hudFragment.hide();

                    boolean stop = controller.getMotionPlan() == null || !controller.getMotionPlan().isResumable();
                    stop &= !controller.hasPausedPlan();
                    hudFragment.toggleEmergencyStopUI(stop);
                }

                setActionMenuEnabled(false);
                stopRobot(false);

                fragment = new AboutFragment();
                fragmentsCreatedCounter = fragmentsCreatedCounter + 1;
                currentfragment=9;

            default:
                break;
        }

        drawerIndex = position;

        try {
            //noinspection ConstantConditions
            ((RosFragment) fragment).initialize(nodeMainExecutor, nodeConfiguration);
        } catch (Exception e) {
            // Ignore
        }

        if (fragment != null) {
            fragment.setArguments(args);

            // Insert the fragment by replacing any existing fragment
            fragmentManager.beginTransaction()
                    .replace(R.id.content_frame, fragment)
                    .commit();

            if (fragment instanceof Savable && savedInstanceState != null)
                ((Savable) fragment).load(savedInstanceState);
        }

        // Highlight the selected item, update the title, and close the drawer
        mDrawerList.setItemChecked(position, true);
        mDrawerLayout.closeDrawer(mDrawerList);
        setTitle(mFeatureTitles[position]);

        // Refresh waypoints
        new AsyncTask<Void, Void, Void>() {
            @Override
            protected Void doInBackground(Void... params) {
                try {
                    Thread.sleep(100L);
                } catch (InterruptedException ignore) {}
                waypointsChanged();
                return null;
            }
        }.execute();
    }

此段代碼根據用戶點擊菜單的item來選擇不同的Fragment並初始化,然後切換主界面的Fragment;後續章節將詳細介紹主要的Fragment.

3. RobotController 代碼主要邏輯講解

   ROS機器人的監控，主要是通過發送和接收RosTopic來實現的，所以在RobotController也主要是實現RosTopic的功能，在變量聲明部分爲每種要處理的topic都定義了相關的訂閱變量，如下圖
   
   圖中紅色框爲定義的Twist消息發佈變量，綠色框是定義的訂閱的消息變量，讀者可以根據自己的需求來訂閱不同的Topic，擴展APP功能
   消息的處理主要在refreshTopics()函數中處理，這段代碼初始化了消息訂閱和發佈的邏輯，由於代碼太長就不全貼出來，只把主要的代碼邏輯貼出來說明

        // Get the correct topic names
        String moveTopic = PreferenceManager.getDefaultSharedPreferences(context)
                .getString(context.getString(R.string.prefs_joystick_topic_edittext_key),
                        context.getString(R.string.joy_topic));

        String navSatTopic = PreferenceManager.getDefaultSharedPreferences(context)
                .getString(context.getString(R.string.prefs_navsat_topic_edittext_key),
                        context.getString(R.string.navsat_topic));

        String batteryTopic = PreferenceManager.getDefaultSharedPreferences(context)
                .getString(context.getString(R.string.prefs_battery_topic_edittext_key),
                        context.getString(R.string.battery_topic));

這段代碼，從配置信息中讀取Topic的名稱

        // Refresh the Move Publisher
        if (movePublisher == null
                || !moveTopic.equals(movePublisher.getTopicName().toString())) {

            if (publisherTimer != null) {
                publisherTimer.cancel();
            }

            if (movePublisher != null) {
                movePublisher.shutdown();
            }

            // Start the move publisher
            movePublisher = connectedNode.newPublisher(moveTopic, Twist._TYPE);
            currentVelocityCommand = movePublisher.newMessage();

            publisherTimer = new Timer();
            publisherTimer.schedule(new TimerTask() {
                @Override
                public void run() { if (publishVelocity) {
                    movePublisher.publish(currentVelocityCommand);
                }
                }
            }, 0, 80);
            publishVelocity = false;
        }

這段代碼初始化了Twist的發佈邏輯，通過定時器來不斷髮送當前的Twist消息

        // Refresh the NavSat Subscriber
        if (navSatFixSubscriber == null
                || !navSatTopic.equals(navSatFixSubscriber.getTopicName().toString())) {

            if (navSatFixSubscriber != null)
                navSatFixSubscriber.shutdown();

            // Start the NavSatFix subscriber
            navSatFixSubscriber = connectedNode.newSubscriber(navSatTopic, NavSatFix._TYPE);
            navSatFixSubscriber.addMessageListener(new MessageListener() {
                @Override
                public void onNewMessage(NavSatFix navSatFix) {
                    setNavSatFix(navSatFix);
                }
            });
        }

這段代碼初始化了GPS Topic的訂閱邏輯，收到GPSTopic後，在通過Android的消息機制來傳遞，只要需要用到GPS Topic的代碼實現此消息接口，便可實施活動GPS信息，並處理。