【PX4无人机仿真】构建自己的ROS节点

本篇文章主要是一个创建自己的ros节点的学习记录，在学会创建简单的节点后，参考一篇关于目标跟踪工作的博客进行了实践，使用自己写的移动小球世界进行小球的跟踪。

Demo参考：无人机跟踪小车

如何创建ROS节点

创建catkin workspace

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make

创建package

$ cd ~/catkin_ws/src
$ catkin_create_pkg [package_name] [dependency]

创建c++代码ros node

#include 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ros_tutorial_node");  //初始化节点,前面的参数固定，最后一个参数为节点名称
    ros::NodeHandle n;  //实例化一个ros节点，句柄
    ros::Rate rate(1);

    while (n.ok())
    {
        ROS_INFO("spin once");
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

---

无人机追踪运动小球Demo

无人机添加摄像机

• 在px4/Firmware/Tools/sitl_gazebo/models下的新建kinect_self模型
  在px4目录下新建文件夹kinect_self
  并在该文件夹中新建文件model.config与model.sdf
• 新建model.config文件

<model>
  <name>Kinect_selfname>
  <version>1.0version>
  <sdf version="1.5">model.sdfsdf>
  <description>
	Work in progress.

	kinetic 3D camera
  description>

model>

新建model.sdf文件

<sdf version="1.5">
  <model name="kinect_self">
    <pose>-2 2.5 1 0 0 0pose>
    <link name="link">
      <inertial>
        <mass>0.01mass>
        <inertia>
          <ixx>4.16666666667e-06ixx>
          <iyy>5.20833333333e-07iyy>
          <izz>3.85416666667e-06izz>
        inertia>
      inertial>

      <collision name="collision">
        <geometry>
          <box>
            <size>0.023000 0.076000 0.032000size>
          box>
        geometry>
      collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.023000 0.076000 0.032000size>
          box>
        geometry>
      visual>
      <sensor type="depth" name="camera">
      <always_on>truealways_on>
      <update_rate>30.0update_rate>
      <camera>
        <horizontal_fov>1.3962634horizontal_fov>　　
        <image>
          <format>B8G8R8format>
          <width>640width>
          <height>480height>
        image>
        <clip>
          <near>0.4near>
          <far>16.0far>
        clip>
      camera>
      <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2baseline>
        <alwaysOn>truealwaysOn>
        
        <updateRate>0.0updateRate>
        <cameraName>camera_ircameraName>
        <imageTopicName>/camera/color/image_rawimageTopicName>
        <cameraInfoTopicName>/camera/color/camera_infocameraInfoTopicName>
        <depthImageTopicName>/camera/depth/image_rawdepthImageTopicName>
        <depthImageCameraInfoTopicName>/camera/depth/camera_infodepthImageCameraInfoTopicName>
        <pointCloudTopicName>/camera/depth/pointspointCloudTopicName>
        <frameName>camera_linkframeName>
        <pointCloudCutoff>0.5pointCloudCutoff>
        <pointCloudCutoffMax>8.0pointCloudCutoffMax>
        <distortionK1>0distortionK1>
        <distortionK2>0distortionK2>
        <distortionK3>0distortionK3>
        <distortionT1>0distortionT1>
        <distortionT2>0distortionT2>
        <CxPrime>0CxPrime>
        <Cx>0Cx>
        <Cy>0Cy>
        <focalLength>0focalLength>
        <hackBaseline>0hackBaseline>
      plugin>
    sensor>
      <gravity>0gravity>
    link>
  model>
  
sdf>

该部分主要参考主要参考　与 px4中自带的深度相机模型r200 (在px4/Firmware/Tools/sitl_gazebo/models)
其中，修改参数
0.5 与
8.0
中的数值可以修改深度相机的工作范围

深度相机模型文件构建完毕，接下来需要添加到无人机上

• 打开iris_fpv_cam下的iris_fpv_cam.sdf

<sdf version='1.5'>
  <model name='iris_fpv_cam'>

    <include>
      <uri>model://irisuri>
    include>
    
    
    <include>
      <uri>model://kinect_selfuri>
      <pose>0.1 0 0 0 0 0pose>
    include>
    <joint name="kinect_joint" type="fixed">
      <child>kinect_self::linkchild>
      <parent>iris::base_linkparent>
      <axis>
        <xyz>0 0 0xyz>
        <limit>
          <upper>0upper>
          <lower>0lower>
        limit>
      axis>
    joint>

　　
    
    <include>
      <uri>model://fpv_cam_downuri>
      <pose>0 0 0 0 1.57 0pose>
    include>
    <joint name="fpv_cam_joint_down" type="fixed">
      <child>fpv_cam_down::linkchild>
      <parent>iris::base_linkparent>
      <axis>
        <xyz>0 0 0xyz>
        <limit>
          <upper>0upper>
          <lower>0lower>
        limit>
      axis>
    joint>

  model>
sdf>

• 修改mavros_posix_sitl.launch文件

<arg name="my_model" default="iris_fpv_cam"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>

    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>

• 验证

$ roslaunch px4 mavros_posix_sitl.launch

合并对象到一个launch文件当中

把launch文件中关于node的描述部分复制粘贴过来即可

    <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
    <arg name="x_pos" default="1.0"/>
    <arg name="y_pos" default="1.0"/>
    <arg name="z_pos" default="0.0"/>
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_burger.urdf.xacro" />


<node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

控制无人机的ros node示例

/*
头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行
*/
#include 
#include 
#include 
#include 
#include 


/*
current_state表示的是无人机的状态，在主函数中订阅了对应的话题，这个状态就会不断更新，表示无人机当前的状态。state_cb就是对应的回调函数，会不断的执行，更新状态。现在先不用管为什么需要这个状态，后面的代码会解释。
*/
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, "offb_node");
    ros::NodeHandle nh;
    
	// 订阅无人机的状态
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    
    /* 
    发布一个geometry_msgs::PoseStamped的消息，需要知道的是，这个消息是控制无人机的一种方式，将指定坐标包裹进这个消息，然后发布出去，无人机就能自动飞行到指定的坐标地点
    */ 
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    
    /*
    无人机有一个锁，如果不解锁，无人机虽然接受了命令但是不会动被锁住了，只有解锁了才能对无人机进行控制，下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息，若没解锁就需要解锁，否则就不用，其用途在这就体现出来
    */
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    
    /*
    无人机飞行有很多种模式，如果需要用代码操控无人机，我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式，若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。
    */
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    /*
    在OFFBOARD模式下，需要以>=2HZ的频率向无人机发送消息，否则无人机会回退到OFFBOARD模式之前所在的模式，因此这里的rate需要设置的比2大就行
    */
    ros::Rate rate(20.0);
	
    // wait for FCU connection
    /*
    等待无人机与控制站连接（代码的方式就是代理），只有连接了才能发送消息
    */
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    /*
    pose就是坐标，本实例是让无人机在2m处悬空，因此z设置为2，z表示的就是高度
    */
    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();
    }

    // 请求的切换模式的消息，设置为OFFBOARD
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁的消息，arm表示解锁，设置为true，disarm是上锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上次请求的时间
    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        // 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换，这里的5s是为了演示清楚设置的延时
        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 {
            // 如果当前未解锁且与请求时间大于5s，就发送请求解锁
            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();
            }
        }
		
        // 不断发送位置消息，但是只有解锁后才能真正开始运动，如果不发送就会退出OFFBOARD模式，因为请求发送速度要>=2HZ
        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

#include 
#include //发布的消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped
#include //CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool
#include //SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode
#include //订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State
#include
using namespace std;
//建立一个订阅消息体类型的变量，用于存储订阅的信息
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, "simple_uav_node");//初始化节点,前面的参数固定，最后一个参数为节点名称
    ros::NodeHandle nh;//创建句柄
    //订阅。<>里面为模板参数，传入的是订阅的消息体类型，
    //()里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    //发布geometry目标点,   
    //发布之前需要公告，并获取句柄，发布的消息体的类型为：geometry_msgs::PoseStamped
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    //启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    //启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    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);//设置循环频率20Hz～0.02s

    // wait for FCU connection//等待FCU连接
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();//设置暂停0.02s
    }
    //先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 2;
    pose.pose.position.y = 2;
    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();//设置暂停0.02s
    }
    //建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        //首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call，
        //然后服务端回应response将模式返回，这就打开了offboard模式
        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 {
            //else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call
            //然后服务端回应response成功解锁，这就解锁了
            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();
            }
        }
        co ut<<current_state<<endl;
        local_pos_pub.publish(pose);//发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
        
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

无人机追踪节点生成和运行步骤

1、新建一个package，包含的依赖有roscpp、std_msgs、geometry_msgs、mavros、cv_bridge、image_transport、sensor_msgs

其中后面几个是为了后续图像处理用的，这里一并导入

2、创建一个cpp文件, ros node文件

3、修改CMakeLists

4、catkin build

5、roslaunch启动gazebo后rosrun该节点

无人机追踪ros node

// 一些全局变量

// 悬空高度（追踪小车的高度）
const double h = 4;
// 调整高度的速度（上升或下降）
const double hv = 0.1;

// 控制无人机的速度
geometry_msgs::Twist velocity;

// 无人机当前的高度
double curH;

// 无人机是否已经稳定在空中的标志
bool start = false;

void doImg(const sensor_msgs::Image::ConstPtr &msg) {
    
    if(!start) return;
    
    // 将无人机发布的图像先转化为灰度图，再进行二值化，就能得到黑白图像，若小车出现，那么在图像内有黑色的像素，否则图像全是白色像素，这也是我将小车改成黑色的原因，若改成其它颜色就不好进行分离
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	cv::Mat img = cv_ptr -> image;
    cv::Mat gray, bin;
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    cv::threshold(gray, bin, 127, 255, cv::THRESH_BINARY);
    
    // 获得图像的宽和高
    static int row = bin.rows, col = bin.cols;
    // 图像中心点的位置，我们假设图像中心点的位置就是无人机的位置，这样就能很方便的发布速度来控制无人机
    static double centX = row / 2, centY = col / 2;
    
    // x y用来记录小车在该帧图像出现的位置
    int x, y;
    // 是否找到小车的标记
    bool findCar = false;
    
    // 遍历图像，若图像内有黑色像素则代表发现了小车，记录下此时的x y位置
    for(int i = 0; i < row; i++) {
        for(int j = 0; j < col; j++) {
            uchar point = bin.at<uchar>(i, j);
            if(point == 0) {
                findCar = true;
                x = i, y = j;
                break;
            }
        }
        if(findCar) break;
    }
    
    // 记录最后一次找到小车的时间
    static ros::Time last_find_time = ros::Time::now();
    if(findCar) {
        ROS_INFO("找到目标位置, x = %d, y = %d", x, y);
        // 将小车（所在像素点）相对无人机（图像中心像素点）的位置归一化到0 ~ 1之间，并以此作为控制无人机的速度，小车离无人机越远，则无人机的速度越大，否则无人机的速度越小
        double vx = abs(centX - x) / centX;
        double vy = abs(centY - y) / centY;
        
        // 经测试，无人机发送的图像的垂直方向是无人机的x方向，图像的水平方向是无人机的y方向
        // 因此，若小车（像素位置）在无人机（像素位置）上方，需要发送一个正的x方向速度，否则要发送一个负方向的速度
        if(x < centX) velocity.linear.x = vx;
        else velocity.linear.x = -vx;
        
		// y方向同理
        if(y < centY) velocity.linear.y = vy;
        else velocity.linear.y = -vy;

        // 若不给无人机发送z方向的速度，无人机会时上时下，因此通过下面这个代码控制无人机高度，若低于一定高度，就发布z方向的速度，若高于某个高度，就发送一个-z方向的速度，让无人机下降
        if(curH < h - 0.5) velocity.linear.z = hv;
        else if(curH < h + 0.5) velocity.linear.z = 0;
        else velocity.linear.z = (curH - h) * -hv;
        ROS_INFO("发布速度 x : %f, y : %f, z : %f", velocity.linear.x, velocity.linear.y, velocity.linear.z);
        // 记录无人机最后一次发现小车的时间，后面有用
        last_find_time = ros::Time::now();
    } else {
        ros::Time now = ros::Time::now();
        velocity.linear.x = 0;
        velocity.linear.y = 0;
        // 无人机丢失目标五秒内，什么都不操作
        if(now - last_find_time < ros::Duration(5)) {
            ROS_INFO("没有找到目标...");
        } else {
            // 无人机丢失目标五秒后，开始向上飞行（扩大视野）来搜寻小车，搜寻的最高高度是无人机跟踪小车高度的两倍，这也是前面代码中控制无人机下降的原因，若无人机在升空过程中发现目标小车，会立刻下降跟踪小车
            if(curH < 2 * h - 1) {
                ROS_INFO("上升高度寻找，当前高度为：%.2f", curH);
                velocity.linear.z = hv;
            } else {
                if(curH > 2 * h + 1) velocity.linear.z = -hv;
                else velocity.linear.z = 0;
                ROS_INFO("目标丢失。。。");
            }
        }
    }
}


void do_H(const mavros_msgs::Altitude::ConstPtr& msg) {
    curH = msg->local;
}

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, "offb_node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");

    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::Publisher local_vec_pub = nh.advertise<geometry_msgs::Twist>
            ("/mavros/setpoint_velocity/cmd_vel_unstamped", 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");
    ros::Subscriber img_sub = nh.subscribe<sensor_msgs::Image>("/camera/rgb/image_raw", 10, doImg);  
    //订阅摄像头的消息，前面这个字符串是摄像头的名字，从rqt_image_view可以查到，要修改

    ros::Subscriber height_sub = nh.subscribe<mavros_msgs::Altitude>
            ("/mavros/altitude", 10, do_H);

    //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 = h;

    velocity.linear.x = 0;
    velocity.linear.y = 0;
    velocity.linear.z = 0;

    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();

    bool takeoff = false;

    while(ros::ok()){
        if(!takeoff) {
            if( current_state.mode != "OFFBOARD" &&
                (ros::Time::now() - last_request > ros::Duration(2.0))){
                if( set_mode_client.call(offb_set_mode) &&
                    offb_set_mode.response.mode_sent){
                    ROS_INFO("Offboard enabled");
                }
                last_request = ros::Time::now();
            }

            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(2.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

            if( current_state.armed && 
                (ros::Time::now() - last_request > ros::Duration(5.0))) {
                    takeoff = true;
                    ROS_INFO("Vehicle stabled");
                    start = true;
                    ROS_INFO("开始追踪...");
                    last_request = ros::Time::now();
                }

            local_pos_pub.publish(pose);

        } else {
            local_vec_pub.publish(velocity);
        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

Demo效果

Demo

---

一些问题解决方法

rqt-image-view打不开： python3安装了过高版本的pyqt5和pyqt5-sim

编译node时需要opencv: 修改CMakeList.txt, 添加库的路径

无人机没有跟踪： 可能是没有修改订阅图像时摄像头的名字

修改摄像头俯仰角: 修改pos倒数第二个参数： 其中 1.57 对应是下视，(1.57/3.14)* 180 = 90 度 参考文章