ros和Android

ros基本要素：

1.节点 ：节点与节点用tcp/ip通信

2.节点管理器 ：（相当于域名解析器）所有节点的枢纽,节点之间要实现互相通信，都要通过节点管理器

3.消息 ：

4.参数服务器  ：

5.主题 ：某一类的节点通信 例如：scan主题,所有扫描的订阅和发布都可以放在这个主题上

6.服务 ：

7.消息记录包：

8.订阅：

9.发布：

 

 

c++中的

例子提取出的步骤

1.创建消息和服务：

消息文件（.msg）一般带Header+基本类型数据+消息文件类型

服务文件（.srv）请求+响应 ---划分，

请求，响应也是由基本类型组成

 

 

// This header defines the standard ROS classes .    #include
       
                int main (int argc ,char** argv ){    // Initialize the ROS system .     ros::init ( argc , argv ," hello _ros ");       // Establ ish this program as a ROS node .     ros::NodeHandle nh ;        // Send some output as a log message .     ROS_INFO_STREAM(" Hello , ␣ ROS! ");    }
       

 

 

c++发布订阅端程序

// This program publishes randomly−generated velocity    // messages for turtlesim .    #include
       
            #include
        
         // For geometry_msgs:: Twist    #include
         
          // For rand() and RAND_MAX        int main (int argc ,char** argv ){    // Initialize the ROS system and become a node .     ros::init ( argc , argv ," publish _ velocity ");     ros::NodeHandle nh ;        // Create a publisher obj ect .     ros::Publisher pub = nh . advertise 
          
           (    " turtle1 /cmd_vel",1000);        // Seed the random number generator .     srand ( time (0));        // Loop at 2Hz until the node is shut down.     ros::Raterate(2);    while( ros::ok ()){    // Create and fill in the message . The other four    // fields , which are ignored by turtl esim , default to 0.     geometry_msgs ::Twist msg ;     msg . linear . x =double( rand ())/double(RAND_MAX);     msg.angular.z =2*double( rand ())/double(RAND_MAX)−1;        // Publish the message .     pub . publish ( msg );        // Send a message to rosout with the details .     ROS_INFO_STREAM("Sending random velocity command : "    <<" linear="<< msg.linear.x    <<" angular="<< msg.angular.z);        // Wait untilit's time for another iteration .     rate.sleep ();    }    }    表3
          
         
        
       

 

订阅者的程序

// This program subscribes to turtle1/pose and shows its    // messages on the screen .    #include
       
            #include
        
             #include
         
          // for std::setprecision and std::fixed        // A callback function . Executed each time a new pose    // message arrives .    void poseMessageReceived (const turtlesim::Pose& msg ){     ROS_INFO_STREAM( std::setprecision (2)<< std::fixed    <<" position =("<< msg . x <<" , "<< msg . y <<" ) "    <<" *direction="<< msg . theta );    }        int main (int argc ,char** argv ){    // Initialize the ROS system and become a node .     ros::init ( argc , argv ," subscribe_to _pose ");     ros::NodeHandle nh ;       // Create a subscri ber obj ect .     ros::Subscriber sub = nh.subscribe (" turtle1/pose ",1000,    &poseMessageReceived );        // Let ROS take over .     ros::spin ();    }
         
        
       

 

 

 

 

 

rosjava发布者的一般写法

 

/*** 写Publisher必须继承NodeMain  重写NodeMain的getDefaultNodeName()*/publicclassImuPublisherimplementsNodeMain{privateImuThread imuThread;privateSensorListener sensorListener;privateSensorManager sensorManager;privatePublisher
      
        publisher;privateclassImuThreadextendsThread{privatefinalSensorManager sensorManager;privateSensorListener sensorListener;privateLooper threadLooper;privatefinalSensor accelSensor;privatefinalSensor gyroSensor;privatefinalSensor quatSensor;privateImuThread(SensorManager sensorManager,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);}publicvoid run(){Looper.prepare();this.threadLooper =Looper.myLooper();this.sensorManager.registerListener(this.sensorListener,this.accelSensor,SensorManager.SENSOR_DELAY_FASTEST);this.sensorManager.registerListener(this.sensorListener,this.gyroSensor,SensorManager.SENSOR_DELAY_FASTEST);this.sensorManager.registerListener(this.sensorListener,this.quatSensor,SensorManager.SENSOR_DELAY_FASTEST);Looper.loop();}publicvoid shutdown(){this.sensorManager.unregisterListener(this.sensorListener);if(this.threadLooper !=null){this.threadLooper.quit();}}}privateclassSensorListenerimplementsSensorEventListener{privatePublisher
       
         publisher;privateImu imu;privateSensorListener(Publisher
        
          publisher,boolean hasAccel,boolean hasGyro,boolean hasQuat){this.publisher = publisher;...this.imu =this.publisher.newMessage();}// @Overridepublicvoid onSensorChanged(SensorEvent event){this.imu.getLinearAcceleration().setX(event.values[0]);this.imu.getLinearAcceleration().setY(event.values[1]);this.imu.getLinearAcceleration().setZ(event.values[2]);this.imu.setLinearAccelerationCovariance(tmpCov);this.imu.getAngularVelocity().setX(event.values[0]);this.imu.getAngularVelocity().setY(event.values[1]);this.imu.getAngularVelocity().setZ(event.values[2]);double[] tmpCov ={0.0025,0,0,0,0.0025,0,0,0,0.0025};// TODO Make Parameterthis.imu.setAngularVelocityCovariance(tmpCov);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[] tmpCov ={0.001,0,0,0,0.001,0,0,0,0.001};// TODO Make Parameterthis.imu.setOrientationCovariance(tmpCov);//组装header// Convert event.timestamp (nanoseconds uptime) into system time, use that as the header stamplong 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.imuthis.imu =this.publisher.newMessage();// Reset timesthis.accelTime =0;this.gyroTime =0;this.quatTime =0;}}}publicImuPublisher(SensorManager manager){this.sensorManager = manager;}publicGraphName getDefaultNodeName(){ //节点名称returnGraphName.of("android_sensors_driver/imuPublisher");}publicvoid onError(Node node,Throwable throwable){}publicvoid onStart(ConnectedNode node){try{//主题为"android/imu" 消息类型为 "sensor_msgs/Imu" ,是标准类型的消息this.publisher = node.newPublisher("android/imu","sensor_msgs/Imu");   ......this.sensorListener = new SensorListener(publisher, hasAccel, hasGyro, hasQuat);this.imuThread.start();   this.imuThread = new ImuThread(this.sensorManager, sensorListener);}catch(Exception e){if(node !=null){node.getLog().fatal(e);}else{e.printStackTrace();}}}//@Overridepublicvoid onShutdown(Node arg0){....}//@Overridepublicvoid onShutdownComplete(Node arg0){}}
        
       
      

 

 

 

其中

NodeMain 必须重写

getDefaultNodeName

()节点名称

publicinterfaceNodeMainextendsNodeListener{    GraphName getDefaultNodeName();    }

 

消息使用的是是Rosjava库里面预值好的一个格式

 

//    // Source code recreated from a .class file by IntelliJ IDEA    // (powered by Fernflower decompiler)    //    package sensor_msgs;    import geometry_msgs.Quaternion;    import geometry_msgs.Vector3;    import org.ros.internal.message.Message;    import std_msgs.Header;    publicinterfaceImuextendsMessage{    String _TYPE ="sensor_msgs/Imu";    String _DEFINITION ="# This is a message to hold data from an IMU (Inertial Measurement Unit)\n#\n# Accelerations should be in m/s^2 (not in g\'s), and rotational velocity should be in rad/sec\n#\n# If the covariance of the measurement is known, it should be filled in (if all you know is the \n# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)\n# A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the\n# data a covariance will have to be assumed or gotten from some other source\n#\n# If you have no estimate for one of the data elements (e.g. your IMU doesn\'t produce an orientation \n# estimate), please set element 0 of the associated covariance matrix to -1\n# If you are interpreting this message, please check for a value of -1 in the first element of each \n# covariance matrix, and disregard the associated estimate.\n\nHeader header\n\ngeometry_msgs/Quaternion orientation\nfloat64[9] orientation_covariance # Row major about x, y, z axes\n\ngeometry_msgs/Vector3 angular_velocity\nfloat64[9] angular_velocity_covariance # Row major about x, y, z axes\n\ngeometry_msgs/Vector3 linear_acceleration\nfloat64[9] linear_acceleration_covariance # Row major x, y z \n";    Header getHeader();    void setHeader(Header var1);    Quaternion getOrientation();    void setOrientation(Quaternion var1);    double[] getOrientationCovariance();    void setOrientationCovariance(double[] var1);    Vector3 getAngularVelocity();    void setAngularVelocity(Vector3 var1);    double[] getAngularVelocityCovariance();    void setAngularVelocityCovariance(double[] var1);    Vector3 getLinearAcceleration();    void setLinearAcceleration(Vector3 var1);    double[] getLinearAccelerationCovariance();    void setLinearAccelerationCovariance(double[] var1);    }

 

 

 

rosjava发布者的使用

 

XActivity .java    publicclass XActivityextendsRosActivity    {    privateImuPublisher imu_pub;    privateSensorManager mSensorManager;    public XActivity()    {    super("Ros Android Sensors Driver","Ros Android Sensors Driver");    }    @Override    protectedvoid onCreate(Bundle savedInstanceState)    {    super.onCreate(savedInstanceState);    setContentView(R.layout.main);    mSensorManager =(SensorManager)this.getSystemService(SENSOR_SERVICE);    }    @Override    protectedvoid onResume()    {    super.onResume();    }    @Override    protectedvoid init(NodeMainExecutor nodeMainExecutor)    {    NodeConfiguration nodeConfiguration3 =NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());    nodeConfiguration3.setMasterUri(getMasterUri());    nodeConfiguration3.setNodeName("android_sensors_driver_imu");    this.imu_pub =newImuPublisher(mSensorManager);    nodeMainExecutor.execute(this.imu_pub, nodeConfiguration3);    }    }

 

其中的

nodeConfiguration3

.

setNodeName

(

"android_sensors_driver_imu"

);

觉得很奇怪，在

ImuPublisher中已经设置了节点名称，到底是谁在起作用呢？

 

ImuPublisher.java    publicGraphName getDefaultNodeName()    {    //节点名称    returnGraphName.of("android_sensors_driver/imuPublisher");    }

 

 

需要跟踪 nodeMainExecutor

.

execute

(

this

.

imu_pub

,

nodeConfiguration3

);方法

XActivity .java    @Override    protectedvoid init(NodeMainExecutor nodeMainExecutor)    {    ...    }

 

 

是从父类继承过来的，父类再启动中会

startServic

 

RosActivity.java    protectedRosActivity(String notificationTicker,String notificationTitle, URI customMasterUri){    super();    this.notificationTicker = notificationTicker;    this.notificationTitle = notificationTitle;        nodeMainExecutorServiceConnection =newNodeMainExecutorServiceConnection(customMasterUri);    }    @Override    protectedvoid onStart(){    super.onStart();        bindNodeMainExecutorService();//启动service    }    //startService+bindService    //如果我们想保持和 Service 的通信，又不想让 Service 随着 Activity 退出而退出呢？你可以先 startService() 然后再 bindService() 。    //当你不需要绑定的时候就执行 unbindService() 方法，执行这个方法只会触发 Service 的 onUnbind() 而不会把这个 Service 销毁。    //这样就可以既保持和 Service 的通信，也不会随着 Activity 销毁而销毁了    protectedvoid bindNodeMainExecutorService(){    Intent intent =newIntent(this,NodeMainExecutorService.class);        intent.setAction(NodeMainExecutorService.ACTION_START);        intent.putExtra(NodeMainExecutorService.EXTRA_NOTIFICATION_TICKER, notificationTicker);        intent.putExtra(NodeMainExecutorService.EXTRA_NOTIFICATION_TITLE, notificationTitle);        startService(intent);//启动服务    Preconditions.checkState(            bindService(intent, nodeMainExecutorServiceConnection, BIND_AUTO_CREATE),//绑定连接状态回调    "Failed to bind NodeMainExecutorService.");    }

 

这个servic连接上的时候会回调，回调会取出

binder，

((

NodeMainExecutorService

.

LocalBinder

)

binder

).

getService

()得到

nodeMainExecutorService 然后

init

();

 

RosActivity.java    privatefinalclassNodeMainExecutorServiceConnectionimplementsServiceConnection{    private URI customMasterUri;    publicNodeMainExecutorServiceConnection(URI customUri){    super();          customMasterUri = customUri;    }    @Override    publicvoid onServiceConnected(ComponentName name,IBinder binder){          nodeMainExecutorService =((NodeMainExecutorService.LocalBinder) binder).getService();//取出发布的操作类    if(customMasterUri !=null){            nodeMainExecutorService.setMasterUri(customMasterUri);            nodeMainExecutorService.setRosHostname(getDefaultHostAddress());    }          nodeMainExecutorService.addListener(newNodeMainExecutorServiceListener(){    @Override    publicvoid onShutdown(NodeMainExecutorService nodeMainExecutorService){    // We may have added multiple shutdown listeners and we only want to    // call finish() once.    if(!RosActivity.this.isFinishing()){    RosActivity.this.finish();    }    }    });    if(getMasterUri()==null){            startMasterChooser();    }else{            init();    }    }    @Override    publicvoid onServiceDisconnected(ComponentName name){    }    };

 

init

()里面就把

nodeMainExecutorService传到了子类，子类（执行发布操作的类）

拿到 

nodeMainExecutor并执行

nodeMainExecutor

.

execute

(

this

.

imu_pub

,

nodeConfiguration3

);

 

RosActivity.java    protectedvoid init(){    // Run init() in a new thread as a convenience since it often requires    // network access.    newAsyncTask
      
       (){    @Override    protectedVoid doInBackground(Void... params){    RosActivity.this.init(nodeMainExecutorService);//调子类的方法，传递发布的操作类    returnnull;    }    }.execute();    }
      

 

所以要去查nodeMainExecutor的来源，就要找到启动的service :NodeMainExecutorService

nodeMainExecutor.execute(this.imu_pub, nodeConfiguration3);调用了如下代码 NodeMainExecutorService.java    @Override    publicvoid execute(NodeMain nodeMain,NodeConfiguration nodeConfiguration,    Collection
      
        nodeListeneners){        nodeMainExecutor.execute(nodeMain, nodeConfiguration, nodeListeneners);//主界面调用的execute到了这里    }    @Override    publicvoid execute(NodeMain nodeMain,NodeConfiguration nodeConfiguration){        execute(nodeMain, nodeConfiguration,null);    }
      

 

其中的

nodeMainExecutor在构造

NodeMainExecutorService的时候就初始化了

 

NodeMainExecutorService.java    publicNodeMainExecutorService(){    super();        rosHostname =null;        nodeMainExecutor =DefaultNodeMainExecutor.newDefault();//初始化        binder =newLocalBinder();        listeners =    newListenerGroup
      
       (                nodeMainExecutor.getScheduledExecutorService());    }
      

 

查看

DefaultNodeMainExecutor

.

newDefault

()是如何初始化的

publicstaticNodeMainExecutor newDefault(){    return newDefault(newDefaultScheduledExecutorService());    }    publicstaticNodeMainExecutor newDefault(ScheduledExecutorService executorService){    returnnewDefaultNodeMainExecutor(newDefaultNodeFactory(executorService), executorService);    }        //真实的初始化执行发布的操作类    privateDefaultNodeMainExecutor(NodeFactory nodeFactory,ScheduledExecutorService scheduledExecutorService){    this.nodeFactory = nodeFactory;    this.scheduledExecutorService = scheduledExecutorService;    this.connectedNodes =Multimaps.synchronizedMultimap(HashMultimap.create());    this.nodeMains =Maps.synchronizedBiMap(HashBiMap.create());    Runtime.getRuntime().addShutdownHook(newThread(newRunnable(){    publicvoid run(){    DefaultNodeMainExecutor.this.shutdown();    }    }));    }

 

实际的执行代码

publicvoid execute(finalNodeMain nodeMain,NodeConfiguration nodeConfiguration,finalCollection
         
           nodeListeners){    finalNodeConfiguration nodeConfigurationCopy =NodeConfiguration.copyOf(nodeConfiguration);            nodeConfigurationCopy.setDefaultNodeName(nodeMain.getDefaultNodeName());    Preconditions.checkNotNull(nodeConfigurationCopy.getNodeName(),"Node name not specified.");    this.scheduledExecutorService.execute(newRunnable(){    publicvoid run(){    ArrayList nodeListenersCopy =Lists.newArrayList();                    nodeListenersCopy.add(DefaultNodeMainExecutor.this.newRegistrationListener(null));                    nodeListenersCopy.add(nodeMain);    if(nodeListeners !=null){                        nodeListenersCopy.addAll(nodeListeners);    }    Node node =DefaultNodeMainExecutor.this.nodeFactory.newNode(nodeConfigurationCopy, nodeListenersCopy);    DefaultNodeMainExecutor.this.nodeMains.put(node, nodeMain);    }    });    }
         

 

nodeConfigurationCopy

.

setDefaultNodeName

(

nodeMain

.

getDefaultNodeName

());

如果前台没配置节点名，就会用发布者默认的名称，

如果前台有，就使用前台的名字作为节点名字

 

NodeConfiguration.java    publicNodeConfiguration setDefaultNodeName(GraphName nodeName){    if(this.nodeName ==null){    this.setNodeName(nodeName);    }    returnthis;    }

 

 

实际上只执行一句

DefaultNodeMainExecutor

.

this

.

nodeMains

.

put

(

node

,

nodeMain

);

node是由前台的节点配置信息和两个监听实例（一个是本页面的，一个发布者的）构造的

1. Node node =DefaultNodeMainExecutor.this.nodeFactory.newNode(nodeConfigurationCopy, nodeListenersCopy);

    

构造里面做的事

 

DefaultNodeFactory.java    publicclassDefaultNodeFactoryimplementsNodeFactory{    privatefinalScheduledExecutorService scheduledExecutorService;    publicDefaultNodeFactory(ScheduledExecutorService scheduledExecutorService){    this.scheduledExecutorService =newSharedScheduledExecutorService(scheduledExecutorService);    }    publicNode newNode(NodeConfiguration nodeConfiguration,Collection
        
          listeners){    returnnewDefaultNode(nodeConfiguration, listeners,this.scheduledExecutorService);    }    publicNode newNode(NodeConfiguration nodeConfiguration){    returnthis.newNode(nodeConfiguration,(Collection)null);    }    }
        

 

 

DefaultNode.java         publicDefaultNode(NodeConfiguration nodeConfiguration,Collection
          
            nodeListeners,ScheduledExecutorService scheduledExecutorService){    this.nodeConfiguration =NodeConfiguration.copyOf(nodeConfiguration);    this.nodeListeners =newListenerGroup(scheduledExecutorService);    this.nodeListeners.addAll(nodeListeners);    this.scheduledExecutorService = scheduledExecutorService;    this.masterUri = nodeConfiguration.getMasterUri();    this.masterClient =newMasterClient(this.masterUri);    this.topicParticipantManager =newTopicParticipantManager();    this.serviceManager =newServiceManager();    this.parameterManager =newParameterManager(scheduledExecutorService);    GraphName basename = nodeConfiguration.getNodeName();    NameResolver parentResolver = nodeConfiguration.getParentResolver();    this.nodeName = parentResolver.getNamespace().join(basename);    this.resolver =newNodeNameResolver(this.nodeName, parentResolver);    this.slaveServer =newSlaveServer(this.nodeName, nodeConfiguration.getTcpRosBindAddress(), nodeConfiguration.getTcpRosAdvertiseAddress(),            nodeConfiguration.getXmlRpcBindAddress(), nodeConfiguration.getXmlRpcAdvertiseAddress(),this.masterClient,this.topicParticipantManager,    this.serviceManager,this.parameterManager, scheduledExecutorService);            //nodeConfiguration.getTcpRosBindAddress()默认情况下端口为0            //nodeConfiguration.getTcpRosAdvertiseAddress()主界面组装时候配置的host（String)组成的TcpRosAdvertiseAddress            //nodeConfiguration.getXmlRpcBindAddress() 默认情况下端口为0            //nodeConfiguration.getXmlRpcAdvertiseAddress()     this.slaveServer.start();    NodeIdentifier nodeIdentifier =this.slaveServer.toNodeIdentifier();    this.parameterTree =DefaultParameterTree.newFromNodeIdentifier(nodeIdentifier,this.masterClient.getRemoteUri(),this.resolver,this.parameterManager);    this.publisherFactory =newPublisherFactory(nodeIdentifier,this.topicParticipantManager, nodeConfiguration.getTopicMessageFactory(), scheduledExecutorService);    this.subscriberFactory =newSubscriberFactory(nodeIdentifier,this.topicParticipantManager, scheduledExecutorService);    this.serviceFactory =newServiceFactory(this.nodeName,this.slaveServer,this.serviceManager, scheduledExecutorService);    this.registrar =newRegistrar(this.masterClient, scheduledExecutorService);    this.topicParticipantManager.setListener(this.registrar);    this.serviceManager.setListener(this.registrar);            scheduledExecutorService.execute(newRunnable(){    publicvoid run(){    DefaultNode.this.start();    }    });    }
          

 

其中this

.

masterClient

=

new

MasterClient

(

this

.

masterUri

);配置服务端的地址

publicClient(URI uri,Class
           
             interfaceClass){    this.uri = uri;    XmlRpcClientConfigImpl config =newXmlRpcClientConfigImpl();    try{     config.setServerURL(uri.toURL());
           

 

 

 

 

 

前台的节点配置信息

NodeConfiguration nodeConfiguration3 =NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());        nodeConfiguration3.setMasterUri(getMasterUri());//如果没有设置默认值为 "http://localhost:11311/"        nodeConfiguration3.setNodeName("android_sensors_driver_imu");

 

 

DefaultNodeMainExecutor.java的监听器

privateclassRegistrationListenerimplementsNodeListener{    privateRegistrationListener(){    }    publicvoid onStart(ConnectedNode connectedNode){    DefaultNodeMainExecutor.this.registerNode(connectedNode);    }    publicvoid onShutdown(Node node){    }    publicvoid onShutdownComplete(Node node){    DefaultNodeMainExecutor.this.unregisterNode(node);    }    publicvoid onError(Node node,Throwable throwable){    DefaultNodeMainExecutor.log.error("Node error.", throwable);    DefaultNodeMainExecutor.this.unregisterNode(node);    }    }    }

 

 

发布者的监听器

//@Override    publicvoid onStart(ConnectedNode node)    {    try    {    this.publisher = node.newPublisher("android/fix","sensor_msgs/NavSatFix");    this.navSatFixListener =newNavSatListener(publisher);    this.navSatThread =newNavSatThread(this.locationManager,this.navSatFixListener);    this.navSatThread.start();    }    catch(Exception e)    {    if(node !=null)    {     node.getLog().fatal(e);    }    else    {     e.printStackTrace();    }    }    }    //@Override    publicvoid onShutdown(Node arg0){    this.navSatThread.shutdown();    try{    this.navSatThread.join();    }catch(InterruptedException e){     e.printStackTrace();    }    }    //@Override    publicvoid onShutdownComplete(Node arg0){    }    publicvoid onError(Node node,Throwable throwable)    {    }