CyberRT使用方法

  • 时间:
  • 来源:互联网
  • 文章标签:

头文件

#include <cyber/cyber.h>

预定义

using adlab::cyber::Writer;
using adlab::cyber::Reader;
using adlab::cyber::Time;
using adlab::cyber::Node;

using adlab::drivers::Gps;
using adlab::drivers::Image;
using adlab::drivers::Imu;
using adlab::drivers::PCLPointCloud2Array;

// reader
using GpsReaderPtr = std::shared_ptr<Reader<Gps>>;
using ImageReaderPtr = std::shared_ptr<Reader<Image>>;
using ImuReaderPtr = std::shared_ptr<Reader<Imu>>;
using PointCloud2ArrayReaderPtr = std::shared_ptr<Reader<PCLPointCloud2Array>>;

// writer
using PointCloud2ArrayWriterPtr = std::shared_ptr<Writer<PCLPointCloud2Array>>;

类定义

class Scheduler {
 public:
  Scheduler();

  ~Scheduler();

  void initialize(const std::vector<std::string>& params);

  void run();

 private:
  void imuHandler(const std::shared_ptr<Imu>& msg);

  void gpsHandler(const std::shared_ptr<Gps>& msg);

  void cloudHandler(const std::shared_ptr<PCLPointCloud2Array>& msg);

 private:
  std::unique_ptr<Node> node_;

  GpsReaderPtr gps_reader_;

  PointCloud2ArrayReaderPtr cloud_reader_;
};

初始化

void Scheduler::initialize() {
  gps_reader_ = node_->CreateReader<Gps>("gps_topic", 5, 
      [this](const std::shared_ptr<Gps>& msg) {
        gpsHandler(msg);
      });

  cloud_reader_ = node_->CreateReader<PCLPointCloud2Array>("cloud_topic", 5,
      [this](const std::shared_ptr<PCLPointCloud2Array>& msg) {
        cloudHandler(msg);
      });

 imu_reader_ = node_->CreateReader<Imu>("imu_topic", 10,
      [this](const std::shared_ptr<Imu>& msg) {
        imuHandler(msg);
      });
}

回调函数

void Scheduler::gpsHandler(const std::shared_ptr<Gps>& msg) {
  double x = pose_msg->pos().x();
  double y = pose_msg->pos().y();
  double z = pose_msg->pos().z();
}

void Scheduler::cloudHandler(const std::shared_ptr<PCLPointCloud2Array>& msg) {
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
  adlab::common::util::fromProto<pcl::PointXYZI>(*(msg->mutable_clouds(0)), cloud->get()));
}

void Scheduler::imuHandler(const std::shared_ptr<Imu>& msg) {
  double time = static_cast<double>(msg->utime()) * 1e-6;

  // acceleration
  const auto& acc = msg->linear_accel();
  Eigen::Vector3d acceleration(acc.x(), acc.y(), acc.z());

  // angular velocity
  const auto& omega = msg->rotation_rate();
  Eigen::Vector3d angular_velocity(omega.x(), omega.y(), omega.z());
}

主函数

#include <stdlib.h>
#include <stdio.h>

DEFINE_string(configs, "configs.yaml", "");


int main(int argc, char** argv) {

  google::ParseCommandLineFlags(&argc, &argv, true);

  std::vector<std::string> arguments;
  arguments.push_back(FLAGS_configs);

  adlab::cyber::Init(argv[0]);
  google::SetLogDestination(google::INFO, "");
  ros::init(argc, argv, "node");

  Scheduler schedular;
  schedular.initialize(arguments);

  adlab::cyber::Rate rate(10);
  while (adlab::cyber::OK()) {
    schedular.run();
    rate.Sleep();
  }

  adlab::cyber::WaitForShutdown();
  return 0;
}

 

本文链接http://www.taodudu.cc/news/show-1944908.html