Skip to content

Commit

Permalink
Switch C++ AutoBuilder std::functions to accept const& (#879)
Browse files Browse the repository at this point in the history
  • Loading branch information
@bhall-ctre
bhall-ctre authored Oct 22, 2024
1 parent 1a9dbcf commit 49fb344
Show file tree
Hide file tree
Showing 11 changed files with 47 additions and 41 deletions.
6 changes: 3 additions & 3 deletions examples/cpp/src/main/cpp/subsystems/SwerveSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@ SwerveSubsystem::SwerveSubsystem() : flModule(), frModule(), blModule(), brModul
// Configure AutoBuilder
AutoBuilder::configure(
[this]() {return this->getPose();},
[this](frc::Pose2d pose) {this->resetPose(pose);},
[this](const frc::Pose2d& pose) {this->resetPose(pose);},
[this]() {return this->getSpeeds();},
[this](frc::ChassisSpeeds robotRelativeSpeeds) {this->driveRobotRelative(robotRelativeSpeeds);},
[this](const frc::ChassisSpeeds& robotRelativeSpeeds) {this->driveRobotRelative(robotRelativeSpeeds);},
std::make_shared<PPHolonomicDriveController>(
SwerveConstants::translationConstants,
SwerveConstants::rotationConstants
Expand All @@ -43,7 +43,7 @@ SwerveSubsystem::SwerveSubsystem() : flModule(), frModule(), blModule(), brModul
);

// Set up custom logging to add the current path to a field 2d widget
PathPlannerLogging::setLogActivePathCallback([this](auto poses) {
PathPlannerLogging::setLogActivePathCallback([this](const auto& poses) {
this->field.GetObject("path")->SetPoses(poses);
});

Expand Down
8 changes: 4 additions & 4 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ using namespace pathplanner;
bool AutoBuilder::m_configured = false;
std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> AutoBuilder::m_pathFollowingCommandBuilder;
std::function<frc::Pose2d()> AutoBuilder::m_poseSupplier;
std::function<void(frc::Pose2d)> AutoBuilder::m_resetPose;
std::function<void(const frc::Pose2d&)> AutoBuilder::m_resetPose;
std::function<bool()> AutoBuilder::m_shouldFlipPath;
bool AutoBuilder::m_isHolonomic = false;

Expand All @@ -32,9 +32,9 @@ std::function<
frc2::CommandPtr(std::shared_ptr<PathPlannerPath>, PathConstraints)> AutoBuilder::m_pathfindThenFollowPathCommandBuilder;

void AutoBuilder::configure(std::function<frc::Pose2d()> poseSupplier,
std::function<void(frc::Pose2d)> resetPose,
std::function<void(const frc::Pose2d&)> resetPose,
std::function<frc::ChassisSpeeds()> robotRelativeSpeedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem) {
Expand Down Expand Up @@ -79,7 +79,7 @@ void AutoBuilder::configure(std::function<frc::Pose2d()> poseSupplier,

void AutoBuilder::configureCustom(std::function<frc::Pose2d()> poseSupplier,
std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> pathFollowingCommandBuilder,
std::function<void(frc::Pose2d)> resetPose, bool isHolonomic,
std::function<void(const frc::Pose2d&)> resetPose, bool isHolonomic,
std::function<bool()> shouldFlipPose) {
if (m_configured) {
FRC_ReportError(frc::err::Error,
Expand Down
6 changes: 3 additions & 3 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ using namespace pathplanner;
FollowPathCommand::FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Requirements requirements) : m_originalPath(path), m_poseSupplier(
Expand Down Expand Up @@ -79,8 +79,8 @@ void FollowPathCommand::Initialize() {
currentPose.Rotation(), m_robotConfig);
}

PathPlannerLogging::logActivePath (m_path);
PPLibTelemetry::setCurrentPath(m_path);
PathPlannerLogging::logActivePath(m_path.get());
PPLibTelemetry::setCurrentPath (m_path);

m_eventScheduler.initialize(m_trajectory);

Expand Down
8 changes: 4 additions & 4 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ PathfindingCommand::PathfindingCommand(
std::shared_ptr<PathPlannerPath> targetPath,
PathConstraints constraints, std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState(
Expand Down Expand Up @@ -59,7 +59,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose,
PathConstraints constraints, units::meters_per_second_t goalEndVel,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose(
targetPose), m_originalTargetPose(targetPose), m_goalEndState(
Expand Down Expand Up @@ -168,8 +168,8 @@ void PathfindingCommand::Execute() {
m_timeOffset = 0.02_s;
}

PathPlannerLogging::logActivePath (m_currentPath);
PPLibTelemetry::setCurrentPath(m_currentPath);
PathPlannerLogging::logActivePath(m_currentPath.get());
PPLibTelemetry::setCurrentPath (m_currentPath);

m_timer.Reset();
m_timer.Start();
Expand Down
6 changes: 3 additions & 3 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PathPlannerLogging.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,6 @@

using namespace pathplanner;

std::function<void(frc::Pose2d)> PathPlannerLogging::m_logCurrentPose;
std::function<void(frc::Pose2d)> PathPlannerLogging::m_logTargetPose;
std::function<void(std::vector<frc::Pose2d>&)> PathPlannerLogging::m_logActivePath;
std::function<void(const frc::Pose2d&)> PathPlannerLogging::m_logCurrentPose;
std::function<void(const frc::Pose2d&)> PathPlannerLogging::m_logTargetPose;
std::function<void(const std::vector<frc::Pose2d>&)> PathPlannerLogging::m_logActivePath;
18 changes: 10 additions & 8 deletions pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,10 @@ class AutoBuilder {
* @param driveSubsystem a pointer to the subsystem for the robot's drive
*/
static void configure(std::function<frc::Pose2d()> poseSupplier,
std::function<void(frc::Pose2d)> resetPose,
std::function<void(const frc::Pose2d&)> resetPose,
std::function<frc::ChassisSpeeds()> robotRelativeSpeedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<
void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem);
Expand All @@ -65,16 +66,17 @@ class AutoBuilder {
* @param driveSubsystem a pointer to the subsystem for the robot's drive
*/
static inline void configure(std::function<frc::Pose2d()> poseSupplier,
std::function<void(frc::Pose2d)> resetPose,
std::function<void(const frc::Pose2d&)> resetPose,
std::function<frc::ChassisSpeeds()> robotRelativeSpeedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
std::function<void(const frc::ChassisSpeeds&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Subsystem *driveSubsystem) {
configure(poseSupplier, resetPose, robotRelativeSpeedsSupplier,
[output](auto speeds, auto feedforwards) {
[output](auto &&speeds, auto &&feedforwards) {
output(speeds);
}, controller, robotConfig, shouldFlipPath, driveSubsystem);
}, std::move(controller), std::move(robotConfig),
shouldFlipPath, driveSubsystem);
}

/**
Expand All @@ -93,7 +95,7 @@ class AutoBuilder {
*/
static void configureCustom(std::function<frc::Pose2d()> poseSupplier,
std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> pathFollowingCommandBuilder,
std::function<void(frc::Pose2d)> resetPose, bool isHolonomic,
std::function<void(const frc::Pose2d&)> resetPose, bool isHolonomic,
std::function<bool()> shouldFlipPose = []() {
return false;
});
Expand Down Expand Up @@ -277,7 +279,7 @@ class AutoBuilder {
static bool m_configured;
static std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> m_pathFollowingCommandBuilder;
static std::function<frc::Pose2d()> m_poseSupplier;
static std::function<void(frc::Pose2d)> m_resetPose;
static std::function<void(const frc::Pose2d&)> m_resetPose;
static std::function<bool()> m_shouldFlipPath;
static bool m_isHolonomic;

Expand Down
5 changes: 3 additions & 2 deletions pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class FollowPathCommand: public frc2::CommandHelper<frc2::Command,
FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<
void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Requirements requirements);
Expand All @@ -63,7 +64,7 @@ class FollowPathCommand: public frc2::CommandHelper<frc2::Command,
std::shared_ptr<PathPlannerPath> m_originalPath;
std::function<frc::Pose2d()> m_poseSupplier;
std::function<frc::ChassisSpeeds()> m_speedsSupplier;
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> m_output;
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> m_output;
std::shared_ptr<PathFollowingController> m_controller;
RobotConfig m_robotConfig;
std::function<bool()> m_shouldFlipPath;
Expand Down
3 changes: 2 additions & 1 deletion pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,8 @@ class PathfindThenFollowPath: public frc2::SequentialCommandGroup {
PathConstraints pathfindingConstraints,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> currentRobotRelativeSpeeds,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<
void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Requirements requirements) {
Expand Down
8 changes: 5 additions & 3 deletions pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
PathConstraints constraints,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<
void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
frc2::Requirements requirements);
Expand All @@ -72,7 +73,8 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
units::meters_per_second_t goalEndVel,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> output,
std::function<
void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
std::shared_ptr<PathFollowingController> controller,
RobotConfig robotConfig, frc2::Requirements requirements);

Expand All @@ -93,7 +95,7 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
PathConstraints m_constraints;
std::function<frc::Pose2d()> m_poseSupplier;
std::function<frc::ChassisSpeeds()> m_speedsSupplier;
std::function<void(frc::ChassisSpeeds, DriveFeedforwards)> m_output;
std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> m_output;
std::shared_ptr<PathFollowingController> m_controller;
RobotConfig m_robotConfig;
std::function<bool()> m_shouldFlipPath;
Expand Down
2 changes: 1 addition & 1 deletion pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -314,7 +314,7 @@ class PathPlannerPath: public std::enable_shared_from_this<PathPlannerPath> {
*
* @return List of poses for each point in this path
*/
inline std::vector<frc::Pose2d> getPathPoses() {
inline std::vector<frc::Pose2d> getPathPoses() const {
std::vector < frc::Pose2d > poses;
for (const PathPoint &point : m_allPoints) {
poses.emplace_back(point.position, frc::Rotation2d());
Expand Down
18 changes: 9 additions & 9 deletions pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,33 +11,33 @@ namespace pathplanner {
class PathPlannerLogging {
public:
static inline void setLogCurrentPoseCallback(
std::function<void(frc::Pose2d)> logCurrentPose) {
std::function<void(const frc::Pose2d&)> logCurrentPose) {
m_logCurrentPose = logCurrentPose;
}

static inline void setLogTargetPoseCallback(
std::function<void(frc::Pose2d)> logTargetPose) {
std::function<void(const frc::Pose2d&)> logTargetPose) {
m_logTargetPose = logTargetPose;
}

static inline void setLogActivePathCallback(
std::function<void(std::vector<frc::Pose2d>)> logActivePath) {
std::function<void(const std::vector<frc::Pose2d>&)> logActivePath) {
m_logActivePath = logActivePath;
}

static inline void logCurrentPose(frc::Pose2d pose) {
static inline void logCurrentPose(const frc::Pose2d &pose) {
if (m_logCurrentPose) {
m_logCurrentPose(pose);
}
}

static inline void logTargetPose(frc::Pose2d targetPose) {
static inline void logTargetPose(const frc::Pose2d &targetPose) {
if (m_logTargetPose) {
m_logTargetPose(targetPose);
}
}

static void logActivePath(std::shared_ptr<PathPlannerPath> path) {
static void logActivePath(const PathPlannerPath *path) {
if (m_logActivePath) {
std::vector < frc::Pose2d > poses;

Expand All @@ -50,8 +50,8 @@ class PathPlannerLogging {
}

private:
static std::function<void(frc::Pose2d)> m_logCurrentPose;
static std::function<void(frc::Pose2d)> m_logTargetPose;
static std::function<void(std::vector<frc::Pose2d>&)> m_logActivePath;
static std::function<void(const frc::Pose2d&)> m_logCurrentPose;
static std::function<void(const frc::Pose2d&)> m_logTargetPose;
static std::function<void(const std::vector<frc::Pose2d>&)> m_logActivePath;
};
}

0 comments on commit 49fb344

Please sign in to comment.