diff --git a/examples/cpp/src/main/cpp/subsystems/SwerveSubsystem.cpp b/examples/cpp/src/main/cpp/subsystems/SwerveSubsystem.cpp index d2d1f685..d2a00cc0 100644 --- a/examples/cpp/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/examples/cpp/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -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( SwerveConstants::translationConstants, SwerveConstants::rotationConstants @@ -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); }); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp index fb086058..92c9c966 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp @@ -16,7 +16,7 @@ using namespace pathplanner; bool AutoBuilder::m_configured = false; std::function)> AutoBuilder::m_pathFollowingCommandBuilder; std::function AutoBuilder::m_poseSupplier; -std::function AutoBuilder::m_resetPose; +std::function AutoBuilder::m_resetPose; std::function AutoBuilder::m_shouldFlipPath; bool AutoBuilder::m_isHolonomic = false; @@ -32,9 +32,9 @@ std::function< frc2::CommandPtr(std::shared_ptr, PathConstraints)> AutoBuilder::m_pathfindThenFollowPathCommandBuilder; void AutoBuilder::configure(std::function poseSupplier, - std::function resetPose, + std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Subsystem *driveSubsystem) { @@ -79,7 +79,7 @@ void AutoBuilder::configure(std::function poseSupplier, void AutoBuilder::configureCustom(std::function poseSupplier, std::function)> pathFollowingCommandBuilder, - std::function resetPose, bool isHolonomic, + std::function resetPose, bool isHolonomic, std::function shouldFlipPose) { if (m_configured) { FRC_ReportError(frc::err::Error, diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index 717eb9bb..0d9ff6af 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp @@ -8,7 +8,7 @@ using namespace pathplanner; FollowPathCommand::FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, - std::function output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) : m_originalPath(path), m_poseSupplier( @@ -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); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index 7edce063..73cb79c7 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -15,7 +15,7 @@ PathfindingCommand::PathfindingCommand( std::shared_ptr targetPath, PathConstraints constraints, std::function poseSupplier, std::function speedsSupplier, - std::function output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState( @@ -59,7 +59,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, PathConstraints constraints, units::meters_per_second_t goalEndVel, std::function poseSupplier, std::function speedsSupplier, - std::function output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose( targetPose), m_originalTargetPose(targetPose), m_goalEndState( @@ -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(); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PathPlannerLogging.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PathPlannerLogging.cpp index f56dd875..8dbccc61 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PathPlannerLogging.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PathPlannerLogging.cpp @@ -2,6 +2,6 @@ using namespace pathplanner; -std::function PathPlannerLogging::m_logCurrentPose; -std::function PathPlannerLogging::m_logTargetPose; -std::function&)> PathPlannerLogging::m_logActivePath; +std::function PathPlannerLogging::m_logCurrentPose; +std::function PathPlannerLogging::m_logTargetPose; +std::function&)> PathPlannerLogging::m_logActivePath; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h b/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h index 47b9dea9..69ef25fa 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h @@ -44,9 +44,10 @@ class AutoBuilder { * @param driveSubsystem a pointer to the subsystem for the robot's drive */ static void configure(std::function poseSupplier, - std::function resetPose, + std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function output, + std::function< + void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Subsystem *driveSubsystem); @@ -65,16 +66,17 @@ class AutoBuilder { * @param driveSubsystem a pointer to the subsystem for the robot's drive */ static inline void configure(std::function poseSupplier, - std::function resetPose, + std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function 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); } /** @@ -93,7 +95,7 @@ class AutoBuilder { */ static void configureCustom(std::function poseSupplier, std::function)> pathFollowingCommandBuilder, - std::function resetPose, bool isHolonomic, + std::function resetPose, bool isHolonomic, std::function shouldFlipPose = []() { return false; }); @@ -277,7 +279,7 @@ class AutoBuilder { static bool m_configured; static std::function)> m_pathFollowingCommandBuilder; static std::function m_poseSupplier; - static std::function m_resetPose; + static std::function m_resetPose; static std::function m_shouldFlipPath; static bool m_isHolonomic; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h index 6585fc68..316eb318 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h @@ -45,7 +45,8 @@ class FollowPathCommand: public frc2::CommandHelper path, std::function poseSupplier, std::function speedsSupplier, - std::function output, + std::function< + void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); @@ -63,7 +64,7 @@ class FollowPathCommand: public frc2::CommandHelper m_originalPath; std::function m_poseSupplier; std::function m_speedsSupplier; - std::function m_output; + std::function m_output; std::shared_ptr m_controller; RobotConfig m_robotConfig; std::function m_shouldFlipPath; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h index dd26ef8e..b457fb65 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h @@ -29,7 +29,8 @@ class PathfindThenFollowPath: public frc2::SequentialCommandGroup { PathConstraints pathfindingConstraints, std::function poseSupplier, std::function currentRobotRelativeSpeeds, - std::function output, + std::function< + void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) { diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h index 3d90a9d4..a7836b04 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h @@ -45,7 +45,8 @@ class PathfindingCommand: public frc2::CommandHelper poseSupplier, std::function speedsSupplier, - std::function output, + std::function< + void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); @@ -72,7 +73,8 @@ class PathfindingCommand: public frc2::CommandHelper poseSupplier, std::function speedsSupplier, - std::function output, + std::function< + void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output, std::shared_ptr controller, RobotConfig robotConfig, frc2::Requirements requirements); @@ -93,7 +95,7 @@ class PathfindingCommand: public frc2::CommandHelper m_poseSupplier; std::function m_speedsSupplier; - std::function m_output; + std::function m_output; std::shared_ptr m_controller; RobotConfig m_robotConfig; std::function m_shouldFlipPath; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h index 86322399..089d5f4c 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -314,7 +314,7 @@ class PathPlannerPath: public std::enable_shared_from_this { * * @return List of poses for each point in this path */ - inline std::vector getPathPoses() { + inline std::vector getPathPoses() const { std::vector < frc::Pose2d > poses; for (const PathPoint &point : m_allPoints) { poses.emplace_back(point.position, frc::Rotation2d()); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h index 50f182e9..c588f2d4 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h @@ -11,33 +11,33 @@ namespace pathplanner { class PathPlannerLogging { public: static inline void setLogCurrentPoseCallback( - std::function logCurrentPose) { + std::function logCurrentPose) { m_logCurrentPose = logCurrentPose; } static inline void setLogTargetPoseCallback( - std::function logTargetPose) { + std::function logTargetPose) { m_logTargetPose = logTargetPose; } static inline void setLogActivePathCallback( - std::function)> logActivePath) { + std::function&)> 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 path) { + static void logActivePath(const PathPlannerPath *path) { if (m_logActivePath) { std::vector < frc::Pose2d > poses; @@ -50,8 +50,8 @@ class PathPlannerLogging { } private: - static std::function m_logCurrentPose; - static std::function m_logTargetPose; - static std::function&)> m_logActivePath; + static std::function m_logCurrentPose; + static std::function m_logTargetPose; + static std::function&)> m_logActivePath; }; }