Skip to content

Commit

Permalink
Fix build for vts
Browse files Browse the repository at this point in the history
  • Loading branch information
@camearle20
camearle20 committed Feb 9, 2024
1 parent ce05966 commit cac4240
Show file tree
Hide file tree
Showing 4 changed files with 106 additions and 31 deletions.
3 changes: 2 additions & 1 deletion trajectory_native/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ find_package(gRPC CONFIG REQUIRED)
find_package(Threads)
find_package(fmt REQUIRED)
find_package(nlohmann_json REQUIRED)
find_package(TrajoptLib 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)
Expand All @@ -17,7 +18,7 @@ target_include_directories(trajectory_native_proto PUBLIC ${CMAKE_CURRENT_BINARY
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})

Expand Down
27 changes: 7 additions & 20 deletions trajectory_native/Earthfile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,28 +21,15 @@ grpc:
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
# Latest commit seems to break casadi at least on arm64
GIT CLONE --branch 95c20be79be7557673d75d631703dc92fe6a191e https://github.com/SleipnirGroup/TrajoptLib TrajoptLib
WORKDIR TrajoptLib
COPY trajoptlib-aarch64.patch .
RUN git apply trajoptlib-aarch64.patch
RUN mkdir build
WORKDIR build
ENV CC=clang
ENV CXX=clang++
RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF ..
Expand Down
85 changes: 75 additions & 10 deletions trajectory_native/src/trajectory_service.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,23 +14,23 @@ 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(),
.modules = {{.x = model.vehicle_length() / 2.0,
.y = model.vehicle_width() / 2.0,
.wheelRadius = model.wheel_radius(),
.wheelMaxAngularVelocity = model.max_wheel_omega(),
.wheelMaxTorque = model.max_wheel_torque()},
{.x = model.vehicle_length(),
.y = -model.vehicle_width(),
{.x = model.vehicle_length() / 2.0,
.y = -model.vehicle_width() / 2.0,
.wheelRadius = model.wheel_radius(),
.wheelMaxAngularVelocity = model.max_wheel_omega(),
.wheelMaxTorque = model.max_wheel_torque()},
{.x = -model.vehicle_length(),
.y = model.vehicle_width(),
{.x = -model.vehicle_length() / 2.0,
.y = model.vehicle_width() / 2.0,
.wheelRadius = model.wheel_radius(),
.wheelMaxAngularVelocity = model.max_wheel_omega(),
.wheelMaxTorque = model.max_wheel_torque()},
{.x = -model.vehicle_length(),
.y = -model.vehicle_width(),
{.x = -model.vehicle_length() / 2.0,
.y = -model.vehicle_width() / 2.0,
.wheelRadius = model.wheel_radius(),
.wheelMaxAngularVelocity = model.max_wheel_omega(),
.wheelMaxTorque = model.max_wheel_torque()}}};
Expand Down Expand Up @@ -74,6 +74,65 @@ void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::Holonomi
}
}

std::string hash_request(const vts::PathRequest &request) {
// Function is somewhat messy but this is the simplest way I could think of to hash the request with
// fixed precision while accounting for optional fields

std::stringstream stream;
stream << std::fixed << std::setprecision(6);
stream << request.model().mass();
stream << request.model().moi();
stream << request.model().vehicle_length();
stream << request.model().vehicle_width();
stream << request.model().wheel_radius();
stream << request.model().max_wheel_omega();
stream << request.model().max_wheel_torque();

for (const vts::PathSegment &segment: request.segments()) {
for (const vts::Waypoint &waypoint: segment.waypoints()) {
stream << waypoint.x();
stream << waypoint.y();

if (waypoint.has_heading_constraint()) {
stream << waypoint.heading_constraint();
}

if (waypoint.has_samples()) {
stream << waypoint.samples();
}

switch (waypoint.velocity_constraint_case()) {
case vts::Waypoint::kZeroVelocity:
stream << "0";
break;
case vts::Waypoint::kVehicleVelocity:
stream << waypoint.vehicle_velocity().vx();
stream << waypoint.vehicle_velocity().vy();
stream << waypoint.vehicle_velocity().omega();
break;
case org::littletonrobotics::vehicletrajectoryservice::Waypoint::VELOCITY_CONSTRAINT_NOT_SET:
break;
}
}

if (segment.has_max_velocity()) {
stream << segment.max_velocity();
}

if (segment.has_max_omega()) {
stream << segment.max_omega();
}

if (segment.straight_line()) {
stream << "1";
} else {
stream << "0";
}
}

return std::to_string(std::hash<std::string>{}(stream.str()));
}

class VehicleTrajectoryService final
: public vts::VehicleTrajectoryService::Service {
public:
Expand Down Expand Up @@ -134,6 +193,13 @@ class VehicleTrajectoryService final
},
trajopt::CoordinateSystem::kField
});
builder.WptConstraint(segment_start_offset + waypoint_idx,
trajopt::AngularVelocityConstraint{
trajopt::IntervalSet1d{
waypoint.vehicle_velocity().omega(),
waypoint.vehicle_velocity().omega()
}
});
break;
}

Expand Down Expand Up @@ -187,8 +253,7 @@ class VehicleTrajectoryService final
trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)};
fmt::print("Generation finished\n");
convert_trajectory(response->mutable_trajectory(), trajectory);
std::size_t hash = std::hash<std::string>{}(request->SerializeAsString());
response->mutable_trajectory()->set_hash_code(std::to_string(hash));
response->mutable_trajectory()->set_hash_code(hash_request(*request));
} catch (std::exception &e) {
fmt::print("Generation failed: {}\n", std::string(e.what()));
response->mutable_error()->set_reason(std::string(e.what()));
Expand Down
22 changes: 22 additions & 0 deletions trajectory_native/trajoptlib-aarch64.patch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
diff --git a/cmake/modules/FetchCasADi.cmake b/cmake/modules/FetchCasADi.cmake
index 631f55f..ccb4511 100644
--- a/cmake/modules/FetchCasADi.cmake
+++ b/cmake/modules/FetchCasADi.cmake
@@ -68,7 +68,7 @@ macro(fetch_casadi)
endif()
set(CASADI_INSTALL_DEST "lib")
elseif(UNIX)
- if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "ARM64")
+ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64")
message(STATUS "Building for Linux arm64")
set(CASADI_URL
https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-linux-aarch64-py311.zip
@@ -79,8 +79,6 @@ macro(fetch_casadi)
${CASADI_LIBDIR}/libipopt.so.3
${CASADI_LIBDIR}/libcoinmumps.so.3
${CASADI_LIBDIR}/libcoinmetis.so.2
- ${CASADI_LIBDIR}/libgfortran-040039e1.so.5.0.0
- ${CASADI_LIBDIR}/libquadmath-96973f99.so.0.0.0
${CASADI_LIBDIR}/libcasadi-tp-openblas.so.0
)
else()

0 comments on commit cac4240

Please sign in to comment.