Skip to content

Commit

Permalink
Choreo event marker updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjansen4857
mjansen4857 committed Oct 11, 2024
1 parent ffa721f commit 1bd6722
Show file tree
Hide file tree
Showing 6 changed files with 69 additions and 103 deletions.
8 changes: 6 additions & 2 deletions pathplannerlib-python/pathplannerlib/auto.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,8 +109,12 @@ def commandFromJson(command_json: dict, load_choreo_paths: bool) -> Command:

@staticmethod
def _waitCommandFromData(data_json: dict) -> Command:
waitTime = float(data_json['waitTime'])
return cmd.waitSeconds(waitTime)
waitJson = data_json['waitTime']
if waitJson is dict:
# Choreo expression
return cmd.waitSeconds(float(waitJson['val']))
else:
return cmd.waitSeconds(float(waitJson))

@staticmethod
def _namedCommandFromData(data_json: dict) -> Command:
Expand Down
37 changes: 11 additions & 26 deletions pathplannerlib-python/pathplannerlib/path.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -545,36 +545,21 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
fullTrajStates.append(state)

fullEvents: List[Event] = []
# Events from pplibCommands
if 'pplibCommands' in fJson:
for m in fJson['pplibCommands']:
dataJson = m['data']
offsetJson = dataJson['offset']

name = str(dataJson['name'])
targetTimestamp = float(dataJson['targetTimestamp'])
offset = float(offsetJson['val'])
timestamp = targetTimestamp + offset

if m['event'] is not None:
eventCommand = CommandUtil.commandFromJson(m['event'], True)
fullEvents.append(ScheduleCommandEvent(timestamp, eventCommand))

fullEvents.append(OneShotTriggerEvent(timestamp, name))

# Events from choreolib events
if 'events' in fJson:
for m in fJson['events']:
dataJson = m['data']
offsetJson = dataJson['offset']
for markerJson in fJson['events']:
name = str(markerJson['name'])

name = str(dataJson['name'])
targetTimestamp = float(dataJson['targetTimestamp'])
offset = float(offsetJson['val'])
timestamp = targetTimestamp + offset
fromJson = markerJson['from']
fromOffsetJson = fromJson['offset']
fromTargetTimestamp = float(fromJson['targetTimestamp'])
fromOffset = float(fromOffsetJson['val'])
fromTimestamp = fromTargetTimestamp + fromOffset

fullEvents.append(OneShotTriggerEvent(timestamp, name))
fullEvents.append(OneShotTriggerEvent(fromTimestamp, name))

if markerJson['event'] is not None:
eventCommand = CommandUtil.commandFromJson(markerJson['event'], True)
fullEvents.append(ScheduleCommandEvent(fromTimestamp, eventCommand))
fullEvents.sort(key=lambda e: e.getTimestamp())

# Add the full path to the cache
Expand Down
11 changes: 9 additions & 2 deletions pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,15 @@ public static Command commandFromJson(JSONObject commandJson, boolean loadChoreo
}

private static Command waitCommandFromData(JSONObject dataJson) {
double waitTime = ((Number) dataJson.get("waitTime")).doubleValue();
return Commands.waitSeconds(waitTime);
try {
double waitTime = ((Number) dataJson.get("waitTime")).doubleValue();
return Commands.waitSeconds(waitTime);
} catch (Exception ignored) {
// Failed to load wait time as a number. This is probably a choreo expression
JSONObject waitTimeJson = (JSONObject) dataJson.get("waitTime");
double waitTime = ((Number) waitTimeJson.get("val")).doubleValue();
return Commands.waitSeconds(waitTime);
}
}

private static Command namedCommandFromData(JSONObject dataJson) {
Expand Down
51 changes: 16 additions & 35 deletions pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -381,43 +381,24 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
}

List<Event> fullEvents = new ArrayList<>();
// Events from pplibCommands
if (json.containsKey("pplibCommands")) {
for (var m : (JSONArray) json.get("pplibCommands")) {
JSONObject markerJson = (JSONObject) m;
JSONObject dataJson = (JSONObject) markerJson.get("data");
JSONObject offsetJson = (JSONObject) dataJson.get("offset");

String name = (String) dataJson.get("name");
double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue();
double offset = ((Number) offsetJson.get("val")).doubleValue();
double timestamp = targetTimestamp + offset;
if (markerJson.get("event") != null) {
Command eventCommand =
CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true);
fullEvents.add(new ScheduleCommandEvent(timestamp, eventCommand));
}

fullEvents.add(new OneShotTriggerEvent(timestamp, name));
}
}

// Events from choreolib events
if (json.containsKey("events")) {
for (var m : (JSONArray) json.get("events")) {
JSONObject markerJson = (JSONObject) m;
JSONObject dataJson = (JSONObject) markerJson.get("data");
JSONObject offsetJson = (JSONObject) dataJson.get("offset");

String name = (String) dataJson.get("name");
double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue();
double offset = ((Number) offsetJson.get("val")).doubleValue();
double timestamp = targetTimestamp + offset;

fullEvents.add(new OneShotTriggerEvent(timestamp, name));
for (var m : (JSONArray) json.get("events")) {
JSONObject markerJson = (JSONObject) m;
String name = (String) markerJson.get("name");

JSONObject fromJson = (JSONObject) markerJson.get("from");
JSONObject fromOffsetJson = (JSONObject) fromJson.get("offset");
double fromTargetTimestamp = ((Number) fromJson.get("targetTimestamp")).doubleValue();
double fromOffset = ((Number) fromOffsetJson.get("val")).doubleValue();
double fromTimestamp = fromTargetTimestamp + fromOffset;

fullEvents.add(new OneShotTriggerEvent(fromTimestamp, name));

if (markerJson.get("event") != null) {
Command eventCommand =
CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true);
fullEvents.add(new ScheduleCommandEvent(fromTimestamp, eventCommand));
}
}

fullEvents.sort(Comparator.comparingDouble(Event::getTimestamp));

// Add the full path to the cache
Expand Down
10 changes: 8 additions & 2 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/CommandUtil.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,14 @@ frc2::CommandPtr CommandUtil::commandFromJson(const wpi::json &json,
}

frc2::CommandPtr CommandUtil::waitCommandFromJson(const wpi::json &json) {
auto waitTime = units::second_t(json.at("waitTime").get<double>());
return frc2::cmd::Wait(waitTime);
auto waitJson = json.at("waitTime");
if (waitJson.is_number()) {
return frc2::cmd::Wait(units::second_t { waitJson.get<double>() });
} else {
// Field is not a number, probably a choreo expression
return frc2::cmd::Wait(units::second_t {
waitJson.at("val").get<double>() });
}
}

frc2::CommandPtr CommandUtil::namedCommandFromJson(const wpi::json &json) {
Expand Down
55 changes: 19 additions & 36 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,50 +193,33 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache(
}

std::vector < std::shared_ptr < Event >> fullEvents;
// Events from pplibCommands
if (json.contains("pplibCommands")) {
for (wpi::json::const_reference m : json.at("pplibCommands")) {
auto dataJson = m.at("data");
auto offsetJson = dataJson.at("offset");

std::string name = dataJson.at("name").get<std::string>();
units::second_t targetTimestamp {
dataJson.at("targetTimestamp").get<double>() };
units::second_t offset { offsetJson.at("val").get<double>() };
units::second_t timestamp = targetTimestamp + offset;
if (json.contains("events")) {
for (wpi::json::const_reference markerJson : json.at("events")) {
std::string name = markerJson.at("name").get<std::string>();

frc2::CommandPtr eventCommand = frc2::cmd::None();
if (!m.at("event").is_null()) {
eventCommand = CommandUtil::commandFromJson(m.at("event"),
true);
}
auto fromJson = markerJson.at("from");
auto fromOffsetJson = markerJson.at("offset");

units::second_t fromTargetTimestamp {
fromJson.at("targetTimestamp").get<double>() };
units::second_t fromOffset { fromOffsetJson.at("val").get<double>() };
units::second_t fromTimestamp = fromTargetTimestamp + fromOffset;

fullEvents.emplace_back(
std::make_shared < OneShotTriggerEvent > (timestamp, name));
std::make_shared < OneShotTriggerEvent
> (fromTimestamp, name));

frc2::CommandPtr eventCommand = frc2::cmd::None();
if (!markerJson.at("event").is_null()) {
eventCommand = CommandUtil::commandFromJson(
markerJson.at("event"), true);
}
fullEvents.emplace_back(
std::make_shared < ScheduleCommandEvent
> (timestamp, std::shared_ptr < frc2::Command
> (fromTimestamp, std::shared_ptr < frc2::Command
> (std::move(eventCommand).Unwrap())));
}
}

// Events from choreolib events
if (json.contains("events")) {
for (wpi::json::const_reference m : json.at("events")) {
auto dataJson = m.at("data");
auto offsetJson = dataJson.at("offset");

std::string name = dataJson.at("name").get<std::string>();
units::second_t targetTimestamp {
dataJson.at("targetTimestamp").get<double>() };
units::second_t offset { offsetJson.at("val").get<double>() };
units::second_t timestamp = targetTimestamp + offset;

fullEvents.emplace_back(
std::make_shared < OneShotTriggerEvent > (timestamp, name));
}
}

std::sort(fullEvents.begin(), fullEvents.end(),
[](auto &left, auto &right) {
return left->getTimestamp() < right->getTimestamp();
Expand Down

0 comments on commit 1bd6722

Please sign in to comment.