pycharm开发Prometheus
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:
pass4.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 = 42 指定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 = 0position_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设置成实地大小
3
// 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("pid_gain/quad_mass" , ctrl_param.quad_mass, 1.0f);
// 【参数】悬停油门
nh.param("pid_gain/hov_percent" , ctrl_param.hov_percent, 0.5f);
// 【参数】XYZ积分上限
nh.param("pid_gain/pxy_int_max" , ctrl_param.int_max[0], 0.5);
nh.param("pid_gain/pxy_int_max" , ctrl_param.int_max[1], 0.5);
nh.param("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("control/location_source", location_source, prometheus_msgs::UAVState::GPS);
// 【参数】最大安全速度
nh.param("control/maximum_safe_vel_xy", maximum_safe_vel_xy, 4.0f);
nh.param("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
好玩