pycharm开发Prometheus

Q_L_Y

已于 2023-04-22 00:34:29 修改

阅读量395

点赞数 1

文章标签： prometheus

于 2023-04-13 20:52:37 首次发布

本文链接：https://blog.csdn.net/q_l_y/article/details/129968267

版权

1.先打开一个文件夹作为项目的根目录

2 引入Prometheus的包

如果最后一个import报错，那么打开Settings里面的Python Interpreter

点击Python 3.6旁边的倒三角，点击“Show All”

点击左侧栏的第四个图标

把最后一个路径加进去。然后点击OK

下一个框要点Apply之后再点OK

然后就发现不报错啦

以上步骤是为了添加python包，也就是site-packages或者dist-packages的文件夹。这里添加了Prometheus的python包，让解释器能依赖这些包去读懂代码。

prometheus_msgs消息类型所在的位置是工作空间根目录（Promethues）下的devel中的include下的prometheus_msgs文件夹

控制模块

1 预备知识

PX4飞控中使用的都是NED坐标系

Mavros和Prometheus_control里都是用ENU坐标系

学习takeoff_land.sh

#!/bin/bash
# 脚本名称: takeoff_land
# 脚本描述: 该脚本为起飞&降落控制demo启动脚本,包含PX4 SITL,Gazebo仿真环境,无人机控制节点以及起飞&降落控制节点

gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e 'bash -c "sleep 5; roslaunch prometheus_gazebo sitl_outdoor_1uav.launch; exec bash"' \
--tab -e 'bash -c "sleep 6; roslaunch prometheus_uav_control uav_control_main_outdoor.launch; exec bash"' \
--tab -e 'bash -c "sleep 7; rosrun prometheus_demo takeoff_land.py; exec bash"' \
#--tab -e 'bash -c "sleep 14; roslaunch prometheus_demo takeoff_land.launch; exec bash"' \

注意这一段代码执行了四个命令，应该开四个终端，这四个命令应该连在一起，中间不能有注释，否则注释后面的代码会执行不了。

第四行执行的是.py文件，第五行执行的是.cpp文件的节点

学习takeoff_land.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制
# 效果说明: 无人机起飞后悬停30秒,然后降落
# 备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用
from math import fabs
from turtle import position
import ros
import rospy
from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState

# 创建无人机相关数据变量
uav_control_state_sv = UAVControlState()
uav_command_pv = UAVCommand()
uav_state_sv = UAVState()

# 无人机状态回调函数
def uavStateCb(msg):
    global uav_state_sv
    uav_state_sv = msg

# 无人机控制状态回调函数
def uavControlStateCb(msg):
    global uav_control_state_sv
    uav_control_state_sv = msg

def main():
    # ROS初始化,设定节点名
    rospy.init_node('takeoff_land_py',anonymous=True)
    # 创建命令发布标志位,命令发布则为true;初始化为false
    cmd_pub_flag = False
    # 创建无人机控制命令发布者
    UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10)
    # 创建无人机控制状态命令订阅者
    rospy.Subscriber("/uav1/prometheus/control_state", UAVControlState, uavControlStateCb)
    # 创建无人机状态命令订阅者
    rospy.Subscriber("/uav1/prometheus/state", UAVState, uavStateCb)
    # 循环频率设置为10HZ
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 检测无人机是否处于[COMMAND_CONTROL]模式
        if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL:
            # 检测控制命令是否发布,没有发布则进行命令的发布
            if not cmd_pub_flag:
                # 时间戳
                uav_command_pv.header.stamp = rospy.Time.now()
                # 坐标系
                uav_command_pv.header.frame_id = 'ENU'  #这个frame_id在UAVCommand.h和UAVCommand.msg都没有找到定义，所以是坐标轴的一个id
                # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度
                #uav_control_indoor.yaml文件里的上锁高度应该就是悬停高度  该高度是0.1
                uav_command_pv.Agent_CMD = 1
                # 发布的命令ID,每发一次,该ID加1   为啥要加一，在UAVCommand.msg里面最后一行有？
                uav_command_pv.Command_ID = 1## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
                # 发布起飞命令
                UavCommandPb.publish(uav_command_pv)
                rate.sleep()
                # 命令发布标志位置为true
                cmd_pub_flag = True
                # 打印无人机起飞相关信息
                rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height'))   get_param的用法？/uav_control_main_1/control/Takeoff_height是一个全局变量
                    获取的是起飞时的高度
            else:
                # 当无人机距离高度目标值±0.1米范围内时认为起飞完成并等待30秒后降落
                if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1:  如果现在的高度与起飞时高度相差0.1米以内就认为起飞成功
                    print(" UAV takeoff successfully and landed after 30 seconds")
                    rospy.sleep(30)  应该是等待30秒以后执行下一步
                    # 时间戳
                    uav_command_pv.header.stamp = rospy.Time.now()
                    # 坐标系
                    uav_command_pv.header.frame_id = "ENU"  为啥又要设置一次
                    # Land降落,从当前位置降落至地面并自动上锁
                    uav_command_pv.Agent_CMD = 3
                    # 发布的命令ID加1
                    uav_command_pv.Command_ID += 1
                    # 发布降落命令
                    UavCommandPb.publish(uav_command_pv)
                    # 打印降落相关信息
                    rospy.loginfo("UAV Land")
                    rospy.loginfo("[takeoff & land tutorial_demo completed]")
                    # 任务结束,关闭该节点
                    rospy.signal_shutdown("shutdown time")
                else:
                    # 打印当前无人机高度信息
                    rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2])
                    rospy.sleep(1)
        else:
            # 在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止
            if cmd_pub_flag:
                rospy.logfatal(" Unknown error! [takeoff & land] tutorial_demo aborted")
            # 命令未发布,等待无人机进入[COMMAND_CONTROL]状态
            else:
                rospy.logwarn(" Wait for UAV to enter [COMMAND_CONTROL] MODE ")
                rospy.sleep(2)
        rate.sleep()
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

4.2.1takeoff_land.py代码理解

该代码在/home/qinluyi/Prometheus/Modules/tutorial_demo/basic/takeoff_land/scripts目录下

takeoff_land.py代码用到的消息类型

！！要看一下消息类型的msg文件，在/home/qinluyi/Prometheus/Modules/common/prometheus_msgs/msg文件夹下。

from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState

消息类型：UAVCommand, UAVControlState，UAVState

UAVControlState uav_control_state_sv

UAVControlState.COMMAND_CONTROL uav_control_state_sv.control_state

# 检测无人机是否处于[COMMAND_CONTROL]模式
        if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL:

UAVCommand 用于发布对无人机的指令的消息类型 uav_command_pv

UAVCommand消息类型对应的话题是/uav1/prometheus/command

# 创建无人机控制命令发布者
    UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10)

                # 时间戳  
                uav_command_pv.header.stamp = rospy.Time.now()
                # 坐标系  
                uav_command_pv.header.frame_id = 'ENU'
                # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 上锁高度Disarm_height : 0.1
                uav_command_pv.Agent_CMD = 1 # home点上方悬停，应该是在0.1的高度停
                                             # Agent_CMD 枚举
                                             #uint8 Init_Pos_Hover=1  # home点上方悬停
               # 发布的命令ID,每发一次,该ID加1
                uav_command_pv.Command_ID = 1
               # 发布起飞命令
                UavCommandPb.publish(uav_command_pv)
 
Q：为啥找不到stamp和frame_id的定义
在UAVCommand.msg中第一行：std_msg/Header header 包括了这个消息，通过std_msg调用的
Q：为啥发布的命令ID,每发一次,该ID加1 
防止接收到错误命令， 编号应该逐次递加。每publish一次，编号增加一次。先增加编号，再发布命令，否则命令不会发布。


# 当无人机距离高度目标值±0.1米范围内时认为起飞完成并等待30秒后降落
                if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1:
                    print(" UAV takeoff successfully and landed after 30 seconds")
                    rospy.sleep(30)
                    # 时间戳
                    uav_command_pv.header.stamp = rospy.Time.now()
                    # 坐标系
                    uav_command_pv.header.frame_id = "ENU"
                    # Land降落,从当前位置降落至地面并自动上锁
                    uav_command_pv.Agent_CMD = 3  #uint8 Land=3
                    # 发布的命令ID加1
                    uav_command_pv.Command_ID += 1
                    # 发布降落命令
                    UavCommandPb.publish(uav_command_pv)
                    # 打印降落相关信息
                    rospy.loginfo("UAV Land")
                    rospy.loginfo("[takeoff & land tutorial_demo completed]")
                    # 任务结束,关闭该节点
                    rospy.signal_shutdown("shutdown time")

UAVState uav_state_sv

# 当无人机距离高度目标值±0.1米范围内时认为起飞完成并等待30秒后降落
#当无人机距离目标高度Takeoff_height距离在0.1米内时，认定为起飞完成
                if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1:

Q：uav_state_sv.position[2]应该是当前高度，也就是Z坐标值

# 打印当前无人机高度信息
                    rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2])

用到的rospy函数

rospy.ROSInterruptException

操作中断的错误异常，经常在 rospy.sleep() and rospy.Rate 中用到

rospy.logfatal

rospy.logwarn

rospy.Rate(10)

设置循环频率频率为10次每秒

rospy.sleep()

rospy.get_param（）

rospy.get_param('/uav_control_main_1/control/Takeoff_height')
# 打印无人机起飞相关信息
rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height'))

Takeoff_height是设置的目标高度，认为当无人机当前位置和目标位置相差0.1m以内时认为起飞完成。这里的目标高度是2。

rospy.Time.now()

rosparam list 查看param

enu_xyz_pos_control.py代码学习

消息类型：UAVCommand

变量名：uav_command_pv

让无人机移动到指定的点

1首先设置Agent_CMD是4，也就是Move移动模式，这样无人机就可以移动到指定位置

注释的语句和下面的语句是一个意思

# Move模式
                    #uav_command_pv.Agent_CMD = UAVCommand.Move
                    uav_command_pv.Agent_CMD = 4

2 指定Move_mode为惯性系定点控制（具体见msg文件中的定义）

# Move_mode
                    #uav_command_pv.Move_mode = UAVCommand.XYZ_POS
                    uav_command_pv.Move_mode = 0
## 移动命令下的子模式枚举
#uint8 XYZ_POS = 0               ### 惯性系定点控制

3 指定要去的点坐标

 # ENU坐标系下的X轴正半轴对应东方,Y轴正半轴对应北方,因此下面的控制数据将会控制无人机在5米的高度移动(3,4,5)
                    uav_command_pv.position_ref[0] = 3
                    uav_command_pv.position_ref[1] = 4
                    uav_command_pv.position_ref[2] = 5
                    uav_command_pv.yaw_ref = 0

position_ref的0 1 2对应坐标系下的XYZ坐标

4 然后发布命令就好啦

# 发布的命令ID加1
                    uav_command_pv.Command_ID += 1
                    # 发布命令
                    UavCommandPb.publish(uav_command_pv)

body_xyz_pos_control.py代码学习

body_xyz_pos_control就是以机体自身为原点的坐标系。

控制模块C++代码

/home/qinluyi/Prometheus/Modules/uav_control/include下有几个.h头文件

uav_controller.h 对应 uav_controller.cpp

uav_controller.cpp 对应在

/home/qinluyi/Prometheus/Modules/uav_control/src目录下

头文件在include文件夹下，对应的cpp文件在src目录下。

1 检测到

2 超出地理围栏，原地降落。

实际飞行的时候把uav_geo_fence设置成实地大小

// 1, 无人机有稳定准确的定位,由uav_state_cb()函数获得
// 2, 无人机知道自己要去哪，即期望位置pos_des等
// HOVER_CONTROL和LAND_CONTROL的指令信息由程序根据当前状态计算得到，COMMAND_CONTROL的指令信息由uav_cmd_cb()函数获得

4 发送期望位置至飞控

/发送位置期望值至飞控（输入: 期望xyz,期望yaw）
void UAV_controller::send_pos_setpoint(const Eigen::Vector3d &pos_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;
    // Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
    // Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    // Bit 10 should set to 0, means is not force sp
    // pos_setpoint.type_mask = 0b100111111000; // 100 111 111 000  xyz + yaw
    pos_setpoint.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
                             mavros_msgs::PositionTarget::IGNORE_VY |
                             mavros_msgs::PositionTarget::IGNORE_VZ |
                             mavros_msgs::PositionTarget::IGNORE_AFX |
                             mavros_msgs::PositionTarget::IGNORE_AFY |
                             mavros_msgs::PositionTarget::IGNORE_AFZ |
                             mavros_msgs::PositionTarget::IGNORE_AFY |
                             mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

    pos_setpoint.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    pos_setpoint.position.x = pos_sp[0];
    pos_setpoint.position.y = pos_sp[1];
    pos_setpoint.position.z = pos_sp[2];
    pos_setpoint.yaw = yaw_sp;
    px4_setpoint_raw_local_pub.publish(pos_setpoint);
}

坐标系旋转函数：机体系到enu系

// body_frame是机体系,enu_frame是惯性系，yaw_angle是当前偏航角[rad]
void UAV_controller::rotation_yaw(double yaw_angle, float body_frame[2], float enu_frame[2])
{
    enu_frame[0] = body_frame[0] * cos(yaw_angle) - body_frame[1] * sin(yaw_angle);
    enu_frame[1] = body_frame[0] * sin(yaw_angle) + body_frame[1] * cos(yaw_angle);
}

/home/qinluyi/Prometheus/Modules/uav_control/include/Position_Controller/pos_controller_PID.h 文件有定悬停油门值，真机飞行的时候从小往大调整

  // 【参数】无人机质量
    nh.param<float>("pid_gain/quad_mass" , ctrl_param.quad_mass, 1.0f);
    // 【参数】悬停油门
    nh.param<float>("pid_gain/hov_percent" , ctrl_param.hov_percent, 0.5f);
    // 【参数】XYZ积分上限
    nh.param<float>("pid_gain/pxy_int_max"  , ctrl_param.int_max[0], 0.5);
    nh.param<float>("pid_gain/pxy_int_max"  , ctrl_param.int_max[1], 0.5);
    nh.param<float>("pid_gain/pz_int_max"   , ctrl_param.int_max[2], 0.5);

Idle就是怠速模式，飞机机翼会旋转但是不会起飞，等待命令进入。

/home/qinluyi/Prometheus/Modules/uav_control/src/uav_control_node.cpp

是控制节点的cpp文件

/home/qinluyi/Prometheus/Modules/uav_control/src/uav_estimator.cpp

有有关位置控制的代码，我觉得把t265作为位置源的话应该要修改文件中的这个位置？

这里应该默认位置源是GPS，然后自己飞的时候要改无人机定位源的话要改这里。

定位源的定义在

/home/qinluyi/Prometheus/Modules/common/prometheus_msgs/msg/UAVState.msg

实际使用的应该是这个.h文件，也有同样的定义。

/home/qinluyi/Prometheus/devel/include/prometheus_msgs/UAVState.h

 // 【参数】定位源, 定义见UAVState.msg
    nh.param<int>("control/location_source", location_source, prometheus_msgs::UAVState::GPS);
    // 【参数】最大安全速度
    nh.param<float>("control/maximum_safe_vel_xy", maximum_safe_vel_xy, 4.0f);
    nh.param<float>("control/maximum_safe_vel_z", maximum_safe_vel_z, 3.0f);

当需要使用外部定位设备（比如t265）时，需要定时发送vision信息至飞控,并保证一定频率。

 if (location_source == prometheus_msgs::UAVState::MOCAP || location_source == prometheus_msgs::UAVState::T265 || location_source == prometheus_msgs::UAVState::GAZEBO || location_source == prometheus_msgs::UAVState::UWB)
    {
        // 【定时器】当需要使用外部定位设备时，需要定时发送vision信息至飞控,并保证一定频率
        timer_px4_vision_pub = nh.createTimer(ros::Duration(0.02), &UAV_estimator::timercb_pub_vision_pose, this);
    }

terminal_control.cpp 文件夹里找不到哇意思就是用终端可以控制无人机的飞行

ground_station.cpp 就是起到地面站的作用，把飞机的状态不断地print出去。

大概在43min第三章第三讲，就是如果更改定位源，先手动把无人机拿前一米，然后看导航信息是不是对的。

要看1 Drone State位置速度对不对，还要看2 Control Command看坐标系对不对，还有偏仰角对不对，在控制输出器 PID算出来的期待姿态角和期待加速度是多少

3 Target Into FCU 看飞控执行命令，就是飞控真正执行的速度加速度要对比2中的PID对不对，

很重要很重要！

_command_to_mavros是什么东西，没找到

看一下launch文件是啥，然后加载的yaml文件是啥

roslaunch prometheus_gazebo sitl_indoor_1uav.launch

好玩

Q_L_Y

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
1
评论
pycharm开发Prometheus

1.先打开一个文件夹作为项目的根目录2 引入Prometheus的包如果最后一个import报错，那么打开Settings里面的Python Interpreter点击Python 3.6旁边的倒三角，点击“Show All”点击左侧栏的第四个图标把最后一个路径加进去。然后点击OK下一个框要点Apply之后再点OK然后就发现不报错啦以上步骤是为了添加python包，也就是site-packages或者dist-packages的文件夹。这里添加了
复制链接

扫一扫