@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();
}
@Override
public void onStart(final ConnectedNode connectedNode) {
Map<GraphName, GraphName> remaps = connectedNode.getResolver().getRemappings();
System.out.println("my namespace is: " + connectedNode.getResolver().getNamespace());
if (remaps.containsKey("noIns")) {
System.out.println("noooooo iiiiiiins " + remaps.get("noIns"));
}
ParameterTree pt = connectedNode.getParameterTree();
ParameterTreeCrawler ptc = new ParameterTreeCrawler(pt);
ptc.printAll();
if (pt.has("noIns")) {
System.out.println("fffffffffff " + pt.getInteger("noIns"));
}
System.out.println(ptc.getAllRemapps(remaps));
System.out.println("reeeeeee " + GraphName.of("ccc").isRelative());
// System.out.println("tried to get param from my NS:
// "+pt.getString(GraphName.of("ccc").toGlobal()));
System.out.println("--a is: " + GraphName.of("b"));
System.out.println("--a is: " + GraphName.of("~b"));
// System.out.println("--a is: "+GraphName.of("_b"));
// System.out.println("--a is: "+GraphName.of("__b"));
// connectedNode.getResolver().getNamespace()
log = connectedNode.getLog();
/*
// register subscribers
subscriberA = connectedNode.newSubscriber(aT, std_msgs.Bool._TYPE);
subscriberA.addMessageListener(new MessageListener<std_msgs.Bool>() {
@Override
public void onNewMessage(Bool message) {
a = message.getData();
//y = copute(a);
send();
}
});// dynamic
// register publisher
publisher = connectedNode.newPublisher(yT, std_msgs.Bool._TYPE);
inited = true;
// infinite loop
connectedNode.executeCancellableLoop(new CancellableLoop() {
@Override
protected void setup() {
}
@Override
protected void loop() throws InterruptedException {
if(SEND){
std_msgs.Bool out = publisher.newMessage();
out.setData(y);
publisher.publish(out);
log.info("Publishing this: \"" + out.getData() + " !! on topic: "+yT);
}
Thread.sleep(sleepTime);
}
});*/
}
