PX4与仿真入门教程-dronedoc-offboard控制例子(python)

PX4与仿真入门教程-dronedoc-offboard控制例子(python)

说明：

• 介绍如何 编写python的ROS 节点来操作模拟中的无人机

步骤：

• 写一个节点,让无人机起飞2m以上
• 新建文件

mkdir ~/dronedoc_ws/src/px4_sim_pkg/script
cd ~/dronedoc_ws/src/px4_sim_pkg/script
vim  offboard_sample.py

• 内容如下：

#!/usr/bin/env python

import rospy

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest
from mavros_msgs.srv import SetMode, SetModeRequest

current_state = State()
def state_cb(msg):
    global current_state
    current_state = msg

def offboard_node():

    rospy.init_node("offb_node")
    r = rospy.Rate(20)

    rospy.Subscriber("mavros/state", State, state_cb)
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local",
                                     PoseStamped,
                                     queue_size=10)
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)

    while not rospy.is_shutdown() and not current_state.connected:
        r.sleep()

    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    for i in range(100):
        local_pos_pub.publish(pose)
        r.sleep()

        if rospy.is_shutdown():
            break

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_request = rospy.Time.now()

    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" \
              and (rospy.Time.now() - last_request > rospy.Duration(5)):

            try:
                offb_set_mode_resp = set_mode_client(offb_set_mode)
                if offb_set_mode_resp.mode_sent:
                    rospy.loginfo("Offboard enabled")
            except rospy.ServiceException as e:
                rospy.logwarn(e)

            last_request = rospy.Time.now()

        else:
          if not current_state.armed \
                and (rospy.Time.now() - last_request > rospy.Duration(5)):

            try:
                arm_cmd_resp = arming_client(arm_cmd)
                if arm_cmd_resp.success:
                    rospy.loginfo("Vehicle armed")
            except rospy.ServiceException as e:
                rospy.logwarn(e)

            last_request = rospy.Time.now()

        local_pos_pub.publish(pose)
        r.sleep()

if __name__ == "__main__":
    try:
        offboard_node()
    except rospy.ROSInterruptException:
        pass

• 赋予权限,编译包

chmod +x offboard_sample.py
cd ~/dronedoc_ws/
catkin_make

• 编写luanch文件
• 进入目录包目录新建文件

cd ~/dronedoc_ws/src/px4_sim_pkg/launch
vim py_offb_sample.launch

• 内容如下：

<launch>

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" />
    <node name="offb_node" pkg="px4_sim_pkg" type="offboard_sample.py" />

</launch>

测试：

• 新终端，启动模拟器和运行节点

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg py_offb_sample.launch

• 效果如下：