/** 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();
}
});
}
/** Converts the given TransformStorage datastructure to a TransformStamped message */
protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
Vector3d tTF = tf.getTranslation();
Quat4d rTF = tf.getRotation();
// convert quaternion and translation vector to corresponding messages
geometry_msgs.Vector3 tMsg =
node.getTopicMessageFactory().newFromType(geometry_msgs.Vector3._TYPE);
geometry_msgs.Quaternion rMsg =
node.getTopicMessageFactory().newFromType(geometry_msgs.Quaternion._TYPE);
tMsg.setX(tTF.x);
tMsg.setY(tTF.y);
tMsg.setZ(tTF.z);
rMsg.setX(rTF.x);
rMsg.setY(rTF.y);
rMsg.setZ(rTF.z);
rMsg.setW(rTF.w);
// create TransformStamped message
TransformStamped msg = node.getTopicMessageFactory().newFromType(TransformStamped._TYPE);
msg.getHeader().setFrameId(tf.getParentFrame().getFrameID());
msg.getHeader().setStamp(new Time(tf.getTimeStamp()));
msg.setChildFrameId(tf.getChildFrame().getFrameID());
msg.getTransform().setTranslation(tMsg);
msg.getTransform().setRotation(rMsg);
return msg;
}
/** Converts transform (a geometry msg) to a TransformStorage object and adds it to the buffer. */
protected boolean setTransform(TransformStamped transform) {
// resolve the frame ID's
String childFrameID = assertResolved(tfPrefix, transform.getChildFrameId());
String frameID = assertResolved(tfPrefix, transform.getHeader().getFrameId());
boolean errorExists = false;
if (childFrameID == frameID) {
node.getLog()
.error(
"TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id \""
+ childFrameID
+ "\" because they are the same");
errorExists = true;
}
if (childFrameID == "/") { // empty frame id will be mapped to "/"
node.getLog()
.error("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
errorExists = true;
}
if (frameID == "/") { // empty parent id will be mapped to "/"
node.getLog()
.error(
"TF_NO_FRAME_ID: Ignoring transform with child_frame_id \""
+ childFrameID
+ "\" because frame_id not set");
errorExists = true;
}
if (errorExists) return false;
// lookup or insert child frame
Frame frame = lookupOrInsertFrame(childFrameID);
// convert tf message to JTransform datastructure
transform.setChildFrameId(childFrameID);
transform.getHeader().setFrameId(frameID);
TransformStorage tf = transformStampedMsgToTF(transform);
// try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
if (!frame.insertData(tf)) {
node.getLog()
.warn(
"TF_OLD_DATA ignoring data from the past for frame \""
+ childFrameID
+ "\" at time "
+ ((double) tf.getTimeStamp() / 1E9));
return false;
}
return true;
}
private void publish(byte level, Object message) {
org.ros.message.rosgraph_msgs.Log m = new org.ros.message.rosgraph_msgs.Log();
m.header.stamp = node.getCurrentTime();
m.level = level;
m.name = node.getName().toString();
m.msg = message.toString();
// TODO(damonkohler): Should return list of all published and subscribed
// topics for the node that created this logger. This helps filter the
// rosoutconsole.
m.topics = Lists.newArrayList();
publisher.publish(m);
}
public void updateTopic(Node node, long seq) {
message.header.seq = seq;
message.header.stamp = node.getCurrentTime();
message.header.frame_id = "/robot";
pressed = touch.isPressed();
message.contact = pressed;
topic.publish(message);
}
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);
}
public void listApps(final ServiceResponseListener<ListApps.Response> callback) {
try {
ServiceClient<ListApps.Request, ListApps.Response> listAppsClient =
node.newServiceClient(resolver.resolve("list_apps"), "app_manager/ListApps");
listAppsClient.call(new ListApps.Request(), callback);
} catch (Throwable ex) {
callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
}
}
/**
* Blocks until App Manager is located.
*
* @param node
* @param robotName
* @return
* @throws AppManagerNotAvailableException
* @throws RosException
*/
public static AppManager create(Node node, String robotName)
throws XmlRpcTimeoutException, AppManagerNotAvailableException, RosException {
NameResolver resolver = node.getResolver().newChild(new GraphName(robotName));
try {
return new AppManager(node, resolver);
} catch (java.lang.RuntimeException ex) {
throw new AppManagerNotAvailableException(ex);
}
}
/**
* Returns the resolves version of the given frame ID, and asserts a debug message if the name was
* not fully resolved.
*/
private String assertResolved(String prefix, String frameID) {
if (!frameID.startsWith("/"))
node.getLog()
.debug(
"TF operating on not fully resolved frame id "
+ frameID
+ ", resolving using local prefix "
+ prefix);
return resolve(prefix, frameID);
}
public void uninstallApp(
final String appName, final ServiceResponseListener<UninstallApp.Response> callback) {
try {
ServiceClient<UninstallApp.Request, UninstallApp.Response> uninstallAppClient =
node.newServiceClient(resolver.resolve("uninstall_app"), "app_manager/UninstallApp");
UninstallApp.Request request = new UninstallApp.Request();
request.name = appName;
uninstallAppClient.call(request, callback);
} catch (Throwable ex) {
callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
}
}
public void listExchangeApps(
boolean remoteUpdate, final ServiceResponseListener<GetInstallationState.Response> callback) {
try {
ServiceClient<GetInstallationState.Request, GetInstallationState.Response> listAppsClient =
node.newServiceClient(
resolver.resolve("list_exchange_apps"), "app_manager/GetInstallationState");
GetInstallationState.Request request = new GetInstallationState.Request();
request.remote_update = remoteUpdate;
listAppsClient.call(request, callback);
} catch (Throwable ex) {
callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
}
}
public void getAppDetails(
final String appName, final ServiceResponseListener<GetAppDetails.Response> callback) {
try {
ServiceClient<GetAppDetails.Request, GetAppDetails.Response> startAppClient =
node.newServiceClient(resolver.resolve("get_app_details"), "app_manager/GetAppDetails");
Log.i("AppManager", "Start app service client created");
GetAppDetails.Request request = new GetAppDetails.Request();
request.name = appName;
startAppClient.call(request, callback);
Log.i("AppManager", "Done call");
} catch (Throwable ex) {
Log.i("AppManager", "Get app details failed: " + ex.toString());
callback.onFailure(new RemoteException(ERROR_STATUS, ex.toString()));
}
}
@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);
}
});
}
/**
* Called when the {@link ConnectedNode} has started shutting down. Shutdown will be delayed,
* although not indefinitely, until all {@link NodeListener}s have returned from this method.
*
* <p>Since this method can potentially delay {@link ConnectedNode} shutdown, it is preferred to
* use {@link #onShutdownComplete(Node)} when {@link ConnectedNode} resources are not required
* during the method call.
*
* @param node the {@link Node} that has started shutting down
*/
@Override
public void onShutdown(Node node) {
Log.i(C.TAG, "[ " + node.getName() + " ] onShutdown [ " + QUEUE_NAME + " ]");
publisher.shutdown();
}
public void publishTopic(Node node) {
topic = node.newPublisher("" + super.getName(), messageType);
}
/**
* Returns the transform from the specified source frame to the target frame at a given time;
* returns null if no transformation could be found.
*/
public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
// resolve the source and target IDs
String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);
// if source and target are the same, return the identity transform
if (resolvedSourceID == resolvedTargetID) {
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
out.frameID = resolvedSourceID;
out.childFrameID = resolvedTargetID;
return out;
}
// lookup source and target frame
Frame sourceFrame = frames.get(resolvedSourceID);
Frame targetFrame = frames.get(resolvedTargetID);
if (sourceFrame == null) {
node.getLog()
.error("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
return null;
}
if (targetFrame == null) {
node.getLog()
.error("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
return null;
}
// list that will contain transformations from source frame to some frame F
LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
// list that will contain transformations from frame F to target frame
LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();
// fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
if (!lookupLists(
targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
// TODO give warning
node.getLog()
.error(
"Cannot transform: source + \""
+ resolvedSourceID
+ "\" and target \""
+ resolvedTargetID
+ "\" are not connected.");
return null;
}
// create an identity transform with the correct time stamp
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
// multiply all transforms from source frame to frame F TODO: right?
for (TransformStorage t : inverseTransforms) {
out.mul(StorageToStampedTransform(t));
}
// multiply all transforms from frame F to target frame TODO: right?
for (TransformStorage t : forwardTransforms) {
out.mul(StorageToStampedTransform(t).invert(), out);
}
// return transform
return out;
}
@Override
public void onShutdown(Node node) {
if (node != null) {
node.shutdown();
}
}
RosoutLogger(Node node) {
this.node = node;
publisher = node.newPublisher(ROSOUT_TOPIC, "rosgraph_msgs/Log");
log = LogFactory.getLog(node.getName().toString());
}
/**
* Called when the {@link Node} experiences an unrecoverable error.
*
* @param node the {@link Node} that experienced the error
* @param throwable the {@link Throwable} describing the error condition
*/
@Override
public void onError(Node node, Throwable throwable) {
Log.w(C.TAG, "[ " + node.getName() + " ] onError [ " + QUEUE_NAME + " ]", throwable);
}
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);
}
// 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);
}
@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();
}
/**
* Called when the {@link Node} has shut down.
*
* @param node the {@link Node} that has shut down
*/
@Override
public void onShutdownComplete(Node node) {
Log.i(C.TAG, "[ " + node.getName() + " ] onShutdownComplete [ " + QUEUE_NAME + " ]");
}