Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Choreo event marker updates #856

Merged
merged 1 commit into from
Oct 11, 2024
Merged

Choreo event marker updates #856

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 6 additions & 2 deletions pathplannerlib-python/pathplannerlib/auto.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading