コード例 #1
0
ファイル: PathFollower.java プロジェクト: dmendelsohn/6.141
  /** 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();
          }
        });
  }
コード例 #2
0
 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);
 }
コード例 #3
0
ファイル: KinectData.java プロジェクト: jefesaurus/rss14
 @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);
         }
       });
 }
コード例 #4
0
ファイル: Vision.java プロジェクト: dmendelsohn/6.141
  @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();
  }
コード例 #5
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);
 }