Rospy初次使用记录-定点飞行
程序员文章站
2022-11-30 14:01:36
由于接触到pytorch,所以用python完成与ROS的通信,下面例子为从程序中摘出来的一部分,用到了ROS消息的订阅与发布,服务的通信,可以作为参考使用:import rospyfrom mavros_msgs.msg import AttitudeTargetfrom geometry_msgs.msg import PoseStamped from mavros_msgs.srv import CommandBoolfrom mavros_msgs.srv import SetModes...
由于接触到pytorch,所以用python完成与ROS的通信,下面例子为从程序中摘出来的一部分,用到了ROS消息的订阅与发布,服务的通信,可以作为参考使用:
import rospy
from mavros_msgs.msg import AttitudeTarget
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode
setpoint_pos = PoseStamped()
setpoint_pos.pose.position.x = 0
setpoint_pos.pose.position.y = 0
setpoint_pos.pose.position.z = 2
local_position = PoseStamped()
locat_attitude_target = AttitudeTarget()
locat_attitude_target.thrust = 0.7
locat_attitude_target.type_mask = 0b10000111
arm_state = CommandBool()
def ros_test():
rospy.init_node('rospy_node',anonymous=True)
thrust_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=1)
state_arm_srv = rospy.ServiceProxy('/mavros/cmd/arming',CommandBool)
state_mode_srv = rospy.ServiceProxy('/mavros/set_mode',SetMode)
rospy.Subscriber('/mavros/state',State,VehicleState_callback)
setpoint_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',PoseStamped,queue_size=1)
rospy.Subscriber('/mavros/local_position/pose',PoseStamped,local_pos_cb)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
# print(vehicle_state)
# rospy.loginfo(vehicle_state.mode.)
if vehicle_state.mode != 'OFFBOARD':
state_mode_srv.call(custom_mode='OFFBOARD')
else:
# print(vehicle_state.armed)
if vehicle_state.armed == False:
state_arm_srv.call(True)
else:
# thrust_pub.publish(locat_attitude_target)
# data = rospy.wait_for_message("/mavros/local_position/pose",PoseStamped,timeout=None)
print(local_position.pose.position.z)
setpoint_pos_pub.publish(setpoint_pos)
rate.sleep()
使用方法:启用gazebo,与mavros,然后运行脚本(本脚本需要修改后使用),可以完成无人机的定点悬停,以及定油门飞行功能
本文地址:https://blog.csdn.net/qq_15390133/article/details/107144356