diff --git a/lib/path/choreo_path.dart b/lib/path/choreo_path.dart index 866cd4a2..bc145e67 100644 --- a/lib/path/choreo_path.dart +++ b/lib/path/choreo_path.dart @@ -81,22 +81,12 @@ class ChoreoPath { .map((e) => (e as num).toInt()) .toList(); - if (splits.isNotEmpty) { - // First split - final splitTraj = PathPlannerTrajectory.fromStates( - path.trajectory.states.sublist(0, splits[0])); - final splitPath = ChoreoPath( - name: '$pathName.0', - trajectory: splitTraj, - fs: fs, - choreoDir: choreoDir, - eventMarkerTimes: [], - ); - paths.add(splitPath); + if (splits.isEmpty || splits.first != 0) { + splits.insert(0, 0); } for (int i = 0; i < splits.length; i++) { - String name = '$pathName.${i + 1}'; + String name = '$pathName.$i'; int startIdx = splits[i]; int endIdx; diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 109e9743..162eb4ed 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -580,18 +580,20 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None: fullPath.name = trajectory_name PathPlannerPath._choreoPathCache[trajectory_name] = fullPath - splits = trajJson['splits'] - for i in range(-1, len(splits)): - name = trajectory_name + '.' + str(i + 1) + splitsJson = trajJson['splits'] + splits = [int(s) for s in splitsJson] + if len(splits) == 0 or int(splits[0]) != 0: + splits.insert(0, 0) + + for i in range(len(splits)): + name = trajectory_name + '.' + str(i) states = [] - splitStartIdx = 0 - if i != -1: - splitStartIdx = int(splits[i]) + splitStartIdx = splits[i] splitEndIdx = len(fullTrajStates) if i < len(splits) - 1: - splitEndIdx = int(splits[i + 1]) + splitEndIdx = splits[i + 1] startTime = fullTrajStates[splitStartIdx].timeSeconds endTime = fullTrajStates[splitEndIdx - 1].timeSeconds diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index dd8ac6c1..45acf25b 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -420,19 +420,25 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) fullPath.name = trajectoryName; choreoPathCache.put(trajectoryName, fullPath); - JSONArray splits = (JSONArray) trajJson.get("splits"); - for (int i = -1; i < splits.size(); i++) { - String name = trajectoryName + "." + (i + 1); + JSONArray splitsJson = (JSONArray) trajJson.get("splits"); + List splits = new ArrayList<>(); + for (Object o : splitsJson) { + splits.add(((Number) o).intValue()); + } + + if (splits.isEmpty() || splits.get(0) != 0) { + splits.add(0, 0); + } + + for (int i = 0; i < splits.size(); i++) { + String name = trajectoryName + "." + i; List states = new ArrayList<>(); - int splitStartIdx = 0; - if (i != -1) { - splitStartIdx = ((Number) splits.get(i)).intValue(); - } + int splitStartIdx = splits.get(i); int splitEndIdx = fullTrajStates.size(); if (i < splits.size() - 1) { - splitEndIdx = ((Number) splits.get(i + 1)).intValue(); + splitEndIdx = splits.get(i + 1); } double startTime = fullTrajStates.get(splitStartIdx).timeSeconds; diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 6809b666..c313da71 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -243,58 +243,63 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( fullPath->name = trajectoryName; PathPlannerPath::getChoreoPathCache().emplace(trajectoryName, fullPath); - auto splits = trajJson.at("splits"); - if (splits.is_array()) { - int splitsSize = static_cast(splits.size()); - for (int i = -1; i < splitsSize; i++) { - std::string name = trajectoryName + "." + std::to_string(i + 1); - std::vector < PathPlannerTrajectoryState > states; - - size_t splitStartIdx = 0; - if (i != -1) { - splitStartIdx = splits[i].get(); - } + auto splitsJson = trajJson.at("splits"); + std::vector < size_t > splits; - size_t splitEndIdx = fullTrajStates.size(); - if (i < splitsSize) { - splitEndIdx = splits[i + 1].get(); - } + if (splitsJson.is_array()) { + for (auto split : splitsJson) { + splits.emplace_back(split.get()); + } + } - auto startTime = fullTrajStates[splitStartIdx].time; - auto endTime = fullTrajStates[splitEndIdx - 1].time; - for (size_t s = splitStartIdx; s < splitEndIdx; s++) { - states.emplace_back( - fullTrajStates[s].copyWithTime( - fullTrajStates[s].time - startTime)); - } + if (splits.empty() || splits[0] != 0) { + splits.insert(splits.begin(), 0); + } - std::vector < std::shared_ptr < Event >> events; - for (auto originalEvent : fullEvents) { - if (originalEvent->getTimestamp() >= startTime - && originalEvent->getTimestamp() < endTime) { - events.emplace_back( - originalEvent->copyWithTimestamp( - originalEvent->getTimestamp() - startTime)); - } - } + for (size_t i = 0; i < splits.size(); i++) { + std::string name = trajectoryName + "." + std::to_string(i); + std::vector < PathPlannerTrajectoryState > states; - auto path = - std::make_shared < PathPlannerPath - > (PathConstraints::unlimitedConstraints(12_V), GoalEndState( - states[states.size() - 1].linearVelocity, - states[states.size() - 1].pose.Rotation())); + size_t splitStartIdx = splits[i]; - std::vector < PathPoint > pathPoints; - for (auto state : states) { - pathPoints.emplace_back(state.pose.Translation()); + size_t splitEndIdx = fullTrajStates.size(); + if (i < splits.size()) { + splitEndIdx = splits[i + 1]; + } + + auto startTime = fullTrajStates[splitStartIdx].time; + auto endTime = fullTrajStates[splitEndIdx - 1].time; + for (size_t s = splitStartIdx; s < splitEndIdx; s++) { + states.emplace_back( + fullTrajStates[s].copyWithTime( + fullTrajStates[s].time - startTime)); + } + + std::vector < std::shared_ptr < Event >> events; + for (auto originalEvent : fullEvents) { + if (originalEvent->getTimestamp() >= startTime + && originalEvent->getTimestamp() < endTime) { + events.emplace_back( + originalEvent->copyWithTimestamp( + originalEvent->getTimestamp() - startTime)); } + } + + auto path = std::make_shared < PathPlannerPath + > (PathConstraints::unlimitedConstraints(12_V), GoalEndState( + states[states.size() - 1].linearVelocity, + states[states.size() - 1].pose.Rotation())); - path->m_allPoints = pathPoints; - path->m_isChoreoPath = true; - path->m_idealTrajectory = PathPlannerTrajectory(states, events); - path->name = name; - PathPlannerPath::getChoreoPathCache().emplace(name, path); + std::vector < PathPoint > pathPoints; + for (auto state : states) { + pathPoints.emplace_back(state.pose.Translation()); } + + path->m_allPoints = pathPoints; + path->m_isChoreoPath = true; + path->m_idealTrajectory = PathPlannerTrajectory(states, events); + path->name = name; + PathPlannerPath::getChoreoPathCache().emplace(name, path); } }