rclnodejs/lib/lifecycle.js

ROS2.0 JavaScript client with Node.js

// Copyright (c) 2020 Wayne Parrott. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

'use strict';

const rclnodejs = require('bindings')('rclnodejs');
const LifecyclePublisher = require('./lifecycle_publisher.js');
const loader = require('./interface_loader.js');
const Context = require('./context.js');
const Node = require('./node.js');
const NodeOptions = require('./node_options.js');
const Service = require('./service.js');

const SHUTDOWN_TRANSITION_LABEL =
  rclnodejs.getLifecycleShutdownTransitionLabel();

// An instance of State message constructor used for accessing State
// state machine constants. This interface is lazy initialized at runtime.
let StateInterface;

// An instance of Transition message constructor used for accessing Transition
// state machine constants. This interface is lazy initialized at runtime.
let TransitionInterface;

function getStateInterface() {
  if (!StateInterface) {
    StateInterface = loader.loadInterface('lifecycle_msgs/msg/State');
  }
  return StateInterface;
}

function getTransitionInterface() {
  if (!TransitionInterface) {
    TransitionInterface = loader.loadInterface('lifecycle_msgs/msg/Transition');
  }
  return TransitionInterface;
}

/**
 * @typedef SerializedState
 * @type {object}
 * @property {number} id - code identifying the type of this state.
 * @property {string} label - readable name of this state.
 */

/**
 * The state of the lifecycle state model.
 */
class State {
  /**
   * Create a state.
   * @param {number} id - The id value.
   * @param {string} label - The label value.
   */
  constructor(id, label) {
    this.id = id;
    this.label = label;
  }

  /**
   * Create a State from a SerializedState
   * @param {SerializedState} aSerializedState - The state object.
   * @returns {State} The State converted from a SerializdState
   * @private
   */
  static fromSerializedState(aSerializedState) {
    return new State(aSerializedState.id, aSerializedState.label);
  }
}

/**
 * The intermediate state of the lifecycle state model during a state
 * transition.
 */
class Transition extends State {}

/**
 * Describes a state transition.
 */
class TransitionDescription {
  /**
   * Create a transition description.
   *
   * @param {Transition} transition - The transition
   * @param {State} startState - The initial
   * @param {State} goalState - The target state of a transition activity
   */
  constructor(transition, startState, goalState) {
    this.transition = transition;
    this.startState = startState;
    this.goalState = goalState;
  }
}

/**
 * The values returned by TransitionCallback.
 * @readonly
 * @enum {number}
 */
const CallbackReturnCode = {
  get SUCCESS() {
    return getTransitionInterface().TRANSITION_CALLBACK_SUCCESS;
  },
  get FAILURE() {
    return getTransitionInterface().TRANSITION_CALLBACK_FAILURE;
  },
  get ERROR() {
    return getTransitionInterface().TRANSITION_CALLBACK_ERROR;
  },
};
Object.freeze(CallbackReturnCode);

/**
 * A ValueHolder for a CallbackReturnCode.
 */
class CallbackReturnValue {
  /**
   * Creates a new instance.
   *
   * @param {number} [value=CallbackReturnCode.SUCCESS] - The value property.
   */
  constructor(value = CallbackReturnCode.SUCCESS) {
    this._value = value;
    this._errorMsg = null;
  }

  /**
   * Access the callbackReturnCode.
   * @returns {number} The CallbackReturnCode.
   */
  get value() {
    return this._value;
  }

  set value(value) {
    this._value = value;
  }

  /**
   * Access an optional error message when value is not SUCCESS.
   */
  get errorMsg() {
    return this._errorMsg;
  }

  /**
   * Assign the error message.
   * @param {string} msg - The error message.
   * @returns {unknown} void.
   */
  set errorMsg(msg) {
    this._errorMsg = msg;
  }

  /**
   * Overrides Object.valueOf() to return the 'value' property.
   * @returns {number} The property value.
   */
  valueOf() {
    return this.value;
  }

  /**
   * A predicate to test if the value is SUCCESS.
   * @returns {boolean} Return true if the value is SUCCESS; otherwise return false.
   */
  isSuccess() {
    return this.value === CallbackReturnCode.SUCCESS;
  }

  /**
   * A predicate to test if the value is FAILURE.
   * @returns {boolean} Return true if the value is FAILURE; otherwise return false.
   */
  isFailure() {
    return this.value === CallbackReturnCode.FAILURE;
  }

  /**
   * A predicate to test if the value is ERROR.
   * @returns {boolean} Return true if the value is ERROR; otherwise return false.
   */
  isError() {
    return this.value === CallbackReturnCode.ERROR;
  }

  /**
   * A predicate to test if an error message has been assigned.
   * @returns {boolean} Return true if an error message has been assigned; otherwise return false.
   */
  hasErrorMsg() {
    return !this.isSuccess() && this._errorMsg;
  }
}

/**
 * This callback is invoked when LifecycleNode is transitioning to a new state.
 * @callback TransitionCallback
 * @param {State} previousState - The previous node lifecycle state.
 * @return {CallbackReturnCode} - The result of the callback.
 *
 * @see [LifecycleNode.registerOnConfigure]{@link LifecycleNode#registerOnConfigure}
 * @see [LifecycleNode.registerOnCleanup]{@link LifecycleNode#registerOnCleanup}
 * @see [LifecycleNode.registerOnActivate]{@link LifecycleNode#registerOnActivate}
 * @see [LifecycleNode.registerOnDeactivate]{@link LifecycleNode#registerOnDeactivate}
 * @see [LifecycleNode.registerOnShutdown]{@link LifecycleNode#registerOnShutdown}
 * @see [LifecycleNode.registerOnError]{@link LifecycleNode#registerOnError}
 */

/**
 * A ROS2 managed Node that implements a well-defined life-cycle state model using the
 * {@link https://github.com/ros2/rcl/tree/master/rcl_lifecycle|ros2 client library (rcl) lifecyle api}.
 * @extends Node
 *
 * This class implments the ROS2 life-cycle state-machine defined by the
 * {@link https://github.com/ros2/rclcpp/tree/master/rclcpp_lifecycle}|ROS2 Managed Nodes Design}
 * and parallels the {@link https://github.com/ros2/rclcpp/tree/master/rclcpp_lifecycle|rclcpp lifecycle node }
 * implementation.
 *
 * The design consists of four primary lifecycle states:
 *   UNCONFIGURED
 *   INACTIVE
 *   ACTIVE
 *   FINALIZED.
 *
 * Transitioning between states is accomplished using an action api:
 *   configure()
 *   activate()
 *   deactivate(),
 *   cleanup()
 *   shutdown()
 *
 * During a state transition, the state-machine is in one of the
 * intermediate transitioning states:
 *   CONFIGURING
 *   ACTIVATING
 *   DEACTIVATING
 *   CLEANINGUP
 *   SHUTTING_DOWN
 *   ERROR_PROCESSING
 *
 * Messaging:
 * State changes are published on the '<node_name>/transition_event' topic.
 * Lifecycle service interfaces are also implemented.
 *
 * You can introduce your own state specific behaviors in the form of a
 * {@link TransitionCallback} functions that you register using:
 *   registerOnConfigure(cb)
 *   registerOnActivate(cb)
 *   registerOnDeactivate(cb)
 *   registerOnCleanup(cb)
 *   registerOnShutdown(cb)
 *   registerOnError(cb)
 */

class LifecycleNode extends Node {
  /**
   * Create a managed Node that implements a well-defined life-cycle state
   * model using the {@link https://github.com/ros2/rcl/tree/master/rcl_lifecycle|ros2 client library (rcl) lifecyle api}.
   * @param {string} nodeName - The name used to register in ROS.
   * @param {string} [namespace=''] - The namespace used in ROS.
   * @param {Context} [context=Context.defaultContext()] - The context to create the node in.
   * @param {NodeOptions} [options=NodeOptions.defaultOptions] - The options to configure the new node behavior.
   * @param {boolean} [enableCommunicationsInterface=true] - Enable lifecycle service interfaces, e.g., GetState.
   * @throws {Error} If the given context is not registered.
   */
  constructor(
    nodeName,
    namespace = '',
    context = Context.defaultContext(),
    options = NodeOptions.defaultOptions,
    enableCommunicationInterface = true
  ) {
    super(nodeName, namespace, context, options);
    this.init(enableCommunicationInterface);
  }

  init(enableCommunicationInterface) {
    // initialize native handle to rcl_lifecycle_state_machine
    this._stateMachineHandle = rclnodejs.createLifecycleStateMachine(
      this.handle,
      enableCommunicationInterface
    );

    // initialize Map<transitionId,TransitionCallback>
    this._callbackMap = new Map();

    if (!enableCommunicationInterface) {
      // do not create lifecycle services
      return;
    }

    // Setup and register the 4 native rcl lifecycle services thar are
    // part of _stateMachineHandle.
    let srvHandleObj = rclnodejs.getLifecycleSrvNameAndHandle(
      this._stateMachineHandle,
      'srv_get_state'
    );
    let service = new Service(
      this.handle,
      srvHandleObj.handle,
      srvHandleObj.name,
      loader.loadInterface('lifecycle_msgs/srv/GetState'),
      this._validateOptions(undefined),
      (request, response) => this._onGetState(request, response)
    );
    this._services.push(service);

    srvHandleObj = rclnodejs.getLifecycleSrvNameAndHandle(
      this._stateMachineHandle,
      'srv_get_available_states'
    );
    service = new Service(
      this.handle,
      srvHandleObj.handle,
      srvHandleObj.name,
      loader.loadInterface('lifecycle_msgs/srv/GetAvailableStates'),
      this._validateOptions(undefined),
      (request, response) => this._onGetAvailableStates(request, response)
    );
    this._services.push(service);

    srvHandleObj = rclnodejs.getLifecycleSrvNameAndHandle(
      this._stateMachineHandle,
      'srv_get_available_transitions'
    );
    service = new Service(
      this.handle,
      srvHandleObj.handle,
      srvHandleObj.name,
      loader.loadInterface('lifecycle_msgs/srv/GetAvailableTransitions'),
      this._validateOptions(undefined),
      (request, response) => this._onGetAvailableTransitions(request, response)
    );
    this._services.push(service);

    srvHandleObj = rclnodejs.getLifecycleSrvNameAndHandle(
      this._stateMachineHandle,
      'srv_change_state'
    );
    service = new Service(
      this.handle,
      srvHandleObj.handle,
      srvHandleObj.name,
      loader.loadInterface('lifecycle_msgs/srv/ChangeState'),
      this._validateOptions(undefined),
      (request, response) => this._onChangeState(request, response)
    );
    this._services.push(service);
    this.syncHandles();
  }

  /**
   * Create a LifecyclePublisher.
   * @param {function|string|object} typeClass - The ROS message class,
        OR a string representing the message class, e.g. 'std_msgs/msg/String',
        OR an object representing the message class, e.g. {package: 'std_msgs', type: 'msg', name: 'String'}
   * @param {string} topic - The name of the topic.
   * @param {object} options - The options argument used to parameterize the publisher.
   * @param {boolean} options.enableTypedArray - The topic will use TypedArray if necessary, default: true.
   * @param {QoS} options.qos - ROS Middleware "quality of service" settings for the publisher, default: QoS.profileDefault.
   * @return {LifecyclePublisher} - An instance of LifecyclePublisher.
   */
  createLifecyclePublisher(typeClass, topic, options) {
    return this._createPublisher(typeClass, topic, options, LifecyclePublisher);
  }

  /**
   * Access the current lifecycle state.
   * @returns {State} The current state.
   */
  get currentState() {
    let currentStateObj = rclnodejs.getCurrentLifecycleState(
      this._stateMachineHandle
    );
    return State.fromSerializedState(currentStateObj);
  }

  /**
   * Retrieve all registered states of the state-machine.
   * @returns {State[]} The states.
   */
  get availableStates() {
    let stateObjs = rclnodejs.getLifecycleStates(this._stateMachineHandle);
    let states = stateObjs.map(
      (stateObj) => new State(stateObj.id, stateObj.label)
    );
    return states;
  }

  /**
   * Retrieve all registered transitions of the state-machine.
   *
   * @returns {TransitionDescription[]} The registered TransitionDescriptions.
   */
  get transitions() {
    let transitionObjs = rclnodejs.getLifecycleTransitions(
      this._stateMachineHandle
    );
    let transitions = transitionObjs.map((transitionDescObj) => {
      let transition = new Transition(
        transitionDescObj.transition.id,
        transitionDescObj.transition.label
      );
      let startState = new State(
        transitionDescObj.start_state.id,
        transitionDescObj.start_state.label
      );
      let goalState = new State(
        transitionDescObj.goal_state.id,
        transitionDescObj.goal_state.label
      );
      return new TransitionDescription(transition, startState, goalState);
    });
    return transitions;
  }

  /**
   * Retrieve the valid transitions available from the current state of the
   * state-machine.
   *
   * @returns {TransitionDescription[]} The available TransitionDescriptions.
   */
  get availableTransitions() {
    let transitionObjs = rclnodejs.getAvailableLifecycleTransitions(
      this._stateMachineHandle
    );
    let transitions = transitionObjs.map((transitionDescObj) => {
      let transition = new Transition(
        transitionDescObj.transition.id,
        transitionDescObj.transition.label
      );
      let startState = new State(
        transitionDescObj.start_state.id,
        transitionDescObj.start_state.label
      );
      let goalState = new State(
        transitionDescObj.goal_state.id,
        transitionDescObj.goal_state.label
      );
      return new TransitionDescription(transition, startState, goalState);
    });
    return transitions;
  }

  /**
   * Register a callback function to be invoked during the configure() action.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void.
   */
  registerOnConfigure(cb) {
    this._callbackMap.set(getStateInterface().TRANSITION_STATE_CONFIGURING, cb);
  }

  /**
   * Register a callback function to be invoked during the activate() action.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void.
   */
  registerOnActivate(cb) {
    this._callbackMap.set(getStateInterface().TRANSITION_STATE_ACTIVATING, cb);
  }

  /**
   * Register a callback function to be invoked during the deactivate() action.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void.
   */
  registerOnDeactivate(cb) {
    this._callbackMap.set(
      getStateInterface().TRANSITION_STATE_DEACTIVATING,
      cb
    );
  }

  /**
   * Register a callback function to be invoked during the cleanup() action.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void.
   */
  registerOnCleanup(cb) {
    this._callbackMap.set(getStateInterface().TRANSITION_STATE_CLEANINGUP, cb);
  }

  /**
   * Register a callback function to be invoked during the shutdown() action.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void
   */
  registerOnShutdown(cb) {
    this._callbackMap.set(
      getStateInterface().TRANSITION_STATE_SHUTTINGDOWN,
      cb
    );
  }

  /**
   * Register a callback function to be invoked when an error occurs during a
   * state transition.
   * @param {TransitionCallback} cb - The callback function to invoke.
   * @returns {unknown} void.
   */
  registerOnError(cb) {
    this._callbackMap.set(
      getStateInterface().TRANSITION_STATE_ERRORPROCESSING,
      cb
    );
  }

  /**
   * Initiate a transition from the UNCONFIGURED state to the INACTIVE state.
   * If an onConfigure callback has been registered it will be invoked.
   *
   * @param {CallbackReturnValue?} callbackReturnValue - value holder for the CallbackReturnCode returned from the callback.
   * @returns {State} The new state, should be INACTIVE.
   * @throws {Error} If transition is invalid for the current state.
   */
  configure(callbackReturnValue) {
    return this._changeState(
      getTransitionInterface().TRANSITION_CONFIGURE,
      callbackReturnValue
    );
  }

  /**
   * Initiate a transition from the INACTIVE state to the ACTIVE state.
   * If an onActivate callback has been registered it will be invoked.
   *
   * @param {CallbackReturnValue?} callbackReturnValue - value holder for the CallbackReturnCode returned from the callback.
   * @returns {State} The new state, should be ACTIVE.
   * @throws {Error} If transition is invalid for the current state.
   */
  activate(callbackReturnValue) {
    return this._changeState(
      getTransitionInterface().TRANSITION_ACTIVATE,
      callbackReturnValue
    );
  }

  /**
   * Initiate a transition from the ACTIVE state to the INACTIVE state.
   * If an onDeactivate callback has been registered it will be invoked.
   *
   * @param {CallbackReturnValue?} callbackReturnValue - value holder for the CallbackReturnCode returned from the callback.
   * @returns {State} The new state, should be INACTIVE.
   * @throws {Error} If transition is invalid for the current state.
   */
  deactivate(callbackReturnValue) {
    return this._changeState(
      getTransitionInterface().TRANSITION_DEACTIVATE,
      callbackReturnValue
    );
  }

  /**
   * Initiate a transition from the INACTIVE state to the UNCONFIGURED state.
   * If an onCleanup callback has been registered it will be invoked.
   *
   * @param {CallbackReturnValue?} callbackReturnValue - value holder for the CallbackReturnCode returned from the callback.
   * @returns {State} The new state, should be INACTIVE.
   * @throws {Error} If transition is invalid for the current state.
   */
  cleanup(callbackReturnValue) {
    return this._changeState(
      getTransitionInterface().TRANSITION_CLEANUP,
      callbackReturnValue
    );
  }

  /**
   * Initiate a transition to the FINALIZED state from any of the following
   * states: UNCONFIGURED, INACTIVE or ACTIVE state. If an onConfigure
   * callback has been registered it will be invoked.
   *
   * @param {CallbackReturnValue?} callbackReturnValue - value holder for the CallbackReturnCode returned from the callback.
   * @returns {State} The new state, should be FINALIZED.
   * @throws {Error} If transition is invalid for the current state.
   */
  shutdown(callbackReturnValue) {
    return this._changeState(SHUTDOWN_TRANSITION_LABEL, callbackReturnValue);
  }

  /**
   * Check if state machine is initialized.
   *
   * @returns {boolean} true if the state machine is initialized; otherwise false.
   */
  get isInitialized() {
    return rclnodejs.isInitialized(this._stateMachineHandle);
  }

  /**
   * Log the state machine data.
   *
   * @returns {undefined} void.
   */
  print() {
    rclnodejs.print(this._stateMachineHandle);
  }

  /**
   * The GetState service handler.
   * @param {Object} request - The GetState service request.
   * @param {Object} response - The GetState service response.
   * @returns {unknown} void.
   * @private
   */
  _onGetState(request, response) {
    let result = response.template;

    result.current_state = this.currentState;

    response.send(result);
  }

  /**
   * The GetAvailableStates service handler.
   * @param {Object} request - The GetAvailableStates service request.
   * @param {Object} response - The GetAvailableStates service response.
   * @returns {unknown} void.
   * @private
   */
  _onGetAvailableStates(request, response) {
    let result = response.template;

    result.available_states = this.availableStates;

    response.send(result);
  }

  /**
   * The GetAvailableTransitions service handler.
   * @param {Object} request - The GetAvailableTransitions service request
   * @param {Object} response - The GetAvailableTranactions service response.
   * @returns {unknown} void.
   */
  _onGetAvailableTransitions(request, response) {
    let result = response.template;

    result.available_transitions = this.availableTransitions;

    response.send(result);
  }

  /**
   * The ChangeState service handler.
   * @param {Object} request - The ChangeState service request.
   * @param {Object} response - The ChangeState service response
   * @returns {unknown} void.
   * @private
   */
  _onChangeState(request, response) {
    let result = response.template;

    let transitionId = request.transition.id;
    if (request.transition.label) {
      let transitionObj = rclnodejs.getLifecycleTransitionByLabel(
        this._stateMachineHandle,
        request.transition.label
      );
      if (transitionObj.id) {
        transitionId = transitionObj.id;
      } else {
        result.success = false;
        response.send(result);
        return;
      }
    }

    let callbackReturnValue = new CallbackReturnValue();
    this._changeState(transitionId, callbackReturnValue);

    result.success = callbackReturnValue.isSuccess();
    response.send(result);
  }

  /**
   * Transition to a new lifecycle state.
   * @param {number|string} transitionIdOrLabel - The id or label of the target transition.
   * @param {CallbackReturnValue} callbackReturnValue - An out parameter that holds the CallbackReturnCode.
   * @returns {State} The new state.
   * @throws {Error} If transition is invalid for the current state.
   * @private
   */
  _changeState(transitionIdOrLabel, callbackReturnValue) {
    let initialState = this.currentState;
    let newStateObj =
      typeof transitionIdOrLabel === 'number'
        ? rclnodejs.triggerLifecycleTransitionById(
            this._stateMachineHandle,
            transitionIdOrLabel
          )
        : rclnodejs.triggerLifecycleTransitionByLabel(
            this._stateMachineHandle,
            transitionIdOrLabel
          );

    if (!newStateObj) {
      throw new Error(
        `No transition available from state ${transitionIdOrLabel}.`
      );
    }

    let newState = State.fromSerializedState(newStateObj);

    let cbResult = this._executeCallback(newState, initialState);
    if (callbackReturnValue) callbackReturnValue.value = cbResult;

    let transitioningLabel = this._transitionId2Label(cbResult);
    newState = State.fromSerializedState(
      rclnodejs.triggerLifecycleTransitionByLabel(
        this._stateMachineHandle,
        transitioningLabel
      )
    );

    if (cbResult == CallbackReturnCode.ERROR) {
      cbResult = this._executeCallback(this.currentState, initialState);
      if (callbackReturnValue) callbackReturnValue.value = cbResult;

      transitioningLabel = this._transitionId2Label(cbResult);
      newState = State.fromSerializedState(
        rclnodejs.triggerLifecycleTransitionByLabel(
          this._stateMachineHandle,
          transitioningLabel
        )
      );
    }

    return newState;
  }

  /**
   * Execute the callback function registered with a transition action,
   * e.g. registerOnConfigure(cb).
   * @param {State} state - The state to which the callback is
   * @param {State} prevState - The start state of the transition.
   * @returns {CallbackReturnCode} The callback return code.
   * @private
   */
  _executeCallback(state, prevState) {
    let result = CallbackReturnCode.SUCCESS;
    let callback = this._callbackMap.get(state.id);

    if (callback) {
      try {
        result = callback(prevState);
      } catch (err) {
        console.log('CB exception occured: ', err);
        result = CallbackReturnCode.ERROR;
      }
    }

    return result;
  }

  /**
   * Find the label for the transition with id == transitionId.
   * @param {number} transitionId - A transition id.
   * @returns {string} The label of the transition with id.
   * @private
   */
  _transitionId2Label(transitionId) {
    return rclnodejs.getLifecycleTransitionIdToLabel(transitionId);
  }
}

const Lifecycle = {
  CallbackReturnCode,
  CallbackReturnValue,
  LifecycleNode,
  State,
  Transition,
  TransitionDescription,
};

module.exports = Lifecycle;