Add vts implementation

trajectory_native/.devcontainer/devcontainer.json

@@ -0,0 +1,12 @@
+{
+  "image": "littletonrobotics/vts-dev:latest",
+  "customizations": {
+    "vscode": {
+      "extensions": [
+        "ms-vscode.cmake-tools",
+        "ms-vscode.cpptools-extension-pack",
+        "zxh404.vscode-proto3"
+      ]
+    }
+  }
+}

trajectory_native/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.24)
+project(trajectory_native)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--whole-archive --allow-multiple-definition")
+
+find_package(protobuf CONFIG REQUIRED)
+find_package(gRPC CONFIG REQUIRED)
+find_package(Threads)
+find_package(fmt REQUIRED)
+find_package(nlohmann_json REQUIRED)
+
+add_library(trajectory_native_proto proto/VehicleTrajectoryService.proto)
+target_link_libraries(trajectory_native_proto PUBLIC protobuf::libprotobuf gRPC::grpc gRPC::grpc++ gRPC::grpc++_reflection)
+target_include_directories(trajectory_native_proto PUBLIC ${CMAKE_CURRENT_BINARY_DIR})
+
+get_target_property(grpc_cpp_plugin_location gRPC::grpc_cpp_plugin LOCATION)
+protobuf_generate(TARGET trajectory_native_proto LANGUAGE cpp)
+protobuf_generate(TARGET trajectory_native_proto LANGUAGE grpc GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc PLUGIN "protoc-gen-grpc=${grpc_cpp_plugin_location}")
+
+find_library(TRAJOPT_LIBRARY NAMES TrajoptLib)
+message(${TRAJOPT_LIBRARY})
+
+
+add_executable(trajectory_native src/trajectory_service.cpp)
+target_link_libraries(trajectory_native PRIVATE ${TRAJOPT_LIBRARY} trajectory_native_proto fmt::fmt nlohmann_json::nlohmann_json)

trajectory_native/Earthfile

@@ -0,0 +1,77 @@
+VERSION 0.7
+FROM debian:bookworm-20240110
+WORKDIR /RobotCode2024/trajectory_native
+
+apt-deps:
+    ENV DEBIAN_FRONTEND=noninteractive
+    RUN apt update && apt install -y wget build-essential cmake autoconf libtool pkg-config git libblas-dev liblapack-dev clang lld gfortran
+    SAVE IMAGE --cache-hint
+
+grpc:
+    FROM +apt-deps
+    RUN git clone --recurse-submodules -b v1.60.0 --depth 1 --shallow-submodules https://github.com/grpc/grpc
+    RUN mkdir grpc/build
+    WORKDIR grpc/build
+    ENV CC=clang
+    ENV CXX=clang++
+    RUN cmake -DgRPC_INSTALL=ON -DgRPC_BUILD_TESTS=OFF ..
+    RUN make -j4
+    RUN make DESTDIR=$(pwd)/installroot_top install
+    RUN mkdir installroot
+    RUN mv installroot_top/usr/local/* installroot/
+    SAVE ARTIFACT installroot
+
+ipopt:
+    FROM +apt-deps
+    RUN wget https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz
+    RUN tar -zvxf 3.14.14.tar.gz
+    RUN mkdir Ipopt-releases-3.14.14/build
+    WORKDIR Ipopt-releases-3.14.14/build
+    ENV CC=clang
+    ENV CXX=clang++
+    RUN ../configure
+    RUN make -j4
+    RUN make DESTDIR=$(pwd)/installroot_top install
+    RUN mkdir installroot
+    RUN mv installroot_top/usr/local/* installroot/
+    SAVE ARTIFACT installroot
+
+trajoptlib:
+    FROM +apt-deps
+    BUILD +ipopt
+    COPY +ipopt/installroot /usr/local/
+    GIT CLONE https://github.com/camearle20/TrajoptLib TrajoptLib
+    RUN mkdir TrajoptLib/build
+    WORKDIR TrajoptLib/build
+    ENV CC=clang
+    ENV CXX=clang++
+    RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF ..
+    RUN make -j4
+    RUN make DESTDIR=$(pwd)/installroot_top install
+    RUN mkdir installroot
+    RUN mv installroot_top/usr/local/* installroot/
+    SAVE ARTIFACT installroot
+
+dev-image:
+    FROM +apt-deps
+    BUILD +grpc
+    BUILD +trajoptlib
+    COPY +grpc/installroot /usr/local/
+    COPY +trajoptlib/installroot /usr/local/
+    ENV CC=clang
+    ENV CXX=clang++
+    SAVE IMAGE littletonrobotics/vts-dev
+
+vts:
+    FROM +dev-image
+    COPY src src
+    COPY proto proto
+    COPY CMakeLists.txt CMakeLists.txt
+    RUN mkdir build
+    WORKDIR build
+    RUN cmake ..
+    RUN make -j4
+    EXPOSE 56328
+    ENV GRPC_VERBOSITY=info
+    ENTRYPOINT ["./trajectory_native"]
+    SAVE IMAGE littletonrobotics/vts

trajectory_native/proto/VehicleTrajectoryService.proto

@@ -0,0 +1,80 @@
+syntax = "proto3";
+
+package org.littletonrobotics.vehicletrajectoryservice;
+
+message VehicleModel {
+  double mass = 1;
+  double moi = 2;
+  double vehicle_length = 3;
+  double vehicle_width = 4;
+  double wheel_radius = 5;
+  double max_wheel_omega = 6;
+  double max_wheel_torque = 7;
+}
+
+message VehicleState {
+  double x = 1;
+  double y = 2;
+  double theta = 3;
+  double vx = 4;
+  double vy = 5;
+  double omega = 6;
+}
+
+message TimestampedVehicleState {
+  double time = 1;
+  VehicleState state = 2;
+}
+
+message VehicleVelocityConstraint {
+  double vx = 1;
+  double vy = 2;
+  double omega = 3;
+}
+
+message ZeroVelocityConstraint {
+}
+
+message Waypoint {
+  reserved 5 to 9;
+  double x = 1;
+  double y = 2;
+  optional double heading_constraint = 3;
+  optional uint32 samples = 4;
+  oneof velocity_constraint {
+    ZeroVelocityConstraint zero_velocity = 10;
+    VehicleVelocityConstraint vehicle_velocity = 11;
+  }
+}
+
+message PathSegment {
+  repeated Waypoint waypoints = 1;
+  optional double max_velocity = 2;
+  optional double max_omega = 3;
+  bool straight_line = 4;
+}
+
+message Trajectory {
+  repeated TimestampedVehicleState states = 1;
+  string hash_code = 2;
+}
+
+message TrajectoryGenerationError {
+  string reason = 1;
+}
+
+message PathRequest {
+  repeated PathSegment segments = 1;
+  VehicleModel model = 2;
+}
+
+message TrajectoryResponse {
+  oneof response {
+    Trajectory trajectory = 1;
+    TrajectoryGenerationError error = 2;
+  }
+}
+
+service VehicleTrajectoryService {
+  rpc GenerateTrajectory(PathRequest) returns (TrajectoryResponse) {}
+}