// robot reference point is the origin public CSpace(Polygon robot, PolygonMap map) { Rectangle2D.Double worldRect = map.getWorldRect(); constructor( robot, worldRect.getMinX(), worldRect.getMinY(), worldRect.getMaxX(), worldRect.getMaxY()); for (PolygonObstacle obstacle : map.getObstacles()) { addObstacle(obstacle); } }
public PolygonObstacle getRobot() { Point2D.Double center = polymap.getRobotStart(); PolygonObstacle robot = new PolygonObstacle(); double x, y, angle; for (int i = 0; i < NUM_SIDES; i++) { angle = i * 2.0 * Math.PI / NUM_SIDES; x = RADIUS * Math.cos(angle); y = RADIUS * Math.sin(angle); robot.addVertex(x, y); } robot.close(); return robot; }
@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(); }