ROS 在工作空间中创建python程序

ROS 在工作空间中创建python程序

基于ros，在工作空间catkin_ws中创建pkg和python程序，并进行编译使其可以用rosrun进行运行，参考：参考

1. 默认前面已经创建了catkin_ws的工作空间并使用catkin_make进行了编译。
2. 创建pkg

catkin_create_pkg uav_ctl std_msgs rospy roscpp

3. 创建python程序所在的文件夹位置，由于python程序属于脚本，因此在刚才的uav_ctl文件夹下，创建scripts文件夹

mkdir scripts
cd scripts

4. 创建python程序进行编辑。官网代码链接官网代码，以及python的更多offboard程序参考：python offboard

touch test.py                #生成文件
chmod +x test.py             #设置可执行
rosed beginner_tutorials test.py #自己输入代码，可直接用nano test.py代替

# python程序风格参考
#!/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")

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)

5. 可直接用python3运行，控制无人机起飞，ctl+c进行停止。
6. 在catkin_ws使用catkin_make之后，可以使用rosrun进行运行

rosrun uav_ctl test.py