Merge pull request #68 from rtbot-dev/RB-249

RB-249 Add ConstantResampler and Function operators

Author: rcarcasses

libs/api/src/FactoryOp.cpp

@@ -399,6 +399,34 @@ void from_json(const json& j, Pipeline<T, V>& p) {
   p = Pipeline<T, V>(j.at("id").get<string>(), j.at("prog").dump());
 }
 
+template <class T, class V>
+void to_json(json& j, const ConstantResampler<T, V>& p) {
+  j = json{{"type", p.typeName()}, {"id", p.id}, {"interval", p.getInterval()}};
+}
+
+template <class T, class V>
+void from_json(const json& j, ConstantResampler<T, V>& p) {
+  p = ConstantResampler<T, V>(j["id"].get<string>(), j["interval"].get<T>());
+}
+
+template <class T, class V>
+void to_json(json& j, const Function<T, V>& p) {
+  vector<vector<V>> points_array;
+  for (const auto& point : p.getPoints()) {
+    points_array.push_back({point.first, point.second});
+  }
+  j = json{{"type", p.typeName()}, {"id", p.id}, {"points", points_array}, {"type", p.getInterpolationType()}};
+}
+
+template <class T, class V>
+void from_json(const json& j, Function<T, V>& p) {
+  vector<pair<V, V>> points;
+  for (const auto& point : j["points"]) {
+    points.push_back({point[0].get<V>(), point[1].get<V>()});
+  }
+  p = Function<T, V>(j["id"].get<string>(), points, j.value("type", "linear"));
+}
+
 /* Operators serialization - deserialization - end */
 
 Bytes FactoryOp::serialize(string const& programId) {
@@ -463,6 +491,8 @@ FactoryOp::FactoryOp() {
   op_registry_add<Pipeline<uint64_t, double>, json>();
   op_registry_add<GreaterThanStream<uint64_t, double>, json>();
   op_registry_add<LessThanStream<uint64_t, double>, json>();
+  op_registry_add<Function<uint64_t, double>, json>();
+  op_registry_add<ConstantResampler<uint64_t, double>, json>();
 }
 
 static FactoryOp factory;

libs/std/include/rtbot/std/ConstantResampler.h

@@ -0,0 +1,120 @@
+#ifndef CONSTANTRESAMPLER_H
+#define CONSTANTRESAMPLER_H
+
+#include "rtbot/Operator.h"
+
+namespace rtbot {
+
+using namespace std;
+
+template <class T, class V>
+struct ConstantResampler : public Operator<T, V> {
+  T dt;            // Time interval between emissions
+  T nextEmit;      // Next time point to emit at
+  bool initiated;  // Whether we've received our first message
+  V lastValue;     // Last value received, used for causal consistency
+
+  ConstantResampler() = default;
+
+  ConstantResampler(string const& id, T interval) : Operator<T, V>(id) {
+    if (interval <= 0) throw runtime_error(typeName() + ": time interval must be positive");
+
+    this->dt = interval;
+    this->initiated = false;
+    this->addDataInput("i1", 1);
+    this->addOutput("o1");
+  }
+
+  virtual Bytes collect() {
+    Bytes bytes = Operator<T, V>::collect();
+
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&dt),
+                 reinterpret_cast<const unsigned char*>(&dt) + sizeof(dt));
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&nextEmit),
+                 reinterpret_cast<const unsigned char*>(&nextEmit) + sizeof(nextEmit));
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&initiated),
+                 reinterpret_cast<const unsigned char*>(&initiated) + sizeof(initiated));
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&lastValue),
+                 reinterpret_cast<const unsigned char*>(&lastValue) + sizeof(lastValue));
+
+    return bytes;
+  }
+
+  virtual void restore(Bytes::const_iterator& it) {
+    Operator<T, V>::restore(it);
+
+    dt = *reinterpret_cast<const T*>(&(*it));
+    it += sizeof(dt);
+    nextEmit = *reinterpret_cast<const T*>(&(*it));
+    it += sizeof(nextEmit);
+    initiated = *reinterpret_cast<const bool*>(&(*it));
+    it += sizeof(initiated);
+    lastValue = *reinterpret_cast<const V*>(&(*it));
+    it += sizeof(lastValue);
+  }
+
+  string typeName() const override { return "ConstantResampler"; }
+
+  OperatorMessage<T, V> processData() override {
+    string inputPort;
+    auto in = this->getDataInputs();
+    if (in.size() == 1)
+      inputPort = in.at(0);
+    else
+      throw runtime_error(typeName() + " : more than 1 input port found");
+
+    Message<T, V> msg = this->getDataInputLastMessage(inputPort);
+
+    // Initialize on first message
+    if (!initiated) {
+      nextEmit = msg.time + dt;
+      lastValue = msg.value;
+      initiated = true;
+      return {};
+    }
+
+    OperatorMessage<T, V> outputMsgs;
+    PortMessage<T, V> toEmit;
+
+    // If message time is past next emit time, we need to emit at all grid points
+    // up to (but not including) the current message time
+    while (nextEmit < msg.time) {
+      Message<T, V> out;
+      out.time = nextEmit;
+      out.value = lastValue;  // Use previous value for causal consistency
+      toEmit.push_back(out);
+      nextEmit += dt;
+    }
+
+    // If current message is exactly on the grid, emit with current value
+    if (nextEmit == msg.time) {
+      Message<T, V> out;
+      out.time = msg.time;
+      out.value = msg.value;  // Use current value for grid-aligned messages
+      toEmit.push_back(out);
+      nextEmit += dt;
+    }
+
+    // If we emitted anything, add it to output messages
+    if (!toEmit.empty()) {
+      outputMsgs.emplace("o1", toEmit);
+    }
+
+    // Update last value for future emissions
+    lastValue = msg.value;
+
+    return outputMsgs;
+  }
+
+  T getInterval() const { return dt; }
+  T getNextEmissionTime() const { return nextEmit; }
+  bool isInitiated() const { return initiated; }
+  V getLastValue() const { return lastValue; }
+
+ private:
+  void reset() { initiated = false; }
+};
+
+}  // namespace rtbot
+
+#endif  // CONSTANTRESAMPLER_H

libs/std/include/rtbot/std/ConstantResampler.md

@@ -0,0 +1,124 @@
+---
+behavior:
+  buffered: true
+  throughput: constant
+view:
+  shape: circle
+  latex:
+    template: |
+      \Delta t = {{interval}}
+jsonschema:
+  type: object
+  properties:
+    id:
+      type: string
+      description: The id of the operator
+    interval:
+      type: integer
+      examples:
+        - 5
+      description: The time interval between emissions.
+      minimum: 1
+  required: ["id", "interval"]
+---
+
+# ConstantResampler
+
+Inputs: `i1`  
+Outputs: `o1`
+
+The `ConstantResampler` operator transforms a variable throughput input signal into a constant throughput output signal by emitting values at fixed time intervals. The operator maintains causal consistency while handling grid-aligned messages with special precision.
+
+## Behavior
+
+### Initialization
+
+When receiving its first message at time t₀, the operator:
+
+- Stores the initial value
+- Sets up first emission point at t₀ + dt
+- Does not emit
+
+### Regular Operation
+
+For subsequent messages, the operator follows these rules:
+
+1. **Grid-Aligned Messages**:
+
+   - If a message arrives exactly at a grid point (t = nextEmit)
+   - Emits immediately with the current message's value
+   - Updates nextEmit to t + dt
+
+2. **Between Grid Points**:
+
+   - If message time is between grid points
+   - Stores the value for future emissions
+   - No immediate emission
+
+3. **Past Grid Points**:
+
+   - If message time is past one or more grid points
+   - Emits at each missed grid point using the last known value
+   - Maintains causal consistency by using values known at each emission time
+
+4. **Large Gaps**:
+   - For large time gaps between messages
+   - Emits at all intermediate grid points
+   - Uses the last known value before each emission point
+
+### Key Characteristics
+
+- Lazy evaluation: only emits when receiving messages
+- Maintains constant time intervals between emissions
+- Preserves causal consistency
+- Special handling for grid-aligned messages
+- Buffers only one message (the most recent value)
+- State is maintained between calls and can be serialized/restored
+
+## Examples
+
+### Basic Grid Alignment
+
+```
+Input: dt = 5
+t=1: Receive(1, 10.0)  -> No emission, nextEmit = 6
+t=3: Receive(3, 30.0)  -> No emission
+t=6: Receive(6, 60.0)  -> Emit(6, 60.0), nextEmit = 11
+t=8: Receive(8, 80.0)  -> No emission
+```
+
+### Large Gap with Multiple Emissions
+
+```
+Input: dt = 3
+t=1: Receive(1, 10.0)  -> No emission, nextEmit = 4
+t=9: Receive(9, 90.0)  -> Emit sequence:
+                          - (4, 10.0)  [using last known value]
+                          - (7, 10.0)  [using last known value]
+                          nextEmit = 10
+```
+
+### Mixed Grid-Aligned and Non-Aligned
+
+```
+Input: dt = 5
+t=1:  Receive(1, 10.0)  -> No emission, nextEmit = 6
+t=6:  Receive(6, 60.0)  -> Emit(6, 60.0)  [grid-aligned]
+t=13: Receive(13, 130.0) -> Emit(11, 60.0) [using previous value]
+```
+
+## Error Handling
+
+- Throws if dt ≤ 0
+- Throws if input port count is incorrect
+
+## State Management
+
+The operator maintains:
+
+- Current interval (dt)
+- Next emission time
+- Last received value
+- Initialization status
+
+All state can be serialized and restored for system persistence.