@Override
public boolean onTouchEvent(VisualizationView view, MotionEvent event) {
if (visible) {
Preconditions.checkNotNull(pose);
if (event.getAction() == MotionEvent.ACTION_MOVE) {
Vector3 orientationVector =
camera
.toWorldCoordinates(new Point((int) event.getX(), (int) event.getY()))
.subtract(pose.getTranslation());
if (orientationVector.length() > 0) {
pose.setRotation(
Quaternion.rotationBetweenVectors(new Vector3(1, 0, 0), orientationVector));
} else {
pose.setRotation(Quaternion.newIdentityQuaternion());
}
shape.setTransform(pose);
requestRender();
return true;
} else if (event.getAction() == MotionEvent.ACTION_UP) {
posePublisher.publish(
pose.toPoseStampedMessage(
camera.getFixedFrame(),
connectedNode.getCurrentTime(),
posePublisher.newMessage()));
visible = false;
requestRender();
return true;
}
}
gestureDetector.onTouchEvent(event);
return false;
}
protected void publish() {
if (publisher_string.isEmpty()) return;
std_msgs.String string = publisher.newMessage();
string.setData(publisher_string);
publisher.publish(string);
hasPublishedMsg = true;
publisher_string = "";
}
public void send_coordinate(float[] p_array) {
string = publisher.newMessage();
StringBuilder stringBuilder = new StringBuilder();
stringBuilder.append("c_");
stringBuilder.append(java.lang.String.valueOf(p_array[0]));
stringBuilder.append('_');
stringBuilder.append(java.lang.String.valueOf(p_array[1]));
string.setData(stringBuilder.toString());
publisher.publish(string);
}
private void publish(byte level, Object message) {
rosgraph_msgs.Log logMessage = publisher.newMessage();
logMessage.getHeader().setStamp(defaultNode.getCurrentTime());
logMessage.setLevel(level);
logMessage.setName(defaultNode.getName().toString());
logMessage.setMsg(message.toString());
// TODO(damonkohler): Should update the topics field with a list of all
// published and subscribed topics for the node that created this logger.
// This helps filter the rosoutconsole.
publisher.publish(logMessage);
}
@GET // Handle HTTP Get requests
@Path("{linear}") // URI components containing the parameter
@Produces("text/plain")
public String setLinearVelocity(@PathParam("linear") double linear) {
System.out.printf("\n\n---------> velocity_publisher %s\n\n\n", velocity_publisher);
geometry_msgs.Twist cmd_vel = velocity_publisher.newMessage();
System.out.printf("\n\n---------> cmd_vel %s\n\n\n", cmd_vel);
cmd_vel.getLinear().setX(linear);
velocity_publisher.publish(cmd_vel);
return "done!";
}
public void handleImage() {
while (true) {
Image src = null;
try {
src = new Image(visionImage.take(), width, height);
} catch (InterruptedException e) {
e.printStackTrace();
continue;
}
// Apply Blob Track to Image
Image dest = new Image(src);
blobTrack.apply(src, dest);
OdometryMsg msg = new OdometryMsg();
if (LOCALIZE && System.currentTimeMillis() - last_localization > LOCALIZATION_INTERVAL) {
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = false;
stopPub.publish(stop_msg);
Point2D.Double curr_point;
synchronized (localization) {
curr_point = localization.localize(odo_x, odo_y, blobTrack.fiducials);
}
msg.x = curr_point.x;
msg.y = curr_point.y;
last_localization = System.currentTimeMillis();
} else {
msg.x = odo_x;
msg.y = odo_y;
}
msg.theta = odo_theta;
localPub.publish(msg);
// Update newly formed vision message
gui.setVisionImage(dest.toArray(), width, height);
try {
Thread.sleep(1000);
} catch (Exception exc) {
exc.printStackTrace();
}
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = true;
stopPub.publish(stop_msg);
}
}
public void updateTopic(Node node, long seq) {
message.header.seq = seq;
message.header.stamp = node.getCurrentTime();
message.header.frame_id = "/robot";
pressed = touch.isPressed();
message.contact = pressed;
topic.publish(message);
}
public TurtlesimVelocityPublisherResource(ConnectedNode connectedNode)
throws InterruptedException {
System.out.println("\n\n---------> TurtlesimVelocityPublisherResource Constructor\n\n\n");
velocity_publisher = connectedNode.newPublisher("/turtle1/cmd_vel", geometry_msgs.Twist._TYPE);
System.out.printf("\n\n---------> velocity_publisher %s\n\n\n", velocity_publisher);
geometry_msgs.Twist cmd_vel = velocity_publisher.newMessage();
System.out.printf("\n\n---------> cmd_vel %s\n\n\n", cmd_vel);
}
private void publish(byte level, Object message) {
org.ros.message.rosgraph_msgs.Log m = new org.ros.message.rosgraph_msgs.Log();
m.header.stamp = node.getCurrentTime();
m.level = level;
m.name = node.getName().toString();
m.msg = message.toString();
// TODO(damonkohler): Should return list of all published and subscribed
// topics for the node that created this logger. This helps filter the
// rosoutconsole.
m.topics = Lists.newArrayList();
publisher.publish(m);
}
// @Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_PRESSURE) {
FluidPressure msg = this.publisher.newMessage();
long time_delta_millis = System.currentTimeMillis() - SystemClock.uptimeMillis();
msg.getHeader().setStamp(Time.fromMillis(time_delta_millis + event.timestamp / 1000000));
msg.getHeader().setFrameId("/android/barometric_pressure"); // TODO Make parameter
msg.setFluidPressure(100.0 * event.values[0]); // Reported in hPa, need to output in Pa
msg.setVariance(0.0);
publisher.publish(msg);
}
}
// take the union of two polygons, assuming no enclosed empty spaces, and that the result will
// be contiguous.
public static Polygon combine(Polygon poly1, Polygon poly2) {
ArrayList<Mat> vertices = new ArrayList<Mat>();
ArrayList<TreeSet<Integer>> edgesTo = new ArrayList<TreeSet<Integer>>();
int sizePoly1 = poly1.vertices.size();
int sizePoly2 = poly2.vertices.size();
int size = sizePoly1 + sizePoly2;
boolean done;
Publisher<GUISegmentMsg> segmentPub =
node.newPublisher("/gui/Segment", "lab5_msgs/GUISegmentMsg");
Publisher<GUIEraseMsg> erasePub = node.newPublisher("/gui/Erase", "lab5_msgs/GUIEraseMsg");
// erasePub.publish(new GUIEraseMsg());
GUISegmentMsg segmentPlot = new GUISegmentMsg();
ColorMsg segmentPlotColor = new ColorMsg();
// add all the vertices in both polygons
vertices.addAll(poly1.vertices);
vertices.addAll(poly2.vertices);
for (int i = 0; i < size; i++) {
edgesTo.add(new TreeSet<Integer>());
}
// add all the edges in both polygons
for (int i = 0; i < sizePoly1; i++) {
edgesTo.get(i).add(new Integer((i + 1) % sizePoly1));
edgesTo.get((i + 1) % sizePoly1).add(new Integer(i));
}
for (int i = 0; i < sizePoly2; i++) {
edgesTo.get(i + sizePoly1).add(new Integer(((i + 1) % sizePoly2) + sizePoly1));
edgesTo.get(((i + 1) % sizePoly2) + sizePoly1).add(new Integer(i + sizePoly1));
}
System.err.println(vertices);
System.err.println(edgesTo);
segmentPlotColor.r = 255;
segmentPlotColor.g = 0;
segmentPlotColor.b = 0;
segmentPlot.color = segmentPlotColor;
for (int e0 = 0; e0 < size; e0++) {
for (int e1 : edgesTo.get(e0)) {
double[] xyStart = Mat.decodePoint(vertices.get(e0));
double[] xyEnd = Mat.decodePoint(vertices.get(e1));
segmentPlot.startX = xyStart[0];
segmentPlot.startY = xyStart[1];
segmentPlot.endX = xyEnd[0];
segmentPlot.endY = xyEnd[1];
segmentPub.publish(segmentPlot);
}
}
// find and merge colocated points
done = false;
while (!done) {
done = true;
checkFMCP:
{
for (int p0 = 0; p0 < size; p0++) {
for (int p1 = 0; p1 < size; p1++) {
if (p0 != p1) {
if (ptsEqual(vertices.get(p0), vertices.get(p1))) {
// System.err.println("found two colocated: " + p0 + " " + p1);
// System.err.println(edgesTo);
edgesTo.get(p0).addAll(edgesTo.get(p1));
edgesTo.get(p0).remove(p0);
vertices.remove(p1);
edgesTo.remove(p1);
size--;
for (int e0 = 0; e0 < size; e0++) {
if (edgesTo.get(e0).contains(new Integer(p1))) {
edgesTo.get(e0).remove(new Integer(p1));
edgesTo.get(e0).add(new Integer(p0));
}
// System.err.println("e0: " + e0);
// System.err.println(edgesTo.get(e0));
TreeSet<Integer> head = new TreeSet(edgesTo.get(e0).headSet(new Integer(p1)));
// System.err.println(head);
for (Integer e1 : edgesTo.get(e0).tailSet(new Integer(p1))) {
head.add(e1 - 1);
}
head.remove(e0);
// System.err.println(head);
edgesTo.set(e0, head);
// System.err.println(edgesTo.get(e0));
}
done = false;
// System.err.println(edgesTo);
break checkFMCP;
}
}
}
}
}
}
System.err.println("merged points");
System.err.println(edgesTo);
/*segmentPlotColor.r = 0;
segmentPlotColor.g = 0;
segmentPlotColor.b = 255;
segmentPlot.color = segmentPlotColor;
for (int e0 = 0; e0 < size; e0++){
for (int e1 : edgesTo.get(e0)) {
double[] xyStart = Mat.decodePoint(vertices.get(e0));
double[] xyEnd = Mat.decodePoint(vertices.get(e1));
segmentPlot.startX = xyStart[0];
segmentPlot.startY = xyStart[1];
segmentPlot.endX = xyEnd[0];
segmentPlot.endY = xyEnd[1];
segmentPub.publish(segmentPlot);
}
}*/
// find and split edges bisected by points
done = false;
while (!done) {
done = true;
checkFSEBP:
{
for (int e0 = 0; e0 < size; e0++) {
for (int e1 : edgesTo.get(e0)) {
for (int p = 0; p < size; p++) {
if (e0 != p && e1 != p) {
if (ptSegIntersect(vertices.get(e0), vertices.get(e1), vertices.get(p))) {
edgesTo.get(p).add(new Integer(e0));
edgesTo.get(p).add(new Integer(e1));
edgesTo.get(e0).remove(new Integer(e1));
edgesTo.get(e0).add(new Integer(p));
edgesTo.get(e1).remove(new Integer(e0));
edgesTo.get(e1).add(new Integer(p));
done = false;
break checkFSEBP;
}
}
}
}
}
}
}
System.err.println("split edges on points");
System.err.println(edgesTo);
System.err.println("GOT HERE!");
int iters = 0;
done = false;
while (!done) {
// find and split intersecting edges
System.err.println("size: " + size);
done = true;
checkFSIE:
{
for (int e00 = 0; e00 < size; e00++) {
for (int e10 = 0; e10 < size; e10++) {
if (e00 != e10) {
for (int e01 : new TreeSet<Integer>(edgesTo.get(e00))) {
if (e01 != e10 && e01 != e00) {
for (int e11 : new TreeSet<Integer>(edgesTo.get(e10))) {
if (e11 != e00 && e11 != e01 && e11 != e10) {
if (lineSegIntersect(
vertices.get(e00),
vertices.get(e01),
vertices.get(e10),
vertices.get(e11))) {
// System.err.println("intersectors for iter " + iters);
// System.err.println(e00);
// System.err.println(edgesTo.get(e00));
// Mat.print(System.err, vertices.get(e00));
// System.err.println(e01);
// System.err.println(edgesTo.get(e01));
// Mat.print(System.err, vertices.get(e01));
// System.err.println(e10);
// System.err.println(edgesTo.get(e10));
// Mat.print(System.err, vertices.get(e10));
// System.err.println(e11);
// System.err.println(edgesTo.get(e11));
// Mat.print(System.err, vertices.get(e11));
if (iters > 10000) {
// F**K!
try {
throw new Exception();
} catch (Exception e) {
e.printStackTrace();
System.err.println("hey there");
System.exit(1);
}
}
iters++;
Mat newVertex =
lineSegIntersection(
vertices.get(e00),
vertices.get(e01),
vertices.get(e10),
vertices.get(e11));
if (ptsEqual(newVertex, vertices.get(e00))) {
edgesTo.get(e10).remove(new Integer(e11));
edgesTo.get(e10).add(new Integer(e00));
edgesTo.get(e00).add(new Integer(e10));
edgesTo.get(e11).remove(new Integer(e10));
edgesTo.get(e11).add(new Integer(e00));
edgesTo.get(e00).add(new Integer(e11));
} else if (ptsEqual(newVertex, vertices.get(e01))) {
edgesTo.get(e10).remove(new Integer(e11));
edgesTo.get(e10).add(new Integer(e01));
edgesTo.get(e01).add(new Integer(e10));
edgesTo.get(e11).remove(new Integer(e10));
edgesTo.get(e11).add(new Integer(e01));
edgesTo.get(e01).add(new Integer(e11));
} else if (ptsEqual(newVertex, vertices.get(e10))) {
edgesTo.get(e00).remove(new Integer(e01));
edgesTo.get(e00).add(new Integer(e10));
edgesTo.get(e10).add(new Integer(e00));
edgesTo.get(e01).remove(new Integer(e00));
edgesTo.get(e01).add(new Integer(e10));
edgesTo.get(e10).add(new Integer(e01));
} else if (ptsEqual(newVertex, vertices.get(e11))) {
edgesTo.get(e00).remove(new Integer(e01));
edgesTo.get(e00).add(new Integer(e11));
edgesTo.get(e11).add(new Integer(e00));
edgesTo.get(e01).remove(new Integer(e00));
edgesTo.get(e01).add(new Integer(e11));
edgesTo.get(e11).add(new Integer(e01));
} else {
vertices.add(newVertex);
edgesTo.add(new TreeSet<Integer>());
edgesTo.get(size).add(new Integer(e00));
edgesTo.get(size).add(new Integer(e01));
edgesTo.get(size).add(new Integer(e10));
edgesTo.get(size).add(new Integer(e11));
edgesTo.get(e00).remove(new Integer(e01));
edgesTo.get(e00).add(new Integer(size));
edgesTo.get(e01).remove(new Integer(e00));
edgesTo.get(e01).add(new Integer(size));
edgesTo.get(e10).remove(new Integer(e11));
edgesTo.get(e10).add(new Integer(size));
edgesTo.get(e11).remove(new Integer(e10));
edgesTo.get(e11).add(new Integer(size));
size++;
}
done = false;
break checkFSIE;
}
}
}
}
}
}
}
}
}
System.err.println("split edges on edges");
System.err.println(edgesTo);
}
System.err.println("GOT HERE TOO!");
System.err.println("begin vertices");
for (Mat vertex : vertices) {
Mat.print(System.err, vertex);
}
System.err.println("end vertices");
System.err.println(edgesTo);
return perimeter(vertices, edgesTo);
}
/**
* Called when the {@link ConnectedNode} has started shutting down. Shutdown will be delayed,
* although not indefinitely, until all {@link NodeListener}s have returned from this method.
*
* <p>Since this method can potentially delay {@link ConnectedNode} shutdown, it is preferred to
* use {@link #onShutdownComplete(Node)} when {@link ConnectedNode} resources are not required
* during the method call.
*
* @param node the {@link Node} that has started shutting down
*/
@Override
public void onShutdown(Node node) {
Log.i(C.TAG, "[ " + node.getName() + " ] onShutdown [ " + QUEUE_NAME + " ]");
publisher.shutdown();
}
private void setVelocities(double trans, double rot) {
org.ros.message.rss_msgs.MotionMsg msg = new org.ros.message.rss_msgs.MotionMsg();
msg.translationalVelocity = trans;
msg.rotationalVelocity = rot;
motionPub.publish(msg);
}
public void sendCloseMenu() {
string = publisher.newMessage();
string.setData("menu_close");
publisher.publish(string);
}
@Override
protected Bitmap processFrame(VideoCapture capture) {
Time[] measureTime = new Time[9];
String[] compDescStrings = {
"Total processFrame",
"Grab a new frame",
"MatToBitmap",
"Publish cameraInfo",
"Create ImageMsg",
"Compress image",
"Transfer to Stream",
"Image.SetData",
"Publish Image",
"Total econds per frame"
};
String[] rawDescStrings = {
"Total processFrame",
"Grab a new frame",
"MatToBitmap",
"Publish cameraInfo",
"Create ImageMsg",
"Pixel to buffer",
"Transfer to Stream",
"Image.SetData",
"Publish Image",
"Total seconds per frame"
};
measureTime[0] = connectedNode.getCurrentTime();
switch (MainActivity.viewMode) {
case MainActivity.VIEW_MODE_GRAY:
// capture.retrieve(mGray, Highgui.CV_CAP_ANDROID_GREY_FRAME);
capture.retrieve(mRgba, Highgui.CV_CAP_ANDROID_GREY_FRAME);
// Imgproc.cvtColor(mGray, mRgba, Imgproc.COLOR_GRAY2RGBA, 4);
break;
case MainActivity.VIEW_MODE_RGBA:
capture.retrieve(mRgba, Highgui.CV_CAP_ANDROID_COLOR_FRAME_RGBA);
// Core.putText(mRgba, "OpenCV + Android", new Point(10, 100), 3, 2, new
// Scalar(255, 0, 0, 255), 3);
break;
case MainActivity.VIEW_MODE_CANNY:
capture.retrieve(mGray, Highgui.CV_CAP_ANDROID_GREY_FRAME);
Imgproc.Canny(mGray, mIntermediateMat, 80, 100);
Imgproc.cvtColor(mIntermediateMat, mRgba, Imgproc.COLOR_GRAY2BGRA, 4);
break;
}
Time currentTime = connectedNode.getCurrentTime();
measureTime[1] = connectedNode.getCurrentTime();
if (bmp == null) bmp = Bitmap.createBitmap(mRgba.cols(), mRgba.rows(), Bitmap.Config.ARGB_8888);
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_NONE
&& bb == null) {
Log.i(TAG, "Buffer 1");
bb = ByteBuffer.allocate(bmp.getRowBytes() * bmp.getHeight());
Log.i(TAG, "Buffer 2");
bb.clear();
Log.i(TAG, "Buffer 3");
}
try {
Utils.matToBitmap(mRgba, bmp);
measureTime[2] = connectedNode.getCurrentTime();
cameraInfo = cameraInfoPublisher.newMessage();
cameraInfo.getHeader().setFrameId("camera");
cameraInfo.getHeader().setStamp(currentTime);
cameraInfo.setWidth(640);
cameraInfo.setHeight(480);
cameraInfoPublisher.publish(cameraInfo);
measureTime[3] = connectedNode.getCurrentTime();
if (MainActivity.imageCompression >= MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG) {
// Compressed image
sensor_msgs.CompressedImage image = imagePublisher.newMessage();
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
image.setFormat("png");
else if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_JPEG)
image.setFormat("jpeg");
image.getHeader().setStamp(currentTime);
image.getHeader().setFrameId("camera");
measureTime[4] = connectedNode.getCurrentTime();
ByteArrayOutputStream baos = new ByteArrayOutputStream();
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
bmp.compress(Bitmap.CompressFormat.PNG, 100, baos);
else if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_JPEG)
bmp.compress(Bitmap.CompressFormat.JPEG, MainActivity.imageCompressionQuality, baos);
measureTime[5] = connectedNode.getCurrentTime();
stream.buffer().writeBytes(baos.toByteArray());
measureTime[6] = connectedNode.getCurrentTime();
image.setData(stream.buffer().copy());
measureTime[7] = connectedNode.getCurrentTime();
stream.buffer().clear();
imagePublisher.publish(image);
measureTime[8] = connectedNode.getCurrentTime();
} else {
// Raw image
Log.i(TAG, "Raw image 1");
sensor_msgs.Image rawImage = rawImagePublisher.newMessage();
rawImage.getHeader().setStamp(currentTime);
rawImage.getHeader().setFrameId("camera");
rawImage.setEncoding("rgba8");
rawImage.setWidth(bmp.getWidth());
rawImage.setHeight(bmp.getHeight());
rawImage.setStep(640);
measureTime[4] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 2");
bmp.copyPixelsToBuffer(bb);
measureTime[5] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 3");
stream.buffer().writeBytes(bb.array());
bb.clear();
measureTime[6] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 4");
rawImage.setData(stream.buffer().copy());
stream.buffer().clear();
measureTime[7] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 5");
rawImagePublisher.publish(rawImage);
measureTime[8] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 6");
}
newTime = connectedNode.getCurrentTime();
stats[9][counter] = (newTime.subtract(oldTime)).nsecs / 1000000.0;
oldTime = newTime;
for (int i = 1; i < 9; i++) {
stats[i][counter] = (measureTime[i].subtract(measureTime[i - 1])).nsecs / 1000000.0;
}
stats[0][counter] = measureTime[8].subtract(measureTime[0]).nsecs / 1000000.0;
counter++;
if (counter == numSamples) {
double[] sts = new double[10];
Arrays.fill(sts, 0.0);
for (int i = 0; i < 10; i++) {
for (int j = 0; j < numSamples; j++) sts[i] += stats[i][j];
sts[i] /= (double) numSamples;
if (MainActivity.imageCompression >= MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
Log.i(TAG, String.format("Mean time for %s:\t\t%4.2fms", compDescStrings[i], sts[i]));
else Log.i(TAG, String.format("Mean time for %s:\t\t%4.2fms", rawDescStrings[i], sts[i]));
}
Log.i(TAG, "\n\n");
counter = 0;
}
return bmp;
} catch (Exception e) {
Log.e(TAG, "Frame conversion and publishing throws an exception: " + e.getMessage());
bmp.recycle();
return null;
}
}
@Override
public void onStart(final ConnectedNode connectedNode) {
this.connectedNode = connectedNode;
try {
serviceClient =
connectedNode.newServiceClient(
"bb_estimator/estimate_bb", srs_env_model_percp.EstimateBB._TYPE);
} catch (ServiceNotFoundException e) {
throw new RosRuntimeException(e);
}
final Publisher<geometry_msgs.Twist> publisherRotate =
connectedNode.newPublisher(MoveTopic.trim(), geometry_msgs.Twist._TYPE);
final Publisher<geometry_msgs.PoseStamped> publisherMove =
connectedNode.newPublisher(GoalTopic.trim(), geometry_msgs.PoseStamped._TYPE);
final geometry_msgs.Twist rotace = publisherRotate.newMessage();
final geometry_msgs.PoseStamped cil = publisherMove.newMessage();
// This CancellableLoop will be canceled automatically when the node shuts down.
connectedNode.executeCancellableLoop(
new CancellableLoop() {
private boolean leftE, rightE, goE, backE, moveE;
private int seq;
@Override
protected void setup() {
leftE = false;
rightE = false;
goE = false;
backE = false;
moveE = false;
seq = 0;
}
@Override
protected void loop() throws InterruptedException {
/* ROTACE */
leftE = left;
rightE = right;
goE = go;
backE = back;
moveE = move;
rotace.getAngular().setZ(0);
rotace.getLinear().setX(0);
if (leftE) {
leftE = false;
left = false;
rotace.getAngular().setZ(-2);
for (int i = 0; i < 6; i++) {
publisherRotate.publish(rotace);
Thread.sleep(300);
}
} else if (rightE) {
rightE = false;
right = false;
rotace.getAngular().setZ(2);
for (int i = 0; i < 6; i++) {
publisherRotate.publish(rotace);
Thread.sleep(300);
}
} else if (goE) {
goE = false;
go = false;
rotace.getLinear().setX(1.3);
publisherRotate.publish(rotace);
} else if (backE) {
backE = false;
back = false;
rotace.getLinear().setX(-1.3);
publisherRotate.publish(rotace);
}
/* Pohyb k cily */
if (moveE) {
moveE = false;
move = false;
seq++;
cil.getHeader().setFrameId("/map");
cil.getHeader().setSeq(seq);
cil.getPose().getPosition().setX(x);
cil.getPose().getPosition().setY(y);
System.out.println("PublisherMove: X:" + x + ",Y:" + y);
cil.getPose().getOrientation().setX(0);
cil.getPose().getOrientation().setY(0);
cil.getPose().getOrientation().setZ(0);
cil.getPose().getOrientation().setW(1);
publisherMove.publish(cil);
// Toast.makeText(ctxt, "Robot is moving to goal", Toast.LENGTH_LONG).show();
}
Thread.sleep(300);
}
});
}
public void sendMessage(java.lang.String p_string) {
string = publisher.newMessage();
string.setData(p_string);
publisher.publish(string);
}
@Override
public void onShutdown(VisualizationView view, Node node) {
posePublisher.shutdown();
}
