admin管理员组文章数量:1660206
MAVROS接收PX4飞控消息
px4飞控,使用板载控制器,实现实时读取飞控信息的功能。部分飞控信息已能够通过mavros接收并读取,例如"mavros/local_position/pose",部分飞控信息则没有通过mavros发送。本篇博客针对这部分信息进行处理,以发布悬停升力hover_thrust为例。
整个流程图如下:
一、飞控信息的发布
在飞控代码中,飞控信息通过UORB发送消息,这部分代码已经存在于Firmware中,例如:~/Firmware/src/modules/mc_hover_thrust_estimator模块中发送悬停升力的消息:
uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
...
_hover_thrust_ekf_pub.publish(status_msg);
二、MAVLink
飞控通过MAVLink模块发送接收到的UORB信息,需要在Firmware中新建自定义的MAVLink消息类型,并添加到发送流中去,具体步骤如下:
①板载控制器上源代码安装MAVLink
安装依赖项:
sudo apt-get install python-wstool python-rosinstall-generator python-catkin-tools
sudo apt-get install python3-pip
pip3 install --user future
sudo apt-get install python3-tk
源代码安装:
wstool init ~/catkin_mav/src
rosinstall_generator mavlink | tee -a /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src
rosdep install --from-paths src --ignore-src --rosdistro melodic
catkin build
②Firmware中自定义MAVLink消息
修改~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml文件,添加自定义消息:
<message id="13000" name="MY_MESSAGE_INMAVLINK">
<description>my message</description>
<field type="float" name="hover_thrust">float32_array</field>
</message>
使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件,在MAVLink的安装目录下运行:
python3 -m mavgenerate
XML:~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml
OUT:~/Firmware/mavlink/include/mavlink/v2.0
Language:C
Protocol:2.0
Validate:勾选
点击Generate按钮,在~/Firmware/mavlink/include/mavlink/v2.0/中会生成common和standard文件夹。
在~/Firmware/mavlink/include/mavlink/v2.0/中生成的common文件夹中能找到生成的.h文件:
mavlink_msg_my_message_inmavlink.h
③添加自定义mavlink消息到发送流
首先,在~/Firmware/src/modules/mavlink/mavlink_messages.cpp中添加UORB头文件
#include <uORB/topics/hover_thrust_estimate.h>
然后,在mavlink_messages.cpp中新建一个消息流的类:
class MavlinkStreamMyMessage : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamMyMessage::get_name_static();
}
static constexpr const char *get_name_static()
{
return "MY_MESSAGE";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_MY_MESSAGE_INMAVLINK;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamMyMessage(mavlink);
}
unsigned get_size() override
{
return _hover_thrust_sub.advertised() ? (MAVLINK_MSG_ID_MY_MESSAGE_INMAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _hover_thrust_sub{ORB_ID(hover_thrust_estimate)};
/* do not allow top copying this class */
MavlinkStreamMyMessage(MavlinkStreamMyMessage &) = delete;
MavlinkStreamMyMessage &operator = (const MavlinkStreamMyMessage &) = delete;
protected:
explicit MavlinkStreamMyMessage(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send(const hrt_abstime t) override
{
hover_thrust_estimate_s _hover_thrust_estimate;
if (_hover_thrust_sub.update(&_hover_thrust_estimate)) {
mavlink_my_message_inmavlink_t msg{};
msg.hover_thrust = _hover_thrust_estimate.hover_thrust;
mavlink_msg_my_message_inmavlink_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
接下来,在mavlink_messages.cpp的文件末尾,将这个消息流的类追加到treams_list
create_stream_list_item<MavlinkStreamMyMessage>()
最后,在~/Firmware/src/modules/mavlink/mavlink_messages.cpp中添加消息的发布频率:
configure_stream_local("MY_MESSAGE", 1.0f);
④make
修改完成,在Firmware目录下执行命令:
make px4_sitl_default
三、MAVROS
板载控制器通过MAVROS读取飞控发布的MAVLink消息,需要板载控制器安装MAVLink,并且新建自定义的与飞控发布的MAVLink消息类型相同的MAVLink消息类型。需要板载控制器安装MAVROS,并且新建自定义MAVROS消息用于发布。
①板载控制器上源代码安装MAVROS
源代码安装:
rosinstall_generator --upstream mavros | tee /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src
rosdep install --from-paths src --ignore-src --rosdistro melodic
catkin build
②板载控制器中自定义MAVLink消息
修改~/catkin_ws/src/mavlink/message_definitions/v1.0/common.xml文件,添加自定义消息:
<message id="13000" name="MY_MESSAGE_INMAVLINK">
<description>my message</description>
<field type="float" name="hover_thrust">float32_array</field>
</message>
使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件,在MAVLink的安装目录下运行:
python3 -m mavgenerate
XML:~/catkin_ws/src/mavlink/message_definitions/v1.0/standard.xml
OUT:~/catkin_ws/src/mavlink/message_definitions/v1.0
Language:C
Protocol:1.0
Validate:勾选
点击Generate按钮,在~/catkin_ws/src/mavlink/message_definitions/v1.0/中会生成common和standard文件夹。
在~/catkin_ws/src/mavlink/message_definitions/v1.0/中生成的common文件夹中能找到生成的.h文件:
mavlink_msg_my_message_inmavlink.h
③板载控制器中自定义MAVROS消息
在~/catkin_ws/src/mavros/mavros_msgs/msg中自定义msg文件:
MyMessage.msg
std_msgs/Header header
float32 hover_thrust
在~/catkin_ws/src/mavros/mavros_msgs/CMakeLists.txt中添加内容:
add_message_files(
MyMessage.msg
)
④板载控制器中创建MAVROS消息处理插件plugin
将接受到的MAVLink消息分配给不同的plugin进行处理。在~/catkin_ws/src/mavros/mavros_extras/src/plugins创建my_message.cpp,代码如下:
#include <mavros/mavros_plugin.h>
#include <mavros_msgs/MyMessage.h>
namespace mavros {
namespace extra_plugins {
class MyMessagePlugin:public plugin::PluginBase {
public:
MyMessagePlugin():PluginBase(),
my_message_nh("~my_message_nh")
{}
void initialize(UAS &uas_) override {
PluginBase::initialize(uas_);
my_message_pub = my_message_nh.advertise<mavros_msgs::MyMessage>("my_message", 10);
}
Subscriptions get_subscriptions() override {
return{
make_handler(&MyMessagePlugin::handle_my_message)
};
}
private:
ros::NodeHandle my_message_nh;
ros::Publisher my_message_pub;
// mavlink::common::msg::MY_MESSAGE_INMAVLINK为自动生成的消息头文件中所定义的,也是依据此来解析收到的mavlink消息。
void handle_my_message(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MY_MESSAGE_INMAVLINK &my_message) {
auto tmp = boost::make_shared<mavros_msgs::MyMessage>();
tmp->hover_thrust = my_message.hover_thrust;
my_message_pub.publish(tmp);//将解析到的消息发布至topic
}
};
}
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::MyMessagePlugin, mavros::plugin::PluginBase)
将自定义的插件添加到插件列表中,用于MAVROS自启动插件:
~/catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml中添加:
<class name="my_message" type="mavros::extra_plugins::MyMessagePlugin" base_class_type="mavros::plugin::PluginBase">
<description>Handle my messages</description>
</class>
在~/catkin_ws/src/mavros/mavros_extras/cmakelists添加编译信息
add_library(mavros_extras
src/plugins/my_message.cpp
)
⑤编译
修改完成,在工作空间catkin_ws下执行命令:
catkin build
四、测试
Firmware文件夹下终端运行:
make px4_sitl_default gazebo
工作空间catkin_ws下终端运行:
source devel/setup.bash
roslaunch mavros px4.launch
在工作空间catkin_ws下另开一个终端运行:
source devel/setup.bash
rostopic list
可以看到已有消息发布:
运行
rostopic echo /mavros/my_message_nh/my_message
即可看到发布的消息:
五、参考
PX4/Pixhawk—uORB深入理解和应用
安装 MAVLink
ubuntu 安装mavros和mavlink
实现MAVROS与px4的自定义通讯功能(一)
[pixhawk笔记]7-MAVLink添加自定义消息
版权声明:本文标题:MAVROS接收PX4飞控消息 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/dianzi/1729851026a1215411.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论