本文将实现在电脑端利用mavros将自定义消息发送至飞控端，并在飞控段将接受到的信息打印至日志输出。
针对mavros二次开发需要对相关ros包进行源码编译，具体步骤如下
- 建立工作空间
#初始化工作空间用于保存mavros和mavlink源码
mkdir -p ./mav/src
cd mav/
catkin init
sudo apt install python-wstool
sudo apt-get install python-catkin-tools python-rosinstall-generator -y
wstool init src
- 在 mav目录下下载源码
rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y
-
运行rosdep install --from-paths src --ignore-src -y 可能会提示先进行rosdep update
-
这里可能会出现网络问题，参考rosdep换源进行换源
sudo rosdep init
rosdep update
wget https://gitee.com/ncnynl/rosdep/raw/master/update_rosdep_tsinghua.sh
chmod +x ./update_rosdep_tsinghua.sh
./update_rosdep_tsinghua.sh
rosdep install --from-paths src --ignore-src -y
sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
#顺利的话这里mav文件夹下应有mavros和mavlink两个文件夹 执行编译
catkin build
- 编译完成后将以下语句添加至~/.bashrc文件中
source ~/mav/devel/setup.bash
- 另开一个终端，执行
roscd mavros
#进入对应目录则源码安装成功
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
#顺利启动则成功
至此完成mavros的源码编译
创建自定义mavros消息及对应Plugin文件
/home/yang/mav/src/mavros/mavros_msgs/msg
目录下新建yjh_mav_data.msg文件，输入以下内容
std_msgs/Header header
float32[10] f_32
int32[10] i_32
/home/yang/mav/src/mavros/mavros_msgs/CMakeLists.txt
文件中插入yjh_mav_data.msg
/home/yang/mav/src/mavros/mavros_extras/src/plugins
目录下新建yjh_data.cpp文件，输入以下内容
#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/yjh_mav_data.h>
namespace mavros {
namespace extra_plugins{
class yjhmavdataPlugin : public plugin::PluginBase {
public:
yjhmavdataPlugin() : PluginBase(),
nh("~yjh_mav_data")
{ };
void initialize(UAS &uas_)
{
PluginBase::initialize(uas_);
send_sub = nh.subscribe("send_data", 10, &yjhmavdataPlugin::yjhmavdata_cb, this);
};
Subscriptions get_subscriptions()
{
return {/* RX disabled */ };
}
private:
ros::NodeHandle nh;
ros::Subscriber send_sub;
void yjhmavdata_cb(const mavros_msgs::yjh_mav_data::ConstPtr &req)
{
mavros::UAS *m_uas_ = static_cast<yjhmavdataPlugin *>(this)->m_uas;
mavlink::common::msg:: YJH_MAV_DATA send_data = {};
send_data.f_32[0] = req->f_32[0];
send_data.f_32[1] = req->f_32[1];
send_data.f_32[2] = req->f_32[2];
send_data.f_32[3] = req->f_32[3];
send_data.f_32[4] = req->f_32[4];
send_data.f_32[5] = req->f_32[5];
send_data.f_32[6] = req->f_32[6];
send_data.f_32[7] = req->f_32[7];
send_data.f_32[8] = req->f_32[8];
send_data.f_32[9] = req->f_32[9];
send_data.i_32[0] = req->i_32[0];
send_data.i_32[1] = req->i_32[1];
send_data.i_32[2] = req->i_32[2];
send_data.i_32[3] = req->i_32[3];
send_data.i_32[4] = req->i_32[4];
send_data.i_32[5] = req->i_32[5];
send_data.i_32[6] = req->i_32[6];
send_data.i_32[7] = req->i_32[7];
send_data.i_32[8] = req->i_32[8];
send_data.i_32[9] = req->i_32[9];
std::cout << "Got data : " << req->f_32[0] << req->f_32[1] << req->f_32[2] << std::endl;
UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
}
};
} // namespace extra_plugins
} // namespace mavros
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::yjhmavdataPlugin, mavros::plugin::PluginBase)
/home/yang/mav/src/mavros/mavros_extras/CMakeLists.txt
文件中插入** src/plugins/yjh_data.cpp**
/home/yang/mav/src/mavros/mavros_extras/mavros_plugins.xml
在上述文件中插入
<class name="yjhmavdata" type="mavros::extra_plugins::yjhmavdataPlugin" base_class_type="mavros::plugin::PluginBase">
<description>defined by yjh</description>
</class>
/home/yang/mav/src/mavlink/message_definitions/v1.0/common.xml
在上述文件中插入
<message id="228" name="YJH_MAV_DATA">
<description>defined by yjh</description>
<field type="uint64_t" name="time_usec" units="us">Timestamp</field>
<field type="float[10]" name="f_32">float32_array</field>
<field type="int32_t[10]" name="i_32">float32_array</field>
</message>
返回**/mav**目录下执行
catkin build
编译
至此完成电脑端mavros相关配置
创建定义数据的节点
上一节所完成的工作是最终的发布数据工作，需要创建节点对发布的数据进行定义
在自己的工作空间的src目录下创建一个包
catkin_create_pkg yjh_msg
/home/yang/px4_ros/src/yjh_msg/src
在新建包的src中新建yjh_data_pub.cpp文件
输入以下内容
#include <time.h>
#include <ros/ros.h>
#include <mavros_msgs/yjh_mav_data.h>
using namespace std;
mavros_msgs::yjh_mav_data data;
int main(int argc, char **argv){
ros::init(argc, argv, "yjh_mav_data");
ros::NodeHandle nh;
float aa = 0;
ros::Publisher test;
test = nh.advertise<mavros_msgs::yjh_mav_data>("/mavros/yjh_mav_data/send_data", 10);
ros::Rate rate(20.0);
while(ros::ok()){
ros::spinOnce();
rate.sleep();
data.f_32[0] = aa+1;
data.f_32[1] = aa+2;
data.f_32[2] = aa+3;
data.f_32[3] = aa+1;
data.f_32[4] = aa+2;
data.f_32[5] = aa+3;
data.f_32[6] = aa+1;
data.f_32[7] = aa+2;
data.f_32[8] = aa+3;
data.f_32[9] = aa+1;
data.i_32[0] = aa+1;
data.i_32[1] = aa+2;
data.i_32[2] = aa+3;
data.i_32[3] = aa+1;
data.i_32[4] = aa+2;
data.i_32[5] = aa+3;
data.i_32[6] = aa+1;
data.i_32[7] = aa+2;
data.i_32[8] = aa+3;
data.i_32[9] = aa+1;
test.publish(data);
aa = aa + 0.01;
}
return 0;
}
这里让发布出去的数据逐渐增大，以方便面进行观察。
/home/yang/px4_ros/src/yjh_msg/CMakeLists.txt
在上述文件中插入以下内容，生成发布数据的节点
find_package(catkin REQUIRED COMPONENTS
mavros
roscpp
std_msgs
mavros_msgs
)
include_directories(include ${catkin_INCLUDE_DIRS} )
add_executable(a src/yjh_data_pub.cpp)
target_link_libraries(a ${catkin_LIBRARIES})
/home/yang/px4_ros/src/yjh_msg/package.xml
在上述文件中插入以下内容
<buildtool_depend>catkin</buildtool_depend>
<build_depend>mavros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>mavros_msgs</build_depend>
<build_export_depend>mavros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>mavros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>mavros_msgs</exec_depend>
返回工作空间根目录catkin_make进行编译
飞控端接收消息
本节将新建一个话题用于接收电脑端发送的消息并将该话题发布至日志
/home/yang/px4/test_mav/PX4-Autopilot/msg
在上述目录下新建yjh_mav_data.msg文件，并输入以下内容
uint64 timestamp # time since system start (microseconds)
float32[10] f_32
int32[10] i_32
/home/yang/px4/test_mav/PX4-Autopilot/msg/CMakeLists.txt
中插入
yjh_mav_data.msg
修改common.xml文件（重要！！！）
/home/yang/px4/test_mav/PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml
上述文件中插入与电脑端common.xml文件相同的内容
<message id="228" name="YJH_MAV_DATA">
<description>defined by yjh</description>
<field type="uint64_t" name="time_usec" units="us">Timestamp</field>
<field type="float[10]" name="f_32">float32_array</field>
<field type="int32_t[10]" name="i_32">float32_array</field>
</message>
执行
make clean
make px4_sitl_default
这里可能会报如下错误
[13/874] Generating uORB topic headers
ninja: build stopped: subcommand failed.
Makefile:232: recipe for target 'px4_sitl' failed
make: *** [px4_sitl] Error 1
按如下操作
make distclean #删除当前的common.xml文件
make px4_fmu-v3 #生成新的common.xml文件
/home/yang/px4/test_mav/PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml
上述文件中插入与电脑端common.xml文件相同的内容
<message id="228" name="YJH_MAV_DATA">
<description>defined by yjh</description>
<field type="uint64_t" name="time_usec" units="us">Timestamp</field>
<field type="float[10]" name="f_32">float32_array</field>
<field type="int32_t[10]" name="i_32">float32_array</field>
</message>
插入后重新执行编译
make px4_sitl
这时会生成
/home/yang/px4/test_mav/PX4-Autopilot/build/px4_sitl_default/mavlink/common/mavlink_msg_yjh_mav_data.h
生成上述文件后才可以进行后续操作
- 编写接收函数
/home/yang/px4/test_mav/PX4-Autopilot/src/modules/mavlink/mavlink_receiver.h
在上述文件相关位置插入以下内容
#include <uORB/topics/yjh_mav_data.h>
class MavlinkReceiver
{
...
private:
void handle_message_yjh_mav_data(mavlink_message_t *msg);
...
orb_advert_t _yjh_mav_data_pub{nullptr};
/home/yang/px4/test_mav/PX4-Autopilot/src/modules/mavlink/mavlink_receiver.cpp
在上述文件相关位置插入以下内容
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
...
case MAVLINK_MSG_ID_YJH_MAV_DATA:
handle_message_yjh_mav_data(msg);
break;
...
}
...
void MavlinkReceiver::handle_message_yjh_mav_data(mavlink_message_t *msg){
mavlink_yjh_mav_data_t man;
mavlink_msg_yjh_mav_data_decode(msg, &man);
struct yjh_mav_data_s data = {};
data.timestamp = hrt_absolute_time();
data.f_32[0] = man.f_32[0];
data.f_32[1] = man.f_32[1];
data.f_32[2] = man.f_32[2];
data.i_32[0] = man.i_32[0];
data.i_32[1] = man.i_32[1];
data.i_32[2] = man.i_32[2];
if (_yjh_mav_data_pub == nullptr) {
_yjh_mav_data_pub = orb_advertise(ORB_ID(yjh_mav_data), &data);
} else {
orb_publish(ORB_ID(yjh_mav_data), _yjh_mav_data_pub, &data);
}
}
- 将yjh_mav_data发布至日志
参考上篇博客添加并发布话题
打开地面站，打开三个终端分别执行以下指令
make px4_sitl gazebo
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
rosrun yjh_msg a
如下图
执行后可以看到通信窗口实时打印发送的消息
下载并观察日志
使用QGC地面站下载仿真过程产生的日志并使用plotjuggler进行查看
得到如下记录
说明飞控端成功将电脑端发送的实时更新的数据成功接收并发布，在其它模块对该消息进行订阅即可。
至此完成飞控端接收电脑端自定义MAVROS消息的全过程。
1046
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
