/** 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();
}
});
}
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);
}
@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);
}
});
}
@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();
}
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);
}
