Primary Pipeline Calibration example

This example is very similar to the Primary Pipeline example. However, it uses a specialized consumer that will analyze the calibration data sent from the robot instead of the ShellConsumer.

Consumer setup

This example uses a specialized type of consumer that stores data about the received robot calibration.

Listing 27 examples/primary_pipeline_calibration.cpp

class CalibrationConsumer : public urcl::comm::IConsumer<urcl::primary_interface::PrimaryPackage>
{
public:
  CalibrationConsumer() : calibrated_(0), have_received_data_(false)
  {
  }
  virtual ~CalibrationConsumer() = default;

  // This will consume any primary package
  virtual bool consume(std::shared_ptr<urcl::primary_interface::PrimaryPackage> product)
  {
    // Try to cast the product to a KinematicsInfo package. If that succeeds, we handle the
    // calibration information.
    auto kin_info = std::dynamic_pointer_cast<urcl::primary_interface::KinematicsInfo>(product);
    if (kin_info != nullptr)
    {
      URCL_LOG_INFO("%s", product->toString().c_str());
      calibrated_ = kin_info->calibration_status_;
      have_received_data_ = true;
    }
    return true;
  }

  bool isCalibrated() const
  {
    const uint32_t linearized = 2;
    return calibrated_ == linearized;
  }

  bool calibrationStatusReceived()
  {
    return have_received_data_;
  }

private:
  uint32_t calibrated_;
  bool have_received_data_;
};

Since the producer is reading every package from the primary interface, the consumer has to be able to consume any primary package.

Assemble the pipeline

The rest of the pipeline setup is the same as in the other pipeline example, just that we create a CalibrationConsumer instead of a ShellConsumer:

Listing 28 examples/primary_pipeline_calibration.cpp

  // The calibration consumer will print the package contents to the shell
  CalibrationConsumer calib_consumer;

  // The notifer will be called at some points during connection setup / loss. This isn't fully
  // implemented atm.
  comm::INotifier notifier;

  // Now that we have all components, we can create and start the pipeline to run it all.
  comm::Pipeline<primary_interface::PrimaryPackage> calib_pipeline(prod, &calib_consumer, "Pipeline", notifier);
  calib_pipeline.run();

Again, product handling will be happening in the background, so in the rest of the program we wait until the calibration consumer received and processed data and then print information about that:

Listing 29 examples/primary_pipeline_calibration.cpp

  while (!calib_consumer.calibrationStatusReceived())
  {
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  if (calib_consumer.isCalibrated())
  {
    printf("The robot on IP: %s is calibrated.\n", robot_ip.c_str());
  }
  else
  {
    printf("The robot controller on IP: %s does not have a valid calibration.\n", robot_ip.c_str());
    printf("Remeber to turn on the robot to get calibration stored on the robot!\n");
  }

  return 0;