Optimize actuator control signal retrieval

core_sim/include/core_sim/actuators/actuator.hpp

@@ -6,6 +6,7 @@
 #ifndef CORE_SIM_INCLUDE_CORE_SIM_ACTUATORS_ACTUATOR_HPP_
 #define CORE_SIM_INCLUDE_CORE_SIM_ACTUATORS_ACTUATOR_HPP_
 
+#include <array>
 #include <memory>
 #include <string>
 #include <vector>
@@ -29,17 +30,23 @@ enum class ActuatorType {
 // Abstract base class
 class Actuator {
  public:
+  static constexpr size_t kMaxControlSignalCount = 3;
+  using ControlSignals = std::array<float, kMaxControlSignalCount>;
+
   virtual ~Actuator() {}
 
   bool IsLoaded() const;
 
   const std::string& GetId() const;
+  size_t GetSignalCount() const;
+  int GetSignalIndex(size_t signal_offset = 0) const;
+  void SetSignalIndex(int signal_index, size_t signal_offset = 0);
   ActuatorType GetType() const;
   bool IsEnabled() const;
   const std::string& GetParentLink() const;
   const std::string& GetChildLink() const;
-  virtual void UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos) = 0;
+  virtual void UpdateActuatorOutput(const ControlSignals& control_signals,
+                                    const TimeNano sim_dt_nanos) = 0;
 
   bool UpdateFaultInjectionEnabledState(bool enabled);
 

core_sim/include/core_sim/actuators/gimbal.hpp

@@ -37,7 +37,7 @@ class Gimbal : public Actuator {
 
   const ActuatedTransforms& GetActuatedTransforms() const;
 
- void UpdateActuatorOutput(std::vector<float> && control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos) override;
 
   const std::string& GetTargetID(void) const;

core_sim/include/core_sim/actuators/lift_drag_control_surface.hpp

@@ -43,8 +43,8 @@ class LiftDragControlSurface : public Actuator {
 
   const float& GetControlAngle() const;
 
- void UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos)override;
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
+                            const TimeNano sim_dt_nanos) override;
 
  private:
   friend class Robot;

core_sim/include/core_sim/actuators/rotor.hpp

@@ -112,8 +112,8 @@ class Rotor : public Actuator {
 
   void SetTilt(Quaternion quat);
 
- void UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos)override;
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
+                            const TimeNano sim_dt_nanos) override;
 
   // These conversion operators allow this object to be passed directly to
   // TransformTree methods

core_sim/include/core_sim/actuators/tilt.hpp

@@ -52,7 +52,7 @@ class Tilt : public Actuator {
 
   const std::string& GetTargetID(void) const;
 
- void UpdateActuatorOutput(std::vector<float> && control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos) override;
 
  private:

core_sim/include/core_sim/actuators/wheel.hpp

@@ -96,7 +96,7 @@ class Wheel : public Actuator {
 
   const float GetPowerConsumption() const;
 
-  void UpdateActuatorOutput(std::vector<float>&& control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos) override;
 
   // These conversion operators allow this object to be passed directly to

core_sim/include/core_sim/runtime_components.hpp

@@ -52,6 +52,18 @@ class IController : public IRuntimeComponent {
   // TODO: Should this be in the base IRuntimeComponent?
   virtual void Update() = 0;
 
+  virtual int GetControlSignalIndex(const std::string& actuator_id) = 0;
+
+  virtual int GetControlSignalIndex(const std::string& actuator_id,
+                                    size_t signal_offset) {
+    return signal_offset == 0 ? GetControlSignalIndex(actuator_id) : -1;
+  }
+
+  virtual void GetControlSignalSnapshot(
+      std::vector<float>& control_signals) = 0;
+
+  virtual std::vector<float> GetControlSignals(int signal_index) = 0;
+
   virtual std::vector<float> GetControlSignals(const std::string& actuator_id) = 0;
 
   virtual const GimbalState& GetGimbalSignal(const std::string& gimbal_id) = 0;
@@ -61,4 +73,4 @@ class IController : public IRuntimeComponent {
 }  // namespace projectairsim
 }  // namespace microsoft
 
-#endif  // CORE_SIM_INCLUDE_CORE_SIM_RUNTIME_COMPONENTS_HPP_
+#endif  // CORE_SIM_INCLUDE_CORE_SIM_RUNTIME_COMPONENTS_HPP_

core_sim/src/actor/robot.cpp

@@ -198,6 +198,7 @@ class Robot::Impl : public ActorImpl {
   float total_power_ = 0.0f;
 
   std::unique_ptr<IController> controller_;
+  std::vector<float> control_signal_snapshot_;
 
   std::vector<std::unique_ptr<Actuator>> actuators_;
   std::vector<std::reference_wrapper<Actuator>> actuators_ref_;
@@ -742,6 +743,18 @@ bool Robot::Impl::SetGroundTruthKinematics(
 }
 
 void Robot::Impl::SetController(std::unique_ptr<IController> controller) {
+  for (auto& actuator : actuators_) {
+    if (actuator->GetType() != ActuatorType::kGimbal) {
+      for (size_t signal_offset = 0; signal_offset < actuator->GetSignalCount();
+           ++signal_offset) {
+        const int signal_index = controller == nullptr
+                                     ? -1
+                                     : controller->GetControlSignalIndex(
+                                           actuator->GetId(), signal_offset);
+        actuator->SetSignalIndex(signal_index, signal_offset);
+      }
+    }
+  }
   controller_ = std::move(controller);
 }
 
@@ -1056,16 +1069,29 @@ void Robot::Impl::UpdateActuators(const TimeNano sim_time,
                                   const TimeNano sim_dt_nanos) {
   std::lock_guard<std::mutex> lock(update_lock_);
 
-  if (actuators_.empty()) return;
+  if (actuators_.empty() || controller_ == nullptr) return;
+
+  controller_->GetControlSignalSnapshot(control_signal_snapshot_);
+  const auto get_control_signals =
+      [this](const Actuator& actuator) -> Actuator::ControlSignals {
+    Actuator::ControlSignals control_signals = {};
+    for (size_t signal_offset = 0; signal_offset < actuator.GetSignalCount();
+         ++signal_offset) {
+      const int signal_index = actuator.GetSignalIndex(signal_offset);
+      if (signal_index >= 0 &&
+          signal_index < static_cast<int>(control_signal_snapshot_.size())) {
+        control_signals[signal_offset] = control_signal_snapshot_[signal_index];
+      }
+    }
+    return control_signals;
+  };
 
   // Update tilt actuator first since they affect other actuators
   for (auto& actuator : actuators_) {
     if (actuator->GetType() == ActuatorType::kTilt) {
       // Call actuator to update its output for its current control signal
-      std::vector<float> control_signals =
-          controller_->GetControlSignals(actuator->GetId());
-
-      actuator->UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      actuator->UpdateActuatorOutput(get_control_signals(*actuator),
+                                     sim_dt_nanos);
     }
   }
 
@@ -1096,10 +1122,8 @@ void Robot::Impl::UpdateActuators(const TimeNano sim_time,
       }
 
       // Call actuator to update its output for its current control signal
-      std::vector<float> control_signals =
-          controller_->GetControlSignals(actuator->GetId());
-
-      actuator->UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      actuator->UpdateActuatorOutput(get_control_signals(*actuator),
+                                     sim_dt_nanos);
     }
 
     // Do post-processing specific to actuator type

core_sim/src/actuators/actuator.cpp

@@ -23,6 +23,16 @@ ActuatorType Actuator::GetType() const { return pimpl_->GetType(); }
 
 const std::string& Actuator::GetId() const { return pimpl_->GetID(); }
 
+size_t Actuator::GetSignalCount() const { return pimpl_->GetSignalCount(); }
+
+int Actuator::GetSignalIndex(size_t signal_offset) const {
+  return pimpl_->GetSignalIndex(signal_offset);
+}
+
+void Actuator::SetSignalIndex(int signal_index, size_t signal_offset) {
+  pimpl_->SetSignalIndex(signal_index, signal_offset);
+}
+
 bool Actuator::IsEnabled() const { return pimpl_->IsEnabled(); }
 
 const std::string& Actuator::GetParentLink() const {

core_sim/src/actuators/actuator_impl.hpp

@@ -43,7 +43,8 @@ class ActuatorImpl : public ComponentWithTopicsAndServiceMethods {
         type_(type),
         enabled_(enabled),
         parent_link_(parent_link),
-        child_link_(child_link) {}
+        child_link_(child_link),
+        signal_indices_({-1, -1, -1}) {}
 
   const bool IsEnabled() const { return enabled_; }
 
@@ -53,6 +54,18 @@ class ActuatorImpl : public ComponentWithTopicsAndServiceMethods {
 
   const std::string& GetChildLink() const { return child_link_; }
 
+  size_t GetSignalCount() const {
+    return type_ == ActuatorType::kWheel ? 3 : 1;
+  }
+
+  int GetSignalIndex(size_t signal_offset) const {
+    return signal_indices_.at(signal_offset);
+  }
+
+  void SetSignalIndex(int signal_index, size_t signal_offset) {
+    signal_indices_.at(signal_offset) = signal_index;
+  }
+
   bool UpdateFaultInjectionEnabledState(bool enabled) {
     is_fault_injected_ = enabled;
     return true;
@@ -255,6 +268,7 @@ class ActuatorImpl : public ComponentWithTopicsAndServiceMethods {
   bool enabled_;
   std::string parent_link_;
   std::string child_link_;
+  std::array<int, Actuator::kMaxControlSignalCount> signal_indices_;
 };
 
 }  // namespace projectairsim

core_sim/src/actuators/gimbal.cpp

@@ -56,7 +56,7 @@ class Gimbal::Impl : public ActuatorImpl {
 
   const ActuatedTransforms& GetActuatedTransforms() const;
 
-   void UpdateActuatorOutput(std::vector<float> && control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos);
 
   void UpdateGimbal(IController::GimbalState& new_state,
@@ -118,9 +118,10 @@ const IController::GimbalState& Gimbal::GetGimbalState() const {
   return static_cast<Gimbal::Impl*>(pimpl_.get())->GetGimbalState();
 };
 
-void Gimbal::UpdateActuatorOutput(std::vector<float> && control_signals, const TimeNano sim_dt_nanos){
+void Gimbal::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                  const TimeNano sim_dt_nanos) {
   static_cast<Gimbal::Impl*>(pimpl_.get())
-      ->UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      ->UpdateActuatorOutput(control_signals, sim_dt_nanos);
 }
 
 //------------------------------------------------------------------------------
@@ -191,7 +192,8 @@ void Gimbal::Impl::UpdateGimbal(IController::GimbalState& new_state,
   }
 }
 
-void Gimbal::Impl::UpdateActuatorOutput(std::vector<float> && control_signals, const TimeNano sim_dt_nanos) {
+void Gimbal::Impl::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                        const TimeNano sim_dt_nanos) {
   return;
 }
 

core_sim/src/actuators/lift_drag_control_surface.cpp

@@ -57,7 +57,8 @@ class LiftDragControlSurface::Impl : public ActuatorImpl {
 
   const float& GetControlAngle() const;
 
-  void UpdateActuatorOutput(std::vector<float> && control_signals, const TimeNano sim_dt_nanos);
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
+                            const TimeNano sim_dt_nanos);
 
  private:
   friend class LiftDragControlSurface::Loader;
@@ -111,10 +112,10 @@ const float& LiftDragControlSurface::GetControlAngle() const {
       ->GetControlAngle();
 }
 
-void LiftDragControlSurface::UpdateActuatorOutput(std::vector<float> && control_signals,
-  const TimeNano sim_dt_nanos){
+void LiftDragControlSurface::UpdateActuatorOutput(
+    const ControlSignals& control_signals, const TimeNano sim_dt_nanos) {
   static_cast<LiftDragControlSurface::Impl*>(pimpl_.get())
-      ->UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      ->UpdateActuatorOutput(control_signals, sim_dt_nanos);
 }
 
 //------------------------------------------------------------------------------
@@ -156,8 +157,8 @@ const float& LiftDragControlSurface::Impl::GetControlAngle() const {
   return control_angle_;
 }
 
-void LiftDragControlSurface::Impl::UpdateActuatorOutput(std::vector<float> && control_signals,
-  const TimeNano sim_dt_nanos){
+void LiftDragControlSurface::Impl::UpdateActuatorOutput(
+    const ControlSignals& control_signals, const TimeNano sim_dt_nanos) {
   // Convert -1.0 ~ +1.0 control signal to control surface angle (like a servo
   // motor but without any dynamics)
   auto control_signal = control_signals[0];

core_sim/src/actuators/rotor.cpp

@@ -75,7 +75,7 @@ class Rotor::Impl : public ActuatorImpl {
 
   const float GetPowerConsumption() const;
 
-  void UpdateActuatorOutput(std::vector<float> && control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos);
 
   void SetAirDensityRatio(float air_density_ratio);
@@ -174,10 +174,10 @@ const ActuatedTransforms& Rotor::GetActuatedTransforms() const {
   return static_cast<Rotor::Impl*>(pimpl_.get())->GetActuatedTransforms();
 }
 
-void Rotor::UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos){
+void Rotor::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                 const TimeNano sim_dt_nanos) {
   static_cast<Rotor::Impl*>(pimpl_.get())
-      ->UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      ->UpdateActuatorOutput(control_signals, sim_dt_nanos);
 }
 
 void Rotor::SetAirDensityRatio(float air_density_ratio) {
@@ -285,8 +285,8 @@ void Rotor::Impl::SetAirDensityRatio(float air_density_ratio) {
 
 void Rotor::Impl::SetTilt(Quaternion quat) { quat_tilt_ = quat; }
 
-void Rotor::Impl::UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos){
+void Rotor::Impl::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                       const TimeNano sim_dt_nanos) {
   // This actuator uses one control signal
   auto control_signal = control_signals[0];
   // Apply first order filter to simulate actuator hardware dynamics

core_sim/src/actuators/tilt.cpp

@@ -62,7 +62,7 @@ class Tilt::Impl : public ActuatorImpl {
 
   const std::string& GetTargetID(void) const { return (settings_.target_id); }
 
-  void UpdateActuatorOutput(std::vector<float> && control_signals,
+  void UpdateActuatorOutput(const ControlSignals& control_signals,
                             const TimeNano sim_dt_nanos);
 
  private:
@@ -125,10 +125,10 @@ const std::string& Tilt::GetTargetID(void) const {
   return static_cast<Tilt::Impl*>(pimpl_.get())->GetTargetID();
 }
 
-void Tilt::UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos){
+void Tilt::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                const TimeNano sim_dt_nanos) {
   static_cast<Tilt::Impl*>(pimpl_.get())
-      -> UpdateActuatorOutput(std::move(control_signals), sim_dt_nanos);
+      ->UpdateActuatorOutput(control_signals, sim_dt_nanos);
 }
 
 //------------------------------------------------------------------------------
@@ -176,8 +176,8 @@ const ActuatedTransforms& Tilt::Impl::GetActuatedTransforms() const {
   return actuated_transforms_;
 }
 
-void Tilt::Impl::UpdateActuatorOutput(std::vector<float> && control_signals,
-                            const TimeNano sim_dt_nanos){
+void Tilt::Impl::UpdateActuatorOutput(const ControlSignals& control_signals,
+                                      const TimeNano sim_dt_nanos) {
   float radians;
   Quaternion quat;