ROS2的cpp程序编写步骤

总体要求

既然ROS2用的c++14的编译环境, 那么就要有c++14的样子, 不能像写ROS一样随心所欲,变量满天飞.

1. 不能有全局变量
2. main程序需要非常简洁
3. 只能有一个大类供ROS2调用, 其他类的调用为同命名空间下的其他类实现.

1. main 程序

main程序需要非常简洁

1. ROS2 初始化

rclcpp::init(argc, argv);

2. GLOG初始化

  google::AllowCommandLineReparsing();
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, false);

3. 实例化大类, 所有的运行和变量必须在这个大类中实现, 不可存在外部函数与全局变量

auto myclass = std::make_shared<MyClass>("my_class", config);

4. 开启死循环

rclcpp::spin(myclass);

5. 结束

rclcpp::shutdown();

2. 大类实现

注意事项:

1. 大类一定要公共继承自rclcpp::Node以实现订阅与发布.

class MyClass : public rclcpp::Node {
...
}

2. 变量都要设置为私有(安全性), 方法尽量设置为公有
3. 消息都要设置为SharedPtr(节约内存)

实现:

1. 构造函数: 配置参数的读取与所有订阅, 发布的初始化, 定时器, 还有主处理函数等等

MyClass(std::string name)
        : Node(name) {
        public:
        MyClass_sub_ =
            this->create_subscription<geometry_msgs::msg::PoseStamped>(
                "pose", 16,
                std::bind(&MyClass::HandlePose, this,
                          std::placeholders::_1));
        ...
        tracked_pose_pub_ =
            this->create_publisher<geometry_msgs::msg::PoseStamped>(
                "localization_pose", 10);
		...
		pub_timer_.start(1000, [this]() { PublishMap(); });
		...
		sync_thread_ = std::thread{std::mem_fn(&MyClass::sync_process),this};
};

2. 析构函数: 重置需要重置的变量, 结束主线程

        ~MyClass()
        {
        	some_flag_ = false;
            if(sync_thread_.joinable())
            {
                process_stop_ = true;
                sync_thread_.join();
            }
        }

4. 回调函数: 订阅话题的回调函数, 传入的msg都必须设置为const

void HandlePose(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) 
{
	...
}

5. 其他函数实现: 包括算法, 辅助功能set get等等, 没啥特殊要求

    Eigen::Quaterniond setRPY(double roll, double pitch, double yaw) {
        double halfYaw = yaw * 0.5;
        double halfPitch = pitch * 0.5;
        double halfRoll = roll * 0.5;

        double cosYaw = cos(halfYaw);
        double sinYaw = sin(halfYaw);
        double cosPitch = cos(halfPitch);
        double sinPitch = sin(halfPitch);
        double cosRoll = cos(halfRoll);
        double sinRoll = sin(halfRoll);

        Eigen::Quaterniond quaterniond;
        quaterniond.x() =
            sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
        quaterniond.y() =
            cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
        quaterniond.z() =
            cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
        quaterniond.w() =
            cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
        return quaterniond;
    }

6. 变量部分: 尽量全部设为私有, 这样类内可以拿到, 对外部又保密, 安全性高. 不要设置初始值, 初始值都在构造函数中设置

private:
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
        tracked_pose_pub_;
...
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
        pose_sub_;
Eigen::Vector3d current_pose_;
double heading;

结语

以上就是写ROS2 的cpp程序的基本步骤了.