因为python的库很丰富,有现成的解方程库,比如sympy。
自己写了一份python版本的自动起飞2m的程序。
假设自定义的包名为offb,在offb下新建scripts文件夹,将py文件放在这个文件夹下,文件名为take_off.py。
python代码为:
#!/usr/bin/env python
import rospy
from mavros_msgs.msg import State, PositionTarget
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode
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
return set_target_local
current_state = State()
def state_cb(msg):
global current_state
current_state = msg
current_pos= PoseStamped()
def pos_cb(msg):
global current_pos
current_pos = msg
if __name__ == "__main__":
rospy.init_node("multi_uav")
rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, pos_cb,queue_size=10)
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)
rate = rospy.Rate(20.0)
Position_Target = PositionTarget()
Position_Target = Intarget_local()
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" :
setModeServer(custom_mode='OFFBOARD')
local_target_pub.publish(Position_Target)
print("Offboard enabled")
else:
if not current_state.armed:
armServer(True)
print("Vehicle armed")
else:
local_target_pub.publish(Position_Target)
print("x:%f,y:%f,z:%f" %(current_pos.pose.position.x,current_pos.pose.position.y,current_pos.pose.position.z))
rate.sleep()
同时修改Cmakelists文件
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/take_off.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
修改py文件的可执行性
chmod +x *.py
运行节点
rosrun offb take_off.py
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)