Esempio n. 1
0
  /** Entry hook for ROS when called as stand-alone node */
  @Override
  public void onStart(Node node) {
    System.out.println("REACHED ROBOT NAVIGATION");

    motionPub = node.newPublisher("/command/PathFollowing", "rss_msgs/MotionMsg");
    localSub = node.newSubscriber("/rss/localization", "rss_msgs/OdometryMsg");
    pathSub = node.newSubscriber("/command/path", "lab6_msgs/GUIPolyMsg");

    pathSub.addMessageListener(
        new MessageListener<org.ros.message.lab6_msgs.GUIPolyMsg>() {
          @Override
          public void onNewMessage(org.ros.message.lab6_msgs.GUIPolyMsg message) {
            for (int i = 0; i < message.numVertices; i++) {
              Point2D.Double point = new Point2D.Double(message.x[i], message.y[i]);
              goalPath.add(point);
            }
          }
        });

    localSub.addMessageListener(
        new MessageListener<org.ros.message.rss_msgs.OdometryMsg>() {
          @Override
          public void onNewMessage(org.ros.message.rss_msgs.OdometryMsg message) {
            System.out.println("Getting Odometry");
            locX = message.x;
            locY = message.y;
            locTheta = message.theta;

            navigateRobot();
          }
        });
  }
Esempio n. 2
0
  /** Converts the given TransformStorage datastructure to a TransformStamped message */
  protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
    Vector3d tTF = tf.getTranslation();
    Quat4d rTF = tf.getRotation();

    // convert quaternion and translation vector to corresponding messages
    geometry_msgs.Vector3 tMsg =
        node.getTopicMessageFactory().newFromType(geometry_msgs.Vector3._TYPE);
    geometry_msgs.Quaternion rMsg =
        node.getTopicMessageFactory().newFromType(geometry_msgs.Quaternion._TYPE);
    tMsg.setX(tTF.x);
    tMsg.setY(tTF.y);
    tMsg.setZ(tTF.z);
    rMsg.setX(rTF.x);
    rMsg.setY(rTF.y);
    rMsg.setZ(rTF.z);
    rMsg.setW(rTF.w);

    // create TransformStamped message
    TransformStamped msg = node.getTopicMessageFactory().newFromType(TransformStamped._TYPE);
    msg.getHeader().setFrameId(tf.getParentFrame().getFrameID());
    msg.getHeader().setStamp(new Time(tf.getTimeStamp()));
    msg.setChildFrameId(tf.getChildFrame().getFrameID());
    msg.getTransform().setTranslation(tMsg);
    msg.getTransform().setRotation(rMsg);

    return msg;
  }
Esempio n. 3
0
  /** Converts transform (a geometry msg) to a TransformStorage object and adds it to the buffer. */
  protected boolean setTransform(TransformStamped transform) {
    // resolve the frame ID's
    String childFrameID = assertResolved(tfPrefix, transform.getChildFrameId());
    String frameID = assertResolved(tfPrefix, transform.getHeader().getFrameId());

    boolean errorExists = false;
    if (childFrameID == frameID) {
      node.getLog()
          .error(
              "TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id  \""
                  + childFrameID
                  + "\" because they are the same");
      errorExists = true;
    }

    if (childFrameID == "/") { // empty frame id will be mapped to "/"
      node.getLog()
          .error("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
      errorExists = true;
    }

    if (frameID == "/") { // empty parent id will be mapped to "/"
      node.getLog()
          .error(
              "TF_NO_FRAME_ID: Ignoring transform with child_frame_id \""
                  + childFrameID
                  + "\" because frame_id not set");
      errorExists = true;
    }

    if (errorExists) return false;

    // lookup or insert child frame
    Frame frame = lookupOrInsertFrame(childFrameID);

    // convert tf message to JTransform datastructure
    transform.setChildFrameId(childFrameID);
    transform.getHeader().setFrameId(frameID);
    TransformStorage tf = transformStampedMsgToTF(transform);

    // try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
    if (!frame.insertData(tf)) {
      node.getLog()
          .warn(
              "TF_OLD_DATA ignoring data from the past for frame \""
                  + childFrameID
                  + "\" at time "
                  + ((double) tf.getTimeStamp() / 1E9));
      return false;
    }

    return true;
  }
 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);
 }
Esempio n. 5
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 void addExchangeListCallback(MessageListener<AppInstallationState> callback)
     throws RosException {
   Subscriber<AppInstallationState> s =
       node.newSubscriber(
           resolver.resolve("exchange_app_list"), "app_manager/AppInstallationState");
   s.addMessageListener(callback);
   subscriptions.add(s);
 }
 public void listApps(final ServiceResponseListener<ListApps.Response> callback) {
   try {
     ServiceClient<ListApps.Request, ListApps.Response> listAppsClient =
         node.newServiceClient(resolver.resolve("list_apps"), "app_manager/ListApps");
     listAppsClient.call(new ListApps.Request(), callback);
   } catch (Throwable ex) {
     callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
   }
 }
 /**
  * Blocks until App Manager is located.
  *
  * @param node
  * @param robotName
  * @return
  * @throws AppManagerNotAvailableException
  * @throws RosException
  */
 public static AppManager create(Node node, String robotName)
     throws XmlRpcTimeoutException, AppManagerNotAvailableException, RosException {
   NameResolver resolver = node.getResolver().newChild(new GraphName(robotName));
   try {
     return new AppManager(node, resolver);
   } catch (java.lang.RuntimeException ex) {
     throw new AppManagerNotAvailableException(ex);
   }
 }
Esempio n. 9
0
 /**
  * Returns the resolves version of the given frame ID, and asserts a debug message if the name was
  * not fully resolved.
  */
 private String assertResolved(String prefix, String frameID) {
   if (!frameID.startsWith("/"))
     node.getLog()
         .debug(
             "TF operating on not fully resolved frame id "
                 + frameID
                 + ", resolving using local prefix "
                 + prefix);
   return resolve(prefix, frameID);
 }
Esempio n. 10
0
 public void uninstallApp(
     final String appName, final ServiceResponseListener<UninstallApp.Response> callback) {
   try {
     ServiceClient<UninstallApp.Request, UninstallApp.Response> uninstallAppClient =
         node.newServiceClient(resolver.resolve("uninstall_app"), "app_manager/UninstallApp");
     UninstallApp.Request request = new UninstallApp.Request();
     request.name = appName;
     uninstallAppClient.call(request, callback);
   } catch (Throwable ex) {
     callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
   }
 }
Esempio n. 11
0
 public void listExchangeApps(
     boolean remoteUpdate, final ServiceResponseListener<GetInstallationState.Response> callback) {
   try {
     ServiceClient<GetInstallationState.Request, GetInstallationState.Response> listAppsClient =
         node.newServiceClient(
             resolver.resolve("list_exchange_apps"), "app_manager/GetInstallationState");
     GetInstallationState.Request request = new GetInstallationState.Request();
     request.remote_update = remoteUpdate;
     listAppsClient.call(request, callback);
   } catch (Throwable ex) {
     callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
   }
 }
Esempio n. 12
0
 public void getAppDetails(
     final String appName, final ServiceResponseListener<GetAppDetails.Response> callback) {
   try {
     ServiceClient<GetAppDetails.Request, GetAppDetails.Response> startAppClient =
         node.newServiceClient(resolver.resolve("get_app_details"), "app_manager/GetAppDetails");
     Log.i("AppManager", "Start app service client created");
     GetAppDetails.Request request = new GetAppDetails.Request();
     request.name = appName;
     startAppClient.call(request, callback);
     Log.i("AppManager", "Done call");
   } catch (Throwable ex) {
     Log.i("AppManager", "Get app details failed: " + ex.toString());
     callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
   }
 }
Esempio n. 13
0
 @Override
 public void onStart(Node node) {
   System.out.println("KINECT NODE STARTED");
   kinSub = node.newSubscriber("/camera/depth_registered/points", "sensor_msgs/PointCloud2");
   kinSub.addMessageListener(
       new MessageListener<org.ros.message.sensor_msgs.PointCloud2>() {
         @Override
         public synchronized void onNewMessage(org.ros.message.sensor_msgs.PointCloud2 message) {
           unpackPointCloudData(
               (int) message.width,
               (int) message.height,
               (int) message.point_step,
               (int) message.row_step,
               message.data);
         }
       });
 }
Esempio n. 14
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();
 }
Esempio n. 15
0
 public void publishTopic(Node node) {
   topic = node.newPublisher("" + super.getName(), messageType);
 }
Esempio n. 16
0
  /**
   * Returns the transform from the specified source frame to the target frame at a given time;
   * returns null if no transformation could be found.
   */
  public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
    // resolve the source and target IDs
    String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
    String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);

    // if source and target are the same, return the identity transform
    if (resolvedSourceID == resolvedTargetID) {
      StampedTransform out = StampedTransform.getIdentity();
      out.timeStamp = time;
      out.frameID = resolvedSourceID;
      out.childFrameID = resolvedTargetID;
      return out;
    }

    // lookup source and target frame
    Frame sourceFrame = frames.get(resolvedSourceID);
    Frame targetFrame = frames.get(resolvedTargetID);

    if (sourceFrame == null) {
      node.getLog()
          .error("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
      return null;
    }

    if (targetFrame == null) {
      node.getLog()
          .error("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
      return null;
    }

    // list that will contain transformations from source frame to some frame F
    LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
    // list that will contain transformations from frame F to target frame
    LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();

    // fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
    if (!lookupLists(
        targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
      // TODO give warning
      node.getLog()
          .error(
              "Cannot transform: source + \""
                  + resolvedSourceID
                  + "\" and target \""
                  + resolvedTargetID
                  + "\" are not connected.");
      return null;
    }

    // create an identity transform with the correct time stamp
    StampedTransform out = StampedTransform.getIdentity();
    out.timeStamp = time;

    // multiply all transforms from source frame to frame F TODO: right?
    for (TransformStorage t : inverseTransforms) {
      out.mul(StorageToStampedTransform(t));
    }

    // multiply all transforms from frame F to target frame TODO: right?
    for (TransformStorage t : forwardTransforms) {
      out.mul(StorageToStampedTransform(t).invert(), out);
    }

    // return transform
    return out;
  }
Esempio n. 17
0
 @Override
 public void onShutdown(Node node) {
   if (node != null) {
     node.shutdown();
   }
 }
 RosoutLogger(Node node) {
   this.node = node;
   publisher = node.newPublisher(ROSOUT_TOPIC, "rosgraph_msgs/Log");
   log = LogFactory.getLog(node.getName().toString());
 }
Esempio n. 19
0
 /**
  * Called when the {@link Node} experiences an unrecoverable error.
  *
  * @param node the {@link Node} that experienced the error
  * @param throwable the {@link Throwable} describing the error condition
  */
 @Override
 public void onError(Node node, Throwable throwable) {
   Log.w(C.TAG, "[ " + node.getName() + " ] onError [ " + QUEUE_NAME + " ]", throwable);
 }
Esempio n. 20
0
 public void addAppListCallback(MessageListener<AppList> callback) throws RosException {
   Subscriber<AppList> s = node.newSubscriber(resolver.resolve("app_list"), "app_manager/AppList");
   s.addMessageListener(callback);
   subscriptions.add(s);
 }
Esempio n. 21
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);
    }
Esempio n. 22
0
  @Override
  public void onStart(Node node) {
    ParameterTree paramTree = node.newParameterTree();
    mapFileName = paramTree.getString(node.resolveName("~/mapFileName"));

    last_localization = System.currentTimeMillis();

    try {
      polymap = new PolygonMap(mapFileName);
    } catch (Exception e) {
      e.printStackTrace();
    }

    Point2D.Double robotStart = polymap.getRobotStart();

    Rectangle2D.Double cSpaceWorld =
        CSpace.CSpaceWorldRect(polymap.getWorldRect(), polymap.getRobotStart(), RADIUS);
    List<PolygonObstacle> cSpaceObstacles =
        CSpace.computeCSpace(polymap, getRobot(), polymap.getRobotStart());

    Map<int[], Point2D.Double> fiducialPairs = new HashMap<int[], Point2D.Double>();

    // Finish adding to fiducial pairs here from map

    localization = new Localization(cSpaceObstacles, cSpaceWorld, robotStart, fiducialPairs);

    blobTrack = new BlobTracking(width, height);
    final boolean reverseRGB = node.newParameterTree().getBoolean("reverse_rgb", false);

    localPub = node.newPublisher("/rss/localization", "rss_msgs/OdometryMsg");
    stopPub = node.newPublisher("/rss/stop", "rss_msgs/ResetMsg");

    vidSub = node.newSubscriber("/rss/video", "sensor_msgs/Image");
    vidSub.addMessageListener(
        new MessageListener<org.ros.message.sensor_msgs.Image>() {
          @Override
          public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
            byte[] rgbData;
            if (reverseRGB) {
              rgbData = Image.RGB2BGR(message.data, (int) message.width, (int) message.height);
            } else {
              rgbData = message.data;
            }
            assert ((int) message.width == width);
            assert ((int) message.height == height);
            handle(rgbData);
          }
        });

    depthSub = node.newSubscriber("/rss/depth", "sensor_msgs/Image");
    depthSub.addMessageListener(
        new MessageListener<org.ros.message.sensor_msgs.Image>() {
          @Override
          public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
            byte[] rgbData;
            if (reverseRGB) {
              rgbData = Image.RGB2BGR(message.data, (int) message.width, (int) message.height);
            } else {
              rgbData = message.data;
            }
            assert ((int) message.width == width);
            assert ((int) message.height == height);
            handle(rgbData);
          }
        });

    odoSub = node.newSubscriber("/rss/odometry", "rss_msgs/OdometryMsg");
    odoSub.addMessageListener(
        new MessageListener<org.ros.message.rss_msgs.OdometryMsg>() {
          @Override
          public void onNewMessage(org.ros.message.rss_msgs.OdometryMsg message) {
            odo_x = message.x;
            odo_y = message.y;
            odo_theta = message.theta;

            Point2D.Double curr_point;
            OdometryMsg msg = new OdometryMsg();
            if (LOCALIZE) {
              synchronized (localization) {
                curr_point = localization.encoderUpdate(odo_x, odo_y);
              }
              msg.x = curr_point.x;
              msg.y = curr_point.y;
            } else {
              msg.x = odo_x;
              msg.y = odo_y;
            }
            msg.theta = odo_theta;
            localPub.publish(msg);
          }
        });

    Thread visionThread =
        new Thread(
            new Runnable() {
              @Override
              public void run() {
                handleImage();
                try {
                  Thread.sleep(10);
                } catch (Exception exc) {
                  exc.printStackTrace();
                }
              }
            });
    visionThread.start();
  }
Esempio n. 23
0
 /**
  * Called when the {@link Node} has shut down.
  *
  * @param node the {@link Node} that has shut down
  */
 @Override
 public void onShutdownComplete(Node node) {
   Log.i(C.TAG, "[ " + node.getName() + " ] onShutdownComplete [ " + QUEUE_NAME + " ]");
 }