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
=
new
MasterClient
(
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)
{
}