Frontend Code for Robotic System with State Management - Part 3

July 1, 2023

In the previous article, I discussed existing libraries for flow management. In this article, I will suggest one way to implement a frontend code by partially borrowing some concepts from Redux. You can find the complete code here (opens in a new tab).

1. Toy scenario

First, let's examine the flows that need to be managed in order to implement the scenario. Please note that the flow discussed here is a simple example of implementation.

Fig. Target flow diagram of autonomous drone cinematographer

I will skip the explanation of each flow (arrows) until transition table. For the moment, please only note that even a simple system can have seemingly convoluted flow diagram.

Possible motion phases of drone

As the above picture overlays, we have six motion phases:

• Idle: This phase represents when nothing is being done, typically when the drone is first booted.
• Hovering: In this phase, when the user requests hovering, the drone tries to reach a defined height. We assume that this phase requires feedback information of the drone's position when calculating a control input. It works similar to a PID controller (opens in a new tab).
• Chasing: When the user requests the start of shooting, the drone tries to (re)plan a flight path for a short horizon. If the plan expires, it needs to replan its chasing flight. This phase works similar to Model predictive control (MPC) (opens in a new tab).
• Exploration: when the target is no longer visible to the drone during the chasing phase, it will attempt to explore the area near the last known location.
• Holding: Any of the above phases will transition into this phase when the safety of the drone is not guaranteed. When the drone's position becomes safe, it will revert to its previous phase. Therefore, we need to remember the previous phase.
• Landing: Landing can be triggered either by a user request or in a low-battery situation. It should be able to override all other phases as it is the most important behavior.

For each phase, various requirements are collected.

Requirements to implement the phases

Based on the phases mentioned above, our frontend code should fulfill the following requirements:

• Provide feedback of robot information for the PID controller (idle).
• Perform periodic replanning for the MPC (chasing).
• Maintain memory of motion phases (holding).
• Handle priority for more important states (landing and holding).

In this article, let's delves into how to write a awesome frontend code for robotic pipeline while inheriting the advantages of redux and achieving the requirements 😊

File structure

As can be found in repo (opens in a new tab), the file structure of suggested pipeline looks like:

include/
└── my_robotics_library
    ├── backend
    │   ├── obstacle_manager.h
    │   ├── planners
    │   │   ├── chasing_planner.h
    │   │   └── height_planner.h
    │   └── types.h
    └── frontend
        └── wrapper.h
src/
├── backend
│   ├── chasing_planner.cc
│   └── height_planner.cc
└── frontend
    └── wrapper.cc
 

Here, Wrapper class located in frontend implements the frontend code. Now, let us have a look how each component is written in a real code, step by step.

Fig. Frontend structure and we will look into five steps 😂

2. Monitor to event definition

Step 1: Sensor information

I defined the below (opens in a new tab) as the sensor information (also can be used as a feedback).

struct SensorInformation {
  TimedVelocity velocity;
  TimedPosition position;
  std::optional<TimedPosition> target_position;
  int battery_level{1};
};
...

In Wrapper, the sensor information will be set from setters (opens in a new tab). The setters will be used in consumers of Wrapper (for example, ros wrapper. See adaptor pattern).

 
class Wrapper {
public:
  Wrapper();
 
  void SetVelocity(const TimedVelocity &velocity);
  void SetPosition(const TimedPosition &position);
  void SetTargetPosition(const std::optional<TimedPosition> &target_position);
  void SetBatteryLevel(int level);
 
  ...
 
private:
  ...
  SensorInformation sensor_information_;
  ...

Step 2: Monitor and state

State

In backend side, I defined the six motion phases:

enum MotionPhase {
  kLanding,
  kHolding,
  kExploration,
  kChasing,
  kHovering,
  kIdle
};

These motion phases are associated with how the robot system computes its actuation, and they are used to decide which event should be detected (see the next section, step 3). The state information is stored historically (opens in a new tab) in Wrapper.

Monitor

In an update loop, we keep updating diagnosis using 1) latest sensor_information_ and 2) current state (its history).

struct Monitor {
  bool is_planning_visible{false};
  bool is_safe_for_short_horizon{true};
  bool is_battery_enough{true};
};

void Wrapper::UpdateMonitor() {
  auto current_motion_phase = state_history_.back().motion_phase;
 
  if ((current_motion_phase != MotionPhase::kChasing &&
       current_motion_phase != MotionPhase::kExploration) ||
      !sensor_information_.target_position.has_value())
    monitor_.is_planning_visible = false;
  else
    monitor_.is_planning_visible = true;
 
  if (state_history_.back().motion_phase == MotionPhase::kHolding)
    monitor_.is_safe_for_short_horizon = true;
  else if (std::abs(sensor_information_.position.x) > 1)
    monitor_.is_safe_for_short_horizon = false;
 
  monitor_.is_battery_enough = sensor_information_.battery_level > 0;
}
 

What is the difference between state and monitor?

• State is the result of transition from changes of monitor.
• State should contain the only information needed to compute motion.

Step 3: Read monitor event

To achieve the flow for our scenario, I define the six events:

enum MonitorEvent { kHover, kLand, kHoldStop, kExplore, kChaseReplan, kNone };

Each event can transition a state to another following the below table:

These events are issued in the below function ReadMonitorEvent:

MonitorEvent Wrapper::ReadMonitorEvent() const {
  if (!monitor_.is_battery_enough &&
      state_history_.back().motion_phase != MotionPhase::kLanding)
    return MonitorEvent::kLand;
 
  if (!monitor_.is_safe_for_short_horizon)
    return MonitorEvent::kHoldStop;
 
  switch (state_history_.back().motion_phase) {
  case MotionPhase::kChasing: {
    if (!monitor_.is_planning_visible)
      return kExplore;
 
    using namespace std::chrono;
    auto elapse_since_planning =
        duration<double>(system_clock::now().time_since_epoch()).count() -
        motion_planning_result_->GetRequestTime();
    if (elapse_since_planning > 0.2)
      return MonitorEvent::kChaseReplan;
  }
  case MotionPhase::kExploration: {
    if (monitor_.is_planning_visible)
      return kChaseReplan;
  }
  case MotionPhase::kHolding: {
    auto previous_state = state_history_[state_history_.size() - 2];
    if (monitor_.is_safe_for_short_horizon) {
      switch (previous_state.motion_phase) {
      case MotionPhase::kExploration:
        return MonitorEvent::kExplore;
      case MotionPhase::kChasing:
        return MonitorEvent::kChaseReplan;
      case MotionPhase::kHovering:
        return MonitorEvent::kHover;
      case MotionPhase::kLanding:
        return MonitorEvent::kLand;
      }
    }
  }
  }
  return MonitorEvent::kNone;
}

Here are some noteworthy points:

• As L4 and L7 show, the events with higher priority take precedences by be positioned at the front. This can achieve the last itme of requirements in a simple manner.

• From L11 to L20, checking expiration can issue Replan monitor event to renew the chasing flight in a MPC manner.

• From L26 to L36, the events issued from state Holding are for resuming the previous state before being held. This also satisfy the third item in requirements.

• The ReadMonitorEvent are computing only necessary MonitorEvent using current state. This could overcome the difficulty described here.

• Given state, issues event using monitor and other small operation

3. Transferring reducers (JS) to robotic frontend (C++)

In this chapter, we briefly discuss how I imported the code used in redux into our c++ frontend code, resolving the language difference between Javascript and c++.

Reducer (JS) -> ProcessEvent (C++)

First of all, we discuss how I imported the structure of reducer (opens in a new tab). As this example shows, the Redux library allows us to define a reducer function without explicitly specifying the types of actions (input argument). The content of the action is referred to as the payload, which can be of any type.

export const orderCreateReducer = (state = {}, action) => {
  switch (action.type) {
    case ORDER_CREATE_REQUEST:
      return {
        loading: true,
      };
 
    case ORDER_CREATE_SUCCESS:
      return {
        loading: false,
        success: true,
        order: action.payload,
      };
 
    case ORDER_CREATE_FAIL:
      return {
        loading: false,
        error: action.payload,
      };
 
    case ORDER_CREATE_RESET:
      return {};
 
    default:
      return state;
  }
};

In order to have a similar structure, I designed the below function (opens in a new tab) in c++:

State Wrapper::ProcessEvent(const State &state, const MonitorEvent &event) {
  State new_state = state;
  switch (event) {
  case kHover:
    new_state = HandleHovering(state);
    break;
 
  case kLand:
    new_state = HandleLanding(state);
    break;
 
  case kHoldStop:
    new_state = HandleHoldStop(state);
    break;
 
  case kExplore:
    new_state = HandleExploration(state);
    break;
 
  case kChaseReplan:
    new_state = HandleChasingPlan(state);
    break;
  }
  return new_state;
};
 

This structure is used to reduce (state transition) MonitorEvent detected in a update loop, observing easy-to-understand state transition design used in redux.

Bottommost (JS) -> Uppermost (C++)

In redux, the transition logics (reducers) are normally defined in a separate file (opens in a new tab), at the bottommost to be included from others. Here, the transitions are defined in the uppermost class wrapper.cc along with other functions. Although seemingly entangling, it is more convenient approach as handlers can access the all the information of the uppermost class Wrapper. Also, as will be discussed below, each handler should update the member variable of Wrapper.

Step 4: Handlers and updates of motion planning

Handlers are subroutine functions of ProcessEvent. Each five event has a corresponding handler. Handlers are responsible for 1) updating member variables of Wrapper. and 2) state transition.

Update members of Wrapper and motion planning

class Wrapper has multiple motion planners (opens in a new tab) to interact with backend logics (motion planning, in this example).

class Wrapper {
...
 
private:
  backend::HeightPlanner height_planner_;
  backend::ChasingPlanner chasing_planner_;
...

The planners update motion_planning_result_ which has the following definition (opens in a new tab):

class MotionPlanningResult {
 
public:
  MotionPlanningResult(MotionPhase motion_phase, double t_request)
      : motion_type_(motion_phase), t_request_(t_request){};
  virtual Control GenerateControl(double t) const = 0;
  virtual std::vector<TimedPosition> GetPlanningTrajectory(double t0,
                                                           double tf) const = 0;
  MotionPhase GetMotionType() const { return motion_type_; };
  double GetRequestTime() const { return t_request_; }
 
private:
  MotionPhase motion_type_;
  double t_request_;
};

This result is computed from ComputeXXXMotion(...):

class ChasingPlanner {
public:
  ChasingPlanner() = default;
  ChasingMotionPlanningResult
  ComputeChasingMotion(const ChasingPlannerInput &planner_input);
};
 

The below code shows how kChaseReplan event is handled inside ProcessEvent and updates `motionplanning_result``.

State Wrapper::HandleChasingPlan(const my_robotics_library::State &state) {
  if (!sensor_information_.target_position.has_value())
    return state;
 
  auto new_state = state;
  new_state.motion_phase = MotionPhase::kChasing; // change state
  backend::ChasingPlannerInput input;
  input.target_position = sensor_information_.target_position.value();
  auto chasing_plan = chasing_planner_.ComputeChasingMotion(input);
  motion_planning_result_.reset(
      new backend::ChasingMotionPlanningResult(chasing_plan)); // update motion generation
  return new_state;
}

Step 5: Getting actuation from motion planning

The updated motion_planning_result_ inside the handler can be used to query the required control at a time t.

Control ChasingMotionPlanningResult::GenerateControl(double t) const {
  Control control;
  control.phase = GetMotionType();
  control.t = t;
  control.input = planned_view_position_.x;
  return control;
}

In this function, we are getting the current control input from a planned entity planned_view_position_. (this is just for a simple demonstration.)

Using feedback of Wrapper even after construction of MotionPlanningResult

What if we need latest sensor_information_ during GenerateControl(double t)? After all, in the above logic, the motion planning result was constructed only when an event is read. For example, let us assume that hovering needs the current position, which is working as a PID controller.

To ensure the feedback synchronization, a motion planner is provided with the latest sensor information when setter of Wrapper is working:

void Wrapper::SetPosition(const my_robotics_library::TimedPosition &position) {
  sensor_information_.position = position;
  height_planner_.SetRobotPosition(position);
}

Then, a planner can pass the pointer (opens in a new tab) to sensing information when computing the result. Thus, our motion generator can use the feedback of its parent class, achieving the first requirement.

Summary

Along with previous articles, I introduced a starter frontend code for your robotic systems. The suggested code was able to implement the whole pipeline represented in the diagram, explained in one by one. A more detailed example on how to use my frontend code can be found this test suite (opens in a new tab). As a future work, I am considering a multi-thread structure with mutex management.