diff --git a/trajectory_native/.devcontainer/devcontainer.json b/trajectory_native/.devcontainer/devcontainer.json new file mode 100644 index 00000000..c48c1214 --- /dev/null +++ b/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" + ] + } + } +} \ No newline at end of file diff --git a/trajectory_native/CMakeLists.txt b/trajectory_native/CMakeLists.txt new file mode 100644 index 00000000..9d4af1d7 --- /dev/null +++ b/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) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile new file mode 100644 index 00000000..d8aadddc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/trajectory_native/mingw-w64.cmake b/trajectory_native/mingw-w64.cmake deleted file mode 100644 index 19fd39c6..00000000 --- a/trajectory_native/mingw-w64.cmake +++ /dev/null @@ -1,16 +0,0 @@ -set(CMAKE_SYSTEM_NAME Windows) -set(TOOLCHAIN_PREFIX x86_64-w64-mingw32) - -# cross compilers to use for C, C++ and Fortran -set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc-posix) -set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++-posix) -set(CMAKE_Fortran_COMPILER ${TOOLCHAIN_PREFIX}-gfortran) -set(CMAKE_RC_COMPILER ${TOOLCHAIN_PREFIX}-windres) - -# target environment on the build host system -set(CMAKE_FIND_ROOT_PATH /usr/${TOOLCHAIN_PREFIX}) - -# modify default behavior of FIND_XXX() commands -set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) -set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) -set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) \ No newline at end of file diff --git a/trajectory_native/proto/VehicleTrajectoryService.proto b/trajectory_native/proto/VehicleTrajectoryService.proto new file mode 100644 index 00000000..f876bf6f --- /dev/null +++ b/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) {} +} \ No newline at end of file diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp new file mode 100644 index 00000000..065e6bcc --- /dev/null +++ b/trajectory_native/src/trajectory_service.cpp @@ -0,0 +1,214 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vts = org::littletonrobotics::vehicletrajectoryservice; + +trajopt::SwerveDrivetrain create_drivetrain(const vts::VehicleModel &model) { + trajopt::SwerveDrivetrain drivetrain{ + .mass = model.mass(), + .moi = model.moi(), + .modules = {{.x = model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}}}; + + return drivetrain; +} + +int guess_control_interval_count(const vts::Waypoint &waypoint, const vts::Waypoint &prev_waypoint, + const vts::VehicleModel &model) { + double dx = waypoint.x() - prev_waypoint.x(); + double dy = waypoint.y() - prev_waypoint.y(); + double dtheta = waypoint.has_heading_constraint() && prev_waypoint.has_heading_constraint() ? + waypoint.heading_constraint() - prev_waypoint.heading_constraint() : 0; + static const double heading_weight = 0.5; + double distance = hypot(dx, dy); + double max_force = model.max_wheel_torque() / model.wheel_radius(); + double max_accel = (max_force * 4) / model.mass(); + double max_vel = model.max_wheel_omega() * model.wheel_radius(); + double distance_at_cruise = distance - (max_vel * max_vel) / max_accel; + double total_time = distance_at_cruise < 0 ? + 2 * (sqrt(distance * max_accel) / max_accel) : + distance / max_vel + max_vel / max_accel; + total_time += heading_weight * abs(dtheta); + return ceil(total_time / 0.1); +} + +void convert_sample(vts::TimestampedVehicleState *state_out, const trajopt::HolonomicTrajectorySample &sample_in) { + state_out->set_time(sample_in.timestamp); + vts::VehicleState *vehicle_state = state_out->mutable_state(); + vehicle_state->set_x(sample_in.x); + vehicle_state->set_y(sample_in.y); + vehicle_state->set_theta(sample_in.heading); + vehicle_state->set_vx(sample_in.velocityX); + vehicle_state->set_vy(sample_in.velocityY); + vehicle_state->set_omega(sample_in.angularVelocity); +} + +void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::HolonomicTrajectory &trajectory_in) { + for (const trajopt::HolonomicTrajectorySample &sample: trajectory_in.samples) { + convert_sample(trajectory_out->add_states(), sample); + } +} + +class VehicleTrajectoryService final + : public vts::VehicleTrajectoryService::Service { +public: + ::grpc::Status GenerateTrajectory(::grpc::ServerContext *context, + const vts::PathRequest *request, + vts::TrajectoryResponse *response) override { + trajopt::SwervePathBuilder builder; + builder.SetDrivetrain(create_drivetrain(request->model())); + std::vector control_intervals; + + int segment_start_offset = 0; + for (int segment_idx = 0; segment_idx < request->segments_size(); segment_idx++) { + fmt::print("Starting segment {}\n", segment_idx); + // Add segment waypoints + const vts::PathSegment &segment = request->segments(segment_idx); + const int last_waypoint_idx = segment.waypoints_size() - 1; + const vts::Waypoint *prev_waypoint; + + for (int waypoint_idx = 0; waypoint_idx < segment.waypoints_size(); waypoint_idx++) { + const vts::Waypoint &waypoint = segment.waypoints(segment_start_offset + waypoint_idx); + + if (waypoint.has_heading_constraint()) { + fmt::print("Adding pose waypoint {} ({}, {}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y(), waypoint.heading_constraint()); + builder.PoseWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y(), + waypoint.heading_constraint()); + } else { + fmt::print("Adding translation waypoint {} ({}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y()); + builder.TranslationWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y()); + } + + switch (waypoint.velocity_constraint_case()) { + case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET: + // If this is the first or last waypoint, add an implicit stop if no other constraint is added + if (waypoint_idx == 0 || waypoint_idx == last_waypoint_idx) { + fmt::print("Adding implicit zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + } + break; + case vts::Waypoint::kZeroVelocity: + fmt::print("Adding zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + break; + case vts::Waypoint::kVehicleVelocity: + fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n", + waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().omega(), segment_start_offset + waypoint_idx); + builder.WptConstraint(segment_start_offset + waypoint_idx, + trajopt::HolonomicVelocityConstraint{ + trajopt::RectangularSet2d{ + {waypoint.vehicle_velocity().vx(), + waypoint.vehicle_velocity().vx()}, + {waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().vy()}, + }, + trajopt::CoordinateSystem::kField + }); + break; + } + + if (segment_idx > 0 || waypoint_idx > 0) { + unsigned int sample_count = waypoint.has_samples() ? waypoint.samples() + : guess_control_interval_count(waypoint, + *prev_waypoint, + request->model()); + fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, waypoint_idx - 1, + waypoint_idx); + control_intervals.push_back(sample_count); + } + + prev_waypoint = &waypoint; + } + + // Segment constraints + if (segment.has_max_velocity()) { + fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx, + segment.max_velocity()); + } + + if (segment.has_max_omega()) { + fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx, + trajopt::AngularVelocityConstraint{segment.max_omega()}); + } + + if (segment.straight_line()) { + double x1 = segment.waypoints(segment_start_offset).x(); + double x2 = segment.waypoints(segment_start_offset + last_waypoint_idx).x(); + double y1 = segment.waypoints(segment_start_offset).y(); + double y2 = segment.waypoints(segment_start_offset + last_waypoint_idx).y(); + double angle = atan2(y2 - y1, x2 - x1); + fmt::print("Adding straight line constraint with angle {} to segment {} (waypoints {}-{})\n", angle, + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx, + angle); + } + + segment_start_offset = last_waypoint_idx; + } + + builder.ControlIntervalCounts(std::move(control_intervals)); + + try { + fmt::print("Generating trajectory\n"); + trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)}; + fmt::print("Generation finished\n"); + convert_trajectory(response->mutable_trajectory(), trajectory); + std::size_t hash = std::hash{}(request->SerializeAsString()); + response->mutable_trajectory()->set_hash_code(std::to_string(hash)); + } catch (std::exception &e) { + fmt::print("Generation failed: {}\n", std::string(e.what())); + response->mutable_error()->set_reason(std::string(e.what())); + } + + return grpc::Status::OK; + } +}; + +int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv) { + VehicleTrajectoryService service; + + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + ::grpc::ServerBuilder builder; + builder.AddListeningPort("0.0.0.0:56328", grpc::InsecureServerCredentials()); + + builder.RegisterService(&service); + + std::unique_ptr server(builder.BuildAndStart()); + server->Wait(); + + return 0; +} \ No newline at end of file