@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!";
  }
Beispiel #6
0
  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);
    }
  }
Beispiel #7
0
 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);
      }
    }
Beispiel #11
0
    // 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);
    }
Beispiel #12
0
 /**
  * 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();
 }
Beispiel #13
0
 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();
 }