基于PX4与ROS Melodic的无人机低速绕Z轴旋转Python实现咨询

1 对C++代码的理解验证 #

你的理解完全正确。这段C++代码的核心逻辑就是保持x、y、z高度不变，每次在当前位姿基础上叠加一个绕Z轴的小角度旋转，再输出新的位姿控制指令，控制周期固定为0.5秒，因此旋转速度非常稳定。

2 现有Python代码问题分析 #

你写的代码存在三个核心问题：

• 旋转触发时机完全依赖上游"Searching"消息的发布频率，没有固定控制周期，导致旋转速度忽快忽慢
• 每次直接重置x/y值为0，没有和当前无人机实际位姿同步，容易出现位置漂移
• 若yaw增量0.1rad的触发频率为10Hz，角速度可达1rad/s（约57度/秒），速度确实过快
• 旋转逻辑和消息回调绑定，添加延时会阻塞回调线程，自然会影响物体检测的响应速度

3 优化后的Python实现方案 #

3.1 实现思路 #

• 新增全局状态位控制是否进入搜索旋转模式，收到"Searching"时置为True，检测到物体时置为False
• 用ROS Timer单独实现固定周期的控制逻辑（和C++一致采用0.5秒周期），仅在搜索模式激活时执行旋转逻辑
• 旋转角度增量和C++对齐，每次转π/10（0.314rad），控制周期0.5s的话角速度为0.628rad/s（约36度/秒），可灵活调整参数降低转速
• 异步控制逻辑不会阻塞订阅回调，检测到物体后可立刻停止旋转，不存在延时问题

3.2 代码示例 #

#!/usr/bin/env python3
import rospy
import math
from geometry_msgs.msg import PoseStamped, String
from tf.transformations import quaternion_from_euler, euler_from_quaternion

class SearchRotator:
    def __init__(self):
        self.searching = False
        self.current_pose = PoseStamped()
        # 初始化位姿，可根据实际需求修改高度、初始坐标
        self.current_pose.pose.position.x = 0.0
        self.current_pose.pose.position.y = 0.0
        self.current_pose.pose.position.z = 1.6
        self.current_pose.pose.orientation.w = 1.0
        
        # 订阅搜索触发消息，替换为实际的话题名
        self.search_sub = rospy.Subscriber("/search_trigger", String, self.search_cb)
        # 订阅物体检测结果话题，替换为实际的检测话题名
        self.detect_sub = rospy.Subscriber("/target_detect_result", String, self.detect_cb)
        # 位姿指令发布者，替换为实际的PX4位姿控制话题名
        self.pose_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)
        
        # 固定0.5s执行一次控制逻辑，和C++逻辑完全对齐
        rospy.Timer(rospy.Duration(0.5), self.control_loop)
    
    def search_cb(self, msg):
        if msg.data == "Searching" and not self.searching:
            self.searching = True
            rospy.loginfo("Searching target...")
    
    def detect_cb(self, msg):
        # 检测到物体后立刻停止旋转，可根据实际检测消息的判断逻辑修改
        if msg.data == "detected":
            self.searching = False
    
    def control_loop(self, event):
        # 非搜索状态保持当前位姿输出，避免无人机漂移
        if not self.searching:
            self.pose_pub.publish(self.current_pose)
            return
        
        # 提取当前yaw角
        orientation_q = self.current_pose.pose.orientation
        r, p, yaw = euler_from_quaternion([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])
        # 叠加旋转角度，要更慢就调小这个值，比如改为math.pi/20对应18度/秒的角速度
        yaw += math.pi / 10
        # 限制yaw在[-pi, pi]范围内避免数值溢出
        yaw = (yaw + math.pi) % (2 * math.pi) - math.pi
        
        # 生成新的四元数，x/y/z位置保持不变
        quat = quaternion_from_euler(r, p, yaw)
        self.current_pose.pose.orientation.x = quat[0]
        self.current_pose.pose.orientation.y = quat[1]
        self.current_pose.pose.orientation.z = quat[2]
        self.current_pose.pose.orientation.w = quat[3]
        # 发布控制指令
        self.pose_pub.publish(self.current_pose)

if __name__ == "__main__":
    rospy.init_node("search_rotator_node")
    rotator = SearchRotator()
    rospy.spin()

4 额外优化建议 #

• 如果需要更精准的位置保持，可以额外订阅无人机的实际位姿话题（比如/mavros/local_position/pose），每次更新current_pose的x/y/z值为实际飞行值，避免累积漂移
• 控制周期可以根据PX4的控制帧率调整，建议不要低于10Hz（也就是0.1s），避免位置控制失稳
• 注意使用前需要先解锁无人机、切换到OFFBOARD控制模式，符合PX4的常规操作流程

内容的提问来源于stack exchange，提问作者Diego