欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

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