相关依赖的安装参考文章 PX4的安装与基本环境的配置

  • rospackage C++ 节点

在catkin工作空间中,建立一个ROS包:

cd ~/catkin_ws/src
catkin_create_pkg offboard

我们将外部控制例程offb_node.cpp放入刚刚生成的src目录下,并且修改CMakeLists.txt,添加以下两行:

add_executable(offb_node src/offb_node.cpp)
target_link_libraries(offb_node ${catkin_LIBRARIES})

编译:

cd ~/catkin_ws
catkin_make

编译成功后,打开终端,运行仿真:

cd PX4-Autopilot
make px4_sitl gazebo 

打开新终端,运行:

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

打开新终端,运行:

cd ~/catkin_ws
source ./devel/setup.bash
rosrun offboard offb_node

飞机实现了offboard模式下的定点起飞:

offb_node.cpp:

/**
 * @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;
}
  • python节点

我们再来尝试用python节点来达到同样的效果

还是先运行仿真:

cd PX4-Autopilot
make px4_sitl gazebo 

打开新终端,运行:

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

打开新终端,直接运行编写好的python节点:

python offb_node.py

飞机就实现定点起飞了:

offb_node.py:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
import rospy
from mavros_msgs.msg import PositionTarget, State, HomePosition
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, String
import time
import math


msg = """
$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
%%%%%%%%%%%%%%%%%%%%%%%
offboard_cotrol
%%%%%%%%%%%%%%%%%%%%%%%
---------------------------
CTRL-C to quit

"""
cur_Position_Target = PositionTarget()
mavros_state = State()
armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
setModeServer = rospy.ServiceProxy('/mavros/set_mode', SetMode)
local_target_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
def __init__():
	rospy.init_node('PX4_AuotFLy' ,anonymous=True)
	rospy.Subscriber("/mavros/state", State, mavros_state_callback)
        #rospy.Subscriber("/mavros/local_position/pose",HomePosition, mavros_pos_callback)
	print("Initialized")

def mavros_state_callback(msg):
	global mavros_state
	mavros_state = msg
#	if mavros_state.armed == 0 :
#		print("disarm")

def mavros_pos_callback(msg):
        global mavros_pos
        mavros_pos = msg

def Intarget_local():
	set_target_local = PositionTarget()
	set_target_local.type_mask = 0b100111111000  
        set_target_local.coordinate_frame = 1
        set_target_local.position.x = 0
        set_target_local.position.y = 0
        set_target_local.position.z = 2
        set_target_local.yaw = 0
	return set_target_local

def run_state_update():
	if mavros_state.mode != 'OFFBOARD':
                setModeServer(custom_mode='OFFBOARD')
                local_target_pub.publish(cur_Position_Target)
        	print("wait offboard")
        else: 
        	local_target_pub.publish(cur_Position_Target)
		print("local_target_pub.publish")

global cur_Position_Target
cur_Position_Target = Intarget_local()

if __name__=="__main__":
	settings = termios.tcgetattr(sys.stdin)
	print (msg)
	__init__()
	if armServer(True) :
			print("Vehicle arming succeed!")
        if setModeServer(custom_mode='OFFBOARD'):
			print("Vehicle offboard succeed!")
	else:
			print("Vehicle offboard failed!")
	while(1):
                run_state_update()
                time.sleep(0.2)