Change C++ Notifier to allow std::function callback.

Also provide templated varags constructor for backwards compatibility and
ease of use.

Update PIDController to use new constructor, eliminating static function
CallCalculate().

Change-Id: Iaeae95aa5953f294f5debc5fc569ef6d4684f223

wpilibc/Athena/include/Notifier.h

@@ -5,14 +5,23 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
+#include <functional>
+
 #include "ErrorBase.h"
 #include "HAL/cpp/priority_mutex.h"
 
-typedef void (*TimerEventHandler)(void *param);
+typedef std::function<void()> TimerEventHandler;
 
 class Notifier : public ErrorBase {
  public:
-  Notifier(TimerEventHandler handler, void *param = nullptr);
+  explicit Notifier(TimerEventHandler handler);
+
+  template <typename Callable, typename Arg, typename... Args>
+  Notifier(Callable&& f, Arg&& arg, Args&&... args)
+      : Notifier(std::bind(std::forward<Callable>(f),
+                           std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
   virtual ~Notifier();
 
   Notifier(const Notifier&) = delete;
@@ -34,8 +43,6 @@ class Notifier : public ErrorBase {
   void *m_notifier;
   // address of the handler
   TimerEventHandler m_handler;
-  // a parameter to pass to the handler
-  void *m_param;
   // the absolute expiration time
   double m_expirationTime = 0;
   // the relative time (either periodic or single)

wpilibc/Athena/src/Notifier.cpp

@@ -16,11 +16,10 @@
  * @param handler The handler is called at the notification time which is set
  * using StartSingle or StartPeriodic.
  */
-Notifier::Notifier(TimerEventHandler handler, void *param) {
+Notifier::Notifier(TimerEventHandler handler) {
   if (handler == nullptr)
     wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
   m_handler = handler;
-  m_param = param;
   int32_t status = 0;
   m_notifier = initializeNotifier(&Notifier::Notify, this, &status);
   wpi_setErrorWithContext(status, getHALErrorMessage(status));
@@ -62,12 +61,11 @@ void Notifier::Notify(uint32_t currentTimeInt, void *param) {
   }
 
   auto handler = notifier->m_handler;
-  auto hparam = notifier->m_param;
 
   notifier->m_handlerMutex.lock();
   notifier->m_processMutex.unlock();
 
-  if (handler) handler(hparam);
+  if (handler) handler();
   notifier->m_handlerMutex.unlock();
 }
 

wpilibc/Athena/src/PIDController.cpp

@@ -55,7 +55,7 @@ PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
 void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
                                PIDSource *source, PIDOutput *output,
                                float period) {
-  m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
 
   m_P = Kp;
   m_I = Ki;
@@ -77,25 +77,9 @@ PIDController::~PIDController() {
   if (m_table != nullptr) m_table->RemoveTableListener(this);
 }
 
-/**
- * Call the Calculate method as a non-static method. This avoids having to
- * prepend
- * all local variables in that method with the class pointer. This way the
- * "this"
- * pointer will be set up and class variables can be called more easily.
- * This method is static and called by the Notifier class.
- * @param controller the address of the PID controller object to use in the
- * background loop
- */
-void PIDController::CallCalculate(void *controller) {
-  PIDController *control = (PIDController *)controller;
-  control->Calculate();
-}
-
 /**
  * Read the input, calculate the output accordingly, and write to the output.
- * This should only be called by the Notifier indirectly through CallCalculate
- * and is created during initialization.
+ * This should only be called by the Notifier.
  */
 void PIDController::Calculate() {
   bool enabled;

wpilibc/shared/include/PIDController.h

@@ -119,7 +119,6 @@ class PIDController : public LiveWindowSendable,
 
   void Initialize(float p, float i, float d, float f, PIDSource *source,
                   PIDOutput *output, float period = 0.05);
-  static void CallCalculate(void *controller);
 
   virtual std::shared_ptr<ITable> GetTable() const override;
   virtual std::string GetSmartDashboardType() const override;

wpilibc/simulation/include/Notifier.h

@@ -5,16 +5,25 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
+#include <atomic>
+#include <functional>
+#include <thread>
+
 #include "ErrorBase.h"
 #include "HAL/cpp/priority_mutex.h"
-#include <thread>
-#include <atomic>
 
-typedef void (*TimerEventHandler)(void *param);
+typedef std::function<void()> TimerEventHandler;
 
 class Notifier : public ErrorBase {
  public:
-  Notifier(TimerEventHandler handler, void *param = nullptr);
+  explicit Notifier(TimerEventHandler handler);
+
+  template <typename Callable, typename Arg, typename... Args>
+  Notifier(Callable&& f, Arg&& arg, Args&&... args)
+      : Notifier(std::bind(std::forward<Callable>(f),
+                           std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
   virtual ~Notifier();
 
   Notifier(const Notifier&) = delete;
@@ -42,8 +51,6 @@ class Notifier : public ErrorBase {
 
   // address of the handler
   TimerEventHandler m_handler;
-  // a parameter to pass to the handler
-  void *m_param;
   // the relative time (either periodic or single)
   double m_period = 0;
   // absolute expiration time for the current event

wpilibc/simulation/src/Notifier.cpp

@@ -20,12 +20,11 @@ std::atomic<bool> Notifier::m_stopped(false);
  * @param handler The handler is called at the notification time which is set
  * using StartSingle or StartPeriodic.
  */
-Notifier::Notifier(TimerEventHandler handler, void *param)
+Notifier::Notifier(TimerEventHandler handler)
 {
 	if (handler == nullptr)
 		wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
 	m_handler = handler;
-	m_param = param;
 	m_periodic = false;
 	m_expirationTime = 0;
 	m_period = 0;
@@ -112,7 +111,7 @@ void Notifier::ProcessQueue(uint32_t mask, void *params)
 			current->m_handlerMutex.lock();
 		}
 
-		current->m_handler(current->m_param);	// call the event handler
+		current->m_handler();	// call the event handler
 		current->m_handlerMutex.unlock();
 	}
 	// reschedule the first item in the queue

wpilibc/simulation/src/PIDController.cpp

@@ -84,7 +84,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
 	m_pidOutput = output;
 	m_period = period;
 
-	m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+	m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
 	m_controlLoop->StartPeriodic(m_period);
 
 	static int32_t instances = 0;
@@ -98,23 +98,9 @@ PIDController::~PIDController() {
 }
 
 /**
- * Call the Calculate method as a non-static method. This avoids having to prepend
- * all local variables in that method with the class pointer. This way the "this"
- * pointer will be set up and class variables can be called more easily.
- * This method is static and called by the Notifier class.
- * @param controller the address of the PID controller object to use in the background loop
+ * Read the input, calculate the output accordingly, and write to the output.
+ * This should only be called by the Notifier.
  */
-void PIDController::CallCalculate(void *controller)
-{
-	PIDController *control = (PIDController*) controller;
-	control->Calculate();
-}
-
- /**
-  * Read the input, calculate the output accordingly, and write to the output.
-  * This should only be called by the Notifier indirectly through CallCalculate
-  * and is created during initialization.
-  */
 void PIDController::Calculate()
 {
 	bool enabled;