Understanding Codelets

Components are the basic building blocks of a robotics application. Isaac SDK includes various components which can use in your application. This tutorial uses an existing component to explain the various important concepts around components.

The public interface of the component DifferentialBaseOdometry, contained in the //packages/planner, directory, is shown below. This component listens to the wheel odometry from a differential base and tries to estimate the pose of the robot.

namespace isaac {
namespace navigation {

// Integrates (2D) odometry for a differential base to estimate it's
// ego motion.
class DifferentialBaseOdometry : public alice::Codelet {
 public:
  void start() override;
  void tick() override;

  // Incoming current dynamic state of the differential base which is
  // used to estimate it's ego motion in an odometry frame.
  ISAAC_PROTO_RX(DifferentialBaseStateProto, state)
  // Outgoing ego motion estimate for the differential base.
  ISAAC_PROTO_TX(Odometry2Proto, odometry)

  // Maximum acceleration to use (helps with noisy data or wrong data
  // from simulation)
  ISAAC_PARAM(double, max_acceleration, 5.0)
  // The name of the source coordinate frame under which to publish
  // the pose estimate.
  ISAAC_PARAM(std::string, odometry_frame, "odom")
  // The name of the target coordinate frame under which to publish
  // the pose estimate.
  ISAAC_PARAM(std::string, robot_frame, "robot")
  // 1 sigma of noise used for prediction model in the following order:
  //   pos_x, pos_y, heading, speed, angular_speed, acceleration
  ISAAC_PARAM(Vector6d, prediction_noise_stddev, \
      (MakeVector<double, 6>({0.05, 0.05, 0.35, 0.05, 1.00, 3.00})));
  // 1 sigma of noise used for observation model in the following order:
  //   speed, angular_speed, acceleration
  ISAAC_PARAM(Vector3d, observation_noise_stddev, \
      (Vector3d{0.25, 0.45, 2.0}));

  // This is the pose under which the ego motion estimation will be
  // written to the pose tree.
  ISAAC_POSE2(odom, robot)

 private:
  ...
};

}  // namespace navigation
}  // namespace isaac

ISAAC_ALICE_REGISTER_CODELET(isaac::navigation::DifferentialBaseOdometry);

The following sections step through DifferentialBaseOdometry, explaining each section.

Codelets and tick

class DifferentialBaseOdometry : public alice::Codelet {

Codelets are very common components which enable you to write code which is executed repeatedly. DifferentialBaseOdometry is derived from the alice::Codelet.

Codelets run in one of the three following ways:

  • Tick periodically: The tick function is executed regularly after a fixed time period. A typical example is a controller which ticks 100 times a second to send control commands to hardware.
  • Tick on message: The tick function is executed whenever a new message is received. A typical example is an image processing algorithm which computes certain information on every new camera image which is captured.
  • Tick blocking: The tick function is executed immediately again after it has finished. A typical example is a hardware driver which reads on a socket in blocking mode.

Always use blocking communication with hardware instead of threads, if possible. Isaac SDK automatically creates and manages the necessary threads.

DifferentialBaseOdometry uses a periodic tick. This is realized in the start function as shown below:

void DifferentialBaseOdometry::start() {
  ...
  tickPeriodically();
  ...
}

The tick period itself is set in the configuration as explained later.

Receiving Messages

Many components receive or transmit messages to other components. Message passing is a powerful way to encapsulate components and ensuring modularity of the codebase.

// Incoming current dynamic state of the differential base which is used to estimate its
// ego motion in an odometry frame.
ISAAC_PROTO_RX(DifferentialBaseStateProto, state)

The ISAAC_PROTO_RX macro is used to define a receiving (RX) channel. The macros take two arguments: the type of the message and the name of the channel. Isaac SDK is not particularly tied to a specific message format, but currently cap’n’proto is in widespread use. For more information see the cap’n’proto website.

A message can be read on an receiving channel for example, as follows:

const auto& rmp_reader = rx_state().getProto();
...
state_.speed() = rmp_reader.getLinearSpeed();

The function rx_state is automatically generated by the ISAAC_PROTO_RX macro, and a DifferentialBaseStateProto message is expected. The function getLinearSpeed is automatically generated from the message schema. All message schemas of the Isaac SDK can be found in the //message folder or in the corresponding section in this document.

Transmitting Messages

At the end of the tick, after all computations are done, a component often wants to send out a new message to whomever is listening.

// Outgoing ego motion estimate for the differential base.
ISAAC_PROTO_TX(Odometry2Proto, odometry)

The ISAAC_PROTO_TX macro is used to define a transmitting (TX) channel. This is very similar to the way in which ISAAC_PROTO_RX macro works.

A message can be created and sent as shown below:

auto odom_builder = tx_odometry().initProto();
ToProto(odom_T_robot, odom_builder.initOdomTRobot());
odom_builder.setSpeed(state_.speed());
...
tx_odometry().publish();

Again the tx_odometry function is automatically created by the ISAAC_PROTO_TX macro. Use initProto to start a new message on this channel. Functions automatically generated by the cap’n’proto schema like initOdomTRobot and setSpeed can be used to write data to the message proto. When the message is complete it can be send via the publish() function. Only one message can be generated at a time.

Configuration Parameters

Complicated algorithms can often be parameterized in various different ways. ISAAC_PARAM allows you to define a configuration parameter which can be set via configuration, read in the code, and changed in the frontend.

// Maximum acceleration to use (helps with noisy data or wrong data
// from simulation)
ISAAC_PARAM(double, max_acceleration, 5.0)

There are three parameters to ISAAC_PARAM:

  • type: This is the type of configuration parameter. The basic types are int, double, bool and std::string. Isaac SDK also provides support for various math types like Pose2/3, SO2/3, and Eigen vectors and matrices. An STD vector of any of those types is also supported.
  • name: The name defines the key under which the parameter is stored in the configuration file and the function name under which it can be accessed in code.
  • default value: In case no value is specified in the configuration file this value is used instead. The default can also be omitted which forces the user to specify a value in the configuration file.

In the example of DifferentialBaseOdometry the tick function starts with retrieving the desired prediction noise used in the Kalman filter:

void DifferentialBaseOdometry::tick() {
  navigation::DifferentialBaseState prediction_noise_stddev;
  prediction_noise_stddev.elements = get_prediction_noise_stddev();

Configuration can be changed in multiple ways:

  • The default configuration parameter can be changed. This should be used with caution, because it’ changes the value for all applications which have not overwritten the value in a configuration file.

  • The value can be set in a JSON configuration file. Most sample applications include a JSON file where various parameters are set. For example in //app/samples/simple_robot, a configuration parameter can be changed by adding the following JSON to the config section:

    {
      "config": {
        ...
        "segway_odometry": {
          "isaac.navigation.DifferentialBaseOdometry": {
            "max_acceleration": 2.0
          }
        }
        ...
      }
    }
    

In this example segway_odometry is the name of the node which contains a component of our type with name isaac.navigation.DifferentialBaseOdometry.

Poses

Isaac SDK automatically has a global pose tree which can be used to compute the relative poses of 3D or 2D coordinate frames. The Isaac SDK pose tree also caches time histories of poses to allow queries relative to different time points.

If a component needs to read a pose it should use the ISAAC_POSE2 or ISAAC_POSE3 macro:

// This is the pose under which the ego motion estimation will be written to the pose tree.
ISAAC_POSE2(odom, robot)

The ISAAC_POSE2/3 macro has two arguments which indicate the two coordinate frames in question. ISAAC_POSE2(lhs, rhs) will give the transformation lhs_T_rhs. This transformation can be used to transform a point in the rhs frame into a point in the lhs frame: p_lhs = lhs_T_rhs * p_rhs;.

In the case of DifferentialBaseOdometry the estimated pose of the robot relative to where it started is computed and written to the pose tree.

const Pose2d odom_T_robot{SO2d::FromAngle(state_.heading()),
                          Vector2d{state_.pos_x(), state_.pos_y()}};
set_odom_T_robot(odom_T_robot, getTickTime());

Note that the function set_odom_T_robot (and similarly get_odom_T_robot) is generated automatically when the macro is used.

The pose is read with respect to a particular point in time. In this example getTickTime is used. Querying poses at various timepoints is crucial for time synchronization of data channels to avoid lag and data mismatch.

If you want to read a pose from the pose tree a similar mechanism could be used:

const Pose2d foo_T_bar = get_foo_T_bar(getTickTime());