使用ROS节点控制PX4——位置控制

上一篇简要介绍了ROS控制PX4并使用仿真环境进行调试的框架

点击打开链接

本篇将详细介绍ROS节点的发布与订阅细节，并使用gazebo进行仿真调试。

任务：实现飞机的自动起飞，然后在两点之间循环飞行数次，最后自动降落。

1.使用RoboWare在已经建立好的工作空间中新建立一个holdheight.cpp用于写控制命令

    ps：有时RoboWare会把之前已经打开的工作空间丢失

    解决方法：进入工作空间目录，使用ls -a查看隐藏文件，将.vscode文件夹删除

rm -rf /.vscode

    然后进入RoboWare，打开已有工作空间，以我的为例，将目录设置为catkin_ws即可，不是进入catkin_ws/src/

2.代码编写

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/TwistStamped.h>
#include <string>
#include <sstream>
#include <iostream>
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, "holdheight");
    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");
    //ros::Publisher velocity_pub = nh.advertise<geometry_msgs::TwistStamped>
           // ("mavros/setpoint_velocity/cmd_vel", 10);
    //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 = 5;

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    int state = 3;
    ros::Time last_request = ros::Time::now();
    while(ros::ok() ){
       
       if( !current_state.armed ){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
            }
        if( current_state.mode != "OFFBOARD"){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.success){
                ROS_INFO("Offboard enabled");
            }
        } 

        if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;

        ros::spinOnce();
        rate.sleep();
    }
    while(state--) {
        last_request = ros::Time::now();
        while(ros::ok()) {
            if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;
           
            pose.pose.position.x = 0;
            pose.pose.position.y = 0;
            local_pos_pub.publish(pose);
            //geometry_msgs::TwistStamped velocity_tw;
            //velocity_pub.publish(velocity_tw);
            ROS_INFO("111111");
            ros::spinOnce();
            rate.sleep();
        }
        last_request = ros::Time::now();
        while(ros::ok()) {
           if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;
           
           pose.pose.position.x = 5;
           pose.pose.position.y = 5;
           local_pos_pub.publish(pose);
           //geometry_msgs::TwistStamped velocity_tw;
           //velocity_pub.publish(velocity_tw);
           ROS_INFO("222222");
           ros::spinOnce();
           rate.sleep();
       }
       ROS_INFO_STREAM("state="<<state);
    }
    offb_set_mode.request.custom_mode = "AUTO.LAND";
    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.success)
    {
        ROS_INFO("AUTO.LAND enabled");
        last_request = ros::Time::now();
    }

    return 0;
}

3.代码说明：

    首先根据节点要实现的功能订阅mavros中相应的节点，因为我要控制PX4的位置，所以要订阅发布位置消息的节点;因为要获取  飞机模式，所以要订阅获取模式的节点。。。

以位置节点为例，对订阅格式进行说明：

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);

其中的Publisher说明我要向mavros的local_position节点发布消息

根据wiki mavros中的说明将<>和（）的内容填写上即可。注意要相应的把该节点地址include一下

#include <geometry_msgs/PoseStamped.h>