PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。

PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。

 也就是这个页面

摘自:https://dev.px4.cc/master/en/ros/mavros_offboard.html

MAVROS Offboard control example

This tutorial shows the basics of Offboard control with MAVROS, using an Iris quadcopter simulated in Gazebo/SITL. At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters.

Offboard control is dangerous. If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.

 

This example uses C++. Similar examples in Python can be found here: integrationtests/python_src/px4_it/mavros.

Code

Create the offb_node.cpp file in your ROS package (by also adding it to your CMakeList.txt so it is compiled), and paste the following inside it:

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

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

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

        local_pos_pub.publish(pose);

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

    return 0;
}

Code explanation

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

The mavros_msgs package contains all of the custom messages required to operate services and topics provided by the MAVROS package. All services and topics as well as their corresponding message types are documented in the mavros wiki.

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

We create a simple callback which will save the current state of the autopilot. This will allow us to check connection, arming and Offboard flags.

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

We instantiate a publisher to publish the commanded local position and the appropriate clients to request arming and mode change. Note that for your own system, the "mavros" prefix might be different as it will depend on the name given to the node in it's launch file.

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

PX4 has a timeout of 500ms between two Offboard commands. If this timeout is exceeded, the commander will fall back to the last mode the vehicle was in before entering Offboard mode. This is why the publishing rate must be faster than 2 Hz to also account for possible latencies. This is also the same reason why it is recommended to enter Offboard mode from Position mode, this way if the vehicle drops out of Offboard mode it will stop in its tracks and hover.

// wait for FCU connection
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}

Before publishing anything, we wait for the connection to be established between MAVROS and the autopilot. This loop should exit as soon as a heartbeat message is received.

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

Even though the PX4 Pro Flight Stack operates in the aerospace NED coordinate frame, MAVROS translates these coordinates to the standard ENU frame and vice-versa. This is why we set z to positive 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();
}

Before entering Offboard mode, you must have already started streaming setpoints. Otherwise the mode switch will be rejected. Here, 100 was chosen as an arbitrary amount.

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

We set the custom mode to OFFBOARD. A list of supported modes is available for reference.

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

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

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

        local_pos_pub.publish(pose);

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

The rest of the code is pretty self explanatory. We attempt to switch to Offboard mode, after which we arm the quad to allow it to fly. We space out the service calls by 5 seconds so to not flood the autopilot with the requests. In the same loop, we continue sending the requested pose at the appropriate rate.

This code has been simplified to the bare minimum for illustration purposes. In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints.

好的,你想了解如何使用mavros控制px4。首先,mavros是一个ROS包,它提供了一系列的ROS节点,用于与PX4飞控进行通信和控制。PX4飞控是一款开源的飞控硬件和软件,它运行在多种无人机平台上,并提供了丰富的功能和接口。 要使用mavros控制px4,你需要先安装ROS和mavros包。安装过程可以参考ROS和mavros的官方文档。安装完成后,你需要启动mavros节点,并配置与PX4飞控的连接。 在启动mavros节点之前,你需要先启动PX4飞控,并确保它与你的计算机通过串口或无线通信连接。然后,在终端中输入以下命令启动mavros节点: ``` roslaunch mavros px4.launch ``` 该命令将启动mavros节点,并与PX4飞控建立连接。 接下来,你可以使用ROS的消息和服务接口来控制PX4飞控。例如,你可以使用mavros的setpoint_raw/local或setpoint_position/local服务来设置无人机的目标姿态或位置。你可以通过发布相应的消息来调整目标姿态或位置。 例如,以下是一段使用mavros控制无人机起飞和降落的Python代码: ```python import rospy from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool, SetMode from geometry_msgs.msg import PoseStamped from std_msgs.msg import Bool # 初始化节点 rospy.init_node('drone_control') # 订阅mavros状态信息 state = State() def state_cb(msg): global state state = msg rospy.Subscriber('mavros/state', State, state_cb) # 发布起飞和降落命令 arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) def set_mode(mode): rospy.wait_for_service('mavros/set_mode') try: res = set_mode_client(custom_mode=mode) return res.mode_sent except rospy.ServiceException as e: print("Service call failed: %s" %e) def arming(value): rospy.wait_for_service('mavros/cmd/arming') try: res = arming_client(value) return res.success except rospy.ServiceException as e: print("Service call failed: %s" %e) # 发布目标位置信息 pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) pose = PoseStamped() # 起飞 def takeoff(): pose.pose.position.z = 2 # 设置目标高度 for i in range(100): pose_pub.publish(pose) # 发布目标位置 rospy.sleep(0.1) set_mode('OFFBOARD') # 设置OFFBOARD模式 arming(True) # 解锁 rospy.sleep(1) # 降落 def land(): pose.pose.position.z = 0 # 设置目标高度 for i in range(100): pose_pub.publish(pose) # 发布目标位置 rospy.sleep(0.1) arming(False) # 上锁 rospy.sleep(1) # 程序入口 if __name__ == '__main__': rate = rospy.Rate(10) while not rospy.is_shutdown(): if state.connected: takeoff() # 起飞 rospy.sleep(10) # 飞行10秒 land() # 降落 break rate.sleep() ``` 这段代码演示了如何使用mavros控制无人机起飞和降落,你可以根据自己的需要修改代码
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值