ros和Android(一)
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<ros / ros.h> 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<ros / ros.h> #include<geometry_msgs /Twist. h>// For geometry_msgs:: Twist #include<stdlib.h>// 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 <geometry_msgs::Twist>( " 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<ros / ros.h> #include<turtlesim /Pose.h> #include<iomanip>// 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<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(); } // @Override publicvoid 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 Parameter this.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 Parameter this.imu.setOrientationCovariance(tmpCov); //组装header // Convert event.timestamp (nanoseconds uptime) into system time, use that as the header stamp 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(); // Reset times this.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(); } } } //@Override publicvoid onShutdown(Node arg0) { .... } //@Override publicvoid 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<Void,Void,Void>(){ @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<NodeListener> 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<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.java publicDefaultNode(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); } } }
发布者的监听器
//@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) { }