OrientationPublisher.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2011 Google Inc.
00003  * 
00004  * Licensed under the Apache License, Version 2.0 (the "License"); you may not
00005  * use this file except in compliance with the License. You may obtain a copy of
00006  * the License at
00007  * 
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  * 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
00013  * License for the specific language governing permissions and limitations under
00014  * the License.
00015  */
00016 
00017 package org.ros.android;
00018 
00019 import android.hardware.Sensor;
00020 import android.hardware.SensorEvent;
00021 import android.hardware.SensorEventListener;
00022 import android.hardware.SensorManager;
00023 import geometry_msgs.PoseStamped;
00024 import org.ros.message.Time;
00025 import org.ros.namespace.GraphName;
00026 import org.ros.node.AbstractNodeMain;
00027 import org.ros.node.ConnectedNode;
00028 import org.ros.node.topic.Publisher;
00029 
00033 public class OrientationPublisher extends AbstractNodeMain {
00034 
00035   private final SensorManager sensorManager;
00036 
00037   private OrientationListener orientationListener;
00038 
00039   private final class OrientationListener implements SensorEventListener {
00040 
00041     private final Publisher<geometry_msgs.PoseStamped> publisher;
00042 
00043     private OrientationListener(Publisher<geometry_msgs.PoseStamped> publisher) {
00044       this.publisher = publisher;
00045     }
00046 
00047     @Override
00048     public void onAccuracyChanged(Sensor sensor, int accuracy) {
00049     }
00050 
00051     @Override
00052     public void onSensorChanged(SensorEvent event) {
00053       if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
00054         float[] quaternion = new float[4];
00055         SensorManager.getQuaternionFromVector(quaternion, event.values);
00056         PoseStamped pose = publisher.newMessage();
00057         pose.getHeader().setFrameId("/map");
00058         // TODO(damonkohler): Should get time from the Node.
00059         pose.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis()));
00060         pose.getPose().getOrientation().setW(quaternion[0]);
00061         pose.getPose().getOrientation().setX(quaternion[1]);
00062         pose.getPose().getOrientation().setY(quaternion[2]);
00063         pose.getPose().getOrientation().setZ(quaternion[3]);
00064         publisher.publish(pose);
00065       }
00066     }
00067   }
00068 
00069   public OrientationPublisher(SensorManager sensorManager) {
00070     this.sensorManager = sensorManager;
00071   }
00072 
00073   @Override
00074   public GraphName getDefaultNodeName() {
00075     return GraphName.of("android/orientiation_sensor");
00076   }
00077 
00078   @Override
00079   public void onStart(ConnectedNode connectedNode) {
00080     try {
00081       Publisher<geometry_msgs.PoseStamped> publisher =
00082           connectedNode.newPublisher("android/orientation", "geometry_msgs/PoseStamped");
00083       orientationListener = new OrientationListener(publisher);
00084       Sensor sensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
00085       // 10 Hz
00086       sensorManager.registerListener(orientationListener, sensor, 500000);
00087     } catch (Exception e) {
00088       connectedNode.getLog().fatal(e);
00089     }
00090   }
00091 }


android_core
Author(s): Damon Kohler
autogenerated on Thu Jun 6 2019 21:20:07