VS C++ 实现发布订阅模式(Publish/Subscribe)——基于旧版PX4 uORB模式

C++实现发布订阅模式
源码已经上传至我的 [Gitee](https://gitee.com/ascloudwalker),可用如下代码下载:
git clone https://gitee.com/ascloudwalker/c-publish--subscribe.git

- 前言

    最近接触到一些工程上的代码,使用全局变量或者函数指针的方式进行数据传递,在某些时候不是很方便:

  • 通过函数一层一层传递数据很麻烦;
  • 修改全局变量不可控;

此时,我想起了PX4的uORB(Micro Object Request Broker,微对象请求代理器)。

- 原理

    uORB本质上是一种发布订阅模式,什么是发布订阅模式?发布订阅模式:假设存在老师T(teacher),黑板B(blackboard),同学S(student).老师T每隔一个时间t1就去黑板B上发布一则通知,学生S每隔一段时间就去看(订阅)黑板上以下老师发布了什么消息。当然,这是最简单的模式,实际应用中实现还有很多附加功能,比如说:实际实现中,学生并不会先去看黑板上的内容,他可能通过某种机制先检查黑板上的内容是否更新,如果没更新就没有必要去看,如果更新了就去看看写了什么。又比如多个老师都可以在黑板上发布消息,也可以有多个同学去查看黑板发布了什么,当然也会有多块黑板。
在这里插入图片描述
    以上是一种通俗的说法,通俗的优点是好理解,缺点是显得Low。如果想要显得更加专业,请参考以下文章。

    再好的想法,只有实现了才能发挥作用,我开始查找现成的源码。

  • 首先,PX4的uORB、ROS等成熟的代码中都有该功能,优点是代码完善、功能齐全、结构完备;缺点是繁杂,不适合初学者理解,就算是看明白了也记不住;
  • 其次,网上一些单独的所谓发布订阅模式实现,与我所想的有些差别。

我决定,将uORB中最基本的架构抽离出来,使用Visual Studio C++实现。首先列出参考资源以示尊敬:

- 实现

干饭人干饭魂,干完代码干工程,我想实现最简单的功能,构建四个线程:

1.  主线程:1000ms/1HZ,打印主线程运行次数; 
2.  线程:50ms/20HZ,发布一次更新数据;
3.  线程1:100ms/10HZ,订阅数据并打印;
4.  线程2:100ms/10HZ,订阅数据并打印

    实现方法,当前的PX4版本,uORB的封装更好,代码结构更加简洁,但是不便于提取结构,本文主要参考了zarathustr /uORB提取出来的一个可移植版本,并在其基础上构建了Visual C++工程:

> step1: 构建黑板

// uORB.h
/// 这相当于定义黑板的基本结构
struct Data {
   
   
	void	*data = nullptr;
	bool	published = false;
};

extern Data *topic_data[1];
// pubsub.cpp
/// 实例化黑板,需要发布多少个主题(topic),就需要实例化多少黑板
const int total_uorb_num = 1;
Data *topic_data[total_uorb_num];
// pubsub.cpp
/// 对象请求代理器初始化,主要是因为结构体指针必须初始化,可以通过断点运行查看该步骤的作用
void orbInit(void)
{
   
   
	for (int i = 0; i < total_uorb_num; ++i)
	{
   <
使用PX4C++实现无人机自主飞行圆轨,可按如下步骤进行: ### 开发环境搭建 要确保PX4开发环境已经正确安装和配置,这里主要涉及到PX4固件的编译环境以及MAVROS功能包的安装。MAVROS是ROS(机器人操作系统)与PX4飞控之间的通信桥梁,通过它可以实现对无人机的控制。以下是一些基本的安装命令示例: ```bash # 安装PX4依赖 sudo apt-get update sudo apt-get install python3-dev python3-pip pip3 install --user toml numpy packaging jinja2 pyyaml # 克隆PX4源码 git clone https://github.com/PX4/PX4-Autopilot.git --recursive cd PX4-Autopilot # 编译PX4固件 make px4_sitl gazebo # 安装MAVROS sudo apt install ros-$ROS_DISTRO-mavros ros-$ROS_DISTRO-mavros-extras wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh chmod +x install_geographiclib_datasets.sh sudo ./install_geographiclib_datasets.sh ``` ### 编写C++代码 编写C++代码实现圆轨飞行逻辑。代码中需要创建ROS节点来与MAVROS进行通信,计算圆轨上的目标位置,并将这些目标位置发送给无人机。以下是一个简单的示例代码: ```cpp #include <ros/ros.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <geometry_msgs/PoseStamped.h> #include <cmath> mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } int main(int argc, char **argv) { ros::init(argc, argv, "circle_flight_node"); ros::NodeHandle nh; ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 2; //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; ros::Time last_request = ros::Time::now(); double radius = 5.0; // 圆轨半径 double angular_speed = 0.1; // 角速度 double center_x = 0.0; double center_y = 0.0; double start_time = ros::Time::now().toSec(); while(ros::ok()){ if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("Offboard enabled"); } last_request = ros::Time::now(); } else { if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); } last_request = ros::Time::now(); } } double current_time = ros::Time::now().toSec() - start_time; double angle = angular_speed * current_time; pose.pose.position.x = center_x + radius * cos(angle); pose.pose.position.y = center_y + radius * sin(angle); local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; } ``` ### 编译和运行代码 将上述代码保存为一个`.cpp`文件,例如`circle_flight.cpp`,并在`CMakeLists.txt`中添加相应的编译规则: ```cmake add_executable(circle_flight_node src/circle_flight.cpp) target_link_libraries(circle_flight_node ${catkin_LIBRARIES}) ``` 然后在工作空间根目录下编译代码: ```bash catkin_make source devel/setup.bash ``` 运行代码和PX4仿真: ```bash roslaunch px4 mavros_posix_sitl.launch fcu_url:="udp://:14540@127.0.0.1:14557" rosrun your_package_name circle_flight_node ``` ### 代码解释 - **ROS节点初始化**:在`main`函数中,使用`ros::init`初始化ROS节点,然后创建ROS节点句柄`nh`,用于后续的话题发布、订阅和服务调用。 - **状态订阅**:通过`ros::Subscriber`订阅`mavros/state`话题,获取无人机的当前状态,如是否连接、是否解锁、当前飞行模式等。 - **目标位置发布**:使用`ros::Publisher`发布`mavros/setpoint_position/local`话题,将计算得到的圆轨上的目标位置发送给无人机。 - **模式切换和解锁**:使用`ros::ServiceClient`调用`mavros/set_mode`和`mavros/cmd/arming`服务,将无人机切换到OFFBOARD模式并解锁。 - **圆轨计算**:在主循环中,根据当前时间计算无人机在圆轨上的位置,使用三角函数`cos`和`sin`来实现。 ### 注意事项 - 代码中的参数,如圆轨半径、角速度等,可以根据实际需求进行调整。 - 确保无人机在仿真环境或实际飞行中处于安全状态,避免发生碰撞等危险情况。 - 在实际飞行前,建议先在仿真环境中进行充分测试。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值