diff --git a/.vscode/launch.json b/.vscode/launch.json index 7c79804..3704543 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -10,9 +10,9 @@ "request": "launch", "module": "car", "env": { - "CAR_LIDAR": "LIDAR_MOCK", "CAR_VEHICLE": "CAR_MOCK", - // "LIDAR_DEVICE": "/dev/tty.usbserial-0001" + // "CAR_LIDAR": "/dev/tty.usbserial-0001", + "CAR_LIDAR": "LIDAR_MOCK" } }, { diff --git a/SwiftyCar/Package.swift b/SwiftyCar/Package.swift index 22eebe7..a46de07 100644 --- a/SwiftyCar/Package.swift +++ b/SwiftyCar/Package.swift @@ -13,13 +13,15 @@ let package = Package( // .package(url: /* package url */, from: "1.0.0"), .package(url: "https://github.com/grpc/grpc-swift.git", from: "1.0.0-alpha.12"), .package(url: "https://github.com/uraimo/SwiftyGPIO.git", from: "1.0.0"), + .package(url: "https://vato.ddns.net/gitlab/vato007/swiftrplidar.git", .branch("master")), + .package(url: "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", .branch("dtr_support")) ], targets: [ // Targets are the basic building blocks of a package. A target can define a module or a test suite. // Targets can depend on other targets in this package, and on products in packages which this package depends on. .target( name: "SwiftyCar", - dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift")]), + dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift"), "SwiftRPLidar"]), .testTarget( name: "SwiftyCarTests", dependencies: ["SwiftyCar"]), diff --git a/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift b/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift new file mode 100644 index 0000000..4397fba --- /dev/null +++ b/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift @@ -0,0 +1,61 @@ +// +// LidarProvider.swift +// +// +// Created by Michael Pivato on 10/7/20. +// + +import Foundation +import GRPC +import NIO +import SwiftProtobuf +import SwiftRPLidar + +class LidarProvider: Persontracking_PersonTrackingProvider { + + private let lidar: SwiftRPLidar + private var shouldScan: Bool = false + + init(lidar: SwiftRPLidar) { + self.lidar = lidar + } + + func set_tracking_group(request: Persontracking_Int32Value, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func stop_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture { + shouldScan = false + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func start_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func record(request: Google_Protobuf_BoolValue, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func save_lidar(request: MotorControl_SaveRequest, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func lidar_stream(request: Persontracking_StreamMessage, context: StreamingResponseCallContext) -> EventLoopFuture { + shouldScan = true + try! lidar.iterScans{scan in + _ = context.sendResponse(.with{protoScan in + protoScan.points = scan.map{ point in + Persontracking_Point.with{ protoPoint in + protoPoint.angle = Double(point.angle) + protoPoint.distance = Double(point.distance) + // Placeholder group number. + protoPoint.groupNumber = 0 + } + } + }) + return shouldScan + } + return context.eventLoop.makeSucceededFuture(.ok) + } +} diff --git a/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift b/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift index fadc40a..e8eb906 100644 --- a/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift +++ b/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift @@ -19,8 +19,8 @@ class MotorProvider: MotorControl_CarControlProvider{ func set_throttle(request: MotorControl_ThrottleRequest, context: StatusOnlyCallContext) -> EventLoopFuture { self.vehicle.throttle = request.throttle - return context.eventLoop.makeSucceededFuture(.with{ - $0.throttleSet = true + return context.eventLoop.makeSucceededFuture(.with{ throttle in + throttle.throttleSet = true }) } diff --git a/SwiftyCar/Sources/SwiftyCar/main.swift b/SwiftyCar/Sources/SwiftyCar/main.swift index 7afc07c..cec54d4 100644 --- a/SwiftyCar/Sources/SwiftyCar/main.swift +++ b/SwiftyCar/Sources/SwiftyCar/main.swift @@ -7,6 +7,8 @@ import NIO import GRPC +import SwiftRPLidar +import SwiftSerial func doServer() throws { // Copied from examples @@ -16,13 +18,18 @@ func doServer() throws { try! group.syncShutdownGracefully() } - + let lidar = createLidar() + lidar.iterMeasurements{measruement in + print(measruement.quality) + return false + } // Create a provider using the features we read. let provider = try MotorProvider(vehicle: getVehicle2D()) + let trackingProvider = LidarProvider(lidar: lidar) // Start the server and print its address once it has started. let server = Server.insecure(group: group) - .withServiceProviders([provider]) + .withServiceProviders([provider, trackingProvider]) .bind(host: "localhost", port: 0) server.map { @@ -37,11 +44,27 @@ func doServer() throws { }.wait() } +func createLidar() -> SwiftRPLidar{ + return try! SwiftRPLidar(onPort: SerialPort(path: "/dev/cu.usbserial0001")) + +} + // Entry-Point to the Swift Car Controller print("Starting Server") do{ -try doServer() + try doServer() } catch{ print("Server failed") } + +extension SerialPort: LidarSerial{ + public func setBaudrate(baudrate: Int) { + // TODO: handle different baudrates. Only need this for now. + switch baudrate{ + default: + setSettings(receiveRate: .baud115200, transmitRate: .baud115200, minimumBytesToRead: 1) + } + + } +} diff --git a/app/build.gradle b/app/build.gradle index a662531..0818652 100644 --- a/app/build.gradle +++ b/app/build.gradle @@ -36,9 +36,9 @@ dependencies { testImplementation 'junit:junit:4.12' androidTestImplementation 'androidx.test:runner:1.2.0' androidTestImplementation 'androidx.test.espresso:espresso-core:3.2.0' - implementation 'io.grpc:grpc-okhttp:1.28.1' // CURRENT_GRPC_VERSION - implementation 'io.grpc:grpc-protobuf-lite:1.28.1' // CURRENT_GRPC_VERSION - implementation 'io.grpc:grpc-stub:1.28.1' // CURRENT_GRPC_VERSION + implementation 'io.grpc:grpc-okhttp:1.29.0' // CURRENT_GRPC_VERSION + implementation 'io.grpc:grpc-protobuf-lite:1.29.0' // CURRENT_GRPC_VERSION + implementation 'io.grpc:grpc-stub:1.29.0' // CURRENT_GRPC_VERSION implementation 'javax.annotation:javax.annotation-api:1.2' implementation 'org.zeromq:jeromq:0.5.2' } diff --git a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java index aad1dc4..0d9c545 100644 --- a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java +++ b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java @@ -2,11 +2,11 @@ package org.vato.carcontroller.LIDAR; import android.content.Context; import android.content.SharedPreferences; -import android.graphics.Bitmap; import android.graphics.Canvas; import android.graphics.Color; import android.graphics.Paint; import android.util.AttributeSet; +import android.util.Log; import android.view.MotionEvent; import android.view.SurfaceHolder; import android.view.SurfaceView; @@ -17,9 +17,14 @@ import com.google.protobuf.Empty; import org.vato.carcontroller.PersonTrackingGrpc; import org.vato.carcontroller.PointScan; +import org.vato.carcontroller.StreamMessage; import org.vato.carcontroller.Updaters.AbstractUpdater; +import org.vato.carcontroller.Updaters.GrpcUpdater; import org.vato.carcontroller.Updaters.ZmqUpdater; +import java.util.HashMap; +import java.util.Map; +import java.util.Objects; import java.util.stream.Collectors; import io.grpc.ManagedChannel; @@ -27,7 +32,8 @@ import io.grpc.ManagedChannelBuilder; import io.grpc.stub.StreamObserver; public class LidarView extends SurfaceView - implements AbstractUpdater.MapChangedListener { + implements AbstractUpdater.MapChangedListener, + GrpcUpdater.GrpcUpdateBootstrapper { private static final String LIDAR_TOPIC = "lidar_map"; @@ -35,10 +41,12 @@ public class LidarView extends SurfaceView private Thread lidarThread; private String port; private SurfaceHolder surfaceHolder; + private boolean useGrpcStreams; PersonTrackingGrpc.PersonTrackingStub stub; + private float timeBetweenMessages; + private Map groupNumPaints = new HashMap<>(); - private int mBitmapX, mBitmapY, mViewWidth, mViewHeight; - private Bitmap mBitmap; + private int mViewWidth, mViewHeight, centreX, centreY; public LidarView(Context context) { super(context); @@ -60,8 +68,15 @@ public class LidarView extends SurfaceView String host = prefs.getString("host", "10.0.0.53"); port = prefs.getString("zmqPort", "5050"); String gRPCPort = prefs.getString("port", "50051"); - lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC, - host, port); + useGrpcStreams = prefs.getBoolean("use_grpc_streams", false); + timeBetweenMessages = prefs.getFloat("lidar_timeout", 0.1f); + + if (useGrpcStreams) { + lidar = new GrpcUpdater<>(PointScan.getDefaultInstance().getParserForType(), this); + } else { + lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC, + host, port); + } lidar.addMapChangedListener(this); surfaceHolder = getHolder(); lidarThread = new Thread(lidar); @@ -74,7 +89,16 @@ public class LidarView extends SurfaceView * Called by MainActivity.onResume() to start a thread. */ public void resume() { - StreamObserver response = new StreamObserver() { + if (useGrpcStreams) { + lidarThread.start(); + } else { + doZmqLidarStream(); + } + } + + private void doZmqLidarStream() { + // use async grpc method, ZMQ doesn't need to connect straight away. + stub.startTracking(Empty.newBuilder().build(), new StreamObserver() { @Override public void onNext(Empty value) { lidarThread.start(); @@ -90,14 +114,16 @@ public class LidarView extends SurfaceView public void onCompleted() { // Don't care. } - }; - // use async grpc method, ZMQ doesn't need to connect straight away. - stub.startTracking(Empty.newBuilder().build(), response); + }); } @Override protected void onSizeChanged(int w, int h, int oldw, int oldh) { super.onSizeChanged(w, h, oldw, oldh); + mViewWidth = w; + mViewHeight = h; + centreX = w / 2; + centreY = h / 2; } @Override @@ -115,11 +141,26 @@ public class LidarView extends SurfaceView public void stop() { - // TODO: Use grpc to tell zmq to stop. lidar.stop(); + StreamObserver responseObserver = new StreamObserver() { + @Override + public void onNext(Empty value) { + } + + @Override + public void onError(Throwable t) { + Log.d("LIDAR", "Failed to stop SLAM", t); + } + + @Override + public void onCompleted() { + } + }; + stub.stopTracking(Empty.newBuilder().build(), responseObserver); try { lidarThread.join(1000); } catch (InterruptedException e) { + Log.d("LIDAR", "Lidar failed to join", e); } } @@ -129,21 +170,62 @@ public class LidarView extends SurfaceView Canvas canvas = surfaceHolder.lockCanvas(); canvas.save(); canvas.drawColor(Color.WHITE); - for (Point point : points.getPointsList().stream().map(Point::fromProtoPoint).collect( - Collectors.toList())) { + // TODO: Do an initial pass to find the max distance, which will be a scale factor for the other points. + double maxDistance = 0; + for (org.vato.carcontroller.Point point : points.getPointsList()) { + if (point.getDistance() > maxDistance) { + maxDistance = point.getDistance(); + } + } + + final double maxDistanceFinal = maxDistance; + + // Apply scaling factor from max distance. + for (Point point : points.getPointsList().stream().map( + point -> org.vato.carcontroller.Point.newBuilder(point).setDistance( + point.getDistance() / maxDistanceFinal * mViewHeight).build()).map( + Point::fromProtoPoint) + .collect( + Collectors.toList())) { // Now for each point, draw a circle for the point (so it's big enough) in the correct spot, // and create a colour for that point to paint it correctly. - // TODO: Dynamically change the colour of the paint object based on the point group number. - canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint()); + if (!groupNumPaints.containsKey(point.groupNumber)) { + Paint paint = new Paint(); + paint.setColor( + Color.HSVToColor(new float[]{convertGroupNumberToHue( + point.groupNumber), 1f, 1f})); + groupNumPaints.put(point.groupNumber, paint); + } + canvas.drawCircle((float) point.x + centreX, + (float) point.y + centreY, 5, + Objects.requireNonNull(groupNumPaints + .get(point.groupNumber))); // Can't be null as we just added it. } canvas.restore(); surfaceHolder.unlockCanvasAndPost(canvas); } } + + /** + * @param groupNumber + * @return + */ + private static int convertGroupNumberToHue(int groupNumber) { + return (43 * groupNumber) % 360; + } + + @Override + public void bootstrap(StreamObserver responseObserver) { + stub.lidarStream( + StreamMessage.newBuilder().setTimeBetweenMessages(timeBetweenMessages).build(), + responseObserver); + } + private static class Point { private double x; private double y; + private int groupNumber; private Point(double x, double y) { this.x = x; @@ -151,7 +233,9 @@ public class LidarView extends SurfaceView } static Point fromProtoPoint(org.vato.carcontroller.Point point) { - return fromHist(point.getDistance(), point.getAngle()); + Point p = fromHist(point.getDistance(), point.getAngle()); + p.groupNumber = point.getGroupNumber(); + return p; } static Point fromHist(double distance, double angle) { @@ -159,8 +243,8 @@ public class LidarView extends SurfaceView } static Point fromHist(double distance, double angle, Point offset) { - return new Point(distance * Math.sin(angle) + offset.x, - distance * Math.cos(angle) + offset.y); + return new Point(distance * Math.sin(Math.toRadians(angle)) + offset.x, + distance * Math.cos(Math.toRadians(angle)) + offset.y); } } diff --git a/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java b/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java index 55635b6..4b60ff6 100644 --- a/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java +++ b/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java @@ -7,6 +7,7 @@ import android.graphics.Canvas; import android.graphics.Color; import android.graphics.Paint; import android.util.AttributeSet; +import android.util.Log; import android.view.SurfaceHolder; import android.view.SurfaceView; @@ -128,6 +129,20 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL public void stop() { // TODO: Use grpc to tell zmq to stop. slam.stop(); + stub.stopStreaming(Empty.newBuilder().build(), new StreamObserver() { + @Override + public void onNext(Empty value) { + } + + @Override + public void onError(Throwable t) { + Log.d("SLAM", "Failed to stop SLAM", t); + } + + @Override + public void onCompleted() { + } + }); try { mapThread.join(1000); } catch (InterruptedException e) { diff --git a/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java index 2ac8231..ab48e8d 100644 --- a/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java +++ b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java @@ -10,13 +10,14 @@ import io.grpc.stub.StreamObserver; public class GrpcUpdater extends AbstractUpdater { GrpcUpdateBootstrapper bootstrapper; - public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) { + public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) { super(parser); this.bootstrapper = bootstrapper; } @Override public void stop() { + // TODO... may not be needed here. } diff --git a/app/src/main/res/xml/root_preferences.xml b/app/src/main/res/xml/root_preferences.xml index 5b0612d..c72974a 100644 --- a/app/src/main/res/xml/root_preferences.xml +++ b/app/src/main/res/xml/root_preferences.xml @@ -46,6 +46,14 @@ + + + + - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 490fda8..62d4c05 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index a4b4429..a4f0001 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.3-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-6.4.1-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew index 2fe81a7..fbd7c51 100755 --- a/gradlew +++ b/gradlew @@ -82,6 +82,7 @@ esac CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + # Determine the Java command to use to start the JVM. if [ -n "$JAVA_HOME" ] ; then if [ -x "$JAVA_HOME/jre/sh/java" ] ; then @@ -129,6 +130,7 @@ fi if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then APP_HOME=`cygpath --path --mixed "$APP_HOME"` CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` # We build the pattern for arguments to be converted via cygpath diff --git a/gradlew.bat b/gradlew.bat index 9109989..a9f778a 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -84,6 +84,7 @@ set CMD_LINE_ARGS=%* set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + @rem Execute Gradle "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% diff --git a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto b/protobuf/src/main/proto/car/tracking/lidar_tracker.proto index b988413..b4806b7 100644 --- a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto +++ b/protobuf/src/main/proto/car/tracking/lidar_tracker.proto @@ -16,7 +16,7 @@ message Int32Value{ message Point{ double angle = 1; - int32 distance = 2; + double distance = 2; int32 group_number = 3; } @@ -24,6 +24,9 @@ message PointScan{ repeated Point points = 1; } +message StreamMessage{ + float time_between_messages = 1; +} service PersonTracking{ rpc set_tracking_group(Int32Value) returns (google.protobuf.Empty) {} @@ -36,4 +39,6 @@ service PersonTracking{ rpc save_lidar(MotorControl.SaveRequest) returns (google.protobuf.Empty) {} + rpc lidar_stream(StreamMessage) returns (stream PointScan) {} + } \ No newline at end of file diff --git a/car/.dockerignore b/pycar/.dockerignore similarity index 100% rename from car/.dockerignore rename to pycar/.dockerignore diff --git a/car/Assets/ControllerSlideIcon.ai b/pycar/Assets/ControllerSlideIcon.ai similarity index 100% rename from car/Assets/ControllerSlideIcon.ai rename to pycar/Assets/ControllerSlideIcon.ai diff --git a/car/Assets/ControllerSlideIcon.svg b/pycar/Assets/ControllerSlideIcon.svg similarity index 100% rename from car/Assets/ControllerSlideIcon.svg rename to pycar/Assets/ControllerSlideIcon.svg diff --git a/car/Assets/ControllerSlideIconHorizontal.svg b/pycar/Assets/ControllerSlideIconHorizontal.svg similarity index 100% rename from car/Assets/ControllerSlideIconHorizontal.svg rename to pycar/Assets/ControllerSlideIconHorizontal.svg diff --git a/car/COCO-classes.txt b/pycar/COCO-classes.txt similarity index 100% rename from car/COCO-classes.txt rename to pycar/COCO-classes.txt diff --git a/car/Dockerfile b/pycar/Dockerfile similarity index 63% rename from car/Dockerfile rename to pycar/Dockerfile index 3de27be..90e11c9 100644 --- a/car/Dockerfile +++ b/pycar/Dockerfile @@ -1,4 +1,7 @@ -FROM python:3.6-slim +FROM vato.ddns.net:8083/python:3 + +ARG PYPI_USERNAME +ARG PYPI_PASSWORD RUN apt-get update # OpenCV has a LOT of dependencies. @@ -17,14 +20,14 @@ RUN apt-get install -y \ && rm -rf /var/lib/apt/lists/* COPY requirements.txt / -RUN pip install --trusted-host pypi.python.org -r requirements.txt +RUN pip install --index-url https://${PYPI_USERNAME}:${PYPI_PASSWORD}@vato.ddns.net/nexus/repository/pypi-grouped/simple -r requirements.txt WORKDIR /app -COPY . /app +COPY ./src /app # We aren't listening, just connecting, so probs won't need this. # EXPOSE 1883 ENV PYTHONPATH=/app -CMD ["python", "DecisionSystem/CentralisedDecision/cameraserver.py", "-V", "/app/HandRecognitionMacbookFixed.mp4"] \ No newline at end of file +CMD ["python", "-m", "car"] \ No newline at end of file diff --git a/car/MyRaft/Experiment/node1/config.json b/pycar/MyRaft/Experiment/node1/config.json similarity index 100% rename from car/MyRaft/Experiment/node1/config.json rename to pycar/MyRaft/Experiment/node1/config.json diff --git a/car/MyRaft/Experiment/node2/config.json b/pycar/MyRaft/Experiment/node2/config.json similarity index 100% rename from car/MyRaft/Experiment/node2/config.json rename to pycar/MyRaft/Experiment/node2/config.json diff --git a/car/MyRaft/Experiment/node3/config.json b/pycar/MyRaft/Experiment/node3/config.json similarity index 100% rename from car/MyRaft/Experiment/node3/config.json rename to pycar/MyRaft/Experiment/node3/config.json diff --git a/car/MyRaft/candidate.py b/pycar/MyRaft/candidate.py similarity index 100% rename from car/MyRaft/candidate.py rename to pycar/MyRaft/candidate.py diff --git a/car/MyRaft/config.ini b/pycar/MyRaft/config.ini similarity index 100% rename from car/MyRaft/config.ini rename to pycar/MyRaft/config.ini diff --git a/car/MyRaft/config.json b/pycar/MyRaft/config.json similarity index 100% rename from car/MyRaft/config.json rename to pycar/MyRaft/config.json diff --git a/car/MyRaft/follower.py b/pycar/MyRaft/follower.py similarity index 100% rename from car/MyRaft/follower.py rename to pycar/MyRaft/follower.py diff --git a/car/MyRaft/leader.py b/pycar/MyRaft/leader.py similarity index 100% rename from car/MyRaft/leader.py rename to pycar/MyRaft/leader.py diff --git a/car/MyRaft/messages.py b/pycar/MyRaft/messages.py similarity index 100% rename from car/MyRaft/messages.py rename to pycar/MyRaft/messages.py diff --git a/car/MyRaft/messagestrategy.py b/pycar/MyRaft/messagestrategy.py similarity index 100% rename from car/MyRaft/messagestrategy.py rename to pycar/MyRaft/messagestrategy.py diff --git a/car/MyRaft/node.py b/pycar/MyRaft/node.py similarity index 100% rename from car/MyRaft/node.py rename to pycar/MyRaft/node.py diff --git a/car/MyRaft/protos/raft.proto b/pycar/MyRaft/protos/raft.proto similarity index 100% rename from car/MyRaft/protos/raft.proto rename to pycar/MyRaft/protos/raft.proto diff --git a/car/MyRaft/state.py b/pycar/MyRaft/state.py similarity index 100% rename from car/MyRaft/state.py rename to pycar/MyRaft/state.py diff --git a/car/MyRaft/test.py b/pycar/MyRaft/test.py similarity index 100% rename from car/MyRaft/test.py rename to pycar/MyRaft/test.py diff --git a/car/MyRaft/voter.py b/pycar/MyRaft/voter.py similarity index 100% rename from car/MyRaft/voter.py rename to pycar/MyRaft/voter.py diff --git a/car/bitbucket-pipelines.yml b/pycar/bitbucket-pipelines.yml similarity index 100% rename from car/bitbucket-pipelines.yml rename to pycar/bitbucket-pipelines.yml diff --git a/car/build.gradle b/pycar/build.gradle similarity index 100% rename from car/build.gradle rename to pycar/build.gradle diff --git a/car/config.json b/pycar/config.json similarity index 100% rename from car/config.json rename to pycar/config.json diff --git a/car/docker-compose.yml b/pycar/docker-compose.yml similarity index 100% rename from car/docker-compose.yml rename to pycar/docker-compose.yml diff --git a/car/malima_SIU06.pdf b/pycar/malima_SIU06.pdf similarity index 100% rename from car/malima_SIU06.pdf rename to pycar/malima_SIU06.pdf diff --git a/car/requirements.txt b/pycar/requirements.txt similarity index 82% rename from car/requirements.txt rename to pycar/requirements.txt index f27a941..4938578 100644 --- a/car/requirements.txt +++ b/pycar/requirements.txt @@ -1,9 +1,10 @@ numpy opencv-python six +wheel paho-mqtt u-msgpack-python grpcio-tools rplidar +breezyslam pyzmq -wheel \ No newline at end of file diff --git a/car/setup.py b/pycar/setup.py similarity index 100% rename from car/setup.py rename to pycar/setup.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/__init__.py b/pycar/src/car/DecisionSystem/CentralisedDecision/__init__.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/__init__.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/__init__.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/ballotvoter.py b/pycar/src/car/DecisionSystem/CentralisedDecision/ballotvoter.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/ballotvoter.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/ballotvoter.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/cameraserver.py b/pycar/src/car/DecisionSystem/CentralisedDecision/cameraserver.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/cameraserver.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/cameraserver.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/central_server.py b/pycar/src/car/DecisionSystem/CentralisedDecision/central_server.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/central_server.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/central_server.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/centralisedinstance.py b/pycar/src/car/DecisionSystem/CentralisedDecision/centralisedinstance.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/centralisedinstance.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/centralisedinstance.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/commander.py b/pycar/src/car/DecisionSystem/CentralisedDecision/commander.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/commander.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/commander.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/messenger.py b/pycar/src/car/DecisionSystem/CentralisedDecision/messenger.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/messenger.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/messenger.py diff --git a/car/src/car/DecisionSystem/CentralisedDecision/videoget.py b/pycar/src/car/DecisionSystem/CentralisedDecision/videoget.py similarity index 100% rename from car/src/car/DecisionSystem/CentralisedDecision/videoget.py rename to pycar/src/car/DecisionSystem/CentralisedDecision/videoget.py diff --git a/car/src/car/DecisionSystem/DecentralisedActivityFusion/voter.py b/pycar/src/car/DecisionSystem/DecentralisedActivityFusion/voter.py similarity index 100% rename from car/src/car/DecisionSystem/DecentralisedActivityFusion/voter.py rename to pycar/src/car/DecisionSystem/DecentralisedActivityFusion/voter.py diff --git a/car/src/car/DecisionSystem/__init__.py b/pycar/src/car/DecisionSystem/__init__.py similarity index 100% rename from car/src/car/DecisionSystem/__init__.py rename to pycar/src/car/DecisionSystem/__init__.py diff --git a/car/src/car/DecisionSystem/messages.py b/pycar/src/car/DecisionSystem/messages.py similarity index 100% rename from car/src/car/DecisionSystem/messages.py rename to pycar/src/car/DecisionSystem/messages.py diff --git a/car/src/car/GestureRecognition/HandRecHSV.py b/pycar/src/car/GestureRecognition/HandRecHSV.py similarity index 100% rename from car/src/car/GestureRecognition/HandRecHSV.py rename to pycar/src/car/GestureRecognition/HandRecHSV.py diff --git a/car/src/car/GestureRecognition/HandRecV2.py b/pycar/src/car/GestureRecognition/HandRecV2.py similarity index 100% rename from car/src/car/GestureRecognition/HandRecV2.py rename to pycar/src/car/GestureRecognition/HandRecV2.py diff --git a/car/src/car/GestureRecognition/IMG_0818.png b/pycar/src/car/GestureRecognition/IMG_0818.png similarity index 100% rename from car/src/car/GestureRecognition/IMG_0818.png rename to pycar/src/car/GestureRecognition/IMG_0818.png diff --git a/car/src/car/GestureRecognition/IMG_0825.jpg b/pycar/src/car/GestureRecognition/IMG_0825.jpg similarity index 100% rename from car/src/car/GestureRecognition/IMG_0825.jpg rename to pycar/src/car/GestureRecognition/IMG_0825.jpg diff --git a/car/src/car/GestureRecognition/Neural Network hand Tracking.pdf b/pycar/src/car/GestureRecognition/Neural Network hand Tracking.pdf similarity index 100% rename from car/src/car/GestureRecognition/Neural Network hand Tracking.pdf rename to pycar/src/car/GestureRecognition/Neural Network hand Tracking.pdf diff --git a/car/src/car/GestureRecognition/SimpleHandRecogniser.py b/pycar/src/car/GestureRecognition/SimpleHandRecogniser.py similarity index 100% rename from car/src/car/GestureRecognition/SimpleHandRecogniser.py rename to pycar/src/car/GestureRecognition/SimpleHandRecogniser.py diff --git a/car/src/car/GestureRecognition/__init__.py b/pycar/src/car/GestureRecognition/__init__.py similarity index 100% rename from car/src/car/GestureRecognition/__init__.py rename to pycar/src/car/GestureRecognition/__init__.py diff --git a/car/src/car/GestureRecognition/frozen_inference_graph.pb b/pycar/src/car/GestureRecognition/frozen_inference_graph.pb similarity index 100% rename from car/src/car/GestureRecognition/frozen_inference_graph.pb rename to pycar/src/car/GestureRecognition/frozen_inference_graph.pb diff --git a/car/src/car/GestureRecognition/graph.pbtxt b/pycar/src/car/GestureRecognition/graph.pbtxt similarity index 100% rename from car/src/car/GestureRecognition/graph.pbtxt rename to pycar/src/car/GestureRecognition/graph.pbtxt diff --git a/car/src/car/GestureRecognition/handrecogniser.py b/pycar/src/car/GestureRecognition/handrecogniser.py similarity index 100% rename from car/src/car/GestureRecognition/handrecogniser.py rename to pycar/src/car/GestureRecognition/handrecogniser.py diff --git a/car/src/car/GestureRecognition/kaleidoscope.py b/pycar/src/car/GestureRecognition/kaleidoscope.py similarity index 100% rename from car/src/car/GestureRecognition/kaleidoscope.py rename to pycar/src/car/GestureRecognition/kaleidoscope.py diff --git a/car/src/car/GestureRecognition/keras_ex.py b/pycar/src/car/GestureRecognition/keras_ex.py similarity index 100% rename from car/src/car/GestureRecognition/keras_ex.py rename to pycar/src/car/GestureRecognition/keras_ex.py diff --git a/car/src/car/GestureRecognition/opencvtensorflowex.py b/pycar/src/car/GestureRecognition/opencvtensorflowex.py similarity index 100% rename from car/src/car/GestureRecognition/opencvtensorflowex.py rename to pycar/src/car/GestureRecognition/opencvtensorflowex.py diff --git a/car/src/car/GestureRecognition/starkaleid.py b/pycar/src/car/GestureRecognition/starkaleid.py similarity index 100% rename from car/src/car/GestureRecognition/starkaleid.py rename to pycar/src/car/GestureRecognition/starkaleid.py diff --git a/car/src/car/Messaging/__init__.py b/pycar/src/car/__init__.py similarity index 100% rename from car/src/car/Messaging/__init__.py rename to pycar/src/car/__init__.py diff --git a/car/src/car/__main__.py b/pycar/src/car/__main__.py similarity index 100% rename from car/src/car/__main__.py rename to pycar/src/car/__main__.py diff --git a/car/src/car/control/PythonRemoteController.py b/pycar/src/car/control/PythonRemoteController.py similarity index 100% rename from car/src/car/control/PythonRemoteController.py rename to pycar/src/car/control/PythonRemoteController.py diff --git a/car/src/car/__init__.py b/pycar/src/car/control/__init__.py similarity index 100% rename from car/src/car/__init__.py rename to pycar/src/car/control/__init__.py diff --git a/car/src/car/control/__init__.py b/pycar/src/car/control/gpio/__init__.py similarity index 100% rename from car/src/car/control/__init__.py rename to pycar/src/car/control/gpio/__init__.py diff --git a/car/src/car/control/gpio/factory.py b/pycar/src/car/control/gpio/factory.py similarity index 100% rename from car/src/car/control/gpio/factory.py rename to pycar/src/car/control/gpio/factory.py diff --git a/car/src/car/control/gpio/mockvehicle.py b/pycar/src/car/control/gpio/mockvehicle.py similarity index 100% rename from car/src/car/control/gpio/mockvehicle.py rename to pycar/src/car/control/gpio/mockvehicle.py diff --git a/car/src/car/control/gpio/recording_vehicle_decorator.py b/pycar/src/car/control/gpio/recording_vehicle_decorator.py similarity index 100% rename from car/src/car/control/gpio/recording_vehicle_decorator.py rename to pycar/src/car/control/gpio/recording_vehicle_decorator.py diff --git a/car/src/car/control/gpio/vehicle.py b/pycar/src/car/control/gpio/vehicle.py similarity index 100% rename from car/src/car/control/gpio/vehicle.py rename to pycar/src/car/control/gpio/vehicle.py diff --git a/car/src/car/control/motor_servicer.py b/pycar/src/car/control/motor_servicer.py similarity index 100% rename from car/src/car/control/motor_servicer.py rename to pycar/src/car/control/motor_servicer.py diff --git a/car/src/car/controller.py b/pycar/src/car/controller.py similarity index 100% rename from car/src/car/controller.py rename to pycar/src/car/controller.py diff --git a/car/src/car/control/gpio/__init__.py b/pycar/src/car/messaging/__init__.py similarity index 100% rename from car/src/car/control/gpio/__init__.py rename to pycar/src/car/messaging/__init__.py diff --git a/car/src/car/Messaging/message_factory.py b/pycar/src/car/messaging/message_factory.py similarity index 100% rename from car/src/car/Messaging/message_factory.py rename to pycar/src/car/messaging/message_factory.py diff --git a/car/src/car/Messaging/messages.py b/pycar/src/car/messaging/messages.py similarity index 100% rename from car/src/car/Messaging/messages.py rename to pycar/src/car/messaging/messages.py diff --git a/car/src/car/Messaging/mqttsession.py b/pycar/src/car/messaging/mqttsession.py similarity index 100% rename from car/src/car/Messaging/mqttsession.py rename to pycar/src/car/messaging/mqttsession.py diff --git a/car/src/car/slam/__init__.py b/pycar/src/car/slam/__init__.py similarity index 100% rename from car/src/car/slam/__init__.py rename to pycar/src/car/slam/__init__.py diff --git a/car/src/car/slam/slam_processor.py b/pycar/src/car/slam/slam_processor.py similarity index 100% rename from car/src/car/slam/slam_processor.py rename to pycar/src/car/slam/slam_processor.py diff --git a/car/src/car/slam/slam_servicer.py b/pycar/src/car/slam/slam_servicer.py similarity index 97% rename from car/src/car/slam/slam_servicer.py rename to pycar/src/car/slam/slam_servicer.py index d4843be..fe53411 100644 --- a/car/src/car/slam/slam_servicer.py +++ b/pycar/src/car/slam/slam_servicer.py @@ -1,6 +1,6 @@ import car.slam.SlamController_pb2_grpc as grpc import car.slam.SlamController_pb2 as proto -import car.empty_pb2 as empty +import google.protobuf.empty_pb2 as empty import car.slam.slam_streamer as slam from .slam_processor import SlamProcessor diff --git a/car/src/car/slam/slam_streamer.py b/pycar/src/car/slam/slam_streamer.py similarity index 100% rename from car/src/car/slam/slam_streamer.py rename to pycar/src/car/slam/slam_streamer.py diff --git a/car/src/car/slam/zmq_pair_testing/pair.py b/pycar/src/car/slam/zmq_pair_testing/pair.py similarity index 100% rename from car/src/car/slam/zmq_pair_testing/pair.py rename to pycar/src/car/slam/zmq_pair_testing/pair.py diff --git a/car/src/car/tracking/__init__.py b/pycar/src/car/tracking/__init__.py similarity index 100% rename from car/src/car/tracking/__init__.py rename to pycar/src/car/tracking/__init__.py diff --git a/car/src/car/tracking/algorithms.py b/pycar/src/car/tracking/algorithms.py similarity index 64% rename from car/src/car/tracking/algorithms.py rename to pycar/src/car/tracking/algorithms.py index f61741c..5120bc7 100644 --- a/car/src/car/tracking/algorithms.py +++ b/pycar/src/car/tracking/algorithms.py @@ -1,10 +1,12 @@ import math +import numpy as np +from . import icp class Group: - def __init__(self, number, points=[]): - self._points = points + def __init__(self, number): + self._points = [] self._number = number self._minX = None self._maxX = None @@ -28,7 +30,7 @@ class Group: def _update_min_max(self, new_point): """ - Updates the in and max points for this group. + Updates the min and max points for this group. This is to determine when assigning groups whether the same group is selected. """ @@ -104,35 +106,46 @@ def calc_groups(scan): Returns ------- - list - List of groups that were found. + ndarray + Array of groups that were found. """ prevPoint = None - currentGroup = None - allGroups = [] + currentGroup = Group(0) + allGroups = [currentGroup] currentGroupNumber = 0 - # assume the list is already sorted. for point in scan: if prevPoint is None: prevPoint = point + currentGroup.add_point(point) continue # Distances are in mm. - # within 1cm makes a group. Will need to play around with this. - if (point[2] - prevPoint[2]) ** 2 < 10 ** 2: - if currentGroup is None: - currentGroup = Group(currentGroupNumber) - allGroups.append(currentGroup) + # within 10cm makes a group. Will need to play around with this. + if (point[2] - prevPoint[2]) ** 2 < 100 ** 2: currentGroup.add_point(point) else: - if currentGroup is not None: - currentGroupNumber += 1 - currentGroup = None + currentGroupNumber += 1 + currentGroup = Group(currentGroupNumber) + currentGroup.add_point(point) + allGroups.append(currentGroup) prevPoint = point + return np.array(allGroups) - return allGroups + +def calc_groups_edge_algorithm(scan): + """ + Calculates groups using an edge algorithm. This takes advantage of numpy arrays + and vectorisation, rather than the primitive python loop grouping, resulting in + faster grouping speeds. + """ + allGroups = [] + scanArray = np.array(scan) + + +def edge_algorithm(): + pass def find_centre(group): @@ -156,13 +169,59 @@ def assign_groups(prev_groups, new_groups): """ Assigns group numbers to a new scan based on the groups of an old scan. """ + max_group_number = 0 + unassigned_groups = [] for group in prev_groups: old_centre = find_centre(group) for new_group in new_groups: new_centre = find_centre(new_group) - # They are considered the same if the new group and old group centres are within 5cm. + # They are considered the same if the new group and old group centres are within 10cm. if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2: new_group.number = group.number + if group.number > max_group_number: + max_group_number = group.number + continue + # If this is reached, then no matching groups were found. + unassigned_groups.append(new_group) + + for group in unassigned_groups: + max_group_number += 1 + group.number = max_group_number + + return new_groups + + +def assign_groups_II(prev_groups, new_groups): + """ + Performs the assign groups algorithm, but instead of being greedy to assign, it will match up the + closest groups for each group. + + Additionally, the centre of mass for a group of points is now used, which is less prone to the effects of + outliers as the existing find_centre algorithm. + + An ICP rotation/translation is not made in this algorithm, as it's assumed that the scans are quick enough for + there to not be a significant difference between scans that would require ICP. + """ + max_group_number = 0 + unassigned_groups = [] + + def centres_from_groups(groups): + return np.array([icp.calc_mass_centre(np.array([convert_lidar_to_cartesian(point) for point in group.get_points()])) for group in groups]) + + old_group_centres = centres_from_groups(prev_groups) + old_group_indexes = np.arange(len(old_group_centres)) + + new_group_centers = centres_from_groups(new_groups) + new_group_indexes = np.arange(len(new_group_centers)) + + closest_points = icp.closest_points(new_group_centers, old_group_centres) + # Now assign the new groups to the closest matching old group, if the distance is within a certain threshold. + for i, point in enumerate(closest_points): + matching_groups = prev_groups[old_group_centres == point] + # TODO: Check the centres are within a certain threshold. + new_groups[i].number = prev_groups[0].number + + # TODO: Go through to put all groups into one (if multiple groups get same number) to avoid splits. return new_groups diff --git a/car/src/car/tracking/all_scans.txt b/pycar/src/car/tracking/all_scans.txt similarity index 100% rename from car/src/car/tracking/all_scans.txt rename to pycar/src/car/tracking/all_scans.txt diff --git a/car/src/car/tracking/animate.py b/pycar/src/car/tracking/animate.py similarity index 95% rename from car/src/car/tracking/animate.py rename to pycar/src/car/tracking/animate.py index 3e31974..0066137 100755 --- a/car/src/car/tracking/animate.py +++ b/pycar/src/car/tracking/animate.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 '''Animates distances and measurment quality''' -from car.tracking.mock_lidar import MockLidar +from car.tracking..devices.mock_lidar import MockLidar import matplotlib.pyplot as plt import numpy as np import matplotlib.animation as animation diff --git a/car/src/car/tracking/animate_alg.py b/pycar/src/car/tracking/animate_alg.py similarity index 91% rename from car/src/car/tracking/animate_alg.py rename to pycar/src/car/tracking/animate_alg.py index e076542..44271c1 100644 --- a/car/src/car/tracking/animate_alg.py +++ b/pycar/src/car/tracking/animate_alg.py @@ -2,7 +2,7 @@ Animates distances and angle of lidar Uses model-free algorithms to track grouping of points (objects/groups) """ -from tracking.mock_lidar import MockLidar +from car.tracking.devices.mock_lidar import MockLidar import matplotlib.pyplot as plt import numpy as np import matplotlib.animation as animation @@ -35,7 +35,7 @@ class Bunch: def run(): - lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle")) + lidar = MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle")) fig = plt.figure() ax = plt.subplot(111, projection='polar') line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX], diff --git a/car/src/car/tracking/devices/__init__.py b/pycar/src/car/tracking/devices/__init__.py similarity index 100% rename from car/src/car/tracking/devices/__init__.py rename to pycar/src/car/tracking/devices/__init__.py diff --git a/car/src/car/tracking/devices/factory.py b/pycar/src/car/tracking/devices/factory.py similarity index 50% rename from car/src/car/tracking/devices/factory.py rename to pycar/src/car/tracking/devices/factory.py index fd17874..d9b1e02 100644 --- a/car/src/car/tracking/devices/factory.py +++ b/pycar/src/car/tracking/devices/factory.py @@ -3,10 +3,9 @@ from .. import lidar_loader as loader import os MOCK_DEVICE = "LIDAR_MOCK" -RPLIDAR = "LIDAR_RPLIDAR" -def get_lidar(device=None, connection='/dev/ttyUSB0'): +def get_lidar(device=None): actual_device = None try: actual_device = device if device is not None else os.environ["CAR_LIDAR"] @@ -14,17 +13,14 @@ def get_lidar(device=None, connection='/dev/ttyUSB0'): print( 'No lidar device specified and the CAR_LIDAR environment variable is not set.') if actual_device == MOCK_DEVICE: - return MockLidar(loader.load_scans_bytes_file("car/src/car/tracking/out.pickle")) - elif actual_device == RPLIDAR: + return MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle")) + elif actual_device != '': try: - # TODO: Cleanup connection setting, probably don't need to pass it into the method. from rplidar import RPLidar - if "LIDAR_DEVICE" in os.environ: - return RPLidar(os.environ['LIDAR_DEVICE']) - return RPLidar(connection) + return RPLidar(device) except ImportError: print('Could not import RPLidar. Have you downloaded rplidar?') else: - print('No valid lidar device found. Please choose one of ' + - MOCK_DEVICE + ' or ' + RPLIDAR) + print('No valid lidar device found. Please choose ' + + MOCK_DEVICE + ' or a dn address for the lidar device.') return None diff --git a/car/src/car/tracking/devices/mock_lidar.py b/pycar/src/car/tracking/devices/mock_lidar.py similarity index 100% rename from car/src/car/tracking/devices/mock_lidar.py rename to pycar/src/car/tracking/devices/mock_lidar.py diff --git a/car/src/car/tracking/devices/recording_lidar.py b/pycar/src/car/tracking/devices/recording_lidar.py similarity index 100% rename from car/src/car/tracking/devices/recording_lidar.py rename to pycar/src/car/tracking/devices/recording_lidar.py diff --git a/pycar/src/car/tracking/icp.py b/pycar/src/car/tracking/icp.py new file mode 100644 index 0000000..384e042 --- /dev/null +++ b/pycar/src/car/tracking/icp.py @@ -0,0 +1,93 @@ +""" +A module that includes algorithms to execute the Iterative Closest Point (ICP) algorithm in Besl (1992) + +This algorithm reshapes (registers) a set of points to match those in another data set. In object tracking, +this basically moves the tracked groups to be in line with the correct position in the new scan. Additionally, it +can provide an estimation as to how the tracked groups have moved. + +NOTE: This module is thus far untested -> it may be used in the future, and will be tested then, but for +now was created to provide a deeper understanding of ICP and related algorithms. +""" + +import math +import numpy as np + + +def closest_points(data_shape, model_shape): + """ + Returns the closest points from data to model in the same order as the data shape. + """ + def calc_closest_point(data, model_vector): + current_closest = np.array([[0, 0]]) + min_distance = -1 + # Could vectorise this, but the speed advantage would be minimal + for point in model_vector: + euclid_d = euclid_distance(data, point) + if min_distance < 0 or euclid_d < min_distance: + current_closest = point + min_distance = euclid_d + return current_closest + + closest_func = np.vectorize(calc_closest_point) + return closest_func(data_shape, model_shape) + + +def euclid_distance(point1, point2): + return math.sqrt(((point1[0] - point2[0]) ** 2) + ((point1[1] - point2[1]) ** 2)) + + +def calc_mse(data, model, quaternion, translation): + """ + Calculates the mean squared error for the points, after a registration. + """ + return (translation - calc_rotation_matrix(quaternion).dot(data) - translation).sum(0) / len(data) + + +def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray: + """ + Calculates the cross covariance matrix of the two point clouds. + """ + return (data.dot(model.T).sum(0) / data.shape[0]) - calc_mass_centre(data).dot(calc_mass_centre(model).T) + + +def calc_delta(cross_cov): + A = (cross_cov - cross_cov.T) + return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]]) + + +def calc_quaternion(cross_cov, delta): + """ + Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3) + """ + Q = np.array([[[cross_cov.trace()], [delta.T]], + [[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]]) + eigen_values, eigen_vectors = np.linalg.eig(Q) + optimal_eigen_index = eigen_values[eigen_values == eigen_values.max()][0] + return eigen_vectors[:, optimal_eigen_index] + + +def calc_translation(optimal_rotation, mass_centre_data, mass_centre_model): + """ + Calculates the optimal translation with the given (optimal) quaternion (which is converted to a rotation + matrix within the funtion) + """ + return mass_centre_model - optimal_rotation.dot(mass_centre_data) + + +def calc_rotation_matrix(q): + """ + Returns the rotation matrix for the given unit quaternion. + """ + return np.array([[[q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2], [2 * (q[1] * q[2] - q[0] * q[3])], [2 * (q[1] * q[3] - q[0] * q[2])]], + [[2 * (q[1] * q[2] + q[0] * q[3])], [q[0] ** 2 + q[2] ** 2 - + q[1] ** 2 - q[3] ** 2], [2 * (q[2] * q[3] - q[0] * q[1])]], + [[2 * (q[1] * q[3] - q[0] * q[2])], [2 * (q[2] * q[3] + q[0] * q[1])], [2 * (q[2] * q[3] + q[0] * q[1])], [q[0] ** 2 + q[3] ** 2 - q[1] ** 2 - q[2] ** 2]]]) + + +def calc_mass_centre(points: np.ndarray) -> np.ndarray: + """ + Calculates the point at the centre of mass for the given set of points. + + The returned vector is in form 1xn, where point set is of shape mxn + """ + return points.sum(0) / len(points) diff --git a/car/src/car/tracking/lidar_cache.py b/pycar/src/car/tracking/lidar_cache.py similarity index 67% rename from car/src/car/tracking/lidar_cache.py rename to pycar/src/car/tracking/lidar_cache.py index 40eb789..5d1df91 100644 --- a/car/src/car/tracking/lidar_cache.py +++ b/pycar/src/car/tracking/lidar_cache.py @@ -2,12 +2,14 @@ from threading import Thread from car.tracking import algorithms import car.tracking.lidar_tracker_pb2 as tracker_pb import zmq +from car.tracking.devices.mock_lidar import MockLidar +import car.tracking.lidar_loader as lidar_loader class LidarCache(): """ A class that retrieves scans from the lidar, - runs grouping algorithms between scans and + runs grouping algorithms on the scans and keeps a copy of the group data. """ @@ -38,7 +40,7 @@ class LidarCache(): # Batch over scans, so we don't need to do our own batching to determine groups # TODO: Implement custom batching, as iter_scans can be unreliable for scan in self.lidar.iter_scans(min_len=self.measurements): - print('Got %d measurments' % (len(scan))) + print('Got %d measurements' % (len(scan))) if len(scan) < self.measurements: # Poor scan, likely since it was the first scan. continue @@ -48,24 +50,18 @@ class LidarCache(): # Now process the groups. if self.currentGroups is not None: - self.currentGroups = algorithms.assign_groups( + self.currentGroups = algorithms.assign_groups_II( self.currentGroups, algorithms.calc_groups(scan)) else: self.currentGroups = algorithms.calc_groups(scan) - self.fireGroupsChanged() + self._fireGroupsChanged() - def fireGroupsChanged(self): - # Send the updated groups to 0MQ socket. - # Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already) - pointScan = tracker_pb.PointScan() - for group in self.currentGroups: - for point in group.get_points(): - pointScan.points.append(tracker_pb.Point( - angle=point[1], distance=point[2], group_number=group.number)) + def _fireGroupsChanged(self): + pointScan = self.current_scan for listener in self._group_listeners: - listener.onGroupsChanged(pointScan) + listener(pointScan) def add_groups_changed_listener(self, listener): """ @@ -76,9 +72,34 @@ class LidarCache(): Parameters ---------- listener - An object that implements the onGroupsChanged(message) method. + An function that takes a PointScan proto object as its argument. """ - self._group_listeners.append(listener) + if(listener not in self._group_listeners): + self._group_listeners.append(listener) + + def clear_listeners(self): + """ + Clear all group change listeners. + """ + self._group_listeners = [] + + @property + def current_scan(self): + pointScan = tracker_pb.PointScan() + tempGroups = self.currentGroups + for group in tempGroups: + for point in group.get_points(): + pointScan.points.append(tracker_pb.Point( + angle=point[1], distance=point[2], group_number=group.number)) + return pointScan def stop_scanning(self): self.run = False + + +if __name__ == '__main__': + lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file( + 'pycar/src/car/tracking/out.pickle'))) + cache = LidarCache(lidar) + # cache.add_groups_changed_listener(lambda a: print(a)) + cache.start_cache() diff --git a/car/src/car/tracking/lidar_loader.py b/pycar/src/car/tracking/lidar_loader.py similarity index 100% rename from car/src/car/tracking/lidar_loader.py rename to pycar/src/car/tracking/lidar_loader.py diff --git a/car/src/car/tracking/lidar_servicer.py b/pycar/src/car/tracking/lidar_servicer.py similarity index 72% rename from car/src/car/tracking/lidar_servicer.py rename to pycar/src/car/tracking/lidar_servicer.py index 9038428..51c4ccc 100644 --- a/car/src/car/tracking/lidar_servicer.py +++ b/pycar/src/car/tracking/lidar_servicer.py @@ -10,6 +10,7 @@ from car.messaging import messages import car.tracking.algorithms as alg import os import google.protobuf.empty_pb2 as empty +import time class LidarServicer(PersonTrackingServicer): @@ -18,7 +19,6 @@ class LidarServicer(PersonTrackingServicer): self._lidar = RecordingLidarDecorator( lidar_factory.get_lidar()) self.cache = LidarCache(self._lidar, measurements=100) - self.cache.add_groups_changed_listener(self) self._mFactory = None self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[ 'CAR_ZMQ_PORT'] @@ -32,18 +32,22 @@ class LidarServicer(PersonTrackingServicer): return empty.Empty() def stop_tracking(self, request, context): + print('Stopping tracking') self._should_stream = False self.cache.stop_scanning() + self.cache.clear_listeners() return empty.Empty() def start_tracking(self, request, context): """Starts the lidar cache, streaming on the provided port.""" self._should_stream = True + # Want to rework this, like the sleam streamer/processer rework. + self.cache.add_groups_changed_listener(self.zmq_stream_listener) self.cache.start_cache() return empty.Empty() def record(self, request, context): - # TODO: Fix this to not require + # TODO: Fix this to not require actually running the cache, just recording the lidar. if request.value: self.cache.start_cache() else: @@ -55,7 +59,17 @@ class LidarServicer(PersonTrackingServicer): self._lidar.save_data(request.file) return empty.Empty() - def onGroupsChanged(self, message): + def lidar_stream(self, request, context): + # Streams the state of the lidar cache. + print('Received Lidar gRPC Stream Start Request') + self._should_stream = True + self.cache.start_cache() + sleep_time = request.time_between_messages if request.time_between_messages > 0.01 else 0.01 + while self._should_stream: + time.sleep(sleep_time) + yield self.cache.current_scan + + def zmq_stream_listener(self, message): if self._mFactory is None: # Create the zmq socket in the thread that it will be used, just to be safe. self._mFactory = mf.getZmqPubSubStreamer(self._port) @@ -64,8 +78,10 @@ class LidarServicer(PersonTrackingServicer): self._mFactory.send_message_topic( "lidar_map", messages.ProtoMessage(message=message.SerializeToString())) + def on_groups_changed(self, message): if self._tracked_group is not None and self._vehicle is not None: # Update vehicle to correctly follow the tracked group. # Leave for now, need to work out exactly how this will change. # alg.dualServoChange(alg.find_centre()) + # Put this in a separate module I think, shouldn't control the can autonomously from the servicer. pass diff --git a/car/src/car/tracking/lidar_tester.py b/pycar/src/car/tracking/lidar_tester.py similarity index 100% rename from car/src/car/tracking/lidar_tester.py rename to pycar/src/car/tracking/lidar_tester.py diff --git a/car/src/car/tracking/out.pickle b/pycar/src/car/tracking/out.pickle similarity index 100% rename from car/src/car/tracking/out.pickle rename to pycar/src/car/tracking/out.pickle diff --git a/car/src/car/tracking/readme.txt b/pycar/src/car/tracking/readme.txt similarity index 100% rename from car/src/car/tracking/readme.txt rename to pycar/src/car/tracking/readme.txt diff --git a/pycar/tests/test_algorithms.py b/pycar/tests/test_algorithms.py new file mode 100644 index 0000000..0cf697a --- /dev/null +++ b/pycar/tests/test_algorithms.py @@ -0,0 +1,33 @@ +import unittest +import car.tracking as tracking +from car.tracking.devices.mock_lidar import MockLidar +import car.tracking.lidar_loader as loader + +class TestAlgorithms(unittest.TestCase): + + def setUp(self): + self.lidar = MockLidar(iter(loader.get_scans('../src/car/tracking/out.pickle'))) + + def test_find_centre(self): + # e.g. + #self.assertEqual + pass + + def test_convert_lidar_cartesian(self): + pass + + def test_convert_cartesian_lidar(self): + pass + + def test_calc_groups_runs(self): + pass + + def test_group_changing(self): + pass + + def tearDown(self): + pass + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/car/tests/test_ballot_voter.py b/pycar/tests/test_ballot_voter.py similarity index 100% rename from car/tests/test_ballot_voter.py rename to pycar/tests/test_ballot_voter.py diff --git a/car/tests/test_commander.py b/pycar/tests/test_commander.py similarity index 100% rename from car/tests/test_commander.py rename to pycar/tests/test_commander.py diff --git a/car/tests/test_hand_recogniser.py b/pycar/tests/test_hand_recogniser.py similarity index 100% rename from car/tests/test_hand_recogniser.py rename to pycar/tests/test_hand_recogniser.py diff --git a/pycar/tests/test_lidar_cache.py b/pycar/tests/test_lidar_cache.py new file mode 100644 index 0000000..7e782b8 --- /dev/null +++ b/pycar/tests/test_lidar_cache.py @@ -0,0 +1,10 @@ +import unittest +import car.tracking as tracking + +class TestLidarCache(unittest.TestCase): + + def test_fake_scanner(self): + pass + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/car/tests/test_messages.py b/pycar/tests/test_messages.py similarity index 100% rename from car/tests/test_messages.py rename to pycar/tests/test_messages.py diff --git a/car/tests/test_mqtt_voter.py b/pycar/tests/test_mqtt_voter.py similarity index 100% rename from car/tests/test_mqtt_voter.py rename to pycar/tests/test_mqtt_voter.py diff --git a/settings.gradle b/settings.gradle index 1b5247a..4e80e75 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,6 +1,6 @@ include ':app' include ':protobuf' -include 'car' +include 'pycar' include 'CarControlleriOS' include 'SwiftyCar' rootProject.name='CarController'