ros和Android(一)
时间:2016-08-19 23:47:19
收藏:0
阅读:1927
ros和Android
ros基本要素:
1.节点 :节点与节点用tcp/ip通信
2.节点管理器 :(相当于域名解析器)所有节点的枢纽,节点之间要实现互相通信,都要通过节点管理器
3.消息 :
4.参数服务器 :
5.主题 :某一类的节点通信 例如:scan主题,所有扫描的订阅和发布都可以放在这个主题上
6.服务 :
7.消息记录包:
8.订阅:
9.发布:
c++中的例子提取出的步骤
1.创建消息和服务:
消息文件(.msg)一般带Header+基本类型数据+消息文件类型
服务文件(.srv)请求+响应 ---划分,请求,响应也是由基本类型组成
4// This header defines the standard ROS classes .5#include<ros / ros.h>67int main (int argc ,char** argv ){8// Initialize the ROS system .9 ros::init ( argc , argv ," hello _ros ");1011// Establ ish this program as a ROS node .12 ros::NodeHandle nh ;1314// Send some output as a log message .15 ROS_INFO_STREAM(" Hello , ? ROS! ");16}
c++发布订阅端程序
1// This program publishes randomly−generated velocity2// messages for turtlesim .3#include<ros / ros.h>4#include<geometry_msgs /Twist. h>// For geometry_msgs:: Twist5#include<stdlib.h>// For rand() and RAND_MAX67int main (int argc ,char** argv ){8// Initialize the ROS system and become a node .9 ros::init ( argc , argv ," publish _ velocity ");10 ros::NodeHandle nh ;1112// Create a publisher obj ect .13 ros::Publisher pub = nh . advertise <geometry_msgs::Twist>(14" turtle1 /cmd_vel",1000);1516// Seed the random number generator .17 srand ( time (0));1819// Loop at 2Hz until the node is shut down.20 ros::Raterate(2);21while( ros::ok ()){22// Create and fill in the message . The other four23// fields , which are ignored by turtl esim , default to 0.24 geometry_msgs ::Twist msg ;25 msg . linear . x =double( rand ())/double(RAND_MAX);26 msg.angular.z =2*double( rand ())/double(RAND_MAX)−1;2728// Publish the message .29 pub . publish ( msg );3031// Send a message to rosout with the details .32 ROS_INFO_STREAM("Sending random velocity command : "33<<" linear="<< msg.linear.x34<<" angular="<< msg.angular.z);3536// Wait untilit‘s time for another iteration .37 rate.sleep ();38}39}表3
订阅者的程序
1// This program subscribes to turtle1/pose and shows its2// messages on the screen .3#include<ros / ros.h>4#include<turtlesim /Pose.h>5#include<iomanip>// for std::setprecision and std::fixed67// A callback function . Executed each time a new pose8// message arrives .9void poseMessageReceived (const turtlesim::Pose& msg ){10 ROS_INFO_STREAM( std::setprecision (2)<< std::fixed11<<" position =("<< msg . x <<" , "<< msg . y <<" ) "12<<" *direction="<< msg . theta );13}1415int main (int argc ,char** argv ){16// Initialize the ROS system and become a node .17 ros::init ( argc , argv ," subscribe_to _pose ");18 ros::NodeHandle nh ;1920// Create a subscri ber obj ect .21 ros::Subscriber sub = nh.subscribe (" turtle1/pose ",1000,22&poseMessageReceived );2324// Let ROS take over .25 ros::spin ();26}
rosjava发布者的一般写法
/*** 写Publisher必须继承NodeMain 重写NodeMain的getDefaultNodeName()*/publicclassImuPublisherimplementsNodeMain{privateImuThread imuThread;privateSensorListener sensorListener;privateSensorManager sensorManager;privatePublisher<Imu> 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<Imu> publisher;privateImu imu;privateSensorListener(Publisher<Imu> 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");}@Overrideprotectedvoid onCreate(Bundle savedInstanceState){super.onCreate(savedInstanceState);setContentView(R.layout.main);mSensorManager =(SensorManager)this.getSystemService(SENSOR_SERVICE);}@Overrideprotectedvoid onResume(){super.onResume();}@Overrideprotectedvoid 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
@Overrideprotectedvoid init(NodeMainExecutor nodeMainExecutor){...}
是从父类继承过来的,父类再启动中会startServic
RosActivity.java
protectedRosActivity(String notificationTicker,String notificationTitle, URI customMasterUri){super();this.notificationTicker = notificationTicker;this.notificationTitle = notificationTitle;nodeMainExecutorServiceConnection =newNodeMainExecutorServiceConnection(customMasterUri);}@Overrideprotectedvoid 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;}@Overridepublicvoid onServiceConnected(ComponentName name,IBinder binder){nodeMainExecutorService =((NodeMainExecutorService.LocalBinder) binder).getService();//取出发布的操作类if(customMasterUri !=null){nodeMainExecutorService.setMasterUri(customMasterUri);nodeMainExecutorService.setRosHostname(getDefaultHostAddress());}nodeMainExecutorService.addListener(newNodeMainExecutorServiceListener(){@Overridepublicvoid 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();}}@Overridepublicvoid 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<Void,Void,Void>(){@OverrideprotectedVoid doInBackground(Void... params){RosActivity.this.init(nodeMainExecutorService);//调子类的方法,传递发布的操作类returnnull;}}.execute();}
所以要去查nodeMainExecutor的来源,就要找到启动的service :NodeMainExecutorService
nodeMainExecutor.execute(this.imu_pub, nodeConfiguration3);调用了如下代码
NodeMainExecutorService.java
@Overridepublicvoid execute(NodeMain nodeMain,NodeConfiguration nodeConfiguration,Collection<NodeListener> nodeListeneners){nodeMainExecutor.execute(nodeMain, nodeConfiguration, nodeListeneners);//主界面调用的execute到了这里}@Overridepublicvoid execute(NodeMain nodeMain,NodeConfiguration nodeConfiguration){execute(nodeMain, nodeConfiguration,null);}
其中的nodeMainExecutor在构造NodeMainExecutorService的时候就初始化了
NodeMainExecutorService.java
publicNodeMainExecutorService(){super();rosHostname =null;nodeMainExecutor =DefaultNodeMainExecutor.newDefault();//初始化binder =newLocalBinder();listeners =newListenerGroup<NodeMainExecutorServiceListener>(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<NodeListener> 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是由前台的节点配置信息和两个监听实例(一个是本页面的,一个发布者的)构造的
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<NodeListener> listeners){returnnewDefaultNode(nodeConfiguration, listeners,this.scheduledExecutorService);}publicNode newNode(NodeConfiguration nodeConfiguration){returnthis.newNode(nodeConfiguration,(Collection)null);}}
DefaultNode.javapublicDefaultNode(NodeConfiguration nodeConfiguration,Collection<NodeListener> 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 =newMasterClient(this.masterUri);配置服务端的地址
publicClient(URI uri,Class<T> 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);}}}
发布者的监听器
//@Overridepublicvoid 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();}}}//@Overridepublicvoid onShutdown(Node arg0){this.navSatThread.shutdown();try{this.navSatThread.join();}catch(InterruptedException e){e.printStackTrace();}}//@Overridepublicvoid onShutdownComplete(Node arg0){}publicvoid onError(Node node,Throwable throwable){}
评论(0)