/** 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();
}
});
}
@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();
}
// 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);
}
public void publishTopic(Node node) {
topic = node.newPublisher("" + super.getName(), messageType);
}
RosoutLogger(Node node) {
this.node = node;
publisher = node.newPublisher(ROSOUT_TOPIC, "rosgraph_msgs/Log");
log = LogFactory.getLog(node.getName().toString());
}