m6wf5oeyl
/
Enjoy
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

code changes cys

Browse Source
pull/16/head
cys 5 months ago
parent c29be1d4ff
commit 9b1df5d91c
2590 changed files with 17227 additions and 418054 deletions
Show Stats Download Patch File Download Diff File

1
src/UAV/AlexeyAB_darknet

@ -0,0 +1 @@
Subproject commit 0968058b546193d2d2d0ce66de1b4bed36f369d1

11
src/UAV/Modules/common/include/control_common.h
Unescape View File

@ -1,11 +0,0 @@
#ifndef CONTROL_COMMON_H
#define CONTROL_COMMON_H
#include <string>
using namespace std;
#define PX4_SENDER 0
#define PX4_POS_CONTROLLER 1
#define PX4_GEO_CONTROLLER 2
#endif

208
src/UAV/Modules/common/include/control_utils.h
Unescape View File

@ -1,208 +0,0 @@
#ifndef CONTROL_UTILS_H
#define CONTROL_UTILS_H
#include <Eigen/Eigen>
#include <math.h>
using namespace std;
// 四元数转欧拉角
Eigen::Vector3d quaternion_to_rpy2(const Eigen::Quaterniond &q)
{
// YPR - ZYX
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
// 从(roll,pitch,yaw)创建四元数 by a 3-2-1 intrinsic Tait-Bryan rotation sequence
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
{
// YPR - ZYX
return Eigen::Quaterniond(
Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX())
);
}
// 将四元数转换至(roll,pitch,yaw) by a 3-2-1 intrinsic Tait-Bryan rotation sequence
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// q0 q1 q2 q3
// w x y z
Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q)
{
float quat[4];
quat[0] = q.w();
quat[1] = q.x();
quat[2] = q.y();
quat[3] = q.z();
Eigen::Vector3d ans;
ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]));
ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1]));
ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]));
return ans;
}
//旋转矩阵转欧拉角
void rotation_to_euler(const Eigen::Matrix3d& dcm, Eigen::Vector3d& euler_angle)
{
double phi_val = atan2(dcm(2, 1), dcm(2, 2));
double theta_val = asin(-dcm(2, 0));
double psi_val = atan2(dcm(1, 0), dcm(0, 0));
double pi = M_PI;
if (fabs(theta_val - pi / 2.0) < 1.0e-3) {
phi_val = 0.0;
psi_val = atan2(dcm(1, 2), dcm(0, 2));
} else if (fabs(theta_val + pi / 2.0) < 1.0e-3) {
phi_val = 0.0;
psi_val = atan2(-dcm(1, 2), -dcm(0, 2));
}
euler_angle(0) = phi_val;
euler_angle(1) = theta_val;
euler_angle(2) = psi_val;
}
Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R)
{
Eigen::Vector4d quat;
double tr = R.trace();
if (tr > 0.0) {
double S = sqrt(tr + 1.0) * 2.0; // S=4*qw
quat(0) = 0.25 * S;
quat(1) = (R(2, 1) - R(1, 2)) / S;
quat(2) = (R(0, 2) - R(2, 0)) / S;
quat(3) = (R(1, 0) - R(0, 1)) / S;
} else if ((R(0, 0) > R(1, 1)) & (R(0, 0) > R(2, 2))) {
double S = sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2.0; // S=4*qx
quat(0) = (R(2, 1) - R(1, 2)) / S;
quat(1) = 0.25 * S;
quat(2) = (R(0, 1) + R(1, 0)) / S;
quat(3) = (R(0, 2) + R(2, 0)) / S;
} else if (R(1, 1) > R(2, 2)) {
double S = sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)) * 2.0; // S=4*qy
quat(0) = (R(0, 2) - R(2, 0)) / S;
quat(1) = (R(0, 1) + R(1, 0)) / S;
quat(2) = 0.25 * S;
quat(3) = (R(1, 2) + R(2, 1)) / S;
} else {
double S = sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)) * 2.0; // S=4*qz
quat(0) = (R(1, 0) - R(0, 1)) / S;
quat(1) = (R(0, 2) + R(2, 0)) / S;
quat(2) = (R(1, 2) + R(2, 1)) / S;
quat(3) = 0.25 * S;
}
return quat;
}
static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m)
{
Eigen::Vector3d v;
// TODO: Sanity checks if m is skew symmetric
v << m(7), m(2), m(3);
return v;
}
Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p)
{
Eigen::Vector4d quat;
quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2),
p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
return quat;
}
Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q)
{
Eigen::Matrix3d rotmat;
rotmat << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3),
2 * q(0) * q(2) + 2 * q(1) * q(3),
2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3),
2 * q(2) * q(3) - 2 * q(0) * q(1),
2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3),
q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);
return rotmat;
}
Eigen::Vector4d acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw)
{
Eigen::Vector4d quat;
Eigen::Vector3d zb_des, yb_des, xb_des, proj_xb_des;
Eigen::Matrix3d rotmat;
proj_xb_des << std::cos(yaw), std::sin(yaw), 0.0;
zb_des = vector_acc / vector_acc.norm();
yb_des = zb_des.cross(proj_xb_des) / (zb_des.cross(proj_xb_des)).norm();
xb_des = yb_des.cross(zb_des) / (yb_des.cross(zb_des)).norm();
rotmat << xb_des(0), yb_des(0), zb_des(0), xb_des(1), yb_des(1), zb_des(1), xb_des(2), yb_des(2), zb_des(2);
quat = rot2Quaternion(rotmat);
return quat;
}
//constrain_function
float constrain_function(float data, float Max)
{
if(abs(data)>Max)
{
return (data > 0) ? Max : -Max;
}
else
{
return data;
}
}
//constrain_function2
float constrain_function2(float data, float Min,float Max)
{
if(data > Max)
{
return Max;
}
else if (data < Min)
{
return Min;
}else
{
return data;
}
}
//sign_function
float sign_function(float data)
{
if(data>0)
{
return 1.0;
}
else if(data<0)
{
return -1.0;
}
else if(data == 0)
{
return 0.0;
}
}
// min function
float min(float data1,float data2)
{
if(data1>=data2)
{
return data2;
}
else
{
return data1;
}
}
#endif

44
src/UAV/Modules/common/include/message_utils.h
Unescape View File

@ -1,44 +0,0 @@
/***************************************************************************************************************************
* message_utils.h
*
* Author: Jario
*
* Update Time: 2020.4.29
***************************************************************************************************************************/
#ifndef PROMETHEUS_MESSAGE_UTILS_H
#define PROMETHEUS_MESSAGE_UTILS_H
#include <string>
#include <prometheus_msgs/Message.h>
using namespace std;
// 消息发布函数
inline void pub_message(ros::Publisher& puber, int msg_type, std::string source_node, std::string msg_content)
{
prometheus_msgs::Message exect_msg;
exect_msg.header.stamp = ros::Time::now();
exect_msg.message_type = msg_type;
exect_msg.source_node = source_node;
exect_msg.content = msg_content;
puber.publish(exect_msg);
}
// 消息打印函数
inline void printf_message(const prometheus_msgs::Message& message)
{
//cout <<">>>>>>>>>>>>>>>>>>>>>>>> Message <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(message.message_type == prometheus_msgs::Message::NORMAL)
{
cout << "[NORMAL]" << "["<< message.source_node << "]:" << message.content <<endl;
}else if(message.message_type == prometheus_msgs::Message::WARN)
{
cout << "[WARN]" << "["<< message.source_node << "]:" <<message.content <<endl;
}else if(message.message_type == prometheus_msgs::Message::ERROR)
{
cout << "[ERROR]" << "["<< message.source_node << "]:" << message.content <<endl;
}
}
#endif

132
src/UAV/Modules/common/include/printf_utils.h
Unescape View File

@ -1,132 +0,0 @@
#ifndef PRINTF_UTILS_H
#define PRINTF_UTILS_H
#include <string>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <ctime>
using namespace std;
#define NUM_POINT 2 // 打印小数点
#define RED "\033[0;1;31m"
#define GREEN "\033[0;1;32m"
#define YELLOW "\033[0;1;33m"
#define BLUE "\033[0;1;34m"
#define PURPLE "\033[0;1;35m"
#define DEEPGREEN "\033[0;1;36m"
#define WHITE "\033[0;1;37m"
#define RED_IN_WHITE "\033[0;47;31m"
#define GREEN_IN_WHITE "\033[0;47;32m"
#define YELLOW_IN_WHITE "\033[0;47;33m"
#define TAIL "\033[0m"
class Print
{
public:
Print(float interval = 0, std::string color = TAIL)
: interval(interval), past_ts(std::chrono::system_clock::now()), color(color)
{
//固定的浮点显示
std::cout.setf(ios::fixed);
// setprecision(n) 设显示小数精度为n位
std::cout << std::setprecision(2);
//左对齐
std::cout.setf(ios::left);
// 强制显示小数点
std::cout.setf(ios::showpoint);
// 强制显示符号
std::cout.setf(ios::showpos);
};
template <typename T>
void operator()(T &&msg)
{
std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now();
auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(now_ts - past_ts).count();
if (this != s_object_name)
{
std::cout << std::endl;
s_object_name = this;
}
if (interval >= 0)
{
// std::cout << this->interval << std::endl;
if (dt < this->interval * 1000)
return;
std::cout << color << msg << TAIL << std::endl;
}
else
{
if (dt < 0.1 * 1000)
return;
char now_char;
switch (times)
{
case 0:
now_char = '\\';
break;
case 1:
now_char = '|';
break;
case 2:
now_char = '/';
break;
case 3:
now_char = '-';
break;
}
times = ++times % 4;
std::cout << color << "\r " << now_char << " " << msg << TAIL << std::endl;
}
this->past_ts = now_ts;
};
float interval;
private:
std::chrono::system_clock::time_point past_ts;
std::string color;
static void *s_object_name;
unsigned int times = 0;
};
void *Print::s_object_name = nullptr;
#define PRINTF_UTILS_CONCAT_(x, y) x##y
#define PRINTF_UTILS_CONCAT(x, y) PRINTF_UTILS_CONCAT_(x, y)
#define PRINTF_UTILS_PCOUT_(interval, color, msg, sign) \
static Print PRINTF_UTILS_CONCAT(print, sign)(interval, color); \
PRINTF_UTILS_CONCAT(print, sign) \
(msg)
#define PCOUT(interval, color, msg) PRINTF_UTILS_PCOUT_(interval, color, msg, __LINE__)
// Example:
// cout << GREEN << "Test for Green text." << TAIL <<endl;
// 字背景颜色范围:40--49 字颜色: 30--39
// 40:黑 30: 黑
// 41:红 31: 红
// 42:绿 32: 绿
// 43:黄 33: 黄
// 44:蓝 34: 蓝
// 45:紫 35: 紫
// 46:深绿 36: 深绿
// 47:白色 37: 白色
// 参考资料：https://blog.csdn.net/u014470361/article/details/81512330
// cout << "\033[0;1;31m" << "Hello World, Red color!" << "\033[0m" << endl;
// printf( "\033[0;30;41m color!!! \033[0m Hello \n");
// ROS_INFO("\033[1;33;41m----> Hightlight color.\033[0m");
// 其中41的位置代表字的背景色, 30的位置是代表字的颜色，0 是字的一些特殊属性，0代表默认关闭，一些其他属性如闪烁、下划线等。
// ROS_INFO_STREAM_ONCE ("\033[1;32m---->Setting to OFFBOARD Mode....\033[0m");//绿色只打印一次
#endif

88
src/UAV/Modules/common/msgs/CMakeLists.txt
Unescape View File

@ -1,88 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(prometheus_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
geometry_msgs
actionlib_msgs
sensor_msgs
nav_msgs
std_msgs
std_srvs
tf2_ros
tf2_eigen
mavros_msgs
serial
)
find_package(Boost REQUIRED COMPONENTS system)
add_message_files(
DIRECTORY msg
FILES
DetectionInfo.msg
MultiDetectionInfo.msg
BoundingBox.msg
BoundingBoxes.msg
ControlCommand.msg
PositionReference.msg
AttitudeReference.msg
DroneState.msg
ControlOutput.msg
Message.msg
LogMessage.msg
LogMessageControl.msg
LogMessagePlanning.msg
LogMessageDetection.msg
SwarmCommand.msg
Formation.msg
ArucoInfo.msg
MultiArucoInfo.msg
IndoorSearch.msg
UgvState.msg
UgvCommand.msg
GimbalTrackError.msg
Cloud_platform.msg
gimbal.msg
GimbalCtl.msg
)
add_action_files(
DIRECTORY action
FILES
CheckForObjects.action
)
generate_messages(
DEPENDENCIES
actionlib_msgs
geometry_msgs
sensor_msgs
std_msgs
)
catkin_package(
CATKIN_DEPENDS
message_runtime
actionlib_msgs
geometry_msgs
sensor_msgs
message_runtime
std_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)

13
src/UAV/Modules/common/msgs/action/CheckForObjects.action
Unescape View File

@ -1,13 +0,0 @@
# Check if objects in image
# Goal definition
int16 id
sensor_msgs/Image image
---
# Result definition
int16 id
prometheus_msgs/BoundingBoxes bounding_boxes
---
# Feedback definition

17
src/UAV/Modules/common/msgs/msg/ArucoInfo.msg
Unescape View File

@ -1,17 +0,0 @@
std_msgs/Header header
## aruco编码
int32 aruco_num
## 是否检测到目标
bool detected
## 目标位置-相机坐标系-(x,y,z)
## 从相机往前看，物体在相机右方x为正，下方y为正，前方z为正
float32[3] position
## 目标姿态-四元数-(qx,qy,qz,qw)
float32[4] orientation
## 视线角度-相机系下-(右方x角度为正,下方y角度为正)
float32[2] sight_angle

7
src/UAV/Modules/common/msgs/msg/AttitudeReference.msg
Unescape View File

@ -1,7 +0,0 @@
std_msgs/Header header
## 位置控制器输出： 期望油门 + 期望角度
float32[3] throttle_sp ## [0-1] 惯性系下的油门量
float32 desired_throttle ## [0-1] 机体系z轴
float32[3] desired_attitude ## [rad]
geometry_msgs/Quaternion desired_att_q ## 四元数

7
src/UAV/Modules/common/msgs/msg/BoundingBox.msg
Unescape View File

@ -1,7 +0,0 @@
string Class
float64 probability
int64 xmin
int64 ymin
int64 xmax
int64 ymax

3
src/UAV/Modules/common/msgs/msg/BoundingBoxes.msg
Unescape View File

@ -1,3 +0,0 @@
Header header
Header image_header
BoundingBox[] bounding_boxes

4
src/UAV/Modules/common/msgs/msg/Cloud_platform.msg
Unescape View File

@ -1,4 +0,0 @@
std_msgs/Header header
uint8[59] received_data
uint8[5] get_data_com
uint8[20] gimbal_control # 控制

23
src/UAV/Modules/common/msgs/msg/ControlCommand.msg
Unescape View File

@ -1,23 +0,0 @@
std_msgs/Header header
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
uint32 Command_ID
## 消息来源
string source
## 控制命令的模式
uint8 Mode
# enum Mode 控制模式枚举
uint8 Idle=0
uint8 Takeoff=1
uint8 Hold=2
uint8 Land=3
uint8 Move=4
uint8 Disarm=5
uint8 User_Mode1=6
uint8 User_Mode2=7
## 控制参考量
## 位置参考量：位置、速度、加速度、加加速度、加加加速度
## 角度参考量：偏航角、偏航角速度、偏航角加速度
PositionReference Reference_State

11
src/UAV/Modules/common/msgs/msg/ControlOutput.msg
Unescape View File

@ -1,11 +0,0 @@
std_msgs/Header header
## 位置控制器输出： 期望推力及期望油门
float32[3] Thrust ## [rad]
float32[3] Throttle
## NE控制器的中间变量
float32[3] u_l ## [0-1] 惯性系下的油门量
float32[3] u_d ## [rad]
float32[3] NE ## [rad]

32
src/UAV/Modules/common/msgs/msg/DetectionInfo.msg
Unescape View File

@ -1,32 +0,0 @@
std_msgs/Header header
## 目标类别名称
string object_name
## 是否检测到目标
bool detected
## 0表示相机坐标系, 1表示机体坐标系, 2表示惯性坐标系
int32 frame
## 目标位置[相机系下：右方x为正，下方y为正，前方z为正]
float32[3] position
## 目标姿态-欧拉角-(z,y,x)
float32[3] attitude
## 目标姿态-四元数-(qx,qy,qz,qw)
float32[4] attitude_q
## 视线角度[相机系下：右方x角度为正，下方y角度为正]
float32[2] sight_angle
## 像素位置[相机系下：右方x为正，下方y为正]
int32[2] pixel_position
## 偏航角误差
float32 yaw_error
## 类别
int32 category

22
src/UAV/Modules/common/msgs/msg/DroneState.msg
Unescape View File

@ -1,22 +0,0 @@
std_msgs/Header header
## 机载电脑是否连接上飞控，true已连接，false则不是
bool connected
## 是否解锁，true为已解锁，false则不是
bool armed
## 是否降落，true为已降落，false则代表在空中
bool landed
## PX4飞控当前飞行模式
string mode
bool odom_valid
## 系统启动时间
float32 time_from_start ## [s]
## 无人机状态量：位置、速度、姿态
float32[3] position ## [m]
float32 rel_alt ## [m] only for outdoor
float32[3] velocity ## [m/s]
float32[3] attitude ## [rad]
geometry_msgs/Quaternion attitude_q ## 四元数
float32[3] attitude_rate ## [rad/s]

6
src/UAV/Modules/common/msgs/msg/Formation.msg
Unescape View File

@ -1,6 +0,0 @@
uint8 HORIZONTAL=0
uint8 TRIANGEL=1
uint8 DIAMOND=2
uint8 DIAMOND_STAGE_1=3
uint8 type

9
src/UAV/Modules/common/msgs/msg/GimbalCtl.msg
Unescape View File

@ -1,9 +0,0 @@
float32 pitch
float32 yaw
float32 zoom
float32 focus
uint16 home
uint16 TakePicture
uint16 cameraModeChange
uint16 yawfollow
uint16 quadhome

12
src/UAV/Modules/common/msgs/msg/GimbalTrackError.msg
Unescape View File

@ -1,12 +0,0 @@
## 是否检测到目标
bool detected
# 坐标
float64 x
float64 y
# 速度
float64 velx
float64 vely
# 积分
float64 Ix
float64 Iy

12
src/UAV/Modules/common/msgs/msg/IndoorSearch.msg
Unescape View File

@ -1,12 +0,0 @@
std_msgs/Header header
ArucoInfo Aruco1
ArucoInfo Aruco2
ArucoInfo Aruco3
ArucoInfo Aruco4
ArucoInfo Aruco5
ArucoInfo Aruco6
ArucoInfo Aruco7
ArucoInfo Aruco8
ArucoInfo Aruco9

17
src/UAV/Modules/common/msgs/msg/LogMessage.msg
Unescape View File

@ -1,17 +0,0 @@
std_msgs/Header header
## 用于log的自定义消息，可自行增加需要记录的话题
float32 time ## [s]
DroneState Drone_State
ControlCommand Control_Command
ControlOutput Control_Output
AttitudeReference Attitude_Reference

20
src/UAV/Modules/common/msgs/msg/LogMessageControl.msg
Unescape View File

@ -1,20 +0,0 @@
std_msgs/Header header
## 用于控制模块的log消息，可自行增加需要记录的话题
float32 time ## [s]
## 0代表px4_sender,1代表px4_pos_controller(姿态控制)
int32 control_type
DroneState Drone_State
ControlCommand Control_Command
ControlOutput Control_Output
AttitudeReference Attitude_Reference
geometry_msgs/PoseStamped ref_pose

17
src/UAV/Modules/common/msgs/msg/LogMessageDetection.msg
Unescape View File

@ -1,17 +0,0 @@
std_msgs/Header header
## 用于控制模块的log消息，可自行增加需要记录的话题
float32 time ## [s]
DroneState Drone_State
ControlCommand Control_Command
ControlOutput Control_Output
AttitudeReference Attitude_Reference

17
src/UAV/Modules/common/msgs/msg/LogMessagePlanning.msg
Unescape View File

@ -1,17 +0,0 @@
std_msgs/Header header
## 用于控制模块的log消息，可自行增加需要记录的话题
float32 time ## [s]
DroneState Drone_State
ControlCommand Control_Command
ControlOutput Control_Output
AttitudeReference Attitude_Reference

12
src/UAV/Modules/common/msgs/msg/Message.msg
Unescape View File

@ -1,12 +0,0 @@
std_msgs/Header header
## 节点回传地面站的消息
uint8 message_type
# enum message_type
uint8 NORMAL = 0
uint8 WARN = 1
uint8 ERROR = 2
string source_node
string content

7
src/UAV/Modules/common/msgs/msg/MultiArucoInfo.msg
Unescape View File

@ -1,7 +0,0 @@
std_msgs/Header header
## 检测到的aruco码数量
int32 num_arucos
## 每个aruco码的检测结果
ArucoInfo[] aruco_infos

10
src/UAV/Modules/common/msgs/msg/MultiDetectionInfo.msg
Unescape View File

@ -1,10 +0,0 @@
Header header
## 检测到的目标数量
int32 num_objs
## Detecting or Tracking (0:detect, 1:track)
int32 detect_or_track
## 每个目标的检测结果
DetectionInfo[] detection_infos

37
src/UAV/Modules/common/msgs/msg/PositionReference.msg
Unescape View File

@ -1,37 +0,0 @@
std_msgs/Header header
## 位置控制参考量
## 默认为 XYZ位置追踪模式 （sub_mode = 0）； 速度追踪启用时，控制器不考虑位置参考量及位置状态反馈
uint8 Move_mode
uint8 XYZ_POS = 0 ##0b00
uint8 XY_POS_Z_VEL = 1 ##0b01
uint8 XY_VEL_Z_POS = 2 ##0b10
uint8 XYZ_VEL = 3 ##0b11
uint8 XYZ_ACC = 4
uint8 TRAJECTORY = 5
uint8 Move_frame
# 默认情况下均为ENU_FRAME,注意轨迹追踪和XYZ_ACC一定是ENU_FRAME
uint8 ENU_FRAME = 0
uint8 BODY_FRAME = 1
uint8 MIX_FRAME = 2 ##2020.4.6 临时增加的模式，后期需要统一整合，该模式下，xy采用机体系控制，z采用enu系控制
## 时刻： 用于轨迹追踪
float32 time_from_start ## [s]
## 参考量：位置、速度、加速度、加加速度、加加加速度
float32[3] position_ref ## [m]
float32[3] velocity_ref ## [m/s]
float32[3] acceleration_ref ## [m/s^2]
## float32[3] jerk_ref ## [m/s^3]
## float32[3] snap_ref ## [m/s^4]
## 角度参考量：偏航角、偏航角速度、偏航角加速度
bool Yaw_Rate_Mode ## True 代表控制偏航角速率
float32 yaw_ref ## [rad]
float32 yaw_rate_ref ## [rad/s]
## float32 yaw_acceleration_ref ## [rad/s]

49
src/UAV/Modules/common/msgs/msg/SwarmCommand.msg
Unescape View File

@ -1,49 +0,0 @@
std_msgs/Header header
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
uint32 Command_ID
## 消息来源
string source
## 控制命令的模式
uint8 Mode
# enum Mode 控制模式枚举
uint8 Idle=0
uint8 Takeoff=1
uint8 Hold=2
uint8 Land=3
uint8 Disarm=4
uint8 Position_Control=5
uint8 Velocity_Control=6
uint8 Accel_Control=7
uint8 Move=8
uint8 User_Mode1=9
## 控制参考量
uint8 swarm_shape
uint8 One_column=0
uint8 Triangle=1
uint8 Square=2
uint8 Circular=3
## 默认为 XYZ位置追踪模式 （sub_mode = 0）； 速度追踪启用时，控制器不考虑位置参考量及位置状态反馈
uint8 Move_mode
uint8 XYZ_POS = 0 ##0b00
uint8 XY_POS_Z_VEL = 1 ##0b01
uint8 XY_VEL_Z_POS = 2 ##0b10
uint8 XYZ_VEL = 3 ##0b11
uint8 XYZ_ACC = 4
uint8 TRAJECTORY = 5
float32 swarm_size
float32[3] position_ref ## [m]
float32[3] velocity_ref ## [m]
float32[3] acceleration_ref
float32 yaw_ref ## [rad]
float32 yaw_rate_ref

24
src/UAV/Modules/common/msgs/msg/UgvCommand.msg
Unescape View File

@ -1,24 +0,0 @@
std_msgs/Header header
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
uint32 Command_ID
## 控制命令的模式
uint8 Mode
# enum Mode 控制模式枚举
uint8 Start=0
uint8 Hold=1
uint8 Disarm=2
uint8 Point_Control=3
uint8 Direct_Control=4
uint8 ENU_Vel_Control=5
uint8 ENU_Pos_Control=6
uint8 Path_Control=7
float32[2] position_ref ## [m]
float32 yaw_ref ## [rad]
float32[2] linear_vel ## [m/s]
float32 angular_vel ## [rad/s]

16
src/UAV/Modules/common/msgs/msg/UgvState.msg
Unescape View File

@ -1,16 +0,0 @@
std_msgs/Header header
## 机载电脑是否连接上飞控，true已连接，false则不是
bool connected
## 是否解锁，true为已解锁，false则不是
bool armed
bool guided
## PX4飞控当前飞行模式
string mode
## 无人机状态量：位置、速度、姿态
float32[3] position ## [m]
float32[3] velocity ## [m/s]
float32[3] attitude ## [rad]
geometry_msgs/Quaternion attitude_q ## 四元数
float32 battery ##

15
src/UAV/Modules/common/msgs/msg/gimbal.msg
Unescape View File

@ -1,15 +0,0 @@
float64 imu0
float64 imu1
float64 imu2
float64 rc0
float64 rc1
float64 rc2
float64 rel0 # r
float64 rel1 # p
float64 rel2 # y
float64 imuvel0
float64 imuvel1
float64 imuvel2
float64 relvel0
float64 relvel1
float64 relvel2

38
src/UAV/Modules/common/msgs/package.xml
Unescape View File

@ -1,38 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>prometheus_msgs</name>
<version>0.0.0</version>
<description>The prometheus_msgs package</description>
<maintainer email="fatmoonqyp@126.com">Yuhua Qi</maintainer>
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
<license>TODO</license>
<url type="website">http://www.amovauto.com/</url>
<url type="repository">https://github.com/amov-lab/Prometheus.git</url>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<build_depend>geometry_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<build_depend>sensor_msgs</build_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>sensor_msgs</exec_depend>
<export>
</export>
</package>

230
src/UAV/Modules/mission/CMakeLists.txt
Unescape View File

@ -1,230 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(prometheus_mission)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
geometry_msgs
sensor_msgs
mavros
nav_msgs
std_msgs
std_srvs
tf2_ros
tf2_eigen
mavros_msgs
prometheus_msgs
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
################################################
## Declare ROS messages, services and actions ##
################################################
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs
sensor_msgs
std_msgs
)
catkin_package(
CATKIN_DEPENDS message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
autonomous_landing
${catkin_INCLUDE_DIRS}
../common/include
../control/include
)
###############################
## 声明可执行cpp文件 ##
###############################
add_executable(gimbal_control_circle gimbal_control/gimbal_control_circle.cpp)
add_dependencies(gimbal_control_circle prometheus_mission_gencpp)
target_link_libraries(gimbal_control_circle ${catkin_LIBRARIES})
add_executable(gimbal_control_demo gimbal_control/gimbal_control_demo.cpp)
add_dependencies(gimbal_control_demo prometheus_mission_gencpp)
target_link_libraries(gimbal_control_demo ${catkin_LIBRARIES})
add_executable(gimbal_control gimbal_control/gimbal_control.cpp)
add_dependencies(gimbal_control prometheus_mission_gencpp)
target_link_libraries(gimbal_control ${catkin_LIBRARIES})
add_executable(pad_tracking test/pad_tracking.cpp)
add_dependencies(pad_tracking prometheus_mission_gencpp)
target_link_libraries(pad_tracking ${catkin_LIBRARIES})
add_executable(mission_cmd_pub test/mission_cmd_pub.cpp)
add_dependencies(mission_cmd_pub prometheus_mission_gencpp)
target_link_libraries(mission_cmd_pub ${catkin_LIBRARIES})
add_executable(turtlebot_formation formation_flight/turtlebot_formation.cpp)
add_dependencies(turtlebot_formation prometheus_mission_gencpp)
target_link_libraries(turtlebot_formation ${catkin_LIBRARIES})
add_executable(circle_crossing circle_crossing/circle_crossing.cpp)
add_dependencies(circle_crossing prometheus_mission_gencpp)
target_link_libraries(circle_crossing ${catkin_LIBRARIES})
add_executable(number_detection number_detection/number_detection.cpp)
add_dependencies(number_detection prometheus_mission_gencpp)
target_link_libraries(number_detection ${catkin_LIBRARIES})
add_executable(turtlebot_follow formation_flight/turtlebot_follow.cpp)
add_dependencies(turtlebot_follow prometheus_mission_gencpp)
target_link_libraries(turtlebot_follow ${catkin_LIBRARIES})
add_executable(color_line_following color_line_following/color_line_following.cpp)
add_dependencies(color_line_following prometheus_mission_gencpp)
target_link_libraries(color_line_following ${catkin_LIBRARIES})
add_executable(planning_mission planning_mission/planning_mission.cpp)
add_dependencies(planning_mission prometheus_mission_gencpp)
target_link_libraries(planning_mission ${catkin_LIBRARIES})
add_executable(pub_goal planning_mission/pub_goal.cpp)
add_dependencies(pub_goal prometheus_mission_gencpp)
target_link_libraries(pub_goal ${catkin_LIBRARIES})
add_executable(pub_goal_swarm planning_mission/pub_goal_swarm.cpp)
add_dependencies(pub_goal_swarm prometheus_mission_gencpp)
target_link_libraries(pub_goal_swarm ${catkin_LIBRARIES})
add_executable(waypoint_tracking waypoint_tracking/waypoint_tracking.cpp)
add_dependencies(waypoint_tracking prometheus_mission_gencpp)
target_link_libraries(waypoint_tracking ${catkin_LIBRARIES})
add_executable(autonomous_landing autonomous_landing/autonomous_landing.cpp)
add_dependencies(autonomous_landing prometheus_mission_gencpp)
target_link_libraries(autonomous_landing ${catkin_LIBRARIES})
add_executable(autonomous_landing_aruco autonomous_landing/autonomous_landing_aruco.cpp)
add_dependencies(autonomous_landing_aruco prometheus_mission_gencpp)
target_link_libraries(autonomous_landing_aruco ${catkin_LIBRARIES})
add_executable(object_tracking object_tracking/object_tracking.cpp)
add_dependencies(object_tracking prometheus_mission_gencpp)
target_link_libraries(object_tracking ${catkin_LIBRARIES})
add_executable(indoor_competition indoor_competition/indoor_competition.cpp)
add_dependencies(indoor_competition prometheus_mission_gencpp)
target_link_libraries(indoor_competition ${catkin_LIBRARIES})
add_executable(fake_detectioninfo test/fake_detectioninfo.cpp)
add_dependencies(fake_detectioninfo prometheus_mission_gencpp)
target_link_libraries(fake_detectioninfo ${catkin_LIBRARIES})
add_executable(formation_getpose formation/formation_getpose.cpp)
add_dependencies(formation_getpose prometheus_mission_gencpp)
target_link_libraries(formation_getpose ${catkin_LIBRARIES})
add_executable(formation_state formation/formation_state.cpp)
add_dependencies(formation_state prometheus_mission_gencpp)
target_link_libraries(formation_state ${catkin_LIBRARIES})
add_executable(formation_move formation/formation_move.cpp)
add_dependencies(formation_move prometheus_mission_gencpp)
target_link_libraries(formation_move ${catkin_LIBRARIES})
add_executable(formation_change formation/formation_change.cpp)
add_dependencies(formation_change prometheus_mission_gencpp)
target_link_libraries(formation_change ${catkin_LIBRARIES})
add_executable(formation_setmode formation/formation_setmode.cpp)
add_dependencies(formation_setmode prometheus_mission_gencpp)
target_link_libraries(formation_setmode ${catkin_LIBRARIES})
add_executable(formation_control formation/formation_control.cpp)
add_dependencies(formation_control prometheus_mission_gencpp)
target_link_libraries(formation_control ${catkin_LIBRARIES})
add_executable(formation_square formation/formation_square.cpp)
add_dependencies(formation_square prometheus_mission_gencpp)
target_link_libraries(formation_square ${catkin_LIBRARIES})
add_executable(pub_goal_from_qgc planning_mission/pub_goal_from_qgc.cpp)
add_dependencies(pub_goal_from_qgc prometheus_mission_gencpp)
target_link_libraries(pub_goal_from_qgc ${catkin_LIBRARIES})
add_executable(circlex_landing_gimbal autonomous_landing/circlex_landing_gimbal.cpp)
add_dependencies(circlex_landing_gimbal prometheus_msgs_generate_message_cpp)
target_link_libraries(circlex_landing_gimbal ${catkin_LIBRARIES})
add_executable(circlex_landing_fixed_camera autonomous_landing/circlex_landing_fixed_camera.cpp)
target_link_libraries(circlex_landing_fixed_camera ${catkin_LIBRARIES})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_mission.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

568
src/UAV/Modules/mission/autonomous_landing/autonomous_landing.cpp
Unescape View File

@ -1,568 +0,0 @@
/***************************************************************************************************************************
* autonomous_landing.cpp
*
* Author: Qyp
*
* Update Time: 2020.12.16
*
* 说明: 自主降落程序（支持Gazebo仿真 + 静态/慢速靶标真实实验）
* 1. 订阅目标位置(来自视觉的ros节点)
* 2. 追踪算法及追踪策略
* 3. 发布上层控制指令 (prometheus_msgs::ControlCommand)
***************************************************************************************************************************/
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include <tf/transform_datatypes.h>
#include "mission_utils.h"
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define NODE_NAME "autonomous_landing"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
bool hold_mode; // 悬停模式，用于测试检测精度
bool sim_mode; // 选择Gazebo仿真模式 或 真实实验模式
bool use_pad_height; // 是否使用降落板绝对高度
float pad_height;
string message;
std_msgs::Bool vision_switch;
geometry_msgs::PoseStamped mission_cmd;
float start_point[3]; // 起始降落位置
float camera_offset[3];
bool moving_target;
float target_vel_xy[2]; // 目标移动速度 enu坐标系 单位：m/s
std_msgs::Bool flag_start;
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState _DroneState; // 无人机状态
Eigen::Matrix3f R_Body_to_ENU; // 无人机机体系至惯性系转换矩阵
//---------------------------------------Vision---------------------------------------------
nav_msgs::Odometry GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
Detection_result landpad_det; // 检测结果
//---------------------------------------Track---------------------------------------------
float kp_land[3]; //控制参数 - 比例参数
// 五种状态机
enum EXEC_STATE
{
WAITING_RESULT,
TRACKING,
LOST,
LANDING,
};
EXEC_STATE exec_state;
float distance_to_pad;
float arm_height_to_ground;
float arm_distance_to_pad;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void landpad_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
landpad_det.object_name = "landpad";
landpad_det.Detection_info = *msg;
// 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 相机安装误差 在mission_utils.h中设置
landpad_det.pos_body_frame[0] = - landpad_det.Detection_info.position[1] + camera_offset[0];
landpad_det.pos_body_frame[1] = - landpad_det.Detection_info.position[0] + camera_offset[1];
landpad_det.pos_body_frame[2] = - landpad_det.Detection_info.position[2] + camera_offset[2];
// 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
landpad_det.pos_body_enu_frame = R_Body_to_ENU * landpad_det.pos_body_frame;
if(use_pad_height)
{
//若已知降落板高度，则无需使用深度信息。
landpad_det.pos_body_enu_frame[2] = pad_height - _DroneState.position[2];
}
// 机体惯性系 -> 惯性系
landpad_det.pos_enu_frame[0] = _DroneState.position[0] + landpad_det.pos_body_enu_frame[0];
landpad_det.pos_enu_frame[1] = _DroneState.position[1] + landpad_det.pos_body_enu_frame[1];
landpad_det.pos_enu_frame[2] = _DroneState.position[2] + landpad_det.pos_body_enu_frame[2];
// 此降落方案不考虑偏航角 （高级版可提供）
landpad_det.att_enu_frame[2] = 0.0;
if(landpad_det.Detection_info.detected)
{
landpad_det.num_regain++;
landpad_det.num_lost = 0;
}else
{
landpad_det.num_regain = 0;
landpad_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(landpad_det.num_lost > VISION_THRES)
{
landpad_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(landpad_det.num_regain > VISION_THRES)
{
landpad_det.is_detected = true;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
}
void switch_cb(const std_msgs::Bool::ConstPtr& msg)
{
flag_start = *msg;
}
void mission_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
mission_cmd = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "autonomous_landing");
ros::NodeHandle nh("~");
// 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
//【订阅】降落板与无人机的相对位置及相对偏航角 单位：米 单位：弧度
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpad_det_cb);
//【订阅】无人机状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【订阅】地面真值，此信息仅做比较使用 不强制要求提供
ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, groundtruth_cb);
//【订阅】用于中断任务，直接降落
ros::Subscriber mission_sub = nh.subscribe<geometry_msgs::PoseStamped>("/prometheus/mission/cmd", 10, mission_cb);
//【订阅】降落程序开关，默认情况下不启用，用于多任务情况
ros::Subscriber switch_sub = nh.subscribe<std_msgs::Bool>("/prometheus/switch/landing", 10, switch_cb);
// 【发布】 视觉模块开关量
ros::Publisher vision_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/ellipse_det", 10);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//强制上锁高度
nh.param<float>("arm_height_to_ground", arm_height_to_ground, 0.4);
//强制上锁距离
nh.param<float>("arm_distance_to_pad", arm_distance_to_pad, 0.3);
// 悬停模式 - 仅用于观察检测结果
nh.param<bool>("hold_mode", hold_mode, false);
// 仿真模式 - 区别在于是否自动切换offboard模式
nh.param<bool>("sim_mode", sim_mode, true);
// 是否使用降落板绝对高度
nh.param<bool>("use_pad_height", use_pad_height, false);
nh.param<float>("pad_height", pad_height, 0.01);
//追踪控制参数
nh.param<float>("kpx_land", kp_land[0], 0.1);
nh.param<float>("kpy_land", kp_land[1], 0.1);
nh.param<float>("kpz_land", kp_land[2], 0.1);
// 初始起飞点
nh.param<float>("start_point_x", start_point[0], 0.0);
nh.param<float>("start_point_y", start_point[1], 0.0);
nh.param<float>("start_point_z", start_point[2], 1.0);
// 相机安装偏移,规定为:相机在机体系(质心原点)的位置
nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
nh.param<float>("camera_offset_z", camera_offset[2], 0.0);
//目标运动或静止
nh.param<bool>("moving_target", moving_target, false);
nh.param<float>("target_vel_x", target_vel_xy[0], 0.0);
nh.param<float>("target_vel_y", target_vel_xy[1], 0.0);
//打印现实检查参数
printf_param();
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
if(sim_mode)
{
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}else
{
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
cout << "Waiting for the offboard mode"<<endl;
ros::Duration(1.0).sleep();
ros::spinOnce();
}
}
// 起飞
cout<<"[autonomous_landing]: "<<"Takeoff to predefined position."<<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Takeoff to predefined position.");
while( _DroneState.position[2] < 0.5)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point[0];
Command_Now.Reference_State.position_ref[1] = start_point[1];
Command_Now.Reference_State.position_ref[2] = start_point[2];
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 等待
ros::Duration(3.0).sleep();
exec_state = EXEC_STATE::WAITING_RESULT;
while (ros::ok())
{
//回调
ros::spinOnce();
static int printf_num = 0;
printf_num++;
// 此处是为了控制打印频率
if(printf_num > 20)
{
if(exec_state == TRACKING)
{
// 正常追踪
char message_chars[256];
sprintf(message_chars, "Tracking the Landing Pad, distance_to_the_pad : %f [m] .", distance_to_pad);
message = message_chars;
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
}
if(sim_mode)
{
printf_result();
}
printf_num = 0;
}
// 接收到中断指令，直接降落
if(mission_cmd.pose.position.x == 99)
{
exec_state = LANDING;
}
// 接收到hold转降落指令,将设置hold模式为false
if(mission_cmd.pose.position.x == 88)
{
hold_mode = false;
}
switch (exec_state)
{
// 初始状态，等待视觉检测结果
case WAITING_RESULT:
{
if(landpad_det.is_detected)
{
exec_state = TRACKING;
message = "Get the detection result.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 发送视觉节点启动指令
vision_switch.data = true;
vision_switch_pub.publish(vision_switch);
message = "Waiting for the detection result.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
ros::Duration(1.0).sleep();
break;
}
// 追踪状态
case TRACKING:
{
// 丢失,进入LOST状态
if(!landpad_det.is_detected && !hold_mode)
{
exec_state = LOST;
message = "Lost the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 抵达上锁点,进入LANDING
distance_to_pad = landpad_det.pos_body_enu_frame.norm();
//　达到降落距离，上锁降落
if(distance_to_pad < arm_distance_to_pad)
{
exec_state = LANDING;
message = "Catched the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
//　达到最低高度，上锁降落
else if(abs(landpad_det.pos_body_enu_frame[2]) < arm_height_to_ground)
{
exec_state = LANDING;
message = "Reach the lowest height.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 机体系速度控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL; //xy velocity z position
for (int i=0; i<3; i++)
{
Command_Now.Reference_State.velocity_ref[i] = kp_land[i] * landpad_det.pos_body_enu_frame[i];
}
//
if(moving_target)
{
Command_Now.Reference_State.velocity_ref[0] += target_vel_xy[0];
Command_Now.Reference_State.velocity_ref[1] += target_vel_xy[1];
}
Command_Now.Reference_State.yaw_ref = 0.0;
//Publish
if (!hold_mode)
{
command_pub.publish(Command_Now);
}
break;
}
case LOST:
{
static int lost_time = 0;
lost_time ++ ;
// 重新获得信息,进入TRACKING
if(landpad_det.is_detected)
{
exec_state = TRACKING;
lost_time = 0;
message = "Regain the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 首先是悬停等待 尝试得到图像, 如果仍然获得不到图像 则原地上升
if(lost_time < 10.0)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
ros::Duration(0.4).sleep();
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_Now.Reference_State.velocity_ref[0] = 0.0;
Command_Now.Reference_State.velocity_ref[1] = 0.0;
Command_Now.Reference_State.velocity_ref[2] = 0.1;
Command_Now.Reference_State.yaw_ref = 0;
// 如果上升超过原始高度，则认为任务失败，则直接降落
if(_DroneState.position[2] >= start_point[2])
{
exec_state = LANDING;
lost_time = 0;
message = "Mission failed, landing... ";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
}
command_pub.publish(Command_Now);
break;
}
case LANDING:
{
if(sim_mode) //？？
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
//Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}
ros::Duration(1.0).sleep();
break;
}
}
rate.sleep();
}
return 0;
}
void printf_result()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<"<< endl;
switch (exec_state)
{
case WAITING_RESULT:
cout << "exec_state: WAITING_RESULT" <<endl;
break;
case TRACKING:
cout << "exec_state: TRACKING" <<endl;
break;
case LOST:
cout << "exec_state: LOST" <<endl;
break;
case LANDING:
cout << "exec_state: LANDING" <<endl;
break;
}
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(landpad_det.is_detected)
{
cout << "is_detected: ture" <<endl;
}else
{
cout << "is_detected: false" <<endl;
}
cout << "Target_pos (body): " << landpad_det.pos_body_frame[0] << " [m] "<< landpad_det.pos_body_frame[1] << " [m] "<< landpad_det.pos_body_frame[2] << " [m] "<<endl;
cout << "Target_pos (body_enu): " << landpad_det.pos_body_enu_frame[0] << " [m] "<< landpad_det.pos_body_enu_frame[1] << " [m] "<< landpad_det.pos_body_enu_frame[2] << " [m] "<<endl;
cout << "Ground_truth(pos): " << GroundTruth.pose.pose.position.x << " [m] "<< GroundTruth.pose.pose.position.y << " [m] "<< GroundTruth.pose.pose.position.z << " [m] "<<endl;
cout << "Detection_ENU(pos): " << landpad_det.pos_enu_frame[0] << " [m] "<< landpad_det.pos_enu_frame[1] << " [m] "<< landpad_det.pos_enu_frame[2] << " [m] "<<endl;
cout << "Detection_ENU(yaw): " << landpad_det.att_enu_frame[2]/3.1415926 *180 << " [deg] "<<endl;
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Land Control State<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "vel_cmd: " << Command_Now.Reference_State.velocity_ref[0] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[1] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "hold_mode : "<< hold_mode << endl;
cout << "sim_mode : "<< sim_mode << endl;
cout << "use_pad_height : "<< use_pad_height << endl;
cout << "arm_distance_to_pad : "<< arm_distance_to_pad << endl;
cout << "arm_height_to_ground : "<< arm_height_to_ground << endl;
cout << "kpx_land : "<< kp_land[0] << endl;
cout << "kpy_land : "<< kp_land[1] << endl;
cout << "kpz_land : "<< kp_land[2] << endl;
cout << "start_point_x : "<< start_point[0] << endl;
cout << "start_point_y : "<< start_point[1] << endl;
cout << "start_point_z : "<< start_point[2] << endl;
cout << "camera_offset_x : "<< camera_offset[0] << endl;
cout << "camera_offset_y : "<< camera_offset[1] << endl;
cout << "camera_offset_z : "<< camera_offset[2] << endl;
cout << "moving_target : "<< moving_target << endl;
cout << "target_vel_x : "<< target_vel_xy[0] << endl;
cout << "target_vel_y : "<< target_vel_xy[1] << endl;
}

383
src/UAV/Modules/mission/autonomous_landing/autonomous_landing_aruco.cpp
Unescape View File

@ -1,383 +0,0 @@
//ROS 头文件
#include <autonomous_landing_aruco.h>
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle nh("~");
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// 悬停模式 - 仅用于观察检测结果
nh.param<bool>("hold_mode", hold_mode, false);
// 仿真模式 - 区别在于是否自动切换offboard模式
nh.param<bool>("sim_mode", sim_mode, true);
// 相机安装偏移,规定为:相机在机体系(质心原点)的位置
nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
nh.param<float>("camera_offset_z", camera_offset[2], 0.0);
//追踪控制参数
nh.param<float>("kpx_land", kp_land[0], 0.1);
nh.param<float>("kpy_land", kp_land[1], 0.1);
nh.param<float>("kpz_land", kp_land[2], 0.1);
// 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
//【订阅】靶标位置
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
aruco_sub = nh.subscribe<prometheus_msgs::ArucoInfo>("/prometheus/object_detection/aruco_det", 10, aruco_cb);
//【订阅】无人机状态
drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【订阅】地面真值，此信息仅做比较使用 不强制要求提供
groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/aruco_marker", 10, groundtruth_cb);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
// 状态机定时器
ros::Timer mainloop_timer = nh.createTimer(ros::Duration(1.0), mainloop_cb);
ros::Timer search_timer = nh.createTimer(ros::Duration(2.0), search_cb);
// 无人机控制定时器 20Hz
ros::Timer control_timer = nh.createTimer(ros::Duration(0.05), control_cb);
// 打印定时器
ros::Timer printf_timer = nh.createTimer(ros::Duration(1.0), printf_cb);
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
num_detected = 0;
num_lost = 0;
is_detected = false;
distance_to_pad = 0;
exec_state = EXEC_STATE::INIT;
//打印参数,用于检查
printf_param();
while(ros::ok)
{
// 无人机结束任务 且上锁后
if(exec_state == EXEC_STATE::DISARM && !_DroneState.armed)
{
return 0;
}
ros::spinOnce();
rate.sleep();
}
}
void search_cb(const ros::TimerEvent& e)
{
if(exec_state == EXEC_STATE::SEARCH)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.position_ref[0] = search_point(0);
Command_Now.Reference_State.position_ref[1] = search_point(1);
Command_Now.Reference_State.position_ref[2] = search_point(2);
command_pub.publish(Command_Now);
}else
{
return;
}
}
void control_cb(const ros::TimerEvent& e)
{
// 水平接近
if(exec_state == EXEC_STATE::HORIZON_APPROACH)
{
// 机体系速度控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS; //xy velocity z position
Command_Now.Reference_State.velocity_ref[0] = kp_land[0] * marker_body_enu[0];
Command_Now.Reference_State.velocity_ref[1] = kp_land[1] * marker_body_enu[1];
Command_Now.Reference_State.velocity_ref[2] = 0.0;
// 当前高度？
Command_Now.Reference_State.position_ref[2] = 2.0;
command_pub.publish(Command_Now);
}
// 下降阶段
else if(exec_state == EXEC_STATE::DECEND)
{
// 机体系速度控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL; //xy velocity z position
Command_Now.Reference_State.velocity_ref[0] = kp_land[0] * marker_body_enu[0];
Command_Now.Reference_State.velocity_ref[1] = kp_land[1] * marker_body_enu[1];
// 定速下降
Command_Now.Reference_State.velocity_ref[2] = -0.2;
command_pub.publish(Command_Now);
}else
{
return;
}
}
void mainloop_cb(const ros::TimerEvent& e)
{
switch (exec_state)
{
case INIT:
if(sim_mode)
{
static int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
// 解锁、切换offboard模式
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
}
// 当无人机进入offboard模式，且处于解锁状态,进入TAKEOFF
if(_DroneState.mode == "OFFBOARD" && _DroneState.armed )
{
exec_state = EXEC_STATE::TAKEOFF;
}else
{
cout << "Waiting OFFBOARD mode and arm ..."<<endl;
}
break;
case TAKEOFF:
if(_DroneState.position[2] < 0.8)
{
// 记录返航点
return_point[0] = _DroneState.position[0];
return_point[1] = _DroneState.position[1];
return_point[2] = _DroneState.position[2];
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Takeoff;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
ros::Duration(1.0).sleep();
}else
{
exec_state = EXEC_STATE::SEARCH;
start_search = true;
}
break;
case SEARCH:
// 进入搜索模式，重置search_id
if(start_search)
{
search_id = 0;
start_search = false;
} // 当检测到目标，进入水平接近
else if(is_detected)
{
exec_state = EXEC_STATE::HORIZON_APPROACH;
break;
}
if(search_id == 0)
{
search_point << 0.0, 0.0, 2.0;
}else if(search_id == 1)
{
search_point << 0.0, 5.0, 2.0;
}else if (search_id == 2)
{
search_point << 2.0, 5.0, 2.0;
}else if (search_id == 3)
{
search_point << 2.0, 0.0, 2.0;
}else if (search_id == 4)
{
search_point << 4.0, 0.0, 2.0;
}else if (search_id == 5)
{
search_point << 4.0, 5.0, 2.0;
}
dis_to_search_point = (mav_pos - search_point).norm();
if(dis_to_search_point < 0.2)
{
search_id++;
// 超出最大搜索点，return
if(search_id >= 6)
{
exec_state = EXEC_STATE::LAND_NOW;
}
}
break;
case HORIZON_APPROACH:
// 水平靠近过程中丢失目标
if(!is_detected)
{
exec_state = EXEC_STATE::SEARCH;
start_search = true;
}
// 水平距离达到阈值，进入下降阶段
else if(marker_body[0]<0.05 && marker_body[1]<0.05)
{
exec_state = EXEC_STATE::DECEND;
}
break;
case DECEND:
if(_DroneState.position[2]<0.2)
{
exec_state = EXEC_STATE::DISARM;
}
break;
case LAND_NOW:
// 原地降落模式
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
if(_DroneState.position[2]<0.2)
{
exec_state = EXEC_STATE::DISARM;
}
break;
case DISARM:
if(sim_mode) //？？
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
//Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}
break;
}
}
void printf_cb(const ros::TimerEvent& e)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<"<< endl;
switch (exec_state)
{
case INIT:
cout << "[INIT]" <<endl;
break;
case TAKEOFF:
cout << "[TAKEOFF]" <<endl;
break;
case SEARCH:
cout << "[SEARCH]" << "ENU_FRAME, XYZ_POS"<<endl;
cout << "Reference [ X Y Z ]: " << Command_Now.Reference_State.position_ref[0] << " [m] "<< Command_Now.Reference_State.position_ref[1] << " [m] "<< Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
break;
case HORIZON_APPROACH:
cout << "[HORIZON_APPROACH]" << "ENU_FRAME, XY_VEL_Z_POS" <<endl;
cout << "Reference [ X Y Z ]: " << Command_Now.Reference_State.velocity_ref[0] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[1] << " [m/s] "<< Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
cout << "distance_to_pad: " << distance_to_pad<< endl;
// char message_chars[256];
// sprintf(message_chars, "Tracking the Landing Pad, distance_to_the_pad : %f [m] .", distance_to_pad);
// message = message_chars;
// cout << message <<endl;
// pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
case DECEND:
cout << "[DECEND]" << "ENU_FRAME, XYZ_VEL" <<endl;
cout << "Reference [ X Y Z ]: " << Command_Now.Reference_State.velocity_ref[0] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[1] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
break;
case LAND_NOW:
cout << "[LAND_NOW]" <<endl;
break;
case DISARM:
cout << "[DISARM]" <<endl;
break;
}
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(is_detected)
{
cout << "detected: [yes]" <<endl;
}else
{
cout << "detected: [no]" <<endl;
}
cout << "Target_pos (body): " << marker_body[0] << " [m] "<< marker_body[1] << " [m] "<< marker_body[2] << " [m] "<<endl;
cout << "Target_pos (body_enu): " << marker_body_enu[0] << " [m] "<< marker_body_enu[1] << " [m] "<< marker_body_enu[2] << " [m] "<<endl;
cout << "Ground_truth(pos): " << GroundTruth.pose.pose.position.x << " [m] "<< GroundTruth.pose.pose.position.y << " [m] "<< GroundTruth.pose.pose.position.z << " [m] "<<endl;
cout << "Detection_ENU(pos): " << marker_enu[0] << " [m] "<< marker_enu[1] << " [m] "<< marker_enu[2] << " [m] "<<endl;
}

140
src/UAV/Modules/mission/autonomous_landing/autonomous_landing_aruco.h
Unescape View File

@ -1,140 +0,0 @@
#ifndef AUTONOMOUS_LANDING_ARUCO_H
#define AUTONOMOUS_LANDING_ARUCO_H
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include <tf/transform_datatypes.h>
#include <prometheus_msgs/ArucoInfo.h>
#include "mission_utils.h"
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define NODE_NAME "autonomous_landing_aruco"
// 参数
bool hold_mode; // 悬停模式，用于测试检测精度
bool sim_mode; // 选择Gazebo仿真模式 或 真实实验模式
float camera_offset[3]; //相机安装偏差
string message;
prometheus_msgs::DroneState _DroneState; // 无人机状态
Eigen::Vector3f mav_pos;
Eigen::Matrix3f R_Body_to_ENU; // 无人机机体系至惯性系转换矩阵
Eigen::Vector3f return_point;
nav_msgs::Odometry GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
prometheus_msgs::ArucoInfo marker_info; // 检测结果
Eigen::Vector3f marker_body,marker_body_enu;
Eigen::Vector3f marker_enu;
bool is_detected;
int num_detected, num_lost;
float distance_to_pad;
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
Eigen::Vector3f search_point;
float dis_to_search_point;
int search_id;
bool start_search;
float kp_land[3]; //控制参数 - 比例参数
// 状态机
enum EXEC_STATE
{
INIT,
TAKEOFF,
SEARCH,
HORIZON_APPROACH,
DECEND,
LAND_NOW,
DISARM,
};
EXEC_STATE exec_state;
ros::Subscriber aruco_sub, drone_state_sub, groundtruth_sub;
ros::Publisher command_pub, message_pub;
void mainloop_cb(const ros::TimerEvent& e);
void search_cb(const ros::TimerEvent& e);
void control_cb(const ros::TimerEvent& e);
void printf_cb(const ros::TimerEvent& e);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void aruco_cb(const prometheus_msgs::ArucoInfo::ConstPtr &msg)
{
marker_info = *msg;
if(marker_info.detected)
{
num_detected++;
num_lost = 0;
}else
{
num_detected = 0;
num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(num_lost > VISION_THRES)
{
is_detected = false;
return;
}
// 当连续一段时间检测到目标时，认定目标得到
if(num_detected > VISION_THRES)
{
is_detected = true;
}
// marker在无人机机体系下的位置
marker_body[0] = - marker_info.position[1] + camera_offset[0];
marker_body[1] = - marker_info.position[0] + camera_offset[1];
marker_body[2] = - marker_info.position[2] + camera_offset[2];
distance_to_pad = marker_body.norm();
// 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
marker_body_enu = R_Body_to_ENU * marker_body;
// 机体惯性系 -> 惯性系
marker_enu[0] = _DroneState.position[0] + marker_body_enu[0];
marker_enu[1] = _DroneState.position[1] + marker_body_enu[1];
marker_enu[2] = _DroneState.position[2] + marker_body_enu[2];
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
mav_pos << _DroneState.position[0], _DroneState.position[1], _DroneState.position[2];
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "hold_mode : "<< hold_mode << endl;
cout << "sim_mode : "<< sim_mode << endl;
cout << "kpx_land : "<< kp_land[0] << endl;
cout << "kpy_land : "<< kp_land[1] << endl;
cout << "kpz_land : "<< kp_land[2] << endl;
cout << "camera_offset_x : "<< camera_offset[0] << endl;
cout << "camera_offset_y : "<< camera_offset[1] << endl;
cout << "camera_offset_z : "<< camera_offset[2] << endl;
}
#endif

332
src/UAV/Modules/mission/autonomous_landing/circlex_landing_fixed_camera.cpp
Unescape View File

@ -1,332 +0,0 @@
/***************************************************************************************************************************
*
* Author: Onxming
*
***************************************************************************************************************************/
// ros头文件
#include <ros/ros.h>
// topic 头文件
#include <iostream>
#include "prometheus_msgs/ControlCommand.h"
#include "prometheus_msgs/DroneState.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <std_srvs/SetBool.h>
#include <std_msgs/Float32MultiArray.h>
#include <fstream>
#include <sstream>
#include <Eigen/Eigen>
using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float pixel_x, pixel_y;
//---------------------------------------Vision---------------------------------------------
// geometry_msgs::Pose pos_target; //目标位置[机体系下：前方x为正，右方y为正，下方z为正]
float curr_pos[3];
float curr_vel[3];
std::string drone_mode;
float drone_pitch;
float height_time = 0.0;
int flag_detected = 0; // 是否检测到目标标志
//---------------------------------------Track---------------------------------------------
float kpx_track; //追踪比例系数
float land_move_scale; //降落移动系数
float x_move_scale;
float y_move_scale;
float land_high;
float static_vel_thresh;
float max_vel;
bool is_gps = false;
int max_count_vision_lost = 0; //视觉丢失计数器
int count_vision_lost; //视觉丢失计数器阈值
int max_high = 10; // 丢失目标后, 上升的最大高度
int hz = 60; //循环频率
float pic_width;
float pic_high;
// 几秒到达最大速度
float vel_smooth_scale = 0.2;
float vel_smooth_thresh = 0.3;
// 目标丢失时速度衰减
float object_lost_vel_weaken;
// 视觉降落计数器
int cv_land_count;
int cv_land_cnt[5]{0, 0, 0, 0, 0};
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_now; //发送给position_control.cpp的命令
prometheus_msgs::ControlCommand Command_past; //发送给position_control.cpp的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void pos_cb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
// Read the Drone Position from the Mavros Package [Frame: ENU]
curr_pos[0] = msg->position[0];
curr_pos[1] = msg->position[1];
if (is_gps)
{
curr_pos[2] = msg->rel_alt;
}
else
{
curr_pos[2] = msg->position[2];
}
curr_vel[0] = msg->velocity[0];
curr_vel[1] = msg->velocity[1];
curr_vel[2] = msg->velocity[2];
drone_mode = msg->mode;
drone_pitch = msg->attitude[1] * 180 / M_PI;
// high_error = msg->position[2] - msg->rel_alt;
}
void sub_diff_callback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
flag_detected = msg->data[0];
pixel_x = msg->data[1] - msg->data[3] / 2;
pixel_y = msg->data[2] - msg->data[4] / 2;
pic_width = msg->data[3];
pic_high = msg->data[4];
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "circlex_landing_fixed_camera");
ros::NodeHandle nh("~");
std::string path = std::string("/home/amov/Prometheus/circlex_detect_") + std::to_string((int)ros::Time::now().toSec()) + ".log";
ofstream file(path.c_str());
// vision_pixel
ros::Subscriber pos_diff_sub = nh.subscribe<std_msgs::Float32MultiArray>("/prometheus/object_detection/circlex_det", 10, sub_diff_callback);
// 获取 无人机ENU下的位置
ros::Subscriber curr_pos_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, pos_cb);
// 【发布】发送给position_control.cpp的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 频率 [20Hz]
// 这个频率取决于视觉程序输出的频率，一般不能低于10Hz，不然追踪效果不好
ros::Rate rate(hz);
int comid = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//视觉丢失次数阈值
//处理视觉丢失时的情况
nh.param<int>("max_count_vision_lost", max_count_vision_lost, 30);
// 速度平滑
nh.param<float>("vel_smooth_scale", vel_smooth_scale, 0.2);
// 使用速度平滑的阈值
nh.param<float>("vel_smooth_thresh", vel_smooth_thresh, 0.3);
// 降落速度
nh.param<float>("land_move_scale", land_move_scale, 0.2);
// 降落速度
nh.param<float>("x_move_scale", x_move_scale, 0.2);
// 降落速度
nh.param<float>("y_move_scale", y_move_scale, 0.2);
// 自动降落高度
nh.param<float>("land_high", land_high, 0.2);
// 目标丢失时速度衰减
nh.param<float>("object_lost_vel_weaken", object_lost_vel_weaken, 0.5);
nh.param<int>("cv_land_count", cv_land_count, 5);
// 静止速度
nh.param<float>("static_vel_thresh", static_vel_thresh, 0.02);
nh.param<float>("max_vel", max_vel, 1);
// 是否室外
nh.param<bool>("is_gps", is_gps, false);
//打印现实检查参数
printf_param();
int check_flag;
cout << "Please check the parameter and setting，\n 1 continue \n else for quit: " << endl;
try
{
std::cin >> check_flag;
if (check_flag != 1)
return -0;
}
catch (std::exception)
{
cout << "Please input number" << std::endl;
return -1;
}
// 丢失时使用的数据
float lost_x_vel, lost_y_vel, lost_z_vel, lost_yaw;
int land_cnt = 0;
bool cv_land = false;
count_vision_lost = max_count_vision_lost;
Command_past.Mode = prometheus_msgs::ControlCommand::Hold;
Command_past.Reference_State.velocity_ref[0] = 0.;
Command_past.Reference_State.velocity_ref[1] = 0.;
Command_past.Reference_State.velocity_ref[2] = 0.;
Command_past.Reference_State.position_ref[0] = 0.;
Command_past.Reference_State.position_ref[1] = 0.;
Command_past.Reference_State.position_ref[2] = 0.;
for (int i = 0; i < 50; i++)
{
ros::spinOnce();
}
while (ros::ok())
{
ros::spinOnce();
float comm[4]{0, 0, 0, 0};
Command_now.Command_ID = comid;
Command_now.header.stamp = ros::Time::now();
Command_now.Mode = prometheus_msgs::ControlCommand::Move;
Command_now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_now.Reference_State.Yaw_Rate_Mode = true;
Command_now.source = "circlex_dectector";
comid++;
std::stringstream ss;
if ((curr_pos[2] < land_high || cv_land) && drone_mode == "OFFBOARD")
{
Command_now.Mode = prometheus_msgs::ControlCommand::Land;
file.close();
}
else if (flag_detected == 0)
{
if (drone_mode != "OFFBOARD")
{
ss << "\n ready into offboard ....";
}
else
{
int tmp_cv = 0;
for (int i = 0; i < cv_land_count; i++)
{
tmp_cv += cv_land_cnt[i];
cv_land_cnt[i] = 0;
}
if (tmp_cv == cv_land_count)
{
cv_land = true;
ss << "\n 目标消失前5次连续检测为X, 进入LAND";
comm[0] = 0;
comm[1] = 0;
comm[2] = 0;
comm[3] = 0;
}
else if (count_vision_lost <= 0) // 进入重新寻回目标模式
{
if (curr_pos[2] > max_high)
{
ss << "\n 目标丢失后达到最大高度, 进入LAND";
cv_land = true;
}
else
{
comm[0] = -lost_x_vel;
comm[1] = -lost_y_vel;
comm[2] = -lost_z_vel;
comm[3] = -lost_yaw;
lost_yaw = 0;
lost_x_vel *= object_lost_vel_weaken;
lost_y_vel *= object_lost_vel_weaken;
ss << "\n>> object lost, back << ";
}
}
else
{
// 记录目标丢失时数据
if (count_vision_lost == max_count_vision_lost)
{
lost_x_vel = Command_past.Reference_State.velocity_ref[0];
lost_y_vel = Command_past.Reference_State.velocity_ref[1];
lost_z_vel = Command_past.Reference_State.velocity_ref[2];
lost_z_vel = -std::fmax(std::abs(lost_z_vel), 0.3);
lost_yaw = Command_past.Reference_State.yaw_ref;
}
comm[0] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[0];
comm[1] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[1];
comm[2] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[2];
ss << "\n object lost, use past command ! ";
comm[3] = 0;
count_vision_lost--;
}
}
}
else
{
// 重置丢失次数
count_vision_lost = max_count_vision_lost;
// 使用真实高度
// float offset = std::max(curr_pos[2] * 0.10, 0.10);
comm[0] = -pixel_y * x_move_scale * curr_pos[2] / 100;
comm[1] = -pixel_x * y_move_scale * curr_pos[2] / 100;
comm[2] = -land_move_scale * curr_pos[2] / std::fmax((comm[1] +comm[0]) * 10,1);
comm[3] = 0.;
ss << "\n"
<< "pixel_error_x: " << pixel_x
<< " pixel_error_y: " << pixel_y
<< " curr_pos: " << curr_pos[2] << " ";
// 视觉降落判断
land_cnt = (land_cnt + 1) % cv_land_count;
cv_land_cnt[land_cnt] = flag_detected == 2 ? 1 : 0;
if (flag_detected == 2 && std::abs(curr_vel[1]) + std::abs(curr_vel[2]) < static_vel_thresh)
{
ss << "\n 检测到X且当前速度被时为静止状态, 进入LAND ";
cv_land = true;
comm[0] = 0;
comm[1] = 0;
comm[2] = 0;
comm[3] = 0;
}
}
ss << "\nnow vel >> x_vel: " << curr_vel[0] << " y_vel: " << curr_vel[1] << " z_vel: " << curr_vel[2] << " hight: " << curr_pos[2];
ss << "\ncommand vel >> x_vel: " << comm[0] << " y_vel: " << comm[1] << " z_vel: " << comm[2] << " yaw_rate: " << Command_now.Reference_State.yaw_rate_ref << " max_vel: " << max_vel;
for (int i = 0; i < 3; i++)
{
Command_now.Reference_State.velocity_ref[i] = comm[i] == 0 ? 0 : (comm[i] / std::abs(comm[i])) * std::fmin(std::abs(comm[i]), max_vel);
}
command_pub.publish(Command_now);
if (Command_now.Mode == prometheus_msgs::ControlCommand::Land)
break;
Command_past = Command_now;
ss << "\n------------------------------------------------------------------------" << std::endl;
std::cout << ss.str() << std::endl;
file << ss.str();
rate.sleep();
}
cout << "land finish, press any key to exit" << endl;
cin >> check_flag;
return 0;
}
void printf_param()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" << endl;
cout << "land_move_scale " << land_move_scale << endl;
cout << "land_high " << land_high << endl;
cout << "max_count_vision_lost " << max_count_vision_lost << endl;
cout << "vel_smooth_scale " << vel_smooth_scale << endl;
cout << "vel_smooth_thresh " << vel_smooth_thresh << endl;
cout << "object_lost_vel_weaken " << object_lost_vel_weaken << endl;
cout << "static_vel_thresh " << static_vel_thresh << endl;
cout << "max_vel " << max_vel << endl;
}

506
src/UAV/Modules/mission/autonomous_landing/circlex_landing_gimbal.cpp
Unescape View File

@ -1,506 +0,0 @@
/***************************************************************************************************************************
*
* Author: Onxming
*
***************************************************************************************************************************/
//ros头文件
#include <ros/ros.h>
//topic 头文件
#include "prometheus_msgs/gimbal.h"
#include <iostream>
#include "prometheus_msgs/ControlCommand.h"
#include "prometheus_msgs/DroneState.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <prometheus_msgs/GimbalCtl.h>
#include <std_srvs/SetBool.h>
#include <std_msgs/Float32MultiArray.h>
#include <fstream>
#include <sstream>
#include <Eigen/Eigen>
using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float pixel_x, pixel_y;
prometheus_msgs::gimbal read_gimbal;
//---------------------------------------Vision---------------------------------------------
// geometry_msgs::Pose pos_target; //目标位置[机体系下：前方x为正，右方y为正，下方z为正]
float curr_pos[3];
float curr_vel[3];
std::string drone_mode;
float drone_pitch;
float height_time = 0.0;
int flag_detected = 0; // 是否检测到目标标志
float horizontal_distance = 0.;
//---------------------------------------Track---------------------------------------------
int Num_StateMachine = 0; // 状态机编号
int Num_StateMachine_Last = 0; // 状态机编号last
float default_pitch;
float kpx_track; //追踪比例系数
float land_move_scale; //降落移动系数
float land_high;
float static_vel_thresh;
float max_vel;
int ignore_error_yaw;
int ignore_error_pitch;
int max_count_vision_lost = 0; //视觉丢失计数器
int count_vision_lost; //视觉丢失计数器阈值
const int yaw_max_count = 10;
int yaw_count = yaw_max_count;
const int horizontal_max_count = 10;
int horizontal_count = horizontal_max_count;
const int vertical_max_count = 10;
int vertical_count = vertical_max_count;
int max_high = 10; // 丢失目标后, 上升的最大高度
int hz = 25; //循环频率
float pic_width;
float pic_high;
// 几秒到达最大速度
float vel_smooth_scale = 0.2;
float vel_smooth_thresh = 0.3;
// 目标丢失时速度衰减
float object_lost_vel_weaken;
// 视觉降落计数器
int cv_land_count;
int cv_land_cnt[5]{0, 0, 0, 0, 0};
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_now; //发送给position_control.cpp的命令
prometheus_msgs::ControlCommand Command_past; //发送给position_control.cpp的命令
// 降落状态量
enum State
{
Horizontal,
Vertical,
};
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result(); //打印函数
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
inline float angle2radians(float angle)
{
return M_PI * angle / 180;
}
void pos_cb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
// Read the Drone Position from the Mavros Package [Frame: ENU]
curr_pos[0] = msg->position[0];
curr_pos[1] = msg->position[1];
// curr_pos[2] = msg->position[2];
curr_pos[2] = msg->rel_alt;
curr_vel[0] = msg->velocity[0];
curr_vel[1] = msg->velocity[1];
curr_vel[2] = msg->velocity[2];
drone_mode = msg->mode;
drone_pitch = msg->attitude[1] * 180 / M_PI;
// high_error = msg->position[2] - msg->rel_alt;
}
void sub_diff_callback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
flag_detected = msg->data[0];
pixel_x = msg->data[1] - msg->data[3] / 2;
pixel_y = msg->data[2] - msg->data[4] / 2;
pic_width = msg->data[3];
pic_high = msg->data[4];
}
void sub_gimbaldata_cb(const prometheus_msgs::gimbal::ConstPtr &msg)
{
read_gimbal.imu0 = msg->imu0;
read_gimbal.imu1 = msg->imu1;
read_gimbal.imu2 = msg->imu2;
read_gimbal.rel0 = msg->rel0;
read_gimbal.rel1 = msg->rel1;
read_gimbal.rel2 = msg->rel2;
read_gimbal.relvel2 = msg->relvel2;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "target_tracking");
ros::NodeHandle nh("~");
std::string path = std::string("/home/amov/Prometheus/circlex_detect_") + std::to_string((int)ros::Time::now().toSec()) + ".log";
ofstream file(path.c_str());
// 【订阅】视觉消息 来自视觉节点
// 方向定义： 目标位置[机体系下：前方x为正，右方y为正，下方z为正]
// 标志位： orientation.w 用作标志位 1代表识别到目标 0代表丢失目标
// 注意这里为了复用程序使用了/vision/target作为话题名字，适用于椭圆、二维码、yolo等视觉算法
// 故同时只能运行一种视觉识别程序，如果想同时追踪多个目标，这里请修改接口话题的名字
//ros::Subscriber vision_sub = nh.subscribe<geometry_msgs::Pose>("/vision/target", 10, vision_cb);
//vision_pixel
ros::Subscriber pos_diff_sub = nh.subscribe<std_msgs::Float32MultiArray>("/prometheus/object_detection/circlex_det", 10, sub_diff_callback);
//gimbal_data, 云台数据
ros::Subscriber gimbaldata_sub = nh.subscribe<prometheus_msgs::gimbal>("/readgimbal", 10, sub_gimbaldata_cb);
// 获取 无人机ENU下的位置
ros::Subscriber curr_pos_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, pos_cb);
// 【发布】发送给position_control.cpp的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 手动控制吊舱. 控制初始角度向下
ros::Publisher gimbal_control_pub = nh.advertise<prometheus_msgs::GimbalCtl>("/GimbalCtl", 10);
// 控制吊舱是否接受圆叉检测器的控制
ros::ServiceClient gimbal_control_flag_client = nh.serviceClient<std_srvs::SetBool>("/prometheus/circlex_gimbal_control");
gimbal_control_flag_client.waitForExistence();
ros::ServiceClient gimbal_change_landing = nh.serviceClient<std_srvs::SetBool>("/prometheus/gimbal/is_land");
gimbal_change_landing.waitForExistence();
// 频率 [20Hz]
// 这个频率取决于视觉程序输出的频率，一般不能低于10Hz，不然追踪效果不好
ros::Rate rate(hz);
// 降落的吊舱初始角度， 朝下
prometheus_msgs::GimbalCtl gimbal_control_;
int comid = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//追踪算法比例参数
nh.param<float>("kpx_track", kpx_track, 0.25);
//视觉丢失次数阈值
//处理视觉丢失时的情况
nh.param<int>("max_count_vision_lost", max_count_vision_lost, 30);
// 速度平滑
nh.param<float>("vel_smooth_scale", vel_smooth_scale, 0.2);
// 使用速度平滑的阈值
nh.param<float>("vel_smooth_thresh", vel_smooth_thresh, 0.3);
//吊舱默认向下的转的角度
nh.param<float>("default_pitch", default_pitch, -60.);
// 降落速度
nh.param<float>("land_move_scale", land_move_scale, 0.2);
// 自动降落高度
nh.param<float>("land_high", land_high, 0.2);
// 目标丢失时速度衰减
nh.param<float>("object_lost_vel_weaken", object_lost_vel_weaken, 0.5);
nh.param<int>("cv_land_count", cv_land_count, 5);
// 忽略的俯仰角误差: 在误差内,判定为无人机已经到达降落板的正上方
nh.param<int>("ignore_error_pitch", ignore_error_pitch, 5);
// 静止速度
nh.param<float>("static_vel_thresh", static_vel_thresh, 0.02);
nh.param<float>("max_vel", max_vel, 1);
//打印现实检查参数
printf_param();
int check_flag;
//输入1,继续，其他，退出程序
cout << "Please check the parameter and setting，\n 1 for (yaw + pitch) control \n 2 for mode (roll + pitch) control \n else for quit: " << endl;
std::cin >> check_flag;
// 吊舱位置初始化
// 摄像头向下90度
std_srvs::SetBool tmp;
gimbal_control_.pitch = default_pitch;
gimbal_control_pub.publish(gimbal_control_);
switch (check_flag)
{
case 1:
tmp.request.data = false;
break;
case 2:
tmp.request.data = true;
break;
default:
return -1;
}
gimbal_change_landing.call(tmp);
gimbal_control_.pitch = 0.;
// 丢失时使用的数据
float lost_x_vel, lost_y_vel, lost_z_vel, lost_yaw;
int land_cnt = 0;
bool cv_land = false;
count_vision_lost = max_count_vision_lost;
State Now_State = State::Horizontal;
Command_past.Mode = prometheus_msgs::ControlCommand::Hold;
Command_past.Reference_State.velocity_ref[0] = 0.;
Command_past.Reference_State.velocity_ref[1] = 0.;
Command_past.Reference_State.velocity_ref[2] = 0.;
Command_past.Reference_State.position_ref[0] = 0.;
Command_past.Reference_State.position_ref[1] = 0.;
Command_past.Reference_State.position_ref[2] = 0.;
for (int i = 0; i < 50; i++)
{
ros::spinOnce();
}
while (ros::ok())
{
ros::spinOnce();
float comm[4]{0, 0, 0, 0};
Command_now.Command_ID = comid;
Command_now.header.stamp = ros::Time::now();
Command_now.Mode = prometheus_msgs::ControlCommand::Move;
Command_now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_now.Reference_State.Yaw_Rate_Mode = true;
Command_now.source = "circlex_dectector";
comid++;
std::stringstream ss;
float r, p, y;
r = read_gimbal.rel0;
p = read_gimbal.rel1 + drone_pitch;
y = read_gimbal.rel2;
horizontal_distance = curr_pos[2] * std::tan(M_PI * (90 - p) / 180);
if ((curr_pos[2] < land_high || cv_land) && drone_mode == "OFFBOARD")
{
Command_now.Mode = prometheus_msgs::ControlCommand::Land;
gimbal_control_.pitch = 0.;
gimbal_control_.yaw = 0.;
gimbal_control_.home = 1.;
gimbal_control_pub.publish(gimbal_control_);
file.close();
}
else if (flag_detected == 0)
{
if (drone_mode != "OFFBOARD")
{
ss << "\n ready into offboard ....";
}
else // 判断降落,还是进入目标寻回状态
{
int tmp_cv = 0;
for (int i = 0; i < cv_land_count; i++)
{
tmp_cv += cv_land_cnt[i];
cv_land_cnt[i] = 0;
}
if (tmp_cv == cv_land_count)
{
cv_land = true;
ss << "\n 目标消失前5次连续检测为X, 进入LAND";
comm[0] = 0;
comm[1] = 0;
comm[2] = 0;
comm[3] = 0;
}
else if (count_vision_lost <= 0) // 进入重新寻回目标模式
{
if (curr_pos[2] > max_high)
{
ss << "\n 目标丢失后达到最大高度, 进入LAND";
cv_land = true;
}
else
{
comm[0] = -lost_x_vel;
comm[1] = -lost_y_vel;
comm[2] = -lost_z_vel;
comm[3] = -lost_yaw;
lost_yaw = 0;
lost_x_vel *= object_lost_vel_weaken;
lost_y_vel *= object_lost_vel_weaken;
ss << "\n>> object lost, back << ";
}
}
else
{
// 记录目标丢失时数据
if (count_vision_lost == max_count_vision_lost)
{
lost_x_vel = Command_past.Reference_State.velocity_ref[0];
lost_y_vel = Command_past.Reference_State.velocity_ref[1];
lost_z_vel = Command_past.Reference_State.velocity_ref[2];
lost_z_vel = -std::fmax(std::abs(lost_z_vel), 0.3);
lost_yaw = Command_past.Reference_State.yaw_ref;
}
comm[0] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[0];
comm[1] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[1];
comm[2] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[2];
ss << "\n object lost, use past command ! ";
comm[3] = 0;
// comm[0] = object_lost_vel_weaken;
// comm[1] = object_lost_vel_weaken;
// comm[2] = object_lost_vel_weaken;
count_vision_lost--;
}
}
}
else
{
//如果捕捉到目标
// 重置丢失次数
count_vision_lost = max_count_vision_lost;
switch (check_flag)
{
case 1:
switch (Now_State)
{
case State::Horizontal:
// 如果无人机在接近降落板的正上方时, 锁定吊舱偏航角, 锁定无人机偏航角对齐吊舱
if (std::abs(90 - p) < ignore_error_pitch)
{
if (horizontal_count > 0)
{
horizontal_count--;
}
else
{
// 1. 关闭吊舱跟随
std_srvs::SetBool tmp;
tmp.request.data = true;
gimbal_control_flag_client.call(tmp);
// 2. 锁死patch -90, 锁死yaw
gimbal_control_.pitch = -90.;
gimbal_control_pub.publish(gimbal_control_);
gimbal_control_.pitch = 0.;
// 3. yaw 回到0度
gimbal_control_.yaw = -y;
gimbal_control_pub.publish(gimbal_control_);
// gimbal_control_.yawfollow = 1;
ss << "\n >> yaw lock, patch lock << ";
Now_State = State::Vertical;
}
}
else // 控yaw, xy速度
{
comm[0] = kpx_track * horizontal_distance / std::fmax(std::abs(y) - 5, 1);
// * std::abs(std::cos(-y));
comm[1] = 0;
comm[2] = 0;
comm[3] = -y;
ss << "\n>> horizontal control << ";
}
break;
case State::Vertical:
// 使用真实高度
// 3. xy定点控制无人机, z速度控制
// float offset = std::max(curr_pos[2] * 0.10, 0.10);
float offset = land_move_scale * curr_pos[2];
// 垂直观测范围:39.8°(近焦) 到 4.2°(远焦)。
// tan(53.2/2)=0.50
comm[0] = -(pixel_y / (pic_high * 0.5)) * std::tan(M_PI * (39.8 / 2) / 180) * offset;
// 水平观测范围:53.2°(近焦) 到 5.65°(远焦)。
// tan(53.2/2)=0.50
comm[1] = -(pixel_x / (pic_width * 0.5)) * std::tan(M_PI * (53.2 / 2) / 180) * offset;
// ((480 + 640)/2)/10 = 56
comm[2] = -offset / std::fmax((90 - p) + (std::abs(pixel_y) + std::abs(pixel_x)) / 56, 1);
// comm[2] = -std::fmax(offset, 0.05);
comm[3] = 0.;
ss << "\n >> high control << "
<< " pixel_error_x: " << pixel_x
<< " pixel_error_y: " << pixel_y;
break;
}
break;
case 2:
Eigen::Matrix3d rotation_matrix3;
rotation_matrix3 = Eigen::AngleAxisd(r * M_PI / 180, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(p * M_PI / 180, Eigen::Vector3d::UnitY());
Eigen::Vector3d euler_angles = rotation_matrix3.eulerAngles(2, 1, 0);
y = euler_angles[0] * 180 / M_PI;
y = r > 0 ? y : (y - 180);
horizontal_distance = (std::tan(angle2radians(90 - p)) / std::abs(std::tan(angle2radians(90 - p)))) *
curr_pos[2] *
std::sqrt(std::pow(std::tan(angle2radians(90 - p)), 2) + std::pow(std::tan(angle2radians(r)), 2));
// 靠近时, 不控航向角
if (std::abs(90 - p) < ignore_error_pitch * 2)
{
// -3 吊舱误差偏移
comm[0] = land_move_scale * curr_pos[2] * std::tan(angle2radians(90 - p - 3));
comm[1] = land_move_scale * curr_pos[2] * std::tan(angle2radians(r));
// -3 容忍误差
comm[2] = -land_move_scale * curr_pos[2] / std::fmax(std::abs(std::abs(90 - p) + std::abs(r)) - 3, 1);
comm[3] = 0;
ss << "\n 近距离模式: ";
}
else
{
// comm[0] = kpx_track * horizontal_distance * std::abs(std::cos(angle2radians(y)));
comm[0] = kpx_track * horizontal_distance / std::fmax(std::abs(y) - 5, 1);
comm[1] = 0;
comm[2] = -kpx_track * curr_pos[2] * std::fmin(1 / horizontal_distance * horizontal_distance, 0.1);
comm[3] = y/2;
ss << "\n 远距离模式: ";
}
break;
}
// 视觉降落判断
land_cnt = (land_cnt + 1) % cv_land_count;
cv_land_cnt[land_cnt] = flag_detected == 2 ? 1 : 0;
if (flag_detected == 2 && std::abs(curr_vel[1]) + std::abs(curr_vel[2]) < static_vel_thresh)
{
ss << "\n 检测到X且当前速度被时为静止状态, 进入LAND ";
cv_land = true;
comm[0] = 0;
comm[1] = 0;
comm[2] = 0;
comm[3] = 0;
}
}
ss << "\nyaw: " << y
<< " pitch: " << p
<< " roll: " << r
<< " drone_pitch: " << drone_pitch
<< " high: " << curr_pos[2]
<< " horizontal_distance: " << horizontal_distance;
ss << "\nnow vel >> x_vel: " << curr_vel[0] << " y_vel: " << curr_vel[1] << " z_vel: " << curr_vel[2];
ss << "\ncommand vel >> x_vel: " << comm[0] << " y_vel: " << comm[1] << " z_vel: " << comm[2] << " max_vel: " << max_vel;
for (int i = 0; i < 3; i++)
{
Command_now.Reference_State.velocity_ref[i] = comm[i] == 0 ? 0 : (comm[i] / std::abs(comm[i])) * std::fmin(std::abs(comm[i]), max_vel);
}
Command_now.Reference_State.yaw_rate_ref = comm[3] / 180.0 * M_PI;
command_pub.publish(Command_now);
if (Command_now.Mode == prometheus_msgs::ControlCommand::Land)
break;
Command_past = Command_now;
ss << "\n------------------------------------------------------------------------" << std::endl;
std::cout << ss.str() << std::endl;
file << ss.str();
rate.sleep();
}
cout << "land finish, press any key to exit" << endl;
cin >> check_flag;
return 0;
}
void printf_param()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" << endl;
cout << "default_pitch " << default_pitch << endl;
cout << "land_move_scale " << land_move_scale << endl;
cout << "land_high " << land_high << endl;
cout << "ignore_error_pitch " << ignore_error_pitch << endl;
cout << "max_count_vision_lost " << max_count_vision_lost << endl;
cout << "vel_smooth_scale " << vel_smooth_scale << endl;
cout << "vel_smooth_thresh " << vel_smooth_thresh << endl;
cout << "object_lost_vel_weaken " << object_lost_vel_weaken << endl;
cout << "static_vel_thresh " << static_vel_thresh << endl;
cout << "max_vel " << max_vel << endl;
}

255
src/UAV/Modules/mission/circle_crossing/circle_crossing.cpp
Unescape View File

@ -1,255 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
using namespace std;
# define NODE_NAME "circle_crossing"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
Detection_result ellipse_det;
prometheus_msgs::DroneState _DroneState; //无人机状态量
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
Eigen::Matrix3f R_Body_to_ENU;
ros::Publisher command_pub,message_pub;
int State_Machine = 0;
float kpx_circle_track,kpy_circle_track,kpz_circle_track; //控制参数 - 比例参数
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void tracking();
void crossing();
void return_start_point();
void detection_result_printf(const struct Detection_result& test);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void ellipse_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr& msg)
{
ellipse_det.object_name = "circle";
ellipse_det.Detection_info = *msg;
ellipse_det.pos_body_frame[0] = ellipse_det.Detection_info.position[2] + FRONT_CAMERA_OFFSET_X;
ellipse_det.pos_body_frame[1] = - ellipse_det.Detection_info.position[0] + FRONT_CAMERA_OFFSET_Y;
ellipse_det.pos_body_frame[2] = - ellipse_det.Detection_info.position[1] + FRONT_CAMERA_OFFSET_Z;
ellipse_det.pos_body_enu_frame = R_Body_to_ENU * ellipse_det.pos_body_frame;
if(ellipse_det.Detection_info.detected)
{
ellipse_det.num_regain++;
ellipse_det.num_lost = 0;
}else
{
ellipse_det.num_regain = 0;
ellipse_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(ellipse_det.num_lost > VISION_THRES)
{
ellipse_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(ellipse_det.num_regain > VISION_THRES)
{
ellipse_det.is_detected = true;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "circle_crossing");
ros::NodeHandle nh("~");
//【订阅】图像识别结果，返回的结果为相机坐标系
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
ros::Subscriber ellipse_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/ellipse_det", 10, ellipse_det_cb);
//【订阅】无人机当前状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message", 10);
nh.param<float>("kpx_circle_track", kpx_circle_track, 0.1);
nh.param<float>("kpy_circle_track", kpy_circle_track, 0.1);
nh.param<float>("kpz_circle_track", kpz_circle_track, 0.1);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Circle Crossing Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to takeoff the drone..."<<endl;
cin >> start_flag;
}
// 起飞
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
while( _DroneState.position[2] < 0.3)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Switch to OFFBOARD and arm ....");
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0.0;
Command_Now.Reference_State.position_ref[1] = 0.0;
Command_Now.Reference_State.position_ref[2] = 1.5;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
while (ros::ok())
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>Circle Crossing Mission<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
if(State_Machine == 0)
{
cout << "Please enter 1 to start the mission ..."<<endl;
cin >> start_flag;
if (start_flag == 1)
{
State_Machine = 1;
}else
{
State_Machine = 0;
}
}else if(State_Machine == 1)
{
tracking();
printf_detection_result(ellipse_det);
}else if(State_Machine == 2)
{
crossing();
cout << "Crossing the circle..." << endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Crossing the circle...");
}else if(State_Machine == 3)
{
return_start_point();
cout << "Returning the start point..." << endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Returning the start point...");
}
//回调
ros::spinOnce();
ros::Duration(0.05).sleep();
}
return 0;
}
void tracking()
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0;
Command_Now.Reference_State.position_ref[1] = 0;
Command_Now.Reference_State.position_ref[2] = 0;
Command_Now.Reference_State.velocity_ref[0] = kpx_circle_track * ellipse_det.pos_body_enu_frame[0];
Command_Now.Reference_State.velocity_ref[1] = kpy_circle_track * ellipse_det.pos_body_enu_frame[1];
Command_Now.Reference_State.velocity_ref[2] = kpz_circle_track * ellipse_det.pos_body_enu_frame[2];
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
if(abs(ellipse_det.pos_body_enu_frame[0]) < 1)
{
State_Machine = 2;
}
}
void crossing()
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_Now.Reference_State.position_ref[0] = 2.0;
Command_Now.Reference_State.position_ref[1] = 0;
Command_Now.Reference_State.position_ref[2] = 0;
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
ros::Duration(5.0).sleep();
State_Machine = 3;
}
void return_start_point()
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_Now.Reference_State.position_ref[0] = 0.0;
Command_Now.Reference_State.position_ref[1] = 3.0;
Command_Now.Reference_State.position_ref[2] = 0.0;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
ros::Duration(3.0).sleep();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0.0;
Command_Now.Reference_State.position_ref[1] = 0.0;
Command_Now.Reference_State.position_ref[2] = 1.5;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
ros::Duration(5.0).sleep();
State_Machine = 0;
}

190
src/UAV/Modules/mission/color_line_following/color_line_following.cpp
Unescape View File

@ -1,190 +0,0 @@
/***************************************************************************************************************************
* color_line_following.cpp
*
* Author: Qyp
*
* Update Time: 2020.4.6
*
***************************************************************************************************************************/
//ROS 头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
#include <tf/transform_datatypes.h>
//topic 头文件
#include <prometheus_msgs/DroneState.h>
#include <geometry_msgs/Pose.h>
#include <prometheus_msgs/ControlCommand.h>
#include <nav_msgs/Odometry.h>
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define FOLLOWING_VEL 0.5
#define FOLLOWING_KP 2.0
# define NODE_NAME "color_line_following"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//---------------------------------------Drone---------------------------------------------
float drone_height;
float drone_yaw;
float start_point_x,start_point_y,start_point_z;
//---------------------------------------Vision---------------------------------------------
int flag_detection;
float error_body_y;
float yaw_sp;
float next_desired_yaw;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void color_line_cb(const geometry_msgs::Pose::ConstPtr &msg)
{
error_body_y = - tan(msg->position.x) * start_point_z;
flag_detection = msg->position.y;
float x1 = msg->orientation.w;
float y1 = msg->orientation.x;
float x2 = msg->orientation.y;
float y2 = msg->orientation.z;
next_desired_yaw = - atan2(y2 - y1, x2 - x1);
yaw_sp = (0.7*yaw_sp + 0.3*next_desired_yaw);
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
drone_yaw = msg->attitude[2];
drone_height = msg->position[2];
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "color_line_following");
ros::NodeHandle nh("~");
//节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
//【订阅】所识别线与相机中心轴的偏移角度
ros::Subscriber color_line_sub = nh.subscribe<geometry_msgs::Pose>("/prometheus/object_detection/color_line_angle", 10, color_line_cb);
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
nh.param<float>("start_point_x", start_point_x, 0.0);
nh.param<float>("start_point_y", start_point_y, 0.0);
nh.param<float>("start_point_z", start_point_z, 2.0);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
printf_param();
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>Color Line Following Mission<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to start... "<<endl;
cin >> start_flag;
}
// 起飞
cout<<"[color_line_following]: "<<"Takeoff to predefined position."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Takeoff to predefined position.");
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
while( drone_height < 0.3)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point_x;
Command_Now.Reference_State.position_ref[1] = start_point_y;
Command_Now.Reference_State.position_ref[2] = start_point_z;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
ros::Duration(5.0).sleep();
// 先读取一些飞控的数据
for(int i=0;i<10;i++)
{
ros::spinOnce();
rate.sleep();
}
while (ros::ok())
{
//回调
ros::spinOnce();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::MIX_FRAME;
Command_Now.Reference_State.velocity_ref[0] = FOLLOWING_VEL;
Command_Now.Reference_State.velocity_ref[1] = FOLLOWING_KP * error_body_y;
Command_Now.Reference_State.position_ref[2] = start_point_z;
Command_Now.Reference_State.yaw_ref = yaw_sp; // 增量yaw
command_pub.publish(Command_Now);
printf_result();
rate.sleep();
}
return 0;
}
void printf_result()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>Color Line Following Mission<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "error_body_y : " << error_body_y << " [ m ] "<<endl;
cout << "yaw_from_vision: " << next_desired_yaw/3.1415926 *180 << " [deg] "<<endl;
cout << "yaw_sp : " << yaw_sp/3.1415926 *180 << " [deg] "<<endl;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "start_point_x : "<< start_point_x << endl;
cout << "start_point_y : "<< start_point_y << endl;
cout << "start_point_z : "<< start_point_z << endl;
cout << "FOLLOWING_KP : "<< FOLLOWING_KP << endl;
}

140
src/UAV/Modules/mission/formation/formation_change.cpp
Unescape View File

@ -1,140 +0,0 @@
/*******************************************************************
* 文件名:formation_change.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.14
*
* 介绍:该cpp文件主要为动捕集群中模式切换相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include <iostream>
void formation::init()
{
//创建队形变化数据发布者
ros::param::param<int>("~DIAMOND_intervals", diamond_intervals, 5);
ros::param::param<string>("Location_source", location_source, "gps");
formation_type_pub = n.advertise<prometheus_msgs::Formation>("/prometheus/formation/change", 10);
}
//是否等待函数
void formation::is_wait(int time)
{
//判断是否需要等待,time变量不为0,则等待
if(time != 0)
{
sleep(time);
}
}
//打印集群队形状态函数
void formation::printf_formation_type(std::string type_name)
{
std::cout << std::endl;
std::cout << ">>>>>>>>Formation Type<<<<<<<<" << std::endl;
std::cout << std::endl;
std::cout << " " << type_name << " " << std::endl;
std::cout << std::endl;
std::cout << "------------------------------" << std::endl;
std::cout << std::endl;
}
void formation::change()
{
//初始化,创建队形变换信息发布者
init();
//创建获取用户输入值的变量
int type_last;
int type_now;
while(ros::ok())
{
//打印提示信息
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< " << std::endl;
std::cout << "Input the formation type: 0 for horizontal formation, 1 for triangle formation, 2 for diamond formation" << std::endl;
//获取用户输入的值
std::cin >> type_now;
//判断值是否正确,正确则进入正常流程,错误则打印警告信息
if((type_now >= 0) && (type_now <= 2))
{
switch(type_now)
{
//一字队形
case 0:
//判断当前队形是否为菱形队形,是菱形则先进入过渡队形
if(type_last == prometheus_msgs::Formation::DIAMOND)
{
formation_data.type = prometheus_msgs::Formation::DIAMOND_STAGE_1;
formation_type_pub.publish(formation_data);
is_wait(diamond_intervals);
}
//切换为一字队形
formation_data.type = prometheus_msgs::Formation::HORIZONTAL;
formation_type_pub.publish(formation_data);
printf_formation_type("Horizontal");
break;
//三角队形
case 1:
//判断当前队形是否为菱形队形,是菱形则先进入过渡队形
if(type_last == prometheus_msgs::Formation::DIAMOND)
{
formation_data.type = prometheus_msgs::Formation::DIAMOND_STAGE_1;
formation_type_pub.publish(formation_data);
is_wait(diamond_intervals);
}
//切换为三角队形
formation_data.type = prometheus_msgs::Formation::TRIANGEL;
formation_type_pub.publish(formation_data);
printf_formation_type("Triangle");
break;
//菱形队形
case 2:
if(location_source == "mocap")
{
//判断当前队形是否为菱形队形,是菱形队形则直接跳出当前队形
if(type_last == type_now)
{
break;
}
//进入过渡队形
formation_data.type = prometheus_msgs::Formation::DIAMOND_STAGE_1;
formation_type_pub.publish(formation_data);
is_wait(diamond_intervals);
//切换为菱形队形
formation_data.type = prometheus_msgs::Formation::DIAMOND;
formation_type_pub.publish(formation_data);
printf_formation_type("Diamond");
break;
}
if(location_source == "uwb" || location_source == "gps")
{
ROS_WARN("not support diamond formation");
type_now = type_last;
break;
}
}
//把当前集群队形变量赋值给上一集群队形变量
type_last = type_now;
}
else
{
type_now = type_last;
//输入错误
ROS_WARN("inpunt error, please input again");
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation_change");
formation Formation;
Formation.change();
return 0;
}

449
src/UAV/Modules/mission/formation/formation_control.cpp
Unescape View File

@ -1,449 +0,0 @@
/*******************************************************************
* 文件名:mocap_formation_control.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.14
*
* 介绍:该cpp文件主要为动捕集群中集群控制相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include "command_to_mavros.h"
#include <unistd.h>
void formation::init()
{
//控制命令数据订阅者
cmd_sub = n.subscribe("/prometheus/control_command", 10, &formation::ControlCallBack, this);
//集群队形数据订阅者
formation_type_sub = n.subscribe("/prometheus/formation/change", 10, &formation::FormationChangeCallBack, this);
//集群五台飞机位置控制数据发布者
uav1_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav1/mavros/setpoint_raw/local", 10);
uav2_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav2/mavros/setpoint_raw/local", 10);
uav3_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav3/mavros/setpoint_raw/local", 10);
uav4_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav4/mavros/setpoint_raw/local", 10);
uav5_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav5/mavros/setpoint_raw/local", 10);
//获取集群X轴间隔距离参数
ros::param::param<double>("~FORMATION_DISTANCE_x", formation_distance_x, 1);
//获取集群Y轴间隔距离参数
ros::param::param<double>("~FORMATION_DISTANCE_y", formation_distance_y, 2);
//获取定位来源
ros::param::param<string>("Location_source", location_source, "fps");
//获取飞控系统
ros::param::param<string>("Flight_controller", flight_controller, "spm");
//获取仿真中位置差值
ros::param::param<double>("uav1_x", uav1_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav1_y", uav1_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav2_x", uav2_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav2_y", uav2_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav3_x", uav3_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav3_y", uav3_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav4_x", uav4_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav4_y", uav4_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav5_x", uav5_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav5_y", uav5_gazebo_offset_pose[1], 0);
//获取是否为仿真参数
ros::param::param<bool>("~sim",sim,false);
check_param();
//初始队形设置为一字形
formation_data.type = prometheus_msgs::Formation::HORIZONTAL;
//设置程序初始时间
begin_time = ros::Time::now();
}
bool formation::check_param()
{
char tmp[100];
strncpy(tmp,location_source.c_str(),location_source.length() + 1);
if((location_source == "gps" || location_source == "uwb") || location_source == "mocap")
{
ROS_INFO("Location source is [%s]\n", tmp);
}
else
{
ROS_ERROR("param [Location_source] value error, value is [%s], not gps, uwb or mocap\n", tmp);
ros::shutdown();
}
strncpy(tmp,flight_controller.c_str(),flight_controller.length() + 1);
if(flight_controller == "apm" || flight_controller == "px4")
{
ROS_INFO("Flight_controller is [%s]\n", tmp);
}
else
{
ROS_ERROR("param [flight_controller] value error, value is [%s], not apm or px4\n", tmp);
ros::shutdown();
}
}
//集群位置控制发布函数
void formation::formation_pos_pub()
{
uav1_local_pub.publish(uav1_desired_pose);
uav2_local_pub.publish(uav2_desired_pose);
uav3_local_pub.publish(uav3_desired_pose);
uav4_local_pub.publish(uav4_desired_pose);
uav5_local_pub.publish(uav5_desired_pose);
}
//获取单台无人机控制数据
void formation::get_uav_cmd(Eigen::Vector3d offset_pose, mavros_msgs::PositionTarget& desired_pose)
{
desired_pose.header.stamp = ros::Time::now();
desired_pose.type_mask = 0b100111111000;
desired_pose.coordinate_frame = 1;
desired_pose.position.x = control_data.Reference_State.position_ref[0] + offset_pose[0];
desired_pose.position.y = control_data.Reference_State.position_ref[1] + offset_pose[1];
desired_pose.position.z = control_data.Reference_State.position_ref[2] + offset_pose[2];
desired_pose.yaw = control_data.Reference_State.yaw_ref;
}
void formation::is_sim()
{
if(!sim)
{
uav1_gazebo_offset_pose[0] = 0;
uav1_gazebo_offset_pose[1] = 0;
uav2_gazebo_offset_pose[0] = 0;
uav2_gazebo_offset_pose[1] = 0;
uav3_gazebo_offset_pose[0] = 0;
uav3_gazebo_offset_pose[1] = 0;
uav4_gazebo_offset_pose[0] = 0;
uav4_gazebo_offset_pose[1] = 0;
uav5_gazebo_offset_pose[0] = 0;
uav5_gazebo_offset_pose[1] = 0;
}
}
//设置一字队形
void formation::set_horizontal()
{
if(location_source == "mocap")
{
//1号机
uav1_offset_pose[0] = 0 - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 2 * formation_distance_y - uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = 0 - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = formation_distance_y - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = 0 - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = -formation_distance_y - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = -2 * formation_distance_y - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
else
{
if(location_source == "uwb" || location_source == "gps")
{
//1号机
uav1_offset_pose[0] = 0 - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 0- uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = 0 - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = 0 - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = 0 - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = 0 - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = 0 - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
}
}
//设置三角队形
void formation::set_triangle()
{
if(location_source == "mocap")
{
//1号机
uav1_offset_pose[0] = 2 * formation_distance_x - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 2 * formation_distance_y - uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = formation_distance_x - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = formation_distance_y - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = formation_distance_x - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = -formation_distance_y - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = -2 * formation_distance_y - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
if(location_source == "uwb" || location_source == "gps")
{
//1号机
uav1_offset_pose[0] = 2 * formation_distance_x - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 0 - uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = formation_distance_x - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = 0 - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = formation_distance_x - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = 0 - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = 0 - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
}
//设置菱形队形
void formation::set_diamond()
{
if(location_source == "mocap")
{
//1号机
uav1_offset_pose[0] = 0 - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 2 * formation_distance_y - uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = 2 * formation_distance_x - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = 0 - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = -2 * formation_distance_x - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = 0 - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = -2 * formation_distance_y - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
else
{
if(location_source == "gps" || location_source == "uwb")
{
ROS_WARN("gps or uwb location not support diamond formation");
}
}
}
//设置菱形队形相关过渡队形
void formation::set_diamond_stage1()
{
//1号机
uav1_offset_pose[0] = 0 - uav1_gazebo_offset_pose[0];
uav1_offset_pose[1] = 0 - uav1_gazebo_offset_pose[1];
uav1_offset_pose[2] = 0;
//2号机
uav2_offset_pose[0] = 0 - uav2_gazebo_offset_pose[0];
uav2_offset_pose[1] = 2 * formation_distance_y - uav2_gazebo_offset_pose[1];
uav2_offset_pose[2] = 0;
//3号机
uav3_offset_pose[0] = 2 * formation_distance_x - uav3_gazebo_offset_pose[0];
uav3_offset_pose[1] = formation_distance_y - uav3_gazebo_offset_pose[1];
uav3_offset_pose[2] = 0;
//4号机
uav4_offset_pose[0] = -2 * formation_distance_x - uav4_gazebo_offset_pose[0];
uav4_offset_pose[1] = -formation_distance_y - uav4_gazebo_offset_pose[1];
uav4_offset_pose[2] = 0;
//5号机
uav5_offset_pose[0] = 0 - uav5_gazebo_offset_pose[0];
uav5_offset_pose[1] = -2 * formation_distance_y - uav5_gazebo_offset_pose[1];
uav5_offset_pose[2] = 0;
}
//集群控制函数
void formation::control()
{
//初始化
init();
is_sim();
if(flight_controller == "px4")
{
while(ros::ok())
{
//处理回调函数
ros::spinOnce();
//获取集群队形
switch(formation_data.type)
{
//设置为一字队形
case prometheus_msgs::Formation::HORIZONTAL:
set_horizontal();
break;
//设置为三角队形
case prometheus_msgs::Formation::TRIANGEL:
set_triangle();
break;
//设置为菱形队形过渡队形
case prometheus_msgs::Formation::DIAMOND_STAGE_1:
set_diamond_stage1();
break;
//设置为菱形队形
case prometheus_msgs::Formation::DIAMOND:
set_diamond();
break;
}
//五台无人机获取控制数据
get_uav_cmd(uav1_offset_pose, uav1_desired_pose);
get_uav_cmd(uav2_offset_pose, uav2_desired_pose);
get_uav_cmd(uav3_offset_pose, uav3_desired_pose);
get_uav_cmd(uav4_offset_pose, uav4_desired_pose);
get_uav_cmd(uav5_offset_pose, uav5_desired_pose);
formation_pos_pub();
//等待0.1秒
usleep(100000);
}
}
else
{
if(flight_controller == "apm")
{
//起飞后开始控制无人机集群飞行
ROS_INFO("When the drone formation takeoff and the control command input,enter 1 to start formation control");
int check_flag;
std::cin >> check_flag;
if(check_flag != 1)
{
ros::shutdown();
}
while(ros::ok())
{
//处理回调函数
ros::spinOnce();
//获取集群队形
switch(formation_data.type)
{
//设置为一字队形
case prometheus_msgs::Formation::HORIZONTAL:
set_horizontal();
break;
//设置为三角队形
case prometheus_msgs::Formation::TRIANGEL:
set_triangle();
break;
//设置为菱形队形过渡队形
case prometheus_msgs::Formation::DIAMOND_STAGE_1:
set_diamond_stage1();
break;
}
//五台无人机获取控制数据
get_uav_cmd(uav1_offset_pose, uav1_desired_pose);
get_uav_cmd(uav2_offset_pose, uav2_desired_pose);
get_uav_cmd(uav3_offset_pose, uav3_desired_pose);
get_uav_cmd(uav4_offset_pose, uav4_desired_pose);
get_uav_cmd(uav5_offset_pose, uav5_desired_pose);
formation_pos_pub();
//等待0.1秒
usleep(100000);
}
}
}
}
//获取无人机控制指令
void formation::ControlCallBack(const prometheus_msgs::ControlCommandConstPtr& control_msgs)
{
control_data = *control_msgs;
}
//获取无人机集群队形指令
void formation::FormationChangeCallBack(const prometheus_msgs::FormationConstPtr& change_msgs)
{
formation_data.type = change_msgs->type;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation_control");
formation Formation;
Formation.control();
return 0;
}

91
src/UAV/Modules/mission/formation/formation_getpose.cpp
Unescape View File

@ -1,91 +0,0 @@
/*******************************************************************
* 文件名:formation_move.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.16
*
* 介绍:该cpp文件主要为动捕集群中获取动捕系统位置数据并转发到飞控相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include <unistd.h>
void formation::init()
{
//动捕系统位置数据订阅者
uav1_mocap_pose_sub = n.subscribe("/mocap/uav1/pose", 10, &formation::Uav1MocapPoseCallBack, this);
uav2_mocap_pose_sub = n.subscribe("/mocap/uav2/pose", 10, &formation::Uav2MocapPoseCallBack, this);
uav3_mocap_pose_sub = n.subscribe("/mocap/uav3/pose", 10, &formation::Uav3MocapPoseCallBack, this);
uav4_mocap_pose_sub = n.subscribe("/mocap/uav4/pose", 10, &formation::Uav4MocapPoseCallBack, this);
uav5_mocap_pose_sub = n.subscribe("/mocap/uav5/pose", 10, &formation::Uav5MocapPoseCallBack, this);
//动捕系统位置数据发布者
uav1_mocap_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/uav1/mavros/vision_pose/pose", 10);
uav2_mocap_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/uav2/mavros/vision_pose/pose", 10);
uav3_mocap_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/uav3/mavros/vision_pose/pose", 10);
uav4_mocap_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/uav4/mavros/vision_pose/pose", 10);
uav5_mocap_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/uav5/mavros/vision_pose/pose", 10);
}
void formation::getpose()
{
//初始化
init();
while(ros::ok())
{
//处理回调函数,
ros::spinOnce();
//打上时间戳后,发布位置数据到飞控
uav1_current_pose.header.stamp = ros::Time::now();
uav2_current_pose.header.stamp = ros::Time::now();
uav3_current_pose.header.stamp = ros::Time::now();
uav4_current_pose.header.stamp = ros::Time::now();
uav5_current_pose.header.stamp = ros::Time::now();
uav1_mocap_pose_pub.publish(uav1_current_pose);
uav2_mocap_pose_pub.publish(uav2_current_pose);
uav3_mocap_pose_pub.publish(uav3_current_pose);
uav4_mocap_pose_pub.publish(uav4_current_pose);
uav5_mocap_pose_pub.publish(uav5_current_pose);
usleep(10000);
}
}
//获取动捕系统下的五台无人机位置数据
//1号机
void formation::Uav1MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav1_current_pose.pose = pose_msgs->pose;
}
//2号机
void formation::Uav2MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav2_current_pose.pose = pose_msgs->pose;
}
//3号机
void formation::Uav3MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav3_current_pose.pose = pose_msgs->pose;
}
//4号机
void formation::Uav4MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav4_current_pose.pose = pose_msgs->pose;
}
//5号机
void formation::Uav5MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav5_current_pose.pose = pose_msgs->pose;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation/mocap/get_pose");
formation Formation;
Formation.getpose();
return 0;
}

86
src/UAV/Modules/mission/formation/formation_move.cpp
Unescape View File

@ -1,86 +0,0 @@
/*******************************************************************
* 文件名:formation_move.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.16
*
* 介绍:该cpp文件主要为动捕集群中终端输入控制命令相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include "unistd.h"
void formation::init()
{
//创建控制命令发布者
cmd_pub = n.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
}
void formation::move()
{
//初始化
init();
while(ros::ok())
{
//打印提示语句,获取用户输入的控制命令
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< " << std::endl;
std::cout << "Please input control command in LOCAL_FRAME: [x]m [y]m [z]m [yaw]radian" << std::endl;
//X轴坐标,单位米
std::cout << "input [x]_m" << std::endl;
std::cin >> control_data.Reference_State.position_ref[0];
//Y轴坐标,单位米
std::cout << "input [y]_m" << std::endl;
std::cin >> control_data.Reference_State.position_ref[1];
//Z轴坐标,单位米
std::cout << "input [z]_m" << std::endl;
//偏航,单位度
std::cin >> control_data.Reference_State.position_ref[2];
std::cout << "input [yaw]_radian" << std::endl;
std::cin >> control_data.Reference_State.yaw_ref;
std::cout << "---------------------" << std::endl;
std::cout << "control command: " << std::endl
<< "x is [" << control_data.Reference_State.position_ref[0] << "]" << std::endl
<< "y is [" << control_data.Reference_State.position_ref[1] << "]" << std::endl
<< "z is [" << control_data.Reference_State.position_ref[2] << "]" << std::endl
<< "yaw is [" << control_data.Reference_State.yaw_ref << "]" << std::endl;
std::cout << "---------------------" << std::endl;
sleep(1);
//检查确认控制命令是否正确
std::cout << "Please check whether the control command is correct, input 0 for continue, input 1 for input again" << std::endl;
int check_flag;
std::cin >> check_flag;
if(check_flag == 1)
{
//打印重新输入的提示信息
ROS_WARN("Input error , input again");
std::cout << std::endl;
sleep(1);
//退出当前循环
continue;
}
if(check_flag == 0)
{
//将控制命令的其他变量进行填充
control_data.header.stamp = ros::Time::now();
control_data.Mode = prometheus_msgs::ControlCommand::Move;
control_data.source = "formation_move";
control_data.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
//发布控制命令数据
cmd_pub.publish(control_data);
ROS_INFO("Control command sent");
std::cout << std::endl;
}
}
}
int main(int argc, char** argv)
{
//ROS初始化
ros::init(argc, argv, "formation_move");
//创建类并执行move函数
formation Formation;
Formation.move();
return 0;
}

517
src/UAV/Modules/mission/formation/formation_setmode.cpp
Unescape View File

@ -1,517 +0,0 @@
/*******************************************************************
* 文件名:formation_setmode.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.14
*
* 介绍:该cpp文件主要为动捕集群中模式切换相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include <unistd.h>
#include <iostream>
void formation::init()
{
//获取相关参数
ros::param::param<int>("~OFFBOARD_intervals", offboard_intervals, 3);
ros::param::param<int>("~LAND_intervals", land_intervals, 3);
ros::param::param<string>("Flight_controller", flight_controller, "spm");
ros::param::param<double>("~Takeoff_height", takeoff_height, 1.0);
}
void formation::is_wait(int time)
{
//判断是否需要等待,time变量不为0,则等待
if(time != 0)
{
sleep(time);
}
}
void formation::set_formation_px4_offboard()
{
//创建1~5号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
command_to_mavros ctm5("uav5");
//设置1~5号机模式变量为offboard,解上锁变量为解锁
ctm1.arm_cmd.request.value = true;
ctm2.arm_cmd.request.value = true;
ctm3.arm_cmd.request.value = true;
ctm4.arm_cmd.request.value = true;
ctm5.arm_cmd.request.value = true;
ctm1.mode_cmd.request.custom_mode = "OFFBOARD";
ctm2.mode_cmd.request.custom_mode = "OFFBOARD";
ctm3.mode_cmd.request.custom_mode = "OFFBOARD";
ctm4.mode_cmd.request.custom_mode = "OFFBOARD";
ctm5.mode_cmd.request.custom_mode = "OFFBOARD";
//集群按照23145的顺序对五台无人机分别解锁并切入offboard模式
//当有一台无人机解锁或者切入offboard模式失败,该函数返回false
//五台无人机成功解锁并切入offboard模式,该函数返回true
if(ctm2.arming_client.call(ctm2.arm_cmd) && ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav2 armed and set offboard mode failed");
}
if(ctm3.arming_client.call(ctm3.arm_cmd) && ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav3 armed and set offboard mode failed");
}
if(ctm1.arming_client.call(ctm1.arm_cmd) && ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav1 armed and set offboard mode failed");
}
if(ctm4.arming_client.call(ctm4.arm_cmd) && ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav4 armed and set offboard mode failed");
}
if(ctm5.arming_client.call(ctm5.arm_cmd) && ctm5.set_mode_client.call(ctm5.mode_cmd))
{
ROS_INFO("uav5 armed and set offboard mode success");
}
else
{
ROS_ERROR("uav5 armed and set offboard mode failed");
}
}
void formation::set_formation_apm_guided()
{
//创建1~5号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
command_to_mavros ctm5("uav5");
//设置1~5号机模式变量为offboard,解上锁变量为解锁以及设置起飞变量高度
ctm1.arm_cmd.request.value = true;
ctm2.arm_cmd.request.value = true;
ctm3.arm_cmd.request.value = true;
ctm4.arm_cmd.request.value = true;
ctm5.arm_cmd.request.value = true;
ctm1.mode_cmd.request.custom_mode = "GUIDED";
ctm2.mode_cmd.request.custom_mode = "GUIDED";
ctm3.mode_cmd.request.custom_mode = "GUIDED";
ctm4.mode_cmd.request.custom_mode = "GUIDED";
ctm5.mode_cmd.request.custom_mode = "GUIDED";
uav1_takeoff_cmd.request.altitude = takeoff_height;
uav2_takeoff_cmd.request.altitude = takeoff_height;
uav3_takeoff_cmd.request.altitude = takeoff_height;
uav4_takeoff_cmd.request.altitude = takeoff_height;
uav5_takeoff_cmd.request.altitude = takeoff_height;
if(ctm2.arming_client.call(ctm2.arm_cmd) && ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 armed and set guided mode success");
sleep(1);
if(uav2_takeoff_client.call(uav2_takeoff_cmd))
{
ROS_INFO("uav2 takeoff success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav2 takeoff failed");
}
}
else
{
ROS_ERROR("uav2 armed and set guided mode failed");
}
if(ctm3.arming_client.call(ctm3.arm_cmd) && ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 armed and set guided mode success");
sleep(1);
if(uav3_takeoff_client.call(uav3_takeoff_cmd))
{
ROS_INFO("uav3 takeoff success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav3 takeoff failed");
}
}
else
{
ROS_ERROR("uav3 armed and set guided mode failed");
}
if(ctm1.arming_client.call(ctm1.arm_cmd) && ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 armed and set guided mode success");
sleep(1);
if(uav1_takeoff_client.call(uav1_takeoff_cmd))
{
ROS_INFO("uav1 takeoff success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav1 takeoff failed");
}
}
else
{
ROS_ERROR("uav3 armed and set guided mode failed");
}
if(ctm4.arming_client.call(ctm4.arm_cmd) && ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 armed and set guided mode success");
sleep(1);
if(uav4_takeoff_client.call(uav4_takeoff_cmd))
{
ROS_INFO("uav4 takeoff success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav4 takeoff failed");
}
}
else
{
ROS_ERROR("uav4 armed and set guided mode failed");
}
if(ctm5.arming_client.call(ctm5.arm_cmd) && ctm5.set_mode_client.call(ctm5.mode_cmd))
{
ROS_INFO("uav5 armed and set guided mode success");
sleep(1);
if(uav5_takeoff_client.call(uav5_takeoff_cmd))
{
ROS_INFO("uav5 takeoff success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav5 takeoff failed");
}
}
else
{
ROS_ERROR("uav5 armed and set guided mode failed");
}
}
void formation::set_formation_px4_land()
{
//创建1~5号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
command_to_mavros ctm5("uav5");
//设置1~5号机模式变量为land
ctm1.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm2.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm3.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm4.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm5.mode_cmd.request.custom_mode = "AUTO.LAND";
//切换为land模式,并对结果进行打印
if(ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav2 set land mode failed");
}
if(ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav3 set land mode failed");
}
if(ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav1 set land mode failed");
}
if(ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav4 set land mode success");
}
if(ctm5.set_mode_client.call(ctm5.mode_cmd))
{
ROS_INFO("uav5 set land mode success");
}
else
{
ROS_ERROR("uav5 set land mode failed");
}
}
void formation::set_formation_apm_land()
{
//创建1~5号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
command_to_mavros ctm5("uav5");
//设置1~5号机模式变量为land
ctm1.mode_cmd.request.custom_mode = "LAND";
ctm2.mode_cmd.request.custom_mode = "LAND";
ctm3.mode_cmd.request.custom_mode = "LAND";
ctm4.mode_cmd.request.custom_mode = "LAND";
ctm5.mode_cmd.request.custom_mode = "LAND";
//切换为land模式,并对结果进行打印
if(ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav2 set land mode failed");
}
if(ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav3 set land mode failed");
}
if(ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav1 set land mode failed");
}
if(ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav4 set land mode success");
}
if(ctm5.set_mode_client.call(ctm5.mode_cmd))
{
ROS_INFO("uav5 set land mode success");
}
else
{
ROS_ERROR("uav5 set land mode failed");
}
}
void formation::set_formation_disarmed()
{
//创建1~5号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
command_to_mavros ctm5("uav5");
//设置1~5号机解上锁变量为false
ctm1.arm_cmd.request.value = false;
ctm1.arm_cmd.request.value = false;
ctm1.arm_cmd.request.value = false;
ctm1.arm_cmd.request.value = false;
ctm1.arm_cmd.request.value = false;
//按照23145的顺序调用上锁服务,并根据结果打印相关提示信息
if(ctm2.arming_client.call(ctm2.arm_cmd))
{
ROS_INFO("uav2 disarmed success");
}
else
{
ROS_ERROR("uav2 disarmed failed");
}
if(ctm3.arming_client.call(ctm3.arm_cmd))
{
ROS_INFO("uav3 disarmed success");
}
else
{
ROS_ERROR("uav3 disarmed failed");
}
if(ctm1.arming_client.call(ctm1.arm_cmd))
{
ROS_INFO("uav1 disarmed success");
}
else
{
ROS_ERROR("uav1 disarmed failed");
}
if(ctm4.arming_client.call(ctm4.arm_cmd))
{
ROS_INFO("uav4 disarmed success");
}
else
{
ROS_ERROR("uav4 disarmed failed");
}
if(ctm5.arming_client.call(ctm5.arm_cmd))
{
ROS_INFO("uav5 disarmed success");
}
else
{
ROS_ERROR("uav5 disarmed failed");
}
}
void formation::set_mode()
{
//初始化,创建服务调用后客户端以及获取参数
init();
while(ros::ok())
{
//创建变量用以获取用户输入的值
int mode;
//打印提示信息
if(flight_controller == "px4")
{
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< " << std::endl;
std::cout << "Input the drone state: 0 for armed and offboard, 1 for land, 2 for disarmed" << std::endl;
//获取用户输入的值
std::cin >> formation_mode;
switch(formation_mode)
{
//offboard模式
case 0:
set_formation_px4_offboard();
break;
//land模式
case 1:
set_formation_px4_land();
break;
//上锁
case 2:
set_formation_disarmed();
break;
}
}
else
{
if(flight_controller == "apm")
{
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< " << std::endl;
std::cout << "Input the drone state: 0 for armed guided and takeoff, 1 for land, 2 for disarmed" << std::endl;
//获取用户输入的值
std::cin >> formation_mode;
switch(formation_mode)
{
//guided模式
case 0:
set_formation_apm_guided();
break;
//land模式
case 1:
set_formation_apm_land();
break;
//上锁
case 2:
set_formation_disarmed();
break;
}
}
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation_set_mode");
formation Formation;
Formation.set_mode();
return 0;
}

507
src/UAV/Modules/mission/formation/formation_square.cpp
Unescape View File

@ -1,507 +0,0 @@
/*******************************************************************
* 文件名:mocap_formation_control.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.11.20
*
* 介绍:该cpp文件主要为动捕集群中四机绕圈相关函数的实现以及程序的运行
* ****************************************************************/
#include "ros/ros.h"
#include "Formation.h"
#include <unistd.h>
#include <Eigen/Eigen>
#include <mavros_msgs/PositionTarget.h>
void formation::init()
{
//集群四台飞机位置控制数据发布者
uav1_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav1/mavros/setpoint_raw/local", 10);
uav2_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav2/mavros/setpoint_raw/local", 10);
uav3_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav3/mavros/setpoint_raw/local", 10);
uav4_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav4/mavros/setpoint_raw/local", 10);
//读取相关参数
ros::param::param<double>("~square_length", square_length, 4);
ros::param::param<double>("~13_height", point13_height, 1);
ros::param::param<double>("~24_height", point24_height, 0.5);
ros::param::param<double>("~hold_time", hold_time, 10);
ros::param::param<double>("~stage1_time", stage1_time, 5);
ros::param::param<int>("~LAND_intervals", land_intervals, 3);
ros::param::param<bool>("~sim", sim, false);
ros::param::param<double>("uav1_x", uav1_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav1_y", uav1_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav2_x", uav2_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav2_y", uav2_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav3_x", uav3_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav3_y", uav3_gazebo_offset_pose[1], 0);
ros::param::param<double>("uav4_x", uav4_gazebo_offset_pose[0], 0);
ros::param::param<double>("uav4_y", uav4_gazebo_offset_pose[1], 0);
}
void formation::is_sim()
{
if(!sim)
{
uav1_gazebo_offset_pose[0] = 0;
uav1_gazebo_offset_pose[1] = 0;
uav2_gazebo_offset_pose[0] = 0;
uav2_gazebo_offset_pose[1] = 0;
uav3_gazebo_offset_pose[0] = 0;
uav3_gazebo_offset_pose[1] = 0;
uav4_gazebo_offset_pose[0] = 0;
uav4_gazebo_offset_pose[1] = 0;
}
}
void formation::square()
{
init();
is_sim();
while(ros::ok())
{
std::cout << "Please input 1 start square formation" << std::endl;
int start_flag;
std::cin >> start_flag;
if(start_flag == 1)
{
//阶段1
int stage = 1;
if(stage == 1)
{
int count = 0;
int num = (stage1_time + hold_time) * 10;
uav1_desired_pose.type_mask = 0b100111111000;
uav1_desired_pose.coordinate_frame = 1;
uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
uav1_desired_pose.position.z = point13_height;
uav2_desired_pose.type_mask = 0b100111111000;
uav2_desired_pose.coordinate_frame = 1;
uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
uav2_desired_pose.position.z = point24_height;
uav3_desired_pose.type_mask = 0b100111111000;
uav3_desired_pose.coordinate_frame = 1;
uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
uav3_desired_pose.position.z = point13_height;
uav4_desired_pose.type_mask = 0b100111111000;
uav4_desired_pose.coordinate_frame = 1;
uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
uav4_desired_pose.position.z = point24_height;
while(ros::ok())
{
uav1_desired_pose.header.stamp = ros::Time::now();
uav1_local_pub.publish(uav1_desired_pose);
uav2_desired_pose.header.stamp = ros::Time::now();
uav2_local_pub.publish(uav2_desired_pose);
uav3_desired_pose.header.stamp = ros::Time::now();
uav3_local_pub.publish(uav3_desired_pose);
uav4_desired_pose.header.stamp = ros::Time::now();
uav4_local_pub.publish(uav4_desired_pose);
if(count == 0)
{
set_formation_px4_offboard();
}
count++;
if(count >= num)
{
stage = 2;
break;
}
ROS_INFO("go to point 1");
usleep(100000);
}
}
if(stage == 2)
{
int count = 0;
int num = hold_time * 10;
uav1_desired_pose.type_mask = 0b100111111000;
uav1_desired_pose.coordinate_frame = 1;
uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
uav1_desired_pose.position.y = -square_length/2 - uav1_gazebo_offset_pose[1];
uav1_desired_pose.position.z = point24_height;
uav2_desired_pose.type_mask = 0b100111111000;
uav2_desired_pose.coordinate_frame = 1;
uav2_desired_pose.position.x = -square_length/2 - uav2_gazebo_offset_pose[0];
uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
uav2_desired_pose.position.z = point13_height;
uav3_desired_pose.type_mask = 0b100111111000;
uav3_desired_pose.coordinate_frame = 1;
uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
uav3_desired_pose.position.y = square_length/2 - uav3_gazebo_offset_pose[1];
uav3_desired_pose.position.z = point24_height;
uav4_desired_pose.type_mask = 0b100111111000;
uav4_desired_pose.coordinate_frame = 1;
uav4_desired_pose.position.x = square_length/2 - uav4_gazebo_offset_pose[0];
uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
uav4_desired_pose.position.z = point13_height;
while(ros::ok())
{
uav1_desired_pose.header.stamp = ros::Time::now();
uav1_local_pub.publish(uav1_desired_pose);
uav2_desired_pose.header.stamp = ros::Time::now();
uav2_local_pub.publish(uav2_desired_pose);
uav3_desired_pose.header.stamp = ros::Time::now();
uav3_local_pub.publish(uav3_desired_pose);
uav4_desired_pose.header.stamp = ros::Time::now();
uav4_local_pub.publish(uav4_desired_pose);
count++;
if(count >= num)
{
stage = 3;
break;
}
ROS_INFO("go to point 2");
usleep(100000);
}
}
if(stage == 3)
{
int count = 0;
int num = hold_time * 10;
uav1_desired_pose.type_mask = 0b100111111000;
uav1_desired_pose.coordinate_frame = 1;
uav1_desired_pose.position.x = -square_length/2 - uav1_gazebo_offset_pose[0];
uav1_desired_pose.position.y = -square_length/2 - uav1_gazebo_offset_pose[1];
uav1_desired_pose.position.z = point13_height;
uav2_desired_pose.type_mask = 0b100111111000;
uav2_desired_pose.coordinate_frame = 1;
uav2_desired_pose.position.x = -square_length/2 - uav2_gazebo_offset_pose[0];
uav2_desired_pose.position.y = square_length/2 - uav2_gazebo_offset_pose[1];
uav2_desired_pose.position.z = point24_height;
uav3_desired_pose.type_mask = 0b100111111000;
uav3_desired_pose.coordinate_frame = 1;
uav3_desired_pose.position.x = square_length/2 - uav3_gazebo_offset_pose[0];
uav3_desired_pose.position.y = square_length/2 - uav3_gazebo_offset_pose[1];
uav3_desired_pose.position.z = point13_height;
uav4_desired_pose.type_mask = 0b100111111000;
uav4_desired_pose.coordinate_frame = 1;
uav4_desired_pose.position.x = square_length/2 - uav4_gazebo_offset_pose[0];
uav4_desired_pose.position.y = -square_length/2 - uav4_gazebo_offset_pose[1];
uav4_desired_pose.position.z = point24_height;
while(ros::ok())
{
uav1_desired_pose.header.stamp = ros::Time::now();
uav1_local_pub.publish(uav1_desired_pose);
uav2_desired_pose.header.stamp = ros::Time::now();
uav2_local_pub.publish(uav2_desired_pose);
uav3_desired_pose.header.stamp = ros::Time::now();
uav3_local_pub.publish(uav3_desired_pose);
uav4_desired_pose.header.stamp = ros::Time::now();
uav4_local_pub.publish(uav4_desired_pose);
count++;
if(count >= num)
{
stage = 4;
break;
}
ROS_INFO("go to point 3");
usleep(100000);
}
}
if(stage == 4)
{
int count = 0;
int num = hold_time * 10;
uav1_desired_pose.type_mask = 0b100111111000;
uav1_desired_pose.coordinate_frame = 1;
uav1_desired_pose.position.x = -square_length/2 - uav1_gazebo_offset_pose[0];
uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
uav1_desired_pose.position.z = point24_height;
uav2_desired_pose.type_mask = 0b100111111000;
uav2_desired_pose.coordinate_frame = 1;
uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
uav2_desired_pose.position.y = square_length/2 - uav2_gazebo_offset_pose[1];
uav2_desired_pose.position.z = point13_height;
uav3_desired_pose.type_mask = 0b100111111000;
uav3_desired_pose.coordinate_frame = 1;
uav3_desired_pose.position.x = square_length/2 - uav3_gazebo_offset_pose[0];
uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
uav3_desired_pose.position.z = point24_height;
uav4_desired_pose.type_mask = 0b100111111000;
uav4_desired_pose.coordinate_frame = 1;
uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
uav4_desired_pose.position.y = -square_length/2 - uav4_gazebo_offset_pose[1];
uav4_desired_pose.position.z = point13_height;
while(ros::ok())
{
uav1_desired_pose.header.stamp = ros::Time::now();
uav1_local_pub.publish(uav1_desired_pose);
uav2_desired_pose.header.stamp = ros::Time::now();
uav2_local_pub.publish(uav2_desired_pose);
uav3_desired_pose.header.stamp = ros::Time::now();
uav3_local_pub.publish(uav3_desired_pose);
uav4_desired_pose.header.stamp = ros::Time::now();
uav4_local_pub.publish(uav4_desired_pose);
count++;
if(count >= num)
{
stage = 5;
break;
}
ROS_INFO("go to point 4");
usleep(100000);
}
}
if(stage == 5)
{
int count = 0;
int num = hold_time * 10;
uav1_desired_pose.type_mask = 0b100111111000;
uav1_desired_pose.coordinate_frame = 1;
uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
uav1_desired_pose.position.z = point13_height;
uav2_desired_pose.type_mask = 0b100111111000;
uav2_desired_pose.coordinate_frame = 1;
uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
uav2_desired_pose.position.z = point24_height;
uav3_desired_pose.type_mask = 0b100111111000;
uav3_desired_pose.coordinate_frame = 1;
uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
uav3_desired_pose.position.z = point13_height;
uav4_desired_pose.type_mask = 0b100111111000;
uav4_desired_pose.coordinate_frame = 1;
uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
uav4_desired_pose.position.z = point24_height;
while(ros::ok())
{
uav1_desired_pose.header.stamp = ros::Time::now();
uav1_local_pub.publish(uav1_desired_pose);
uav2_desired_pose.header.stamp = ros::Time::now();
uav2_local_pub.publish(uav2_desired_pose);
uav3_desired_pose.header.stamp = ros::Time::now();
uav3_local_pub.publish(uav3_desired_pose);
uav4_desired_pose.header.stamp = ros::Time::now();
uav4_local_pub.publish(uav4_desired_pose);
count++;
if(count >= num)
{
//四机降落
set_formation_px4_land();
break;
}
ROS_INFO("go to point 1");
usleep(100000);
}
}
break;
}
else
{
ROS_WARN("input error, please input again");
}
}
}
void formation::is_wait(int time)
{
//判断是否需要等待,time变量不为0,则等待
if(time != 0)
{
sleep(time);
}
}
void formation::set_formation_px4_offboard()
{
//创建1~4号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
//设置1~4号机模式变量为offboard,解上锁变量为解锁
ctm1.arm_cmd.request.value = true;
ctm2.arm_cmd.request.value = true;
ctm3.arm_cmd.request.value = true;
ctm4.arm_cmd.request.value = true;
ctm1.mode_cmd.request.custom_mode = "OFFBOARD";
ctm2.mode_cmd.request.custom_mode = "OFFBOARD";
ctm3.mode_cmd.request.custom_mode = "OFFBOARD";
ctm4.mode_cmd.request.custom_mode = "OFFBOARD";
//集群按照1234的顺序对五台无人机分别解锁并切入offboard模式
//当有一台无人机解锁或者切入offboard模式失败,该函数返回false
//五台无人机成功解锁并切入offboard模式,该函数返回true
if(ctm1.arming_client.call(ctm1.arm_cmd) && ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav1 armed and set offboard mode failed");
}
if(ctm2.arming_client.call(ctm2.arm_cmd) && ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav2 armed and set offboard mode failed");
}
if(ctm3.arming_client.call(ctm3.arm_cmd) && ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav3 armed and set offboard mode failed");
}
if(ctm4.arming_client.call(ctm4.arm_cmd) && ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 armed and set offboard mode success");
is_wait(offboard_intervals);
}
else
{
ROS_ERROR("uav4 armed and set offboard mode failed");
}
}
void formation::set_formation_px4_land()
{
//创建1~4号机的command_to_mavros类
command_to_mavros ctm1("uav1");
command_to_mavros ctm2("uav2");
command_to_mavros ctm3("uav3");
command_to_mavros ctm4("uav4");
//设置1~4号机模式变量为land
ctm1.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm2.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm3.mode_cmd.request.custom_mode = "AUTO.LAND";
ctm4.mode_cmd.request.custom_mode = "AUTO.LAND";
//切换为land模式,并对结果进行打印
if(ctm1.set_mode_client.call(ctm1.mode_cmd))
{
ROS_INFO("uav1 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav1 set land mode failed");
}
if(ctm2.set_mode_client.call(ctm2.mode_cmd))
{
ROS_INFO("uav2 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav2 set land mode failed");
}
if(ctm3.set_mode_client.call(ctm3.mode_cmd))
{
ROS_INFO("uav3 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav3 set land mode failed");
}
if(ctm4.set_mode_client.call(ctm4.mode_cmd))
{
ROS_INFO("uav4 set land mode success");
is_wait(land_intervals);
}
else
{
ROS_ERROR("uav4 set land mode success");
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation_square");
formation Formation;
Formation.square();
return 0;
}

263
src/UAV/Modules/mission/formation/formation_state.cpp
Unescape View File

@ -1,263 +0,0 @@
/*******************************************************************
* 文件名:formation_control.cpp
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.14
*
* 介绍:该cpp文件主要为动捕集群中集群控制相关函数的实现以及程序的运行
* ****************************************************************/
#include "Formation.h"
#include <unistd.h>
void formation::init()
{
//集群五台飞机状态订阅回调函数
uav1_state_sub = n.subscribe("/uav1/mavros/state", 10, &formation::Uav1StateCallBack, this);
uav2_state_sub = n.subscribe("/uav2/mavros/state", 10, &formation::Uav2StateCallBack, this);
uav3_state_sub = n.subscribe("/uav3/mavros/state", 10, &formation::Uav3StateCallBack, this);
uav4_state_sub = n.subscribe("/uav4/mavros/state", 10, &formation::Uav4StateCallBack, this);
uav5_state_sub = n.subscribe("/uav5/mavros/state", 10, &formation::Uav5StateCallBack, this);
uav1_pose_sub = n.subscribe("/uav1/mavros/local_position/pose", 10, &formation::Uav1PoseCallBack, this);
uav2_pose_sub = n.subscribe("/uav2/mavros/local_position/pose", 10, &formation::Uav2PoseCallBack, this);
uav3_pose_sub = n.subscribe("/uav3/mavros/local_position/pose", 10, &formation::Uav3PoseCallBack, this);
uav4_pose_sub = n.subscribe("/uav4/mavros/local_position/pose", 10, &formation::Uav4PoseCallBack, this);
uav5_pose_sub = n.subscribe("/uav5/mavros/local_position/pose", 10, &formation::Uav5PoseCallBack, this);
//设置程序初始时间
begin_time = ros::Time::now();
}
//获取1号机位置数据回调函数
void formation::Uav1PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav1_current_pose = *pose_msgs;
}
//获取2号机位置数据回调函数
void formation::Uav2PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav2_current_pose = *pose_msgs;
}
//获取3号机位置数据回调函数
void formation::Uav3PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav3_current_pose = *pose_msgs;
}
//获取4号机位置数据回调函数
void formation::Uav4PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav4_current_pose = *pose_msgs;
}
//获取5号机位置数据回调函数
void formation::Uav5PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs)
{
uav5_current_pose = *pose_msgs;
}
//打印无人机状态函数
void formation::printf_formation_state()
{
//固定的浮点显示
std::cout.setf(std::ios::fixed);
//设置小数点后精度为2位
std::cout<<std::setprecision(2);
//左对齐
std::cout.setf(std::ios::left);
//显示小数点
std::cout.setf(std::ios::showpoint);
//强制显示符号
std::cout.setf(std::ios::showpos);
//打印时间戳
std::cout << "Time : " << ros::Time::now().sec - begin_time.sec << std::endl;
//1号机
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV1 State<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
//是否与飞控连接
if(uav1_state.connected == true)
{
std::cout << " [Connected] ";
}
else
{
std::cout << " [Unconnected] ";
}
//是否上锁
if(uav1_state.armed == true)
{
std::cout << " [ Armed ] ";
}
else
{
std::cout << " [ DisArmed ] ";
}
//1号机当前模式
std::cout << " [ " << uav1_state.mode << " ] " << std::endl;
//1号机当前位置
std::cout << "Position_uav1 [X Y Z]: " << uav1_current_pose.pose.position.x << "[ m ]" << uav1_current_pose.pose.position.y << "[ m ]" << uav1_current_pose.pose.position.z << "[ m ]" << std::endl;
//2号机
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV2 State<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
//是否与飞控连接
if(uav2_state.connected == true)
{
std::cout << " [Connected] ";
}
else
{
std::cout << " [Unconnected] ";
}
//是否上锁
if(uav2_state.armed == true)
{
std::cout << " [ Armed ] ";
}
else
{
std::cout << " [ DisArmed ] ";
}
//2号机当前模式
std::cout << " [ " << uav2_state.mode << " ] " << std::endl;
//2号机当前位置
std::cout << "Position_uav2 [X Y Z]: " << uav2_current_pose.pose.position.x << "[ m ]" << uav2_current_pose.pose.position.y << "[ m ]" << uav2_current_pose.pose.position.z << "[ m ]" << std::endl;
//3号机
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV3 State<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
//是否与飞控连接
if(uav3_state.connected == true)
{
std::cout << " [Connected] ";
}
else
{
std::cout << " [Unconnected] ";
}
//是否上锁
if(uav3_state.armed == true)
{
std::cout << " [ Armed ] ";
}
else
{
std::cout << " [ DisArmed ] ";
}
//3号机当前模式
std::cout << " [ " << uav3_state.mode << " ] " << std::endl;
//3号机当前位置
std::cout << "Position_uav3 [X Y Z]: " << uav3_current_pose.pose.position.x << "[ m ]" << uav3_current_pose.pose.position.y << "[ m ]" << uav3_current_pose.pose.position.z << "[ m ]" << std::endl;
//4号机
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV4 State<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
//是否与飞控连接
if(uav4_state.connected == true)
{
std::cout << " [Connected] ";
}
else
{
std::cout << " [Unconnected] ";
}
//是否上锁
if(uav4_state.armed == true)
{
std::cout << " [ Armed ] ";
}
else
{
std::cout << " [ DisArmed ] ";
}
//4号机当前模式
std::cout << " [ " << uav4_state.mode << " ] " << std::endl;
//4号机当前位置
std::cout << "Position_uav4 [X Y Z]: " << uav4_current_pose.pose.position.x << "[ m ]" << uav4_current_pose.pose.position.y << "[ m ]" << uav4_current_pose.pose.position.z << "[ m ]" << std::endl;
//5号机
std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV5 State<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
//是否与飞控连接
if(uav5_state.connected == true)
{
std::cout << " [Connected] ";
}
else
{
std::cout << " [Unconnected] ";
}
//是否上锁
if(uav5_state.armed == true)
{
std::cout << " [ Armed ] ";
}
else
{
std::cout << " [ DisArmed ] ";
}
//5号机当前模式
std::cout << " [ " << uav5_state.mode << " ] " << std::endl;
//5号机当前位置
std::cout << "Position_uav5 [X Y Z]: " << uav5_current_pose.pose.position.x << "[ m ]" << uav5_current_pose.pose.position.y << "[ m ]" << uav5_current_pose.pose.position.z << "[ m ]" << std::endl;
}
//集群控制函数
void formation::state()
{
//初始化
init();
while(ros::ok())
{
//处理回调函数
ros::spinOnce();
//打印无人机相关信息
printf_formation_state();
//等待0.05秒
usleep(50000);
}
}
//集群五台无人机状态读取
//1号机
void formation::Uav1StateCallBack(const mavros_msgs::StateConstPtr &state_msgs)
{
uav1_state = *state_msgs;
}
//2号机
void formation::Uav2StateCallBack(const mavros_msgs::StateConstPtr &state_msgs)
{
uav2_state = *state_msgs;
}
//3号机
void formation::Uav3StateCallBack(const mavros_msgs::StateConstPtr &state_msgs)
{
uav3_state = *state_msgs;
}
//4号机
void formation::Uav4StateCallBack(const mavros_msgs::StateConstPtr &state_msgs)
{
uav4_state = *state_msgs;
}
//5号机
void formation::Uav5StateCallBack(const mavros_msgs::StateConstPtr &state_msgs)
{
uav5_state = *state_msgs;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "formation_state");
formation Formation;
Formation.state();
return 0;
}

180
src/UAV/Modules/mission/formation_flight/turtlebot_follow.cpp
Unescape View File

@ -1,180 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
//topic 头文件
#include <geometry_msgs/Point.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/PositionReference.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include "message_utils.h"
using namespace std;
# define NODE_NAME "turtlebot_formation"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
nav_msgs::Odometry tb3_1_odom;
nav_msgs::Odometry tb3_2_odom;
nav_msgs::Odometry tb3_3_odom;
nav_msgs::Odometry tb3_4_odom;
geometry_msgs::Twist tb3_1_cmd;
geometry_msgs::Twist tb3_2_cmd;
geometry_msgs::Twist tb3_3_cmd;
geometry_msgs::Twist tb3_4_cmd;
prometheus_msgs::DroneState uav1_state;
prometheus_msgs::DroneState uav2_state;
prometheus_msgs::DroneState uav3_state;
prometheus_msgs::DroneState uav4_state;
void tb3_1_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_1_odom = *msg;
}
void tb3_2_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_2_odom = *msg;
}
void tb3_3_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_3_odom = *msg;
}
void tb3_4_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_4_odom = *msg;
}
void uav1_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
uav1_state = *msg;
}
void uav2_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
uav2_state = *msg;
}
void uav3_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
uav3_state = *msg;
}
void uav4_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
uav4_state = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "turtlebot_formation");
ros::NodeHandle nh("~");
ros::Subscriber tb3_1_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_1/odom", 10, tb3_1_odom_cb);
ros::Subscriber tb3_2_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_2/odom", 10, tb3_2_odom_cb);
ros::Subscriber tb3_3_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_3/odom", 10, tb3_3_odom_cb);
ros::Subscriber tb3_4_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_4/odom", 10, tb3_4_odom_cb);
ros::Subscriber uav1_sub = nh.subscribe<prometheus_msgs::DroneState>("/uav1/prometheus/drone_state", 1, uav1_cb);
ros::Subscriber uav2_sub = nh.subscribe<prometheus_msgs::DroneState>("/uav2/prometheus/drone_state", 1, uav2_cb);
ros::Subscriber uav3_sub = nh.subscribe<prometheus_msgs::DroneState>("/uav3/prometheus/drone_state", 1, uav3_cb);
ros::Subscriber uav4_sub = nh.subscribe<prometheus_msgs::DroneState>("/uav4/prometheus/drone_state", 1, uav4_cb);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
ros::Publisher tb3_1_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);
ros::Publisher tb3_2_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_2/cmd_vel", 10);
ros::Publisher tb3_3_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_3/cmd_vel", 10);
ros::Publisher tb3_4_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_4/cmd_vel", 10);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(4);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
ros::Rate rate(50.0);
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>turtlebot_formation Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to move the turtlebot."<<endl;
cin >> start_flag;
}
float time_trajectory = 0.0;
ros::Duration(2.0).sleep();
float kp = 0.5;
float x_sp,y_sp;
while (ros::ok())
{
// turtlebot3命令发布
tb3_1_cmd.linear.x = kp * (uav1_state.position[0] - tb3_1_odom.pose.pose.position.x);
tb3_1_cmd.linear.y = kp * (uav1_state.position[1] - tb3_1_odom.pose.pose.position.y);
tb3_1_cmd.linear.z = 0.0;
tb3_1_cmd.angular.x = 0.0;
tb3_1_cmd.angular.y = 0.0;
tb3_1_cmd.angular.z = 0.0;
tb3_2_cmd.linear.x = kp * (uav2_state.position[0] - tb3_2_odom.pose.pose.position.x);
tb3_2_cmd.linear.y = kp * (uav2_state.position[1] - tb3_2_odom.pose.pose.position.x);
tb3_2_cmd.linear.z = 0.0;
tb3_2_cmd.angular.x = 0.0;
tb3_2_cmd.angular.y = 0.0;
tb3_2_cmd.angular.z = 0.0;
tb3_3_cmd.linear.x = kp * (uav3_state.position[0] - tb3_3_odom.pose.pose.position.x);
tb3_3_cmd.linear.y = kp * (uav3_state.position[1] - tb3_3_odom.pose.pose.position.x);
tb3_3_cmd.linear.z = 0.0;
tb3_3_cmd.angular.x = 0.0;
tb3_3_cmd.angular.y = 0.0;
tb3_3_cmd.angular.z = 0.0;
tb3_4_cmd.linear.x = kp * (uav4_state.position[0] - tb3_4_odom.pose.pose.position.x);
tb3_4_cmd.linear.y = kp * (uav4_state.position[1] - tb3_4_odom.pose.pose.position.x);
tb3_4_cmd.linear.z = 0.0;
tb3_4_cmd.angular.x = 0.0;
tb3_4_cmd.angular.y = 0.0;
tb3_4_cmd.angular.z = 0.0;
tb3_1_cmd_pub.publish(tb3_1_cmd);
tb3_2_cmd_pub.publish(tb3_2_cmd);
tb3_3_cmd_pub.publish(tb3_3_cmd);
tb3_4_cmd_pub.publish(tb3_4_cmd);
rate.sleep();
ros::spinOnce();
}
return 0;
}

151
src/UAV/Modules/mission/formation_flight/turtlebot_formation.cpp
Unescape View File

@ -1,151 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
//topic 头文件
#include <geometry_msgs/Point.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/PositionReference.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include "message_utils.h"
using namespace std;
# define NODE_NAME "turtlebot_formation"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
nav_msgs::Odometry tb3_1_odom;
nav_msgs::Odometry tb3_2_odom;
nav_msgs::Odometry tb3_3_odom;
nav_msgs::Odometry tb3_4_odom;
geometry_msgs::Twist tb3_1_cmd;
geometry_msgs::Twist tb3_2_cmd;
geometry_msgs::Twist tb3_3_cmd;
geometry_msgs::Twist tb3_4_cmd;
void tb3_1_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_1_odom = *msg;
}
void tb3_2_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_2_odom = *msg;
}
void tb3_3_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_3_odom = *msg;
}
void tb3_4_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
tb3_4_odom = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "turtlebot_formation");
ros::NodeHandle nh("~");
ros::Subscriber tb3_1_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_1/odom", 10, tb3_1_odom_cb);
ros::Subscriber tb3_2_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_2/odom", 10, tb3_2_odom_cb);
ros::Subscriber tb3_3_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_3/odom", 10, tb3_3_odom_cb);
ros::Subscriber tb3_4_odom_sub = nh.subscribe<nav_msgs::Odometry>("/tb3_4/odom", 10, tb3_4_odom_cb);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
ros::Publisher tb3_1_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);
ros::Publisher tb3_2_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_2/cmd_vel", 10);
ros::Publisher tb3_3_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_3/cmd_vel", 10);
ros::Publisher tb3_4_cmd_pub = nh.advertise<geometry_msgs::Twist>("/tb3_4/cmd_vel", 10);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(4);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
ros::Rate rate(50.0);
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>turtlebot_formation Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to move the turtlebot."<<endl;
cin >> start_flag;
}
float time_trajectory = 0.0;
ros::Duration(2.0).sleep();
float x_sp,y_sp;
while (ros::ok())
{
// turtlebot3命令发布
tb3_1_cmd.linear.x = 0.4;
tb3_1_cmd.linear.y = 0.0;
tb3_1_cmd.linear.z = 0.0;
tb3_1_cmd.angular.x = 0.0;
tb3_1_cmd.angular.y = 0.0;
tb3_1_cmd.angular.z = 0.2;
tb3_2_cmd.linear.x = 0.2;
tb3_2_cmd.linear.y = 0.0;
tb3_2_cmd.linear.z = 0.0;
tb3_2_cmd.angular.x = 0.0;
tb3_2_cmd.angular.y = 0.0;
tb3_2_cmd.angular.z = 0.2;
tb3_3_cmd.linear.x = 0.2;
tb3_3_cmd.linear.y = 0.0;
tb3_3_cmd.linear.z = 0.0;
tb3_3_cmd.angular.x = 0.0;
tb3_3_cmd.angular.y = 0.0;
tb3_3_cmd.angular.z = 0.2;
tb3_4_cmd.linear.x = 0.4;
tb3_4_cmd.linear.y = 0.0;
tb3_4_cmd.linear.z = 0.0;
tb3_4_cmd.angular.x = 0.0;
tb3_4_cmd.angular.y = 0.0;
tb3_4_cmd.angular.z = 0.2;
tb3_1_cmd_pub.publish(tb3_1_cmd);
tb3_2_cmd_pub.publish(tb3_2_cmd);
tb3_3_cmd_pub.publish(tb3_3_cmd);
tb3_4_cmd_pub.publish(tb3_4_cmd);
rate.sleep();
ros::spinOnce();
}
return 0;
}

85
src/UAV/Modules/mission/gimbal_control/gimbal_control.cpp
Unescape View File

@ -1,85 +0,0 @@
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include "gimbal_control.h"
using namespace std;
using namespace Eigen;
#define NODE_NAME "gimbal_control"
#define PI 3.1415926
Eigen::Vector3d gimbal_att_sp;
Eigen::Vector3d gimbal_att;
Eigen::Vector3d gimbal_att_deg;
Eigen::Vector3d gimbal_att_rate;
Eigen::Vector3d gimbal_att_rate_deg;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "gimbal_control");
ros::NodeHandle nh("~");
// 节点运行频率： 10hz
ros::Rate rate(10.0);
gimbal_control gimbal_control_;
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
float gimbal_att_sp_deg[3];
//
while(ros::ok())
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Gimbal Control Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter gimbal attitude: "<<endl;
cout << "Roll [deg] "<<endl;
cin >> gimbal_att_sp_deg[0];
cout << "Pitch [deg] "<<endl;
cin >> gimbal_att_sp_deg[1];
cout << "Yaw [deg] "<<endl;
cin >> gimbal_att_sp_deg[2];
// 注意必须要解锁飞机后才可控制云台
// 注意：使用角度值直接进行控制
// 面向相机朝向的方向，逆时针旋转roll为正，但roll通道一般不控制
// pitch向上抬头为正
// 面向相机朝向的方向，向右转头yaw为正(与飞机yaw相反)
gimbal_att_sp[0] = gimbal_att_sp_deg[0];
gimbal_att_sp[1] = gimbal_att_sp_deg[1];
gimbal_att_sp[2] = gimbal_att_sp_deg[2];
gimbal_control_.send_mount_control_command(gimbal_att_sp);
cout << "gimbal_att_sp : " << gimbal_att_sp_deg[0] << " [deg] "<< gimbal_att_sp_deg[1] << " [deg] "<< gimbal_att_sp_deg[2] << " [deg] "<<endl;
for (int i=0; i<10; i++)
{
gimbal_att = gimbal_control_.get_gimbal_att();
gimbal_att_deg = gimbal_att/PI*180;
cout << "gimbal_att : " << gimbal_att_deg[0] << " [deg] "<< gimbal_att_deg[1] << " [deg] "<< gimbal_att_deg[2] << " [deg] "<<endl;
gimbal_att_rate = gimbal_control_.get_gimbal_att_rate();
gimbal_att_rate_deg = gimbal_att_rate/PI*180;
cout << "gimbal_att_rate : " << gimbal_att_rate_deg[0] << " [deg/s] "<< gimbal_att_rate_deg[1] << " [deg/s] "<< gimbal_att_rate_deg[2] << " [deg/s] "<<endl;
ros::spinOnce();
rate.sleep();
}
}
return 0;
}

267
src/UAV/Modules/mission/gimbal_control/gimbal_control_circle.cpp
Unescape View File

@ -1,267 +0,0 @@
//无人机追踪目标（此处先直接用目标位置进行追踪）
//无人机吊舱根据视觉算法的反馈调节角度，保证目标在相机中心位置
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include "mission_utils.h"
#include "gimbal_control.h"
#include "message_utils.h"
#include <prometheus_msgs/PositionReference.h>
using namespace std;
using namespace Eigen;
#define NODE_NAME "gimbal_control_demo"
#define PI 3.1415926
bool hold_mode;
Eigen::Vector3d gimbal_att_sp_deg;
Eigen::Vector3d gimbal_att;
Eigen::Vector3d gimbal_att_deg;
Eigen::Vector3d gimbal_att_rate;
Eigen::Vector3d gimbal_att_rate_deg;
nav_msgs::Odometry GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
float sight_angle[2];
float start_point[3]; // 起始降落位置
Eigen::Vector3d roi_point;
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState _DroneState; // 无人机状态
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
Eigen::Vector3d mav_pos_;
Eigen::Vector3f circle_center;
float circle_radius = 2.0;
float linear_vel = 1.0;
float direction = 1.0;
prometheus_msgs::PositionReference Circle_trajectory_generation(float time_from_start);
void printf_result();
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
circle_center[0] = GroundTruth.pose.pose.position.x;
circle_center[1] = GroundTruth.pose.pose.position.y;
circle_center[2] = GroundTruth.pose.pose.position.z + 2.0;
roi_point[0] = GroundTruth.pose.pose.position.x;
roi_point[1] = GroundTruth.pose.pose.position.y;
roi_point[2] = GroundTruth.pose.pose.position.z;
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
mav_pos_ << _DroneState.position[0],_DroneState.position[1],_DroneState.position[2];
Eigen::Vector3d error_vec;
double distance_2d;
error_vec = roi_point - mav_pos_;
distance_2d = std::sqrt(error_vec(0) * error_vec(0) + error_vec(1) * error_vec(1));
gimbal_att_sp_deg[0] = 0.0;
gimbal_att_sp_deg[1] = std::atan2(error_vec(2), distance_2d)/PI*180; //pitch
gimbal_att_sp_deg[2] = -std::atan2(error_vec(1), error_vec(0))/PI*180;//yaw
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "gimbal_control_circle");
ros::NodeHandle nh("~");
// 节点运行频率： 20hz
ros::Rate rate(20.0);
roi_point << 0,0,0;
// 初始起飞点
nh.param<float>("start_point_x", start_point[0], 0.0);
nh.param<float>("start_point_y", start_point[1], 1.0);
nh.param<float>("start_point_z", start_point[2], 1.5);
gimbal_control gimbal_control_;
//【订阅】地面真值，此信息仅做比较使用 不强制要求提供
ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/marker", 10, groundtruth_cb);
//【订阅】无人机状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
bool sim_mode = true;
if(sim_mode)
{
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Gimbal Tracking Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}
// 起飞
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Takeoff to predefined position.");
while( _DroneState.position[2] < 0.5)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point[0];
Command_Now.Reference_State.position_ref[1] = start_point[1];
Command_Now.Reference_State.position_ref[2] = start_point[2];
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "Takeoff to start point..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 等待
ros::Duration(3.0).sleep();
float time_trajectory = 0.0;
float trajectory_total_time = 10.0;
while(ros::ok())
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Gimbal Tracking Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter time：[s]: "<<endl;
cin >> trajectory_total_time;
time_trajectory = 0.0;
// 追圆形
while(time_trajectory < trajectory_total_time)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State = Circle_trajectory_generation(time_trajectory);
command_pub.publish(Command_Now);
time_trajectory = time_trajectory + 0.05;
cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time << " [ s ]" <<endl;
gimbal_att = gimbal_control_.get_gimbal_att();
gimbal_att_deg = gimbal_att/PI*180;
cout << "gimbal_att : " << gimbal_att_deg[0] << " [deg] "<< gimbal_att_deg[1] << " [deg] "<< gimbal_att_deg[2] << " [deg] "<<endl;
cout << "gimbal_att_sp : " << gimbal_att_sp_deg[0] << " [deg] "<< gimbal_att_sp_deg[1] << " [deg] "<< gimbal_att_sp_deg[2] << " [deg] "<<endl;
gimbal_control_.send_mount_control_command(gimbal_att_sp_deg);
printf_result();
ros::spinOnce();
ros::Duration(0.05).sleep();
}
printf_result();
ros::spinOnce();
rate.sleep();
}
return 0;
}
void printf_result()
{
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
}
prometheus_msgs::PositionReference Circle_trajectory_generation(float time_from_start)
{
prometheus_msgs::PositionReference Circle_trajectory;
float omega;
if( circle_radius != 0)
{
omega = direction * fabs(linear_vel / circle_radius);
}else
{
omega = 0.0;
}
const float angle = time_from_start * omega;
const float cos_angle = cos(angle);
const float sin_angle = sin(angle);
// cout << "omega : " << omega * 180/M_PI <<" [deg/s] " <<endl;
// cout << "angle : " << angle * 180/M_PI <<" [deg] " <<endl;
Circle_trajectory.header.stamp = ros::Time::now();
Circle_trajectory.time_from_start = time_from_start;
Circle_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
Circle_trajectory.position_ref[0] = circle_radius * cos_angle + circle_center[0];
Circle_trajectory.position_ref[1] = circle_radius * sin_angle + circle_center[1];
Circle_trajectory.position_ref[2] = circle_center[2];
Circle_trajectory.velocity_ref[0] = - circle_radius * omega * sin_angle;
Circle_trajectory.velocity_ref[1] = circle_radius * omega * cos_angle;
Circle_trajectory.velocity_ref[2] = 0;
Circle_trajectory.acceleration_ref[0] = - circle_radius * pow(omega, 2.0) * cos_angle;
Circle_trajectory.acceleration_ref[1] = - circle_radius * pow(omega, 2.0) * sin_angle;
Circle_trajectory.acceleration_ref[2] = 0;
// Circle_trajectory.jerk_ref[0] = circle_radius * pow(omega, 3.0) * sin_angle;
// Circle_trajectory.jerk_ref[1] = - circle_radius * pow(omega, 3.0) * cos_angle;
// Circle_trajectory.jerk_ref[2] = 0;
// Circle_trajectory.snap_ref[0] = circle_radius * pow(omega, 4.0) * cos_angle;
// Circle_trajectory.snap_ref[1] = circle_radius * pow(omega, 4.0) * sin_angle;
// Circle_trajectory.snap_ref[2] = 0;
Circle_trajectory.yaw_ref = 0;
// Circle_trajectory.yaw_rate_ref = 0;
// Circle_trajectory.yaw_acceleration_ref = 0;
return Circle_trajectory;
}

418
src/UAV/Modules/mission/gimbal_control/gimbal_control_demo.cpp
Unescape View File

@ -1,418 +0,0 @@
//无人机追踪目标（此处先直接用目标位置进行追踪）
//无人机吊舱根据视觉算法的反馈调节角度，保证目标在相机中心位置
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include "mission_utils.h"
#include "gimbal_control.h"
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define NODE_NAME "gimbal_control_demo"
#define PI 3.1415926
#define VISION_THRES_TRACKING 100
bool hold_mode;
bool ignore_vision;
Eigen::Vector3d gimbal_att_sp;
Eigen::Vector3d gimbal_att;
Eigen::Vector3d gimbal_att_deg;
Eigen::Vector3d gimbal_att_rate;
Eigen::Vector3d gimbal_att_rate_deg;
nav_msgs::Odometry GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
float sight_angle[2];
float desired_yaw;
float kp_track[3]; //控制参数 - 比例参数
float kpyaw_track;
float start_point[3]; // 起始降落位置
Eigen::Vector3d roi_point;
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState _DroneState; // 无人机状态
Eigen::Matrix3f R_Body_to_ENU,R_camera_to_body; // 无人机机体系至惯性系转换矩阵
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
float gimbal_rate;
Eigen::Vector3d mav_pos_;
float distance_to_target;
float integral = 0;
float ki_track;
void printf_result();
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
roi_point[0] = GroundTruth.pose.pose.position.x;
roi_point[1] = GroundTruth.pose.pose.position.y;
roi_point[2] = GroundTruth.pose.pose.position.z;
}
int num_regain = 0;
int num_lost = 0;
bool is_detected = false;
Eigen::Vector3d aruco_pos_enu;
prometheus_msgs::ArucoInfo aruco_info;
void aruco_cb(const prometheus_msgs::ArucoInfo::ConstPtr& msg)
{
aruco_info = *msg;
// 暂不考虑无人机姿态的影响
aruco_pos_enu[0] = mav_pos_[0] - aruco_info.position[1];
aruco_pos_enu[1] = mav_pos_[1] - aruco_info.position[0];
// 相机安装在无人机下方10cm处，需减去该偏差
aruco_pos_enu[2] = mav_pos_[2] - aruco_info.position[2];
if(aruco_info.detected)
{
num_regain++;
num_lost = 0;
}else
{
num_regain = 0;
num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(num_lost > VISION_THRES_TRACKING)
{
is_detected = false;
//　丢失后　对sight_angle清零，否则云台会移动
sight_angle[0] = 0.0;
sight_angle[1] = 0.0;
}
// 当连续一段时间检测到目标时，认定目标得到
if(num_regain > 2)
{
is_detected = true;
}
if(aruco_info.detected)
{
cout << "Aruco_ID: [" << aruco_info.aruco_num << "] detected: [yes] " << endl;
cout << "Pos [camera]: "<< aruco_info.position[0] << " [m] "<< aruco_info.position[1] << " [m] "<< aruco_info.position[2] << " [m] "<<endl;
cout << "Pos [enu] : "<< aruco_pos_enu[0] << " [m] "<< aruco_pos_enu[1] << " [m] "<< aruco_pos_enu[2] << " [m] "<<endl;
// cout << "Att [camera]: "<< aruco_info.position[0] << " [m] "<< aruco_info.position[1] << " [m] "<< aruco_info.position[2] << " [m] "<<endl;
cout << "Sight Angle : "<< aruco_info.sight_angle[0]/3.14*180 << " [deg] "<< aruco_info.sight_angle[1]/3.14*180 << " [deg] " <<endl;
}else
{
cout << "Aruco_ID: [" << aruco_info.aruco_num << "] detected: [no] " << endl;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
mav_pos_ << _DroneState.position[0],_DroneState.position[1],_DroneState.position[2];
}
void gimbal_control_cb(const ros::TimerEvent& e)
{
// 几个思考的点： 云台控制频率！！！ 设置定时函数，降低云台控制频率
// 通过什么控制云台
// 云台角度反馈如何控制无人机速度， 或者是加速度？ 是否需要积分项？
// 多了姿态反馈后 如何使得与降落版垂直
// 增加追踪人的场景
if(ignore_vision)
{
Eigen::Vector3d error_vec;
double distance_2d;
error_vec = roi_point - mav_pos_;
distance_2d = std::sqrt(error_vec(0) * error_vec(0) + error_vec(1) * error_vec(1));
// 理想的吊舱控制情况
gimbal_att_sp[0] = 0.0;
gimbal_att_sp[1] = std::atan2(error_vec(2), distance_2d)/PI*180; //pitch
//desired_yaw = -std::atan2(error_vec(1), error_vec(0))/PI*180;//yaw
gimbal_att_sp[2] = 0.0;
}else
{
// 使用目标位置的视场角控制云台
// 品灵吊舱原做法：根据像素来控制云台
// 传统做法(Jin Ren)：使用视场角误差来反馈，控制吊舱角速度(PD control)
// 但我们这里只能直接控制云台角度（不能直接用于品灵吊舱的控制），注意sight_angle是误差值
//　遇到的问题１：由于传进来的sight_angle有噪音，导致云台有一点点抽搐（可以考虑做一个平滑滤波，或者降低控制频率）
// 云台滚转角不控制
gimbal_att_sp[0] = 0.0;
// 云台俯仰角
gimbal_att_sp[1] = gimbal_att_deg[1] - 0.1 * sight_angle[1]/PI*180; //pitch
// 云台偏航角 取决于左右夹角
//gimbal_att_sp[2] = gimbal_att_deg[2] + 0.1 * sight_angle[0]/PI*180;
gimbal_att_sp[2] = 0.0;
}
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "gimbal_control");
ros::NodeHandle nh("~");
// 节点运行频率： 20hz
ros::Rate rate(20.0);
bool moving_target;
nh.param<bool>("hold_mode", hold_mode, false);
nh.param<bool>("ignore_vision", ignore_vision, false);
nh.param<bool>("moving_target", moving_target, false);
nh.param<float>("gimbal_rate", gimbal_rate, 0.1);
//追踪控制参数
nh.param<float>("kpx_track", kp_track[0], 0.1);
nh.param<float>("kpy_track", kp_track[1], 0.1);
nh.param<float>("kpz_track", kp_track[2], 0.1);
nh.param<float>("ki_track", ki_track, 0.02);
nh.param<float>("kpyaw_track", kpyaw_track, 0.1);
// 初始起飞点
nh.param<float>("start_point_x", start_point[0], 0.0);
nh.param<float>("start_point_y", start_point[1], 1.0);
nh.param<float>("start_point_z", start_point[2], 1.5);
//【订阅】地面真值，此信息仅做比较使用 不强制要求提供
ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/marker", 10, groundtruth_cb);
//【订阅】
ros::Subscriber aruco_sub = nh.subscribe<prometheus_msgs::ArucoInfo>("/prometheus/object_detection/aruco_det", 10, aruco_cb);
//【订阅】无人机状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
//　吊舱控制timer
ros::Timer timer = nh.createTimer(ros::Duration(gimbal_rate), gimbal_control_cb);
gimbal_control gimbal_control_;
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
float gimbal_att_sp_deg[3];
bool sim_mode = true;
if(sim_mode)
{
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Gimbal Tracking Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}
// 起飞
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Takeoff to predefined position.");
while( _DroneState.position[2] < 0.5)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point[0];
Command_Now.Reference_State.position_ref[1] = start_point[1];
Command_Now.Reference_State.position_ref[2] = start_point[2];
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "Takeoff to start point..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 等待
ros::Duration(3.0).sleep();
while(ros::ok())
{
//　云台数据打印
gimbal_att = gimbal_control_.get_gimbal_att();
gimbal_att_deg = gimbal_att/PI*180;
cout << "gimbal_att : " << gimbal_att_deg[0] << " [deg] "<< gimbal_att_deg[1] << " [deg] "<< gimbal_att_deg[2] << " [deg] "<<endl;
gimbal_att_rate = gimbal_control_.get_gimbal_att_rate();
gimbal_att_rate_deg = gimbal_att_rate/PI*180;
cout << "gimbal_att_rate : " << gimbal_att_rate_deg[0] << " [deg/s] "<< gimbal_att_rate_deg[1] << " [deg/s] "<< gimbal_att_rate_deg[2] << " [deg/s] "<<endl;
R_camera_to_body = get_rotation_matrix(gimbal_att[0], gimbal_att[1], gimbal_att[2]);
gimbal_control_.send_mount_control_command(gimbal_att_sp);
// 无人机追踪目标
//　但如果是地面目标（或高度确定目标），则应当定高飞行，即XY_VEL_Z_POS
//　如果是空中目标（高度会变化的目标），应当使用云台角度控制ｚ轴速度，即XYZ_VEL
if (!hold_mode)
{
if(ignore_vision)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
//　由于无人机偏航打死，因此　此处使用ENU_FRAME　而非　BODY_FRAME (实际中应当考虑使用机体坐标系进行控制)
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
//float error = GroundTruth.pose.pose.position.x - 2.0 - mav_pos_[0];
// 策略！！ 策略！！
// 如：进入指定距离后 控制反馈变小
// 仍有很大进步空间，但是实际实验可能还要继续调试
distance_to_target = (roi_point - mav_pos_).norm();
float error = distance_to_target - 3.0;
cout << "distance_to_target: " << distance_to_target << " [m] "<<endl;
integral = integral + error;
if (integral > 10.0)
{
integral = 10;
}
Command_Now.Reference_State.velocity_ref[0] = kp_track[0] * error + ki_track*integral;
if (Command_Now.Reference_State.velocity_ref[0] <0 )
{
Command_Now.Reference_State.velocity_ref[0] = 0;
}
if(moving_target)
{
Command_Now.Reference_State.velocity_ref[0] = Command_Now.Reference_State.velocity_ref[0] +0.3;
}
// 如果不控制无人机偏航角（即yaw_ref = 0），可根据云台偏航角控制无人机y轴速度
// y轴速度应当根据视觉解算的目标姿态来调整？ 待定
Command_Now.Reference_State.velocity_ref[1] = 0.0;
// z轴速度取决与当前云台俯仰角度（俯仰角速度） 注意gimbal_att_deg的角度是deg
// 高度上保持一致，还是保持一个10度俯仰角
//　如果是高度会变化的目标，应当使用云台角度控制高度
//　但如果是地面目标（或高度确定目标），则应当定高飞行
// Command_Now.Reference_State.velocity_ref[2] = kp_track[2] * (gimbal_att_deg[１] + 2);
Command_Now.Reference_State.position_ref[2] = 2.5;
// 偏航角 取决于当前云台偏航角
//　由于仿真云台使用的是与无人机的相对夹角，应此不能控制无人机偏航角或者偏航角速度，需锁定在0度
// 或者仿真中：不控制云台偏航角，但是仅控制无人机偏航角
Command_Now.Reference_State.Yaw_Rate_Mode = false;
Eigen::Vector3d error_vec;
error_vec = roi_point - mav_pos_;
desired_yaw = std::atan2(error_vec(1), error_vec(0));//yaw
//cout << "desired_yaw: " << desired_yaw/PI*180 << " [deg] "<<endl;
Command_Now.Reference_State.yaw_ref = desired_yaw;
//Command_Now.Reference_State.yaw_rate_ref = kpyaw_track * (desired_yaw - _DroneState.attitude[2]);
}else
{
// 和降落不同，此处即使丢失也要继续追，不然没法召回目标
if(is_detected)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
// 此处暂时使用真值，实际中应当使用机体坐标系进行控制
//float error = GroundTruth.pose.pose.position.x - 2.0 - mav_pos_[0];
distance_to_target = (roi_point - mav_pos_).norm();
float error = distance_to_target - 3.0;
cout << "distance_to_target: " << distance_to_target << " [m] "<<endl;
integral = integral + error;
if (integral > 10.0)
{
integral = 10;
}
Command_Now.Reference_State.velocity_ref[0] = kp_track[0] * error + ki_track*integral;
if (Command_Now.Reference_State.velocity_ref[0] <0 )
{
Command_Now.Reference_State.velocity_ref[0] = 0;
}
if(moving_target)
{
Command_Now.Reference_State.velocity_ref[0] = Command_Now.Reference_State.velocity_ref[0] +0.3;
}
// 如果不控制无人机偏航角（即yaw_ref = 0），可根据云台偏航角控制无人机y轴速度
Command_Now.Reference_State.velocity_ref[1] = 0.0;
// y轴速度应当根据视觉解算的目标姿态来调整？ 待定
// Command_Now.Reference_State.velocity_ref[1] = - kp_track[1] * gimbal_att_deg[2];
// z轴速度取决与当前云台俯仰角度（俯仰角速度） 注意gimbal_att_deg的角度是deg
Command_Now.Reference_State.position_ref[2] = 2.5;
// 偏航角 取决于当前云台偏航角
//　由于仿真云台使用的是与无人机的相对夹角，应次不能控制无人机偏航角或者偏航角速度，需锁定在0度
Command_Now.Reference_State.Yaw_Rate_Mode = true;
Command_Now.Reference_State.yaw_ref = 0.0;
Command_Now.Reference_State.yaw_rate_ref = - kpyaw_track * sight_angle[0];
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
}
}
command_pub.publish(Command_Now);
}
printf_result();
ros::spinOnce();
rate.sleep();
}
return 0;
}
void printf_result()
{
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
cout << ">>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<"<< endl;
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if( is_detected)
{
cout << "is_detected: ture" <<endl;
}else
{
cout << "is_detected: false" <<endl;
}
// cout << "Target_pos (camera): " << pos_camera_frame[0] << " [m] "<< pos_camera_frame[1] << " [m] "<< pos_camera_frame[2] << " [m] "<<endl;
// cout << "Target_pos (body): " << pos_body_frame[0] << " [m] "<< pos_body_frame[1] << " [m] "<< pos_body_frame[2] << " [m] "<<endl;
// cout << "Target_pos (body_enu): " << pos_body_enu_frame[0] << " [m] "<< pos_body_enu_frame[1] << " [m] "<< pos_body_enu_frame[2] << " [m] "<<endl;
// cout << "Detection_ENU(pos): " << pos_enu_frame[0] << " [m] "<< pos_enu_frame[1] << " [m] "<< pos_enu_frame[2] << " [m] "<<endl;
cout << "Ground_truth(pos): " << GroundTruth.pose.pose.position.x << " [m] "<< GroundTruth.pose.pose.position.y << " [m] "<< GroundTruth.pose.pose.position.z << " [m] "<<endl;
}

417
src/UAV/Modules/mission/include/Formation.h
Unescape View File

@ -1,417 +0,0 @@
/*******************************************************************
* 文件名:Formation.h
*
* 作者: BOSHEN97
*
* 更新时间: 2020.10.12
*
* 介绍:该头文件主要声明动捕系统定位下的无人机集群功能当中的变量以及函数等,包含以下功能模块:
* 1. 模式切换
* 2. 队形变换
* 3. 集群控制
* ****************************************************************/
#ifndef FORMATION_H
#define FORMATION_H
#include "command_to_mavros.h"
#include <ros/ros.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/Formation.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Int8.h>
#include <Eigen/Eigen>
class formation
{
public:
//初始化函数,创建发布者,订阅者,变量初始化等
void init();
//无人机集群解锁以及切入offboard模式(px4)
void set_formation_px4_offboard();
//无人机集群解锁以及切入guided模式并起飞(apm)
void set_formation_apm_guided();
//无人机集群切入LAND模式(px4)
void set_formation_px4_land();
//无人机集群切入LAND模式(apm)
void set_formation_apm_land();
//无人机集群上锁
void set_formation_disarmed();
//程序是否等待函数
void is_wait(int time);
//模式设置函数
void set_mode();
//队形变换函数
void change();
//获取动捕系统位置数据函数
void getpose();
//是否为仿真函数
void is_sim();
//状态函数
void state();
//终端输入控制数据函数
void move();
//切换为三角队形函数
void set_triangle();
//切换为一字队形函数
void set_horizontal();
//切换为菱形队形函数
void set_diamond();
//切换为菱形队形过渡队形函数
void set_diamond_stage1();
//打印集群队形函数
void printf_formation_type(std::string type_name);
//集群位置控制发布函数
void formation_pos_pub();
//打印飞机状态函数
void printf_formation_state();
//集群控制函数
void control();
//四边形绕圈函数
void square();
//参数检查函数
bool check_param();
//获取单台无人机控制数据
void get_uav_cmd(Eigen::Vector3d offset_pose, mavros_msgs::PositionTarget& desired_pose);
//集群控制命令回调函数
void ControlCallBack(const prometheus_msgs::ControlCommandConstPtr& control_msgs);
//队形变换命令回调函数
void FormationChangeCallBack(const prometheus_msgs::FormationConstPtr& change_msgs);
//1号机状态数据回调函数
void Uav1StateCallBack(const mavros_msgs::StateConstPtr &state_msgs);
//2号机状态数据回调函数
void Uav2StateCallBack(const mavros_msgs::StateConstPtr &state_msgs);
//3号机状态数据回调函数
void Uav3StateCallBack(const mavros_msgs::StateConstPtr &state_msgs);
//4号机状态数据回调函数
void Uav4StateCallBack(const mavros_msgs::StateConstPtr &state_msgs);
//5号机状态数据回调函数
void Uav5StateCallBack(const mavros_msgs::StateConstPtr &state_msgs);
//1号机动捕位置数据回调函数
void Uav1MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//2号机动捕位置数据回调函数
void Uav2MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//3号机动捕位置数据回调函数
void Uav3MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//4号机动捕位置数据回调函数
void Uav4MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//5号机动捕位置数据回调函数
void Uav5MocapPoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//1号机位置数据回调函数
void Uav1PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//2号机位置数据回调函数
void Uav2PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//3号机位置数据回调函数
void Uav3PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//4号机位置数据回调函数
void Uav4PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
//5号机位置数据回调函数
void Uav5PoseCallBack(const geometry_msgs::PoseStampedConstPtr &pose_msgs);
private:
//创建句柄
ros::NodeHandle n;
/*******************订阅者*******************/
//控制命令订阅者
ros::Subscriber cmd_sub;
//队形变换订阅者
ros::Subscriber formation_type_sub;
//1号机状态数据订阅者
ros::Subscriber uav1_state_sub;
//2号机状态数据订阅者
ros::Subscriber uav2_state_sub;
//3号机状态数据订阅者
ros::Subscriber uav3_state_sub;
//4号机状态数据订阅者
ros::Subscriber uav4_state_sub;
//5号机状态数据订阅者
ros::Subscriber uav5_state_sub;
//1号机动捕位置数据订阅者
ros::Subscriber uav1_mocap_pose_sub;
//2号机动捕位置数据订阅者
ros::Subscriber uav2_mocap_pose_sub;
//3号机动捕位置数据订阅者
ros::Subscriber uav3_mocap_pose_sub;
//4号机动捕位置数据订阅者
ros::Subscriber uav4_mocap_pose_sub;
//5号机动捕位置数据订阅者
ros::Subscriber uav5_mocap_pose_sub;
//1号机位置数据订阅者
ros::Subscriber uav1_pose_sub;
//2号机位置数据订阅者
ros::Subscriber uav2_pose_sub;
//3号机位置数据订阅者
ros::Subscriber uav3_pose_sub;
//4号机位置数据订阅者
ros::Subscriber uav4_pose_sub;
//5号机位置数据订阅者
ros::Subscriber uav5_pose_sub;
/*******************客户端*******************/
ros::ServiceClient uav1_takeoff_client;
ros::ServiceClient uav2_takeoff_client;
ros::ServiceClient uav3_takeoff_client;
ros::ServiceClient uav4_takeoff_client;
ros::ServiceClient uav5_takeoff_client;
/*******************发布者*******************/
//队形变换发布者
ros::Publisher formation_type_pub;
//控制命令发布者
ros::Publisher cmd_pub;
//1号机动捕位置数据发布者
ros::Publisher uav1_mocap_pose_pub;
//2号机动捕位置数据发布者
ros::Publisher uav2_mocap_pose_pub;
//3号机动捕位置数据发布者
ros::Publisher uav3_mocap_pose_pub;
//4号机动捕位置数据发布者
ros::Publisher uav4_mocap_pose_pub;
//5号机动捕位置数据发布者
ros::Publisher uav5_mocap_pose_pub;
//1号机控制指令发布者
ros::Publisher uav1_local_pub;
//1号机控制指令发布者
ros::Publisher uav2_local_pub;
//1号机控制指令发布者
ros::Publisher uav3_local_pub;
//1号机控制指令发布者
ros::Publisher uav4_local_pub;
//1号机控制指令发布者
ros::Publisher uav5_local_pub;
/*******************变量*******************/
//1号机期望位置
mavros_msgs::PositionTarget uav1_desired_pose;
//2号机期望位置
mavros_msgs::PositionTarget uav2_desired_pose;
//3号机期望位置
mavros_msgs::PositionTarget uav3_desired_pose;
//4号机期望位置
mavros_msgs::PositionTarget uav4_desired_pose;
//5号机期望位置
mavros_msgs::PositionTarget uav5_desired_pose;
//1号机当前位置
geometry_msgs::PoseStamped uav1_current_pose;
//2号机当前位置
geometry_msgs::PoseStamped uav2_current_pose;
//3号机当前位置
geometry_msgs::PoseStamped uav3_current_pose;
//4号机当前位置
geometry_msgs::PoseStamped uav4_current_pose;
//5号机当前位置
geometry_msgs::PoseStamped uav5_current_pose;
//1号机集群队形位置差值
Eigen::Vector3d uav1_offset_pose;
//2号机集群队形位置差值
Eigen::Vector3d uav2_offset_pose;
//3号机集群队形位置差值
Eigen::Vector3d uav3_offset_pose;
//4号机集群队形位置差值
Eigen::Vector3d uav4_offset_pose;
//5号机集群队形位置差值
Eigen::Vector3d uav5_offset_pose;
//1号机仿真位置差值
Eigen::Vector3d uav1_gazebo_offset_pose;
//2号机仿真位置差值
Eigen::Vector3d uav2_gazebo_offset_pose;
//3号机仿真位置差值
Eigen::Vector3d uav3_gazebo_offset_pose;
//4号机仿真位置差值
Eigen::Vector3d uav4_gazebo_offset_pose;
//5号机仿真位置差值
Eigen::Vector3d uav5_gazebo_offset_pose;
//1号机当前状态
mavros_msgs::State uav1_state;
//2号机当前状态
mavros_msgs::State uav2_state;
//3号机当前状态
mavros_msgs::State uav3_state;
//4号机当前状态
mavros_msgs::State uav4_state;
//5号机当前状态
mavros_msgs::State uav5_state;
//1号机起飞客户端变量(apm)
mavros_msgs::CommandTOL uav1_takeoff_cmd;
//2号机起飞客户端变量(apm)
mavros_msgs::CommandTOL uav2_takeoff_cmd;
//3号机起飞客户端变量(apm)
mavros_msgs::CommandTOL uav3_takeoff_cmd;
//4号机起飞客户端变量(apm)
mavros_msgs::CommandTOL uav4_takeoff_cmd;
//5号机起飞客户端变量(apm)
mavros_msgs::CommandTOL uav5_takeoff_cmd;
//集群队形X轴间隔距离:一字队形以及三角队形飞机之间X轴间隔距离为1倍该变量,菱形队形为2倍
double formation_distance_x;
//集群队形Y轴间隔距离:一字队形以及三角队形飞机之间Y轴间隔距离为1倍该变量,菱形队形为2倍
double formation_distance_y;
//集群队形Z轴间隔距离:Z轴距离未设置,暂留该接口
double formation_distance_z;
//集群解锁切入offboard模式时,无人机之间的时间间隔
int offboard_intervals;
//集群降落时,无人机之间的时间间隔
int land_intervals;
//集群切换为菱形队形或由菱形队形切换为其他队形会存在碰撞风险,因此需加入中间过渡队形,该变量为过渡队形保持的时间
int diamond_intervals;
//集群队形
prometheus_msgs::Formation formation_data;
//集群控制
prometheus_msgs::ControlCommand control_data;
//集群模式
int formation_mode;
//是否为仿真.true代表为仿真,false代表真机
bool sim;
//飞行控制系统:px4 或者 apm
std::string flight_controller;
//定位数据来源
std::string location_source;
//起飞高度(apm)
double takeoff_height;
//程序运行初始时间
ros::Time begin_time;
/***************四机正方形绕圈变量***************/
//正方形边长
double square_length;
//1,3号点飞行高度
double point13_height;
//2,4号点飞行高度
double point24_height;
//发布目标点的保持时间
double hold_time;
//发布第一个目标点的延长时间
double stage1_time;
};
#endif //FORMATION_H

141
src/UAV/Modules/mission/include/gimbal_control.h
Unescape View File

@ -1,141 +0,0 @@
#ifndef GIMBAL_CONTROL_H
#define GIMBAL_CONTROL_H
// PX4云台控制类
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <math.h>
#include <mavros_msgs/MountControl.h>
#include <geometry_msgs/Quaternion.h>
#include <prometheus_msgs/ArucoInfo.h>
#include "mission_utils.h"
using namespace std;
class gimbal_control
{
public:
gimbal_control(void):
nh("~")
{
// 订阅云台当前角度
gimbal_att_sub = nh.subscribe<geometry_msgs::Quaternion>("/mavros/mount_control/orientation", 10, &gimbal_control::gimbal_att_cb,this);
// 云台控制：本话题要发送至飞控(通过Mavros_extra功能包 /plugins/mount_control.cpp发送)
mount_control_pub = nh.advertise<mavros_msgs::MountControl>( "/mavros/mount_control/command", 1);
// 云台角度初始化
gimbal_att = Eigen::Vector3d(0.0,0.0,0.0);
gimbal_att_last = Eigen::Vector3d(0.0,0.0,0.0);
begin_time = ros::Time::now();
dt_time = 0.0;
last_time = get_time_in_sec(begin_time);
}
// 云台角度
Eigen::Vector3d gimbal_att;
// 上一时刻云台角度
Eigen::Vector3d gimbal_att_last;
// 估算的云台角速度
Eigen::Vector3d gimbal_att_rate;
// 估算云台角速度
ros::Time begin_time;
float last_time;
float dt_time;
//发送云台控制指令API函数
void send_mount_control_command(const Eigen::Vector3d& gimbal_att_sp);
Eigen::Vector3d get_gimbal_att();
Eigen::Vector3d get_gimbal_att_rate();
private:
ros::NodeHandle nh;
ros::Subscriber gimbal_att_sub;
ros::Publisher mount_control_pub;
Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q);
float get_time_in_sec(const ros::Time& begin_time);
void gimbal_att_cb(const geometry_msgs::Quaternion::ConstPtr& msg)
{
Eigen::Quaterniond gimbal_att_quat;
gimbal_att_quat = Eigen::Quaterniond(msg->w, msg->x, msg->y, msg->z);
//Transform the Quaternion to euler Angles
gimbal_att = quaternion_to_euler(gimbal_att_quat);
float cur_time = get_time_in_sec(begin_time);
dt_time = cur_time - last_time;
dt_time = constrain_function2(dt_time, 0.01, 0.03);
last_time = cur_time;
gimbal_att_rate = (gimbal_att - gimbal_att_last)/dt_time;
gimbal_att_last = gimbal_att;
}
};
void gimbal_control::send_mount_control_command(const Eigen::Vector3d& gimbal_att_sp)
{
mavros_msgs::MountControl mount_setpoint;
//
mount_setpoint.header.stamp = ros::Time::now();
mount_setpoint.header.frame_id = "map";
mount_setpoint.mode = 2;
mount_setpoint.roll = gimbal_att_sp[0]; // Gimbal Roll [deg]
mount_setpoint.pitch = gimbal_att_sp[1]; // Gimbal Pitch[deg]
mount_setpoint.yaw = gimbal_att_sp[2]; // Gimbal Yaw [deg]
mount_control_pub.publish(mount_setpoint);
}
Eigen::Vector3d gimbal_control::get_gimbal_att_rate()
{
return gimbal_att_rate;
}
Eigen::Vector3d gimbal_control::get_gimbal_att()
{
return gimbal_att;
}
Eigen::Vector3d gimbal_control::quaternion_to_euler(const Eigen::Quaterniond &q)
{
float quat[4];
quat[0] = q.w();
quat[1] = q.x();
quat[2] = q.y();
quat[3] = q.z();
Eigen::Vector3d ans;
ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]));
ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1]));
ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]));
return ans;
}
float gimbal_control::get_time_in_sec(const ros::Time& begin_time)
{
ros::Time time_now = ros::Time::now();
float currTimeSec = time_now.sec - begin_time.sec;
float currTimenSec = time_now.nsec / 1e9 - begin_time.nsec / 1e9;
return (currTimeSec + currTimenSec);
}
#endif

188
src/UAV/Modules/mission/include/mission_utils.h
Unescape View File

@ -1,188 +0,0 @@
/***************************************************************************************************************************
* mission_utils.h
*
* Author: Qyp
*
* Update Time: 2020.1.12
*
* Introduction: mission_utils
*
***************************************************************************************************************************/
#ifndef MISSION_UTILS_H
#define MISSION_UTILS_H
#include <Eigen/Eigen>
#include <math.h>
//topic 头文件
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/MultiDetectionInfo.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/Message.h>
#include <mavros_msgs/ActuatorControl.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
using namespace std;
#define DIS_THRES 0.1
#define VISION_THRES 10
//相机安装OFFSET
#define FRONT_CAMERA_OFFSET_X 0.2
#define FRONT_CAMERA_OFFSET_Y 0.0
#define FRONT_CAMERA_OFFSET_Z -0.05
#define DOWN_CAMERA_OFFSET_X 0.0
#define DOWN_CAMERA_OFFSET_Y 0.0
#define DOWN_CAMERA_OFFSET_Z -0.1
// 定义视觉检测结构体
//
struct Detection_result
{
string object_name;
// 视觉检测原始信息，返回的结果为相机坐标系
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
prometheus_msgs::DetectionInfo Detection_info;
// 目标在相机系位置
Eigen::Vector3f pos_camera_frame;
// 目标在机体系位置
Eigen::Vector3f pos_body_frame;
// 目标在机体-惯性系位置 (原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系)
Eigen::Vector3f pos_body_enu_frame;
// 目标在惯性系位置 (原点位于起飞点，x轴指向前方，y轴指向左，z轴指向上的坐标系)
Eigen::Vector3f pos_enu_frame;
// 目标在机体系姿态
Eigen::Vector3f att_body_frame;
// 目标在惯性系姿态
Eigen::Vector3f att_enu_frame;
// 目标识别标志位,阈值:VISION_THRES
bool is_detected = false;
int num_lost = 0; //视觉丢失计数器
int num_regain = 0;
};
//打印视觉检测消息
void printf_detection_result(const struct Detection_result& det_info)
{
cout << "Object_name : " << det_info.object_name << endl;
if(det_info.is_detected)
{
cout << "is_detected : " << "True" << endl;
}else
{
cout << "is_detected : " << "False" << endl;
}
cout << "Camera_frame : " << det_info.Detection_info.position[0] << " [m] "<< det_info.Detection_info.position[1] << " [m] "<< det_info.Detection_info.position[2] << " [m] "<<endl;
cout << "Body_frame : " << det_info.pos_body_frame[0] << " [m] "<< det_info.pos_body_frame[1] << " [m] "<< det_info.pos_body_frame[2] << " [m] "<<endl;
cout << "BodyENU_frame : " << det_info.pos_body_enu_frame[0] << " [m] "<< det_info.pos_body_enu_frame[1] << " [m] "<< det_info.pos_body_enu_frame[2] << " [m] "<<endl;
cout << "ENU_frame : " << det_info.pos_enu_frame[0] << " [m] "<< det_info.pos_enu_frame[1] << " [m] "<< det_info.pos_enu_frame[2] << " [m] "<<endl;
}
float cal_distance(const Eigen::Vector3f& pos_drone,const Eigen::Vector3f& pos_target)
{
Eigen::Vector3f relative;
relative = pos_target - pos_drone;
return relative.norm();
}
float cal_distance_tracking(const Eigen::Vector3f& pos_drone,const Eigen::Vector3f& pos_target,const Eigen::Vector3f& delta)
{
Eigen::Vector3f relative;
relative = pos_target - pos_drone - delta;
return relative.norm();
}
//constrain_function
float constrain_function(float data, float Max)
{
if(abs(data)>Max)
{
return (data > 0) ? Max : -Max;
}
else
{
return data;
}
}
//constrain_function2
float constrain_function2(float data, float Min,float Max)
{
if(data > Max)
{
return Max;
}
else if (data < Min)
{
return Min;
}else
{
return data;
}
}
//sign_function
float sign_function(float data)
{
if(data>0)
{
return 1.0;
}
else if(data<0)
{
return -1.0;
}
else if(data == 0)
{
return 0.0;
}
}
// min function
float min(float data1,float data2)
{
if(data1>=data2)
{
return data2;
}
else
{
return data1;
}
}
//旋转矩阵：机体系到惯性系
Eigen::Matrix3f get_rotation_matrix(float phi, float theta, float psi)
{
Eigen::Matrix3f R_Body_to_ENU;
float r11 = cos(theta)*cos(psi);
float r12 = - cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi);
float r13 = sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi);
float r21 = cos(theta)*sin(psi);
float r22 = cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);
float r23 = - sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi);
float r31 = - sin(theta);
float r32 = sin(phi)*cos(theta);
float r33 = cos(phi)*cos(theta);
R_Body_to_ENU << r11,r12,r13,r21,r22,r23,r31,r32,r33;
return R_Body_to_ENU;
}
#endif

343
src/UAV/Modules/mission/include/ukf_car.h
Unescape View File

@ -1,343 +0,0 @@
/***************************************************************************************************************************
* ukf_car.h
*
* Author: Qyp
*
* Update Time: 2020.2.10
*
* 说明: UKF_CAR类,用于目标状态估计
*
* 系统状态方程：
* 观测方程：
***************************************************************************************************************************/
#ifndef UKF_CAR_H
#define UKF_CAR_H
#include <Eigen/Eigen>
#include <math.h>
#include <prometheus_msgs/DetectionInfo.h>
using namespace std;
using namespace Eigen;
class UKF_CAR
{
public:
//构造函数
UKF_CAR(void):
UKF_nh("~")
{
is_initialized = false;
// Read the noise std
UKF_nh.param<double>("UKF_CAR/std_a_", CAR_proc_noise.std_a_, 0.0);
UKF_nh.param<double>("UKF_CAR/std_yaw_dotdot_", CAR_proc_noise.std_yaw_dotdot_, 0.0);
UKF_nh.param<double>("UKF_CAR/std_px_", CAR_meas_noise.std_px_, 0.0);
UKF_nh.param<double>("UKF_CAR/std_py_", CAR_meas_noise.std_py_, 0.0);
UKF_nh.param<double>("UKF_CAR/std_yaw_", CAR_meas_noise.std_yaw_, 0.0);
cout<<"CAR_proc_noise.std_a_="<<CAR_proc_noise.std_a_<<endl;
cout<<"CAR_proc_noise.std_yaw_dotdot_="<<CAR_proc_noise.std_yaw_dotdot_<<endl;
cout<<"CAR_meas_noise.std_px_="<<CAR_meas_noise.std_px_<<endl;
cout<<"CAR_meas_noise.std_py_="<<CAR_meas_noise.std_py_<<endl;
cout<<"CAR_meas_noise.std_yaw_="<<CAR_meas_noise.std_yaw_<<endl;
n_x_ = 5;
n_noise_ = 2;
n_aug_ = n_x_ + n_noise_;
n_z_ = 3;
x_ = VectorXd(n_x_);
x_pre = VectorXd(n_x_);
z_ = VectorXd(n_z_);
P_pre = MatrixXd(n_x_,n_x_);
P_ = MatrixXd(n_x_,n_x_);
Q_ = MatrixXd(2,2);
R_ = MatrixXd(n_z_,n_z_);
P_ << CAR_meas_noise.std_px_*CAR_meas_noise.std_px_, 0, 0, 0, 0,
0, CAR_meas_noise.std_py_*CAR_meas_noise.std_py_, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, CAR_meas_noise.std_yaw_*CAR_meas_noise.std_yaw_, 0,
0, 0, 0, 0, 1;
Q_ << CAR_proc_noise.std_a_*CAR_proc_noise.std_a_, 0,
0,CAR_proc_noise.std_yaw_dotdot_*CAR_proc_noise.std_yaw_dotdot_;
R_ << CAR_meas_noise.std_px_*CAR_meas_noise.std_px_, 0, 0,
0, CAR_meas_noise.std_py_*CAR_meas_noise.std_py_, 0,
0, 0,CAR_meas_noise.std_yaw_*CAR_meas_noise.std_yaw_;
cout<<"P_="<<endl<<P_<<endl<<endl;
cout<<"Q_="<<endl<<Q_<<endl<<endl;
cout<<"R_="<<endl<<R_<<endl<<endl;
kamma_ = 3 - n_aug_;
//set weights
W_s = VectorXd(2*n_aug_+1);
W_s(0) = kamma_/(kamma_+n_aug_);
for(int i=1; i<2*n_aug_+1; ++i)
{
W_s(i) = 1/(2*kamma_+2*n_aug_);
}
Xsig_pred_ = MatrixXd(n_x_,2*n_aug_+1);
cout<<"[UKF]: "<<"CAR model selected."<<endl;
}
//initially set to false, set to ture in first call of Run()
bool is_initialized;
int n_x_; //系统状态维数
int n_noise_; //过程噪声维数
int n_aug_; //增广维数 = 系统状态维数 + 过程噪声维数
int n_z_; //测量状态维数
VectorXd x_; //系统状态变量 即 x(k)
VectorXd x_pre; //预测的系统状态变量 即 x(k|k-1)
VectorXd z_; //测量值
MatrixXd Q_; //过程噪声协方差矩阵
MatrixXd R_; //测量噪声协方差矩阵
MatrixXd P_pre; //预测状态误差协方差矩阵 即 P(k|k-1)
MatrixXd P_; //状态后验协方差矩阵
double kamma_; //sigma点缩放系数
VectorXd W_s; //sigma点权重
MatrixXd Xsig_pred_; //sigma点预测矩阵
// process noise standard deviation
// car model
struct
{
double std_a_;
double std_yaw_dotdot_;
}CAR_proc_noise;
// measurement noise standard deviation
// Vision measurement
struct
{
double std_px_;
double std_py_;
double std_yaw_;
}CAR_meas_noise;
// Process Measurement
void ProcessMeasurement();
// Prediction
// delta_t in s
void Prediction(double delta_t);
// Update Vision Measurement
void UpdateVision(const prometheus_msgs::DetectionInfo& mesurement);
// UKF main function
VectorXd Run(const prometheus_msgs::DetectionInfo& mesurement, double delta);
private:
ros::NodeHandle UKF_nh;
};
VectorXd UKF_CAR::Run(const prometheus_msgs::DetectionInfo& mesurement, double delta_t)
{
if(!is_initialized)
{
is_initialized = true;
x_[0] = mesurement.position[0];
x_[1] = mesurement.position[1];
x_[2] = 0.0;
x_[3] = mesurement.attitude[2];
x_[4] = 0.0;
cout<<"[UKF]: "<<"CAR model is initialized."<<endl;
}
// 预测
Prediction(delta_t);
UpdateVision(mesurement);
return x_;
}
void UKF_CAR::Prediction(double delta_t)
{
// 【UKF第一步】 构造sigma点
// x_aug为x_的增广状态向量 维度 = 原系统维度+系统噪声维度
VectorXd x_aug = VectorXd(n_aug_);
x_aug.head(5) = x_;
x_aug[5] = 0.0;
x_aug[6] = 0.0;
// P_aug为P_阵的增广矩阵
MatrixXd P_aug = MatrixXd(n_aug_,n_aug_);
P_aug.fill(0.0);
P_aug.topLeftCorner(5,5) = P_;
P_aug.bottomRightCorner(2,2) = Q_;
cout<<"P_aug="<<endl<<P_aug<<endl<<endl;
//Xsig_aug为产生的2na+1个sigma点
MatrixXd Xsig_aug = MatrixXd(n_aug_, 2*n_aug_+1);
Xsig_aug.fill(0.0);
//llt()是Cholesky 分解
//Cholesky分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解
//即 P_aug = L*L^t;
MatrixXd L = P_aug.llt().matrixL();
for(int i=0; i<2*n_aug_+1; ++i)
{
//第i列
Xsig_aug.col(i) = x_aug;
}
//matrix.block<p,q>(i,j) : 从 (i,j) 开始，大小为 (p,q) 矩阵块
Xsig_aug.block<7,7>(0,1) += sqrt(kamma_+n_aug_)*L;
Xsig_aug.block<7,7>(0,n_aug_+1) -= sqrt(kamma_+n_aug_)*L;
//cout<<"Xsig_aug="<<endl<<Xsig_aug<<endl<<endl;
// 【UKF第二步】 时间更新（预测） - 利用系统方程对状态预测
for(int i=0; i<2*n_aug_+1; ++i)
{
double p_x = Xsig_aug(0,i);
double p_y = Xsig_aug(1,i);
double v = Xsig_aug(2,i);
double yaw = Xsig_aug(3,i);
double yaw_dot = Xsig_aug(4,i);
double nu_a = Xsig_aug(5,i);
double nu_yaw_dotdot = Xsig_aug(6,i);
double px_pred, py_pred;
if (fabs(yaw_dot) > 0.001)
{
px_pred = p_x + v/yaw_dot * (sin(yaw+yaw_dot*delta_t) - sin(yaw));
py_pred = p_y - v/yaw_dot * (cos(yaw+yaw_dot*delta_t) - cos(yaw));
}
else
{
px_pred = p_x + v*cos(yaw)*delta_t;
py_pred = p_y + v*sin(yaw)*delta_t;
}
double v_pred = v;
double yaw_pred = yaw + yaw_dot*delta_t;
double yaw_dot_pred = yaw_dot;
//add noise
px_pred += 0.5*nu_a*delta_t*delta_t * cos(yaw);
py_pred += 0.5*nu_a*delta_t*delta_t * sin(yaw);
v_pred += nu_a*delta_t;
yaw_pred += 0.5*nu_yaw_dotdot*delta_t*delta_t;
yaw_dot_pred += nu_yaw_dotdot*delta_t;
// Xsig_pred_为 sigma点经过系统方程的非线性变化后得到
Xsig_pred_(0,i) = px_pred;
Xsig_pred_(1,i) = py_pred;
Xsig_pred_(2,i) = v_pred;
Xsig_pred_(3,i) = yaw_pred;
Xsig_pred_(4,i) = yaw_dot_pred;
}
//cout<<"Xsig_pred_="<<endl<<Xsig_pred_<<endl<<endl;
// 预测状态
x_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
x_pre += W_s(i)*Xsig_pred_.col(i);
}
cout<<"x_pre="<<endl<<x_pre<<endl<<endl;
// 预测协方差矩阵
P_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
// state difference
VectorXd x_diff = Xsig_pred_.col(i) - x_pre;
// angle normalization （偏航角）
while (x_diff(3)>M_PI) x_diff(3)-=2.*M_PI;
while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
P_pre += W_s(i)*x_diff*x_diff.transpose();
}
cout<<"P_pre="<<endl<<P_pre<<endl<<endl;
}
void UKF_CAR::UpdateVision(const prometheus_msgs::DetectionInfo& mesurement)
{
// 【UKF第三步】 测量更新
z_[0] = mesurement.position[0];
z_[1] = mesurement.position[1];
z_[2] = mesurement.attitude[2];
// Zsig为 Xsig_pred_经过测量方程的非线性变化后得到
MatrixXd Zsig = MatrixXd(n_z_, 2*n_aug_+1);
//观测预测值 - 观测方程
Zsig.fill(0.0);
for (int i=0; i<2*n_aug_+1; i++)
{
double p_x = Xsig_pred_(0,i);
double p_y = Xsig_pred_(1,i);
double yaw = Xsig_pred_(3,i);
Zsig(0,i) = p_x;
Zsig(1,i) = p_y;
Zsig(2,i) = yaw;
}
// z_pred为预测观测值
MatrixXd z_pred = VectorXd(n_z_);
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug_+1; ++i)
{
z_pred = z_pred + W_s(i) * Zsig.col(i);
}
MatrixXd S_ = MatrixXd(n_z_,n_z_); //预测测量误差协方差矩阵
MatrixXd T_ = MatrixXd(n_x_,n_z_); //状态与测量空间相关函数
S_.fill(0.0);
T_.fill(0.0);
for (int i = 0; i < 2 * n_aug_ + 1; ++i) { // 2n+1 simga points
// residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
S_ = S_ + W_s(i) * z_diff * z_diff.transpose();
VectorXd x_diff = Xsig_pred_.col(i) - x_;
// angle normalization
while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
T_ = T_ + W_s(i) * x_diff * z_diff.transpose();
}
S_ = S_ + R_; // add measurement noise covariance matrix
MatrixXd K_= MatrixXd(n_x_,n_z_); //卡尔曼增益K_
K_ = T_ * S_.inverse(); // Kalman gain K;
VectorXd z_diff = z_ - z_pred; // residual
while(z_diff(1)>M_PI) z_diff(1) -= 2.*M_PI;
while(z_diff(1)<-M_PI) z_diff(1) += 2.*M_PI; // angle normalization
// 【UKF第四步】 更新状态及P_阵
//zheli duima ?
x_ = x_pre + K_*z_diff;
P_ = P_pre - K_*S_*K_.transpose();
cout<<"x_="<<endl<<x_<<endl<<endl;
cout<<"P_="<<endl<<P_<<endl<<endl;
}
#endif

373
src/UAV/Modules/mission/include/ukf_nca.h
Unescape View File

@ -1,373 +0,0 @@
/***************************************************************************************************************************
* ukf_nca.h
*
* Author: Qyp
*
* Update Time: 2020.2.12
*
* 说明: UKF类,用于目标状态估计
*
* 系统状态方程：
* 观测方程：
***************************************************************************************************************************/
#ifndef UKF_H
#define UKF_H
#include <Eigen/Eigen>
#include <math.h>
#include <prometheus_msgs/DetectionInfo.h>
using namespace std;
using namespace Eigen;
class UKF_NCA
{
public:
//构造函数
UKF_NCA(void):
UKF_nh("~")
{
is_initialized = false;
// Read the noise std
UKF_nh.param<double>("UKF_NCA/std_ax_", NCA_proc_noise.std_ax_, 0.0);
UKF_nh.param<double>("UKF_NCA/std_ay_", NCA_proc_noise.std_ay_, 0.0);
UKF_nh.param<double>("UKF_NCA/std_az_", NCA_proc_noise.std_az_, 0.0);
UKF_nh.param<double>("UKF_NCA/std_px_", NCA_meas_noise.std_px_, 0.0);
UKF_nh.param<double>("UKF_NCA/std_py_", NCA_meas_noise.std_py_, 0.0);
UKF_nh.param<double>("UKF_NCA/std_pz_", NCA_meas_noise.std_pz_, 0.0);
cout<<"NCA_proc_noise.std_ax_="<<CAR_proc_noise.std_ax_<<endl;
cout<<"NCA_proc_noise.std_ay_="<<CAR_proc_noise.std_ay_<<endl;
cout<<"NCA_proc_noise.std_az_="<<CAR_proc_noise.std_az_<<endl;
cout<<"NCA_meas_noise.std_px_="<<CAR_meas_noise.std_px_<<endl;
cout<<"NCA_meas_noise.std_py_="<<CAR_meas_noise.std_py_<<endl;
cout<<"NCA_meas_noise.std_pz_="<<CAR_meas_noise.std_pz_<<endl;
n_x_ = 9;
n_noise_ = 3;
n_aug_ = n_x_ + n_noise_;
n_z_ = 3;
x_ = VectorXd(n_x_);
x_pre = VectorXd(n_x_);
z_ = VectorXd(n_z_);
P_pre = MatrixXd(n_x_,n_x_);
P_ = MatrixXd(n_x_,n_x_);
Q_ = MatrixXd(2,2);
R_ = MatrixXd(n_z_,n_z_);
P_ << NCA_meas_noise.std_px_*NCA_meas_noise.std_px_, 0, 0, 0, 0, 0, 0, 0, 0,
0, NCA_meas_noise.std_py_*NCA_meas_noise.std_py_, 0, 0, 0, 0, 0, 0, 0,
0, 0, NCA_meas_noise.std_pz_*NCA_meas_noise.std_pz_, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1;
Q_ << CAR_proc_noise.std_ax_*CAR_proc_noise.std_ax_, 0, 0,
0, CAR_proc_noise.std_ay_*CAR_proc_noise.std_ay_, 0,
0, 0, CAR_proc_noise.std_az_*CAR_proc_noise.std_az_;
R_ << NCA_meas_noise.std_px_*NCA_meas_noise.std_px_, 0, 0,
0, NCA_meas_noise.std_py_*NCA_meas_noise.std_py_, 0,
0, 0, NCA_meas_noise.std_pz_*NCA_meas_noise.std_pz_;
cout<<"P_="<<endl<<P_<<endl<<endl;
cout<<"Q_="<<endl<<Q_<<endl<<endl;
cout<<"R_="<<endl<<R_<<endl<<endl;
kamma_ = 3 - n_aug_;
//set weights
W_s = VectorXd(2*n_aug_+1);
W_s(0) = kamma_/(kamma_+n_aug_);
for(int i=1; i<2*n_aug_+1; ++i)
{
W_s(i) = 1/(2*kamma_+2*n_aug_);
}
Xsig_pred_ = MatrixXd(n_x_,2*n_aug_+1);
cout<<"[UKF]: "<<"NCA model selected."<<endl;
}
//initially set to false, set to ture in first call of Run()
bool is_initialized;
int n_x_; //系统状态维数
int n_noise_; //过程噪声维数
int n_aug_; //增广维数 = 系统状态维数 + 过程噪声维数
int n_z_; //测量状态维数
VectorXd x_; //系统状态变量 即 x(k)
VectorXd x_pre; //预测的系统状态变量 即 x(k|k-1)
VectorXd z_; //测量值
MatrixXd Q_; //过程噪声协方差矩阵
MatrixXd R_; //测量噪声协方差矩阵
MatrixXd P_pre; //预测状态误差协方差矩阵 即 P(k|k-1)
MatrixXd P_; //状态后验协方差矩阵
double kamma_; //sigma点缩放系数
VectorXd W_s; //sigma点权重
MatrixXd Xsig_pred_; //sigma点预测矩阵
// process noise standard deviation
// NCA model
struct
{
double std_ax_;
double std_ay_;
double std_az_;
}NCA_proc_noise;
// measurement noise standard deviation
// Vision measurement
struct
{
double std_px_;
double std_py_;
double std_pz_;
}NCA_meas_noise;
// Process Measurement
void ProcessMeasurement();
// Prediction
// delta_t in s
void Prediction(double delta_t);
// Update Vision Measurement
void UpdateVision(const prometheus_msgs::DetectionInfo& mesurement);
// UKF main function
VectorXd Run(const prometheus_msgs::DetectionInfo& mesurement, double delta);
private:
ros::NodeHandle UKF_nh;
};
VectorXd UKF::Run(const prometheus_msgs::DetectionInfo& mesurement, double delta_t)
{
if(!is_initialized)
{
is_initialized = true;
x_[0] = mesurement.position[0];
x_[1] = mesurement.position[1];
x_[2] = mesurement.position[2];
x_[3] = 0.0;
x_[4] = 0.0;
x_[5] = 0.0;
x_[6] = 0.0;
x_[7] = 0.0;
x_[8] = 0.0;
cout<<"[UKF]: "<<"NCA model is initialized."<<endl;
}
// 预测
Prediction(delta_t);
UpdateVision(mesurement);
return x_;
}
void UKF::Prediction(double delta_t)
{
// 【UKF第一步】 构造sigma点
// x_aug为x_的增广状态向量 维度 = 原系统维度+系统噪声维度
VectorXd x_aug = VectorXd(n_aug_);
x_aug.head(9) = x_;
x_aug[9] = 0.0;
x_aug[10] = 0.0;
x_aug[11] = 0.0;
// P_aug为P_阵的增广矩阵
MatrixXd P_aug = MatrixXd(n_aug_,n_aug_);
P_aug.fill(0.0);
P_aug.topLeftCorner(9,9) = P_;
P_aug.bottomRightCorner(3,3) = Q_;
cout<<"P_aug="<<endl<<P_aug<<endl<<endl;
//Xsig_aug为产生的2na+1个sigma点
MatrixXd Xsig_aug = MatrixXd(n_aug_, 2*n_aug_+1);
Xsig_aug.fill(0.0);
//llt()是Cholesky 分解
//Cholesky分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解
//即 P_aug = L*L^t;
MatrixXd L = P_aug.llt().matrixL();
for(int i=0; i<2*n_aug_+1; ++i)
{
//第i列
Xsig_aug.col(i) = x_aug;
}
//matrix.block<p,q>(i,j) : 从 (i,j) 开始，大小为 (p,q) 矩阵块
Xsig_aug.block<11,11>(0,1) += sqrt(kamma_+n_aug_)*L;
Xsig_aug.block<11,11>(0,n_aug_+1) -= sqrt(kamma_+n_aug_)*L;
//cout<<"Xsig_aug="<<endl<<Xsig_aug<<endl<<endl;
// 【UKF第二步】 时间更新（预测） - 利用系统方程对状态预测
for(int i=0; i<2*n_aug_+1; ++i)
{
double p_x = Xsig_aug(0,i);
double p_y = Xsig_aug(1,i);
double p_z = Xsig_aug(2,i);
double v_x = Xsig_aug(3,i);
double v_y = Xsig_aug(4,i);
double v_z = Xsig_aug(5,i);
double a_x = Xsig_aug(6,i);
double a_y = Xsig_aug(7,i);
double a_z = Xsig_aug(8,i);
double nu_a_x = Xsig_aug(9,i);
double nu_a_y = Xsig_aug(10,i);
double nu_a_z = Xsig_aug(11,i);
double px_pred, py_pred, pz_pred;
double vx_pred, vy_pred, vz_pred;
px_pred = p_x + v_x*delta_t + 0.5*a_x*delta_t*delta_t;
py_pred = p_y + v_y*delta_t + 0.5*a_y*delta_t*delta_t;
pz_pred = p_z + v_z*delta_t + 0.5*a_z*delta_t*delta_t;
vx_pred = v_x + a_x*delta_t;
vy_pred = v_y + a_y*delta_t;
vz_pred = v_z + a_z*delta_t;
//add noise
px_pred += 0.5*nu_a*delta_t*delta_t * cos(yaw);
py_pred += 0.5*nu_a*delta_t*delta_t * sin(yaw);
v_pred += nu_a*delta_t;
yaw_pred += 0.5*nu_yaw_dotdot*delta_t*delta_t;
yaw_dot_pred += nu_yaw_dotdot*delta_t;
// Xsig_pred_为 sigma点经过系统方程的非线性变化后得到
Xsig_pred_(0,i) = px_pred;
Xsig_pred_(1,i) = py_pred;
Xsig_pred_(2,i) = pz_pred;
Xsig_pred_(3,i) = vx_pred;
Xsig_pred_(4,i) = vy_pred;
Xsig_pred_(5,i) = vz_pred;
Xsig_pred_(6,i) = ax_pred;
Xsig_pred_(7,i) = ay_pred;
Xsig_pred_(8,i) = az_pred;
}
//cout<<"Xsig_pred_="<<endl<<Xsig_pred_<<endl<<endl;
// 预测状态
x_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
x_pre += W_s(i)*Xsig_pred_.col(i);
}
cout<<"x_pre="<<endl<<x_pre<<endl<<endl;
// 预测协方差矩阵
P_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
// state difference
VectorXd x_diff = Xsig_pred_.col(i) - x_pre;
// angle normalization （偏航角）
while (x_diff(3)>M_PI) x_diff(3)-=2.*M_PI;
while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
P_pre += W_s(i)*x_diff*x_diff.transpose();
}
cout<<"P_pre="<<endl<<P_pre<<endl<<endl;
}
void UKF::UpdateVision(const prometheus_msgs::DetectionInfo& mesurement)
{
if (model_type == NCA)
{
}
// car model
else if (model_type == CAR)
{
// 【UKF第三步】 测量更新
z_[0] = mesurement.position[0];
z_[1] = mesurement.position[1];
z_[2] = mesurement.attitude[2];
// Zsig为 Xsig_pred_经过测量方程的非线性变化后得到
MatrixXd Zsig = MatrixXd(n_z_, 2*n_aug_+1);
//观测预测值 - 观测方程
Zsig.fill(0.0);
for (int i=0; i<2*n_aug_+1; i++)
{
double p_x = Xsig_pred_(0,i);
double p_y = Xsig_pred_(1,i);
double yaw = Xsig_pred_(3,i);
Zsig(0,i) = p_x;
Zsig(1,i) = p_y;
Zsig(2,i) = yaw;
}
// z_pred为预测观测值
MatrixXd z_pred = VectorXd(n_z_);
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug_+1; ++i)
{
z_pred = z_pred + W_s(i) * Zsig.col(i);
}
MatrixXd S_ = MatrixXd(n_z_,n_z_); //预测测量误差协方差矩阵
MatrixXd T_ = MatrixXd(n_x_,n_z_); //状态与测量空间相关函数
S_.fill(0.0);
T_.fill(0.0);
for (int i = 0; i < 2 * n_aug_ + 1; ++i) { // 2n+1 simga points
// residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
S_ = S_ + W_s(i) * z_diff * z_diff.transpose();
VectorXd x_diff = Xsig_pred_.col(i) - x_;
// angle normalization
while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
T_ = T_ + W_s(i) * x_diff * z_diff.transpose();
}
S_ = S_ + R_; // add measurement noise covariance matrix
MatrixXd K_= MatrixXd(n_x_,n_z_); //卡尔曼增益K_
K_ = T_ * S_.inverse(); // Kalman gain K;
VectorXd z_diff = z_ - z_pred; // residual
while(z_diff(1)>M_PI) z_diff(1) -= 2.*M_PI;
while(z_diff(1)<-M_PI) z_diff(1) += 2.*M_PI; // angle normalization
// 【UKF第四步】 更新状态及P_阵
//zheli duima ?
x_ = x_pre + K_*z_diff;
P_ = P_pre - K_*S_*K_.transpose();
cout<<"x_="<<endl<<x_<<endl<<endl;
cout<<"P_="<<endl<<P_<<endl<<endl;
}
}
#endif

337
src/UAV/Modules/mission/include/ukf_ncv.h
Unescape View File

@ -1,337 +0,0 @@
/***************************************************************************************************************************
* ukf_ncv.h
*
* Author: Qyp
*
* Update Time: 2020.2.12
*
* 说明: UKF类,用于目标状态估计
*
* 系统状态方程：
* 观测方程：
***************************************************************************************************************************/
#ifndef UKF_H
#define UKF_H
#include <Eigen/Eigen>
#include <math.h>
#include <prometheus_msgs/DetectionInfo.h>
using namespace std;
using namespace Eigen;
class UKF_NCV
{
public:
//构造函数
UKF_NCV(void):
UKF_nh("~")
{
is_initialized = false;
// Read the noise std
UKF_nh.param<double>("UKF_NCV/std_ax_", NCV_proc_noise.std_ax_, 0.0);
UKF_nh.param<double>("UKF_NCV/std_ay_", NCV_proc_noise.std_ay_, 0.0);
UKF_nh.param<double>("UKF_NCV/std_az_", NCV_proc_noise.std_az_, 0.0);
UKF_nh.param<double>("UKF_NCV/std_px_", NCV_meas_noise.std_px_, 0.0);
UKF_nh.param<double>("UKF_NCV/std_py_", NCV_meas_noise.std_py_, 0.0);
UKF_nh.param<double>("UKF_NCV/std_pz_", NCV_meas_noise.std_pz_, 0.0);
cout<<"NCV_proc_noise.std_ax_="<<NCV_proc_noise.std_ax_<<endl;
cout<<"NCV_proc_noise.std_ay_="<<NCV_proc_noise.std_ay_<<endl;
cout<<"NCV_proc_noise.std_az_="<<NCV_proc_noise.std_az_<<endl;
cout<<"NCV_meas_noise.std_px_="<<NCV_meas_noise.std_px_<<endl;
cout<<"NCV_meas_noise.std_py_="<<NCV_meas_noise.std_py_<<endl;
cout<<"NCV_meas_noise.std_pz_="<<NCV_meas_noise.std_pz_<<endl;
n_x_ = 6;
n_noise_ = 3;
n_aug_ = n_x_ + n_noise_;
n_z_ = 3;
x_ = VectorXd(n_x_);
x_pre = VectorXd(n_x_);
z_ = VectorXd(n_z_);
P_pre = MatrixXd(n_x_,n_x_);
P_ = MatrixXd(n_x_,n_x_);
Q_ = MatrixXd(2,2);
R_ = MatrixXd(n_z_,n_z_);
P_ << NCV_meas_noise.std_px_*NCV_meas_noise.std_px_, 0, 0, 0, 0, 0,
0, NCV_meas_noise.std_py_*NCV_meas_noise.std_py_, 0, 0, 0, 0,
0, 0, NCV_meas_noise.std_pz_*NCV_meas_noise.std_pz_, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;
Q_ << NCV_proc_noise.std_ax_*NCV_proc_noise.std_ax_, 0, 0,
0, NCV_proc_noise.std_ay_*NCV_proc_noise.std_ay_, 0,
0, 0, NCV_proc_noise.std_az_*NCV_proc_noise.std_az_;
R_ << NCV_meas_noise.std_px_*NCV_meas_noise.std_px_, 0, 0,
0, NCV_meas_noise.std_py_*NCV_meas_noise.std_py_, 0,
0, 0, NCV_meas_noise.std_pz_*NCV_meas_noise.std_pz_;
cout<<"P_="<<endl<<P_<<endl<<endl;
cout<<"Q_="<<endl<<Q_<<endl<<endl;
cout<<"R_="<<endl<<R_<<endl<<endl;
kamma_ = 3 - n_aug_;
//set weights
W_s = VectorXd(2*n_aug_+1);
W_s(0) = kamma_/(kamma_+n_aug_);
for(int i=1; i<2*n_aug_+1; ++i)
{
W_s(i) = 1/(2*kamma_+2*n_aug_);
}
Xsig_pred_ = MatrixXd(n_x_,2*n_aug_+1);
cout<<"[UKF]: "<<"NCV model selected."<<endl;
}
//initially set to false, set to ture in first call of Run()
bool is_initialized;
int n_x_; //系统状态维数
int n_noise_; //过程噪声维数
int n_aug_; //增广维数 = 系统状态维数 + 过程噪声维数
int n_z_; //测量状态维数
VectorXd x_; //系统状态变量 即 x(k)
VectorXd x_pre; //预测的系统状态变量 即 x(k|k-1)
VectorXd z_; //测量值
MatrixXd Q_; //过程噪声协方差矩阵
MatrixXd R_; //测量噪声协方差矩阵
MatrixXd P_pre; //预测状态误差协方差矩阵 即 P(k|k-1)
MatrixXd P_; //状态后验协方差矩阵
double kamma_; //sigma点缩放系数
VectorXd W_s; //sigma点权重
MatrixXd Xsig_pred_; //sigma点预测矩阵
// process noise standard deviation
// NCV model
struct
{
double std_ax_;
double std_ay_;
double std_az_;
}NCV_proc_noise;
// measurement noise standard deviation
// Vision measurement
struct
{
double std_px_;
double std_py_;
double std_pz_;
}NCV_meas_noise;
// Process Measurement
void ProcessMeasurement();
// Prediction
// delta_t in s
void Prediction(double delta_t);
// Update Vision Measurement
void UpdateVision(const prometheus_msgs::DetectionInfo& mesurement);
// UKF main function
VectorXd Run(const prometheus_msgs::DetectionInfo& mesurement, double delta);
private:
ros::NodeHandle UKF_nh;
};
VectorXd UKF_NCV::Run(const prometheus_msgs::DetectionInfo& mesurement, double delta_t)
{
if(!is_initialized)
{
is_initialized = true;
x_[0] = mesurement.position[0];
x_[1] = mesurement.position[1];
x_[2] = mesurement.position[2];
x_[3] = 0.0;
x_[4] = 0.0;
x_[5] = 0.0;
cout<<"[UKF]: "<<"NCV model is initialized."<<endl;
}
// 预测
Prediction(delta_t);
UpdateVision(mesurement);
return x_;
}
void UKF_NCV::Prediction(double delta_t)
{
// 【UKF第一步】 构造sigma点
// x_aug为x_的增广状态向量 维度 = 原系统维度+系统噪声维度
VectorXd x_aug = VectorXd(n_aug_);
x_aug.head(6) = x_;
x_aug[7] = 0.0;
x_aug[8] = 0.0;
x_aug[9] = 0.0;
// P_aug为P_阵的增广矩阵
MatrixXd P_aug = MatrixXd(n_aug_,n_aug_);
P_aug.fill(0.0);
P_aug.topLeftCorner(6,6) = P_;
P_aug.bottomRightCorner(3,3) = Q_;
cout<<"P_aug="<<endl<<P_aug<<endl<<endl;
//Xsig_aug为产生的2na+1个sigma点
MatrixXd Xsig_aug = MatrixXd(n_aug_, 2*n_aug_+1);
Xsig_aug.fill(0.0);
//llt()是Cholesky 分解
//Cholesky分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解
//即 P_aug = L*L^t;
MatrixXd L = P_aug.llt().matrixL();
for(int i=0; i<2*n_aug_+1; ++i)
{
//第i列
Xsig_aug.col(i) = x_aug;
}
//matrix.block<p,q>(i,j) : 从 (i,j) 开始，大小为 (p,q) 矩阵块
Xsig_aug.block<9,9>(0,1) += sqrt(kamma_+n_aug_)*L;
Xsig_aug.block<9,9>(0,n_aug_+1) -= sqrt(kamma_+n_aug_)*L;
//cout<<"Xsig_aug="<<endl<<Xsig_aug<<endl<<endl;
// 【UKF第二步】 时间更新（预测） - 利用系统方程对状态预测
for(int i=0; i<2*n_aug_+1; ++i)
{
double p_x = Xsig_aug(0,i);
double p_y = Xsig_aug(1,i);
double p_z = Xsig_aug(2,i);
double v_x = Xsig_aug(3,i);
double v_y = Xsig_aug(4,i);
double v_z = Xsig_aug(5,i);
double nu_a_x = Xsig_aug(6,i);
double nu_a_y = Xsig_aug(7,i);
double nu_a_z = Xsig_aug(8,i);
double px_pred, py_pred, pz_pred;
double vx_pred, vy_pred, vz_pred;
px_pred = p_x + v_x*delta_t;
py_pred = p_y + v_y*delta_t;
pz_pred = p_z + v_z*delta_t;
vx_pred = v_x;
vy_pred = v_y;
vz_pred = v_z;
//add noise
px_pred += 0.5*nu_a_x*delta_t*delta_t;
py_pred += 0.5*nu_a_y*delta_t*delta_t;
pz_pred += 0.5*nu_a_z*delta_t*delta_t;
vx_pred += nu_a_x*delta_t;
vy_pred += nu_a_y*delta_t;
vz_pred += nu_a_z*delta_t;
// Xsig_pred_为 sigma点经过系统方程的非线性变化后得到
Xsig_pred_(0,i) = px_pred;
Xsig_pred_(1,i) = py_pred;
Xsig_pred_(2,i) = pz_pred;
Xsig_pred_(3,i) = vx_pred;
Xsig_pred_(4,i) = vy_pred;
Xsig_pred_(5,i) = vz_pred;
}
//cout<<"Xsig_pred_="<<endl<<Xsig_pred_<<endl<<endl;
// 预测状态
x_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
x_pre += W_s(i)*Xsig_pred_.col(i);
}
cout<<"x_pre="<<endl<<x_pre<<endl<<endl;
// 预测协方差矩阵
P_pre.fill(0.0);
for (int i=0; i<2*n_aug_+1; ++i)
{
// state difference
VectorXd x_diff = Xsig_pred_.col(i) - x_pre;
P_pre += W_s(i)*x_diff*x_diff.transpose();
}
cout<<"P_pre="<<endl<<P_pre<<endl<<endl;
}
void UKF_NCV::UpdateVision(const prometheus_msgs::DetectionInfo& mesurement)
{
// 【UKF第三步】 测量更新
z_[0] = mesurement.position[0];
z_[1] = mesurement.position[1];
z_[2] = mesurement.attitude[2];
// Zsig为 Xsig_pred_经过测量方程的非线性变化后得到
MatrixXd Zsig = MatrixXd(n_z_, 2*n_aug_+1);
//观测预测值 - 观测方程
Zsig.fill(0.0);
for (int i=0; i<2*n_aug_+1; i++)
{
double p_x = Xsig_pred_(0,i);
double p_y = Xsig_pred_(1,i);
double p_z = Xsig_pred_(2,i);
Zsig(0,i) = p_x;
Zsig(1,i) = p_y;
Zsig(2,i) = p_z;
}
// z_pred为预测观测值
MatrixXd z_pred = VectorXd(n_z_);
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug_+1; ++i)
{
z_pred = z_pred + W_s(i) * Zsig.col(i);
}
MatrixXd S_ = MatrixXd(n_z_,n_z_); //预测测量误差协方差矩阵
MatrixXd T_ = MatrixXd(n_x_,n_z_); //状态与测量空间相关函数
S_.fill(0.0);
T_.fill(0.0);
for (int i = 0; i < 2 * n_aug_ + 1; ++i) { // 2n+1 simga points
// residual
VectorXd z_diff = Zsig.col(i) - z_pred;
S_ = S_ + W_s(i) * z_diff * z_diff.transpose();
VectorXd x_diff = Xsig_pred_.col(i) - x_;
T_ = T_ + W_s(i) * x_diff * z_diff.transpose();
}
S_ = S_ + R_; // add measurement noise covariance matrix
MatrixXd K_= MatrixXd(n_x_,n_z_); //卡尔曼增益K_
K_ = T_ * S_.inverse(); // Kalman gain K;
VectorXd z_diff = z_ - z_pred; // residual
// 【UKF第四步】 更新状态及P_阵
//zheli duima ?
x_ = x_pre + K_*z_diff;
P_ = P_pre - K_*S_*K_.transpose();
cout<<"x_="<<endl<<x_<<endl<<endl;
cout<<"P_="<<endl<<P_<<endl<<endl;
}
#endif

627
src/UAV/Modules/mission/indoor_competition/indoor_competition.cpp
Unescape View File

@ -1,627 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
using namespace std;
#define START_POINT_X -6.5
#define START_POINT_Y 0.0
#define START_POINT_Z 1.7
#define START_POINT_YAW 0.0
#define CIRCLE_POINT_X -3.5
#define CIRCLE_POINT_Y 0.0
#define CIRCLE_POINT_Z 1.8
#define CIRCLE_POINT_YAW 0.0
#define PILLAR_POINT_X 1.5
#define PILLAR_POINT_Y 0.0
#define PILLAR_POINT_Z 1.8
#define PILLAR_POINT_YAW 0.0
#define CORRIDOR_POINT_X 9.5
#define CORRIDOR_POINT_Y -1.0
#define CORRIDOR_POINT_Z 1.8
#define CORRIDOR_POINT_YAW 0.0
#define NUM_POINT_X 17
#define NUM_POINT_Y 0.0
#define NUM_POINT_Z 1.8
#define NUM_POINT_YAW 0.0
#define FOLLOWING_VEL 0.5
#define FOLLOWING_KP 2.0
#define LAND_POINT_X 23
#define LAND_POINT_Y 0.0
#define LAND_POINT_Z 1.8
#define LAND_POINT_YAW 0.0
#define LANDPAD_HEIGHT 0.0
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
Eigen::Vector3f start_point;
//椭圆穿越
Detection_result ellipse_det;
//避障任务
geometry_msgs::Point desired_vel;
int flag_get_cmd = 0;
int flag_get_cmd_a = 0;
//走廊穿越
struct global_planner
{
// 规划路径
nav_msgs::Path path_cmd;
int Num_total_wp;
int wp_id;
int start_id;
}A_star;
//数字识别+颜色巡线
int flag_detection;
float error_body_y;
float yaw_sp;
//自主降落
Detection_result landpad_det;
Eigen::Vector3f pos_des_prev;
float kpx_land,kpy_land,kpz_land; //控制参数 - 比例参数
//无人机状态
prometheus_msgs::DroneState _DroneState; //无人机状态量
Eigen::Matrix3f R_Body_to_ENU;
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub,goal_pub;
ros::Publisher local_planner_switch_pub,global_planner_switch_pub,circle_switch_pub, num_det_switch_pub, color_det_switch_pub, pad_det_switch_pub;
std_msgs::Bool switch_on;
std_msgs::Bool switch_off;
// 状态机
int State_Machine = 0;
float kpx_circle_track,kpy_circle_track,kpz_circle_track; //控制参数 - 比例参数
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void A_star_planner();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void ellipse_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr& msg)
{
ellipse_det.object_name = "circle";
ellipse_det.Detection_info = *msg;
ellipse_det.pos_body_frame[0] = ellipse_det.Detection_info.position[2] + FRONT_CAMERA_OFFSET_X;
ellipse_det.pos_body_frame[1] = - ellipse_det.Detection_info.position[0] + FRONT_CAMERA_OFFSET_Y;
ellipse_det.pos_body_frame[2] = - ellipse_det.Detection_info.position[1] + FRONT_CAMERA_OFFSET_Z;
ellipse_det.pos_body_enu_frame = R_Body_to_ENU * ellipse_det.pos_body_frame;
if(ellipse_det.Detection_info.detected)
{
ellipse_det.num_regain++;
ellipse_det.num_lost = 0;
}else
{
ellipse_det.num_regain = 0;
ellipse_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(ellipse_det.num_lost > VISION_THRES)
{
ellipse_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(ellipse_det.num_regain > VISION_THRES)
{
ellipse_det.is_detected = true;
}
}
void local_planner_cmd_cb(const geometry_msgs::Point::ConstPtr& msg)
{
flag_get_cmd = 1;
desired_vel = *msg;
}
void global_planner_cmd_cb(const nav_msgs::Path::ConstPtr& msg)
{
flag_get_cmd_a = flag_get_cmd_a + 1;
A_star.path_cmd = *msg;
A_star.Num_total_wp = A_star.path_cmd.poses.size();
//选择与当前无人机所在位置最近的点,并从该点开始追踪
A_star.start_id = 0;
float distance_to_wp_min = abs(A_star.path_cmd.poses[0].pose.position.x - _DroneState.position[0])
+ abs(A_star.path_cmd.poses[0].pose.position.y - _DroneState.position[1]);
for (int j=1;j<A_star.Num_total_wp;j++)
{
float distance_to_wp = abs(A_star.path_cmd.poses[j].pose.position.x - _DroneState.position[0])
+ abs(A_star.path_cmd.poses[j].pose.position.y - _DroneState.position[1]);
if(distance_to_wp < distance_to_wp_min)
{
distance_to_wp_min = distance_to_wp;
A_star.start_id = j;
}
}
//这里增大开始路径点是为了解决当得到新路径时,无人机会回头的问题
A_star.wp_id = A_star.start_id + 1;
if(A_star.Num_total_wp - A_star.start_id > 8)
{
A_star.wp_id = A_star.start_id + 7;
}
}
void color_line_cb(const geometry_msgs::Pose::ConstPtr &msg)
{
error_body_y = - tan(msg->position.x) * NUM_POINT_Z;
flag_detection = msg->position.y;
float x1 = msg->orientation.w;
float y1 = msg->orientation.x;
float x2 = msg->orientation.y;
float y2 = msg->orientation.z;
float next_desired_yaw = - atan2(y2 - y1, x2 - x1);
yaw_sp = (0.7*yaw_sp + 0.3*next_desired_yaw);
}
void landpad_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
landpad_det.object_name = "landpad";
landpad_det.Detection_info = *msg;
landpad_det.pos_body_frame[0] = - landpad_det.Detection_info.position[1] + DOWN_CAMERA_OFFSET_X;
landpad_det.pos_body_frame[1] = - landpad_det.Detection_info.position[0] + DOWN_CAMERA_OFFSET_Y;
landpad_det.pos_body_frame[2] = - landpad_det.Detection_info.position[2] + DOWN_CAMERA_OFFSET_Z;
landpad_det.pos_body_enu_frame = R_Body_to_ENU * landpad_det.pos_body_frame;
//若已知降落板高度，则无需使用深度信息。
landpad_det.pos_body_enu_frame[2] = LANDPAD_HEIGHT - _DroneState.position[2];
landpad_det.pos_enu_frame[0] = _DroneState.position[0] + landpad_det.pos_body_enu_frame[0];
landpad_det.pos_enu_frame[1] = _DroneState.position[1] + landpad_det.pos_body_enu_frame[1];
landpad_det.pos_enu_frame[2] = _DroneState.position[2] + landpad_det.pos_body_enu_frame[2];
// landpad_det.att_enu_frame[2] = _DroneState.attitude[2] + Detection_raw.attitude[2];
landpad_det.att_enu_frame[2] = 0.0;
if(landpad_det.Detection_info.detected)
{
landpad_det.num_regain++;
landpad_det.num_lost = 0;
}else
{
landpad_det.num_regain = 0;
landpad_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(landpad_det.num_lost > VISION_THRES)
{
landpad_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(landpad_det.num_regain > VISION_THRES)
{
landpad_det.is_detected = true;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "indoor_competition");
ros::NodeHandle nh("~");
//【订阅】椭圆识别结果,用于形状穿越
ros::Subscriber ellipse_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/ellipse_det", 10, ellipse_det_cb);
ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpad_det_cb);
//ros::Subscriber num_det_sub = nh.subscribe<prometheus_msgs::MultiDetectionInfo>("/prometheus/object_detection/num_det", 10, num_det_cb);
ros::Subscriber color_line_sub = nh.subscribe<geometry_msgs::Pose>("/prometheus/object_detection/color_line_angle", 10, color_line_cb);
//【订阅】局部路径规划结果,用于避开障碍物 柱子
ros::Subscriber local_planner_sub = nh.subscribe<geometry_msgs::Point>("/prometheus/local_planner/desired_vel", 50, local_planner_cmd_cb);
//【订阅】全局路径规划结果,用于避开障碍物 走廊
ros::Subscriber global_planner_sub = nh.subscribe<nav_msgs::Path>("/prometheus/global_planner/path_cmd", 50, global_planner_cmd_cb);
//【订阅】无人机当前状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/planning/goal", 10);
// 为了避免同时运行过多程序导致电脑奔溃，设置程序运行/休眠 开关，但这个功能尚未完全启用
local_planner_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/local_planner", 10);
global_planner_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/global_planner", 10);
circle_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/circle_crossing", 10);
num_det_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/num_det", 10);
color_det_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/color_det", 10);
pad_det_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/pad_det", 10);
switch_on.data = true;
switch_off.data = false;
//关闭所有节点
local_planner_switch_pub.publish(switch_off);
global_planner_switch_pub.publish(switch_off);
circle_switch_pub.publish(switch_off);
num_det_switch_pub.publish(switch_off);
color_det_switch_pub.publish(switch_off);
pad_det_switch_pub.publish(switch_off);
nh.param<float>("kpx_circle_track", kpx_circle_track, 0.1);
nh.param<float>("kpy_circle_track", kpy_circle_track, 0.1);
nh.param<float>("kpz_circle_track", kpz_circle_track, 0.1);
nh.param<float>("kpx_land", kpx_land, 0.1);
nh.param<float>("kpy_land", kpy_land, 0.1);
nh.param<float>("kpz_land", kpz_land, 0.1);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Indoor Competition<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to start the mission..."<<endl;
cin >> start_flag;
}
// 阶段1: 解锁并起飞
Command_Now.Command_ID = 1;
while( State_Machine == 0)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = START_POINT_X;
Command_Now.Reference_State.position_ref[1] = START_POINT_Y;
Command_Now.Reference_State.position_ref[2] = START_POINT_Z;
Command_Now.Reference_State.yaw_ref = START_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
float dis = cal_distance(Eigen::Vector3f(_DroneState.position[0],_DroneState.position[1],_DroneState.position[2]),
Eigen::Vector3f(START_POINT_X, START_POINT_Y, START_POINT_Z));
ros::spinOnce();
if(dis < DIS_THRES)
{
State_Machine = 1;
}
}
//阶段2: 过门,准备穿圆
while(State_Machine == 1)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = CIRCLE_POINT_X;
Command_Now.Reference_State.position_ref[1] = CIRCLE_POINT_Y;
Command_Now.Reference_State.position_ref[2] = CIRCLE_POINT_Z;
Command_Now.Reference_State.yaw_ref = CIRCLE_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Moving to CIRCLE_POINT ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
float dis = cal_distance(Eigen::Vector3f(_DroneState.position[0],_DroneState.position[1],_DroneState.position[2]),
Eigen::Vector3f(CIRCLE_POINT_X, CIRCLE_POINT_Y, CIRCLE_POINT_Z));
if(ellipse_det.is_detected && dis < DIS_THRES)
{
State_Machine = 2;
}
}
//阶段3: 传圆
circle_switch_pub.publish(switch_on);
while(State_Machine == 2)
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0;
Command_Now.Reference_State.position_ref[1] = 0;
Command_Now.Reference_State.position_ref[2] = 0;
Command_Now.Reference_State.velocity_ref[0] = kpx_circle_track * ellipse_det.pos_body_enu_frame[0];
Command_Now.Reference_State.velocity_ref[1] = kpy_circle_track * ellipse_det.pos_body_enu_frame[1];
Command_Now.Reference_State.velocity_ref[2] = kpz_circle_track * ellipse_det.pos_body_enu_frame[2];
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
cout << "Tracking the cricle ..."<<endl;
printf_detection_result(ellipse_det);
ros::spinOnce();
ros::Duration(0.05).sleep();
if(abs(ellipse_det.pos_body_enu_frame[0]) < 1)
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_Now.Reference_State.position_ref[0] = 2.0;
Command_Now.Reference_State.position_ref[1] = 0;
Command_Now.Reference_State.position_ref[2] = 0;
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
ros::Duration(5.0).sleep();
State_Machine = 3;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = PILLAR_POINT_X;
Command_Now.Reference_State.position_ref[1] = PILLAR_POINT_Y;
Command_Now.Reference_State.position_ref[2] = PILLAR_POINT_Z;
Command_Now.Reference_State.yaw_ref = PILLAR_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Moving to PILLAR_POINT ..."<<endl;
}
}
circle_switch_pub.publish(switch_off);
//发布目标
geometry_msgs::PoseStamped goal;
goal.pose.position.x = CORRIDOR_POINT_X;
goal.pose.position.y = CORRIDOR_POINT_Y;
goal.pose.position.z = CORRIDOR_POINT_Z;
while(flag_get_cmd == 0)
{
goal_pub.publish(goal);
local_planner_switch_pub.publish(switch_on);
cout << "Turn on the local planner and pub the goal point ..."<<endl;
ros::spinOnce();
ros::Duration(0.05).sleep();
}
while(State_Machine == 3)
{
// 高度改为定高飞行
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.velocity_ref[0] = desired_vel.x;
Command_Now.Reference_State.velocity_ref[1] = desired_vel.y;
Command_Now.Reference_State.position_ref[2] = CORRIDOR_POINT_Z;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "APF planner:"<<endl;
cout << "desired_vel: " << desired_vel.x << " [m/s] "<< desired_vel.y << " [m/s] "<< desired_vel.z << " [m/s] "<<endl;
cout << "drone_pos: " << _DroneState.position[0] << " [m] "<< _DroneState.position[1] << " [m] "<< _DroneState.position[2] << " [m] "<<endl;
cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
ros::spinOnce();
ros::Duration(0.05).sleep();
float dis = abs(_DroneState.position[0] - goal.pose.position.x);
if(dis < (DIS_THRES+0.3))
{
State_Machine = 4;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = CORRIDOR_POINT_X;
Command_Now.Reference_State.position_ref[1] = CORRIDOR_POINT_Y;
Command_Now.Reference_State.position_ref[2] = CORRIDOR_POINT_Z;
Command_Now.Reference_State.yaw_ref = CORRIDOR_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Moving to CORRIDOR_POINT ..."<<endl;
ros::Duration(2.0).sleep();
}
}
//发布目标
goal.pose.position.x = NUM_POINT_X;
goal.pose.position.y = NUM_POINT_Y;
goal.pose.position.z = NUM_POINT_Z;
while(flag_get_cmd_a == 0)
{
goal_pub.publish(goal);
local_planner_switch_pub.publish(switch_off);
global_planner_switch_pub.publish(switch_on);
cout << "Turn on the global planner and pub the goal point ..."<<endl;
ros::spinOnce();
ros::Duration(0.05).sleep();
}
while(State_Machine == 4)
{
A_star_planner();
float dis = abs(_DroneState.position[0] - goal.pose.position.x);
if(dis < 0.4)
{
State_Machine = 5;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = NUM_POINT_X;
Command_Now.Reference_State.position_ref[1] = NUM_POINT_Y;
Command_Now.Reference_State.position_ref[2] = NUM_POINT_Z;
Command_Now.Reference_State.yaw_ref = NUM_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Moving to NUM_POINT ..."<<endl;
ros::Duration(2.0).sleep();
}
}
global_planner_switch_pub.publish(switch_off);
color_det_switch_pub.publish(switch_on);
//巡线
while(State_Machine == 5)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_VEL_Z_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::MIX_FRAME;
Command_Now.Reference_State.velocity_ref[0] = FOLLOWING_VEL;
Command_Now.Reference_State.velocity_ref[1] = FOLLOWING_KP * error_body_y;
Command_Now.Reference_State.position_ref[2] = NUM_POINT_Z;
Command_Now.Reference_State.yaw_ref = yaw_sp;
//Publish
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
command_pub.publish(Command_Now);
cout << "Color Line Following... "<< endl;
cout << "error_body_y: " << error_body_y << " [m] "<<endl;
cout << "yaw_sp: " << yaw_sp/3.1415926 *180 << " [deg] "<<endl;
ros::spinOnce();
ros::Duration(0.05).sleep();
if(_DroneState.position[0] > LAND_POINT_X - 1.0)
{
State_Machine = 6;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = LAND_POINT_X;
Command_Now.Reference_State.position_ref[1] = LAND_POINT_Y;
Command_Now.Reference_State.position_ref[2] = LAND_POINT_Z;
Command_Now.Reference_State.yaw_ref = LAND_POINT_YAW;
command_pub.publish(Command_Now);
cout << "Moving to LAND_POINT ..."<<endl;
ros::Duration(4.0).sleep();
}
}
pos_des_prev[0] = _DroneState.position[0];
pos_des_prev[1] = _DroneState.position[1];
pos_des_prev[2] = _DroneState.position[2];
color_det_switch_pub.publish(switch_off);
pad_det_switch_pub.publish(switch_on);
while(State_Machine == 6)
{
float distance_to_setpoint = landpad_det.pos_body_enu_frame.norm();
cout <<"[autonomous_landing]: Tracking the Landing Pad, distance_to_setpoint : "<< distance_to_setpoint << " [m] " << endl;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS; //xy velocity z position
Command_Now.Reference_State.yaw_ref = 0.0;
Eigen::Vector3f vel_command;
vel_command[0] = kpx_land * landpad_det.pos_body_enu_frame[0];
vel_command[1] = kpy_land * landpad_det.pos_body_enu_frame[1];
vel_command[2] = kpz_land * landpad_det.pos_body_enu_frame[2];
for (int i=0; i<3; i++)
{
Command_Now.Reference_State.position_ref[i] = pos_des_prev[i] + vel_command[i]* 0.05;
pos_des_prev[i] = Command_Now.Reference_State.position_ref[i];
}
command_pub.publish(Command_Now);
cout << "Autonomous Landing... "<< endl;
ros::spinOnce();
ros::Duration(0.05).sleep();
while(_DroneState.position[2] < 0.4)
{
State_Machine = 7;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
command_pub.publish(Command_Now);
cout << "Landing and disarm ..."<<endl;
cout << "The end of the indoor competition ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}
pad_det_switch_pub.publish(switch_off);
return 0;
}
void A_star_planner()
{
float current_cmd_id = flag_get_cmd_a;
//执行给定航点
while( A_star.wp_id < A_star.Num_total_wp && flag_get_cmd_a == current_cmd_id)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = A_star.path_cmd.poses[A_star.wp_id].pose.position.x;
Command_Now.Reference_State.position_ref[1] = A_star.path_cmd.poses[A_star.wp_id].pose.position.y;
Command_Now.Reference_State.position_ref[2] = A_star.path_cmd.poses[A_star.wp_id].pose.position.z;
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
cout << "A star planner:"<<endl;
cout << "Moving to Waypoint: [ " << A_star.wp_id << " / "<< A_star.Num_total_wp<< " ] "<<endl;
cout << "desired_point: " << A_star.path_cmd.poses[A_star.wp_id].pose.position.x << " [m] "
<< A_star.path_cmd.poses[A_star.wp_id].pose.position.y << " [m] "
<< A_star.path_cmd.poses[A_star.wp_id].pose.position.z << " [m] "<<endl;
cout << "drone_pos: " << _DroneState.position[0] << " [m] "<< _DroneState.position[1] << " [m] "<< _DroneState.position[2] << " [m] "<<endl;
float wait_time = 0.25;
ros::spinOnce();
A_star.wp_id++;
ros::Duration(wait_time).sleep();
}
}

153
src/UAV/Modules/mission/number_detection/number_detection.cpp
Unescape View File

@ -1,153 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
//topic 头文件
#include <geometry_msgs/Point.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/MultiDetectionInfo.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
#include <nav_msgs/Path.h>
#include <math.h>
#include "message_utils.h"
using namespace std;
#define MIN_DIS 0.1
#define FLY_HEIGHT 1.0
# define NODE_NAME "number_detection"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
prometheus_msgs::MultiDetectionInfo Detection_info;
float size_Detection_info;
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
prometheus_msgs::DroneState _DroneState; //无人机状态量
ros::Publisher command_pub;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void num_det_cb(const prometheus_msgs::MultiDetectionInfo::ConstPtr& msg)
{
Detection_info = *msg;
size_Detection_info = Detection_info.num_objs;
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "number_detection");
ros::NodeHandle nh("~");
ros::Subscriber num_det_sub = nh.subscribe<prometheus_msgs::MultiDetectionInfo>("/prometheus/object_detection/num_det", 10, num_det_cb);
//【订阅】无人机当前状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(3);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Number Tracking Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 to takeoff the drone..."<<endl;
cin >> start_flag;
}
// 起飞
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
while( _DroneState.position[2] < 0.3)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0.0;
Command_Now.Reference_State.position_ref[1] = 0.0;
Command_Now.Reference_State.position_ref[2] = 1.5;
Command_Now.Reference_State.yaw_ref = 0.5 * M_PI;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
while (ros::ok())
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>Number Tracking Mission<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
cout << "Total_Num: " << size_Detection_info << endl;
//回调
ros::spinOnce();
for(int i=0;i<10;i++)
{
cout << "Num: " << i << endl;
//判断是否识别到
int idx = -1;
for (int j=0;j<size_Detection_info;j++)
{
if(Detection_info.detection_infos[j].category == i)
{
idx = j;
break;
}
}
if(idx != -1)
{
cout << "relative_pos: " << Detection_info.detection_infos[idx].position[0] << " [m] "<< Detection_info.detection_infos[idx].position[1] << " [m] "<< Detection_info.detection_infos[idx].position[2] << " [m] "<<endl;
}else{
cout << "Not detected!!!" << endl;
}
}
ros::Duration(1.0).sleep();
}
return 0;
}

315
src/UAV/Modules/mission/object_tracking/object_tracking.cpp
Unescape View File

@ -1,315 +0,0 @@
/***************************************************************************************************************************
* object_tracking.cpp
*
* Author: Qyp
*
* Update Time: 2020.2.11
*
* 说明: 目标追踪示例程序
* 1. 订阅目标位置(来自视觉的ros节点)
* 2. 追踪算法及追踪策略
* 3. 发布上层控制指令 (prometheus_msgs::ControlCommand)
***************************************************************************************************************************/
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
//topic 头文件
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include "message_utils.h"
using namespace std;
using namespace Eigen;
# define NODE_NAME "object_tracking"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
prometheus_msgs::DroneState _DroneState;
Eigen::Vector3f drone_pos;
//---------------------------------------Vision---------------------------------------------
prometheus_msgs::DetectionInfo Detection_raw; //目标位置[机体系下：前方x为正，右方y为正，下方z为正]
Eigen::Vector3f pos_body_frame;
Eigen::Vector3f pos_body_enu_frame; //原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系
Eigen::Vector3f pos_des_prev;
float kpx_track,kpy_track,kpz_track; //控制参数 - 比例参数
float start_point_x,start_point_y,start_point_z,start_yaw;
bool is_detected = false; // 是否检测到目标标志
int num_count_vision_lost = 0; //视觉丢失计数器
int num_count_vision_regain = 0; //视觉丢失计数器
int Thres_vision = 0; //视觉丢失计数器阈值
Eigen::Vector3f camera_offset;
//---------------------------------------Track---------------------------------------------
float distance_to_setpoint;
Eigen::Vector3f tracking_delta;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result(); //打印函数
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void vision_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
Detection_raw = *msg;
pos_body_frame[0] = Detection_raw.position[2] + camera_offset[0];
pos_body_frame[1] = - Detection_raw.position[0] + camera_offset[1];
pos_body_frame[2] = - Detection_raw.position[1] + camera_offset[2];
Eigen::Matrix3f R_Body_to_ENU;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
pos_body_enu_frame = R_Body_to_ENU * pos_body_frame;
if(Detection_raw.detected)
{
num_count_vision_regain++;
num_count_vision_lost = 0;
}else
{
num_count_vision_regain = 0;
num_count_vision_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(num_count_vision_lost > Thres_vision)
{
is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(num_count_vision_regain > Thres_vision)
{
is_detected = true;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
drone_pos[0] = _DroneState.position[0];
drone_pos[1] = _DroneState.position[1];
drone_pos[2] = _DroneState.position[2];
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "object_tracking");
ros::NodeHandle nh("~");
//节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
// 【订阅】视觉消息 来自视觉节点
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
// 注意这里为了复用程序使用了/prometheus/object_detection/kcf_tracker作为话题名字，适用于椭圆、二维码、yolo等视觉算法
// 故同时只能运行一种视觉识别程序，如果想同时追踪多个目标，这里请修改接口话题的名字
ros::Subscriber vision_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/kcf_tracker", 10, vision_cb);
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//视觉丢失次数阈值
nh.param<int>("Thres_vision", Thres_vision, 10);
//追踪的前后间隔
nh.param<float>("tracking_delta_x", tracking_delta[0], 0.0);
nh.param<float>("tracking_delta_y", tracking_delta[1], 0.0);
nh.param<float>("tracking_delta_z", tracking_delta[2], 0.0);
nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
nh.param<float>("camera_offset_z", camera_offset[2], 0.0);
//追踪控制参数
nh.param<float>("kpx_track", kpx_track, 0.1);
nh.param<float>("kpy_track", kpy_track, 0.1);
nh.param<float>("kpz_track", kpz_track, 0.1);
nh.param<float>("start_point_x", start_point_x, 0.0);
nh.param<float>("start_point_y", start_point_y, 0.0);
nh.param<float>("start_point_z", start_point_z, 2.0);
nh.param<float>("start_yaw", start_yaw, 0.0);
//打印现实检查参数
printf_param();
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Object Tracking Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
// 起飞
cout<<"[object tracking]: "<<"Takeoff to predefined position."<<endl;
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
while( _DroneState.position[2] < 0.3)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point_x;
Command_Now.Reference_State.position_ref[1] = start_point_y;
Command_Now.Reference_State.position_ref[2] = start_point_z;
Command_Now.Reference_State.yaw_ref = start_yaw/180*3.1415926;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 先读取一些飞控的数据
for(int i=0;i<10;i++)
{
ros::spinOnce();
rate.sleep();
}
pos_des_prev[0] = drone_pos[0];
pos_des_prev[1] = drone_pos[1];
pos_des_prev[2] = drone_pos[2];
ros::Duration(3.0).sleep();
while (ros::ok())
{
//回调
ros::spinOnce();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
printf_result();
distance_to_setpoint = pos_body_frame.norm();
if(!is_detected)
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
pos_des_prev[0] = drone_pos[0];
pos_des_prev[1] = drone_pos[1];
pos_des_prev[2] = drone_pos[2];
cout <<"[object_tracking]: Lost the Target "<< endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Lost the Target.");
}else
{
cout <<"[object_tracking]: Tracking the Target, distance_to_setpoint : "<< distance_to_setpoint << " [m] " << endl;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS; //xy velocity z position
Eigen::Vector3f vel_command;
vel_command[0] = kpx_track * (pos_body_enu_frame[0] - tracking_delta[0]);
vel_command[1] = kpy_track * (pos_body_enu_frame[1] - tracking_delta[1]);
vel_command[2] = kpz_track * (pos_body_enu_frame[2] - tracking_delta[2]);
for (int i=0; i<3; i++)
{
Command_Now.Reference_State.position_ref[i] = pos_des_prev[i] + vel_command[i]* 0.05;
}
Command_Now.Reference_State.yaw_ref = start_yaw/180*3.1415926;
for (int i=0; i<3; i++)
{
pos_des_prev[i] = Command_Now.Reference_State.position_ref[i];
}
}
//Publish
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
rate.sleep();
}
return 0;
}
void printf_result()
{
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(4);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>Obeject Tracking<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(is_detected)
{
cout << "is_detected: ture" <<endl;
}else
{
cout << "is_detected: false" <<endl;
}
cout << "Detection_raw: " << Detection_raw.position[0] << " [m] "<< Detection_raw.position[1] << " [m] "<< Detection_raw.position[2] << " [m] "<<endl;
cout << "Detection_raw: " << Detection_raw.attitude[2]/3.1415926 *180 << " [du] "<<endl;
cout << "pos_body_frame: " << pos_body_frame[0] << " [m] "<< pos_body_frame[1] << " [m] "<< pos_body_frame[2] << " [m] "<<endl;
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Land Control State<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "pos_des: " << Command_Now.Reference_State.position_ref[0] << " [m] "<< Command_Now.Reference_State.position_ref[1] << " [m] "<< Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "Thres_vision : "<< Thres_vision << endl;
cout << "tracking_delta_x : "<< tracking_delta[0] << endl;
cout << "tracking_delta_y : "<< tracking_delta[1] << endl;
cout << "tracking_delta_z : "<< tracking_delta[2] << endl;
cout << "kpx_track : "<< kpx_track << endl;
cout << "kpy_track : "<< kpy_track << endl;
cout << "kpz_track : "<< kpz_track << endl;
cout << "start_point_x : "<< start_point_x << endl;
cout << "start_point_y : "<< start_point_y << endl;
cout << "start_point_z : "<< start_point_z << endl;
}

35
src/UAV/Modules/mission/object_tracking/object_tracking.yaml
Unescape View File

@ -1,35 +0,0 @@
##Parameter for object_tracking.cpp
##视觉丢失次数阈值
Thres_vision_lost : 10
##追踪的间隔
tracking_delta_x : 1.5
tracking_delta_y : 0.0
tracking_delta_z : -0.3
camera_offset_x: 0.2
camera_offset_y: 0.0
camera_offset_z: -0.05
##比例参数
kpx_track: 0.3
kpy_track: 0.3
kpz_track: 0.3
##降落起始点 静止目标建议起飞点：[1,2,6] 移动目标建议起飞点[-5,0,5]
start_point_x: -3.0
start_point_y: 0
start_point_z: 1.5
start_yaw: 0.0
UKF_NCV:
std_ax_: 0.01
std_ay_: 0.01
std_az_: 0.01
std_px_: 0.02
std_py_: 0.02
std_pz_: 0.02

28
src/UAV/Modules/mission/package.xml
Unescape View File

@ -1,28 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>prometheus_mission</name>
<version>0.0.0</version>
<description>The prometheus_mission package</description>
<maintainer email="fatmoonqyp@126.com">Yuhua Qi</maintainer>
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
<license>TODO</license>
<url type="website">http://www.amovauto.com/</url>
<url type="repository">https://github.com/amov-lab/Prometheus.git</url>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>std_msgs</exec_depend>
<export>
</export>
</package>

244
src/UAV/Modules/mission/planning_mission/planning_mission.cpp
Unescape View File

@ -1,244 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
//topic 头文件
#include <geometry_msgs/Point.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/PositionReference.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
using namespace std;
#define MIN_DIS 0.1
# define NODE_NAME "planning_mission"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
prometheus_msgs::DroneState _DroneState; //无人机状态量
ros::Publisher command_pub;
geometry_msgs::PoseStamped goal; // goal
prometheus_msgs::PositionReference fast_planner_cmd; // fast planner cmd
bool sim_mode;
bool control_yaw_flag;
int flag_get_cmd = 0;
int flag_get_goal = 0;
float desired_yaw = 0; //[rad]
float distance_to_goal = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Fast_planner();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void fast_planner_cmd_cb(const prometheus_msgs::PositionReference::ConstPtr& msg)
{
flag_get_cmd = 1;
fast_planner_cmd = *msg;
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
distance_to_goal = sqrt( pow(_DroneState.position[0] - goal.pose.position.x, 2)
+ pow(_DroneState.position[1] - goal.pose.position.y, 2) );
}
void goal_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
goal = *msg;
flag_get_goal = 1;
cout << "Get a new goal!"<<endl;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "planning_mission");
ros::NodeHandle nh("~");
nh.param<bool>("planning_mission/control_yaw_flag", control_yaw_flag, true);
// 是否为仿真模式
nh.param<bool>("planning_mission/sim_mode", sim_mode, false);
//【订阅】无人机当前状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【订阅】来自planning的指令
ros::Subscriber fast_planner_sub = nh.subscribe<prometheus_msgs::PositionReference>("/prometheus/fast_planner/position_cmd", 50, fast_planner_cmd_cb);
//【订阅】目标点
ros::Subscriber goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/prometheus/planning/goal", 10,goal_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//　仿真模式下直接发送切换模式与起飞指令
if(sim_mode == true)
{
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Fast Planner<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please input 1 for start:"<<endl;
cin >> start_flag;
}
// 起飞
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Takeoff;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
}else
{
//　真实飞行情况：等待飞机状态变为offboard模式，然后发送起飞指令
while(_DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = 1 ;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
cout << "Waiting for the offboard mode"<<endl;
ros::Duration(1.0).sleep();
ros::spinOnce();
}
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Takeoff;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
}
while (ros::ok())
{
static int exec_num=0;
exec_num++;
// 若goal为99，则降落并退出任务
if(goal.pose.position.x == 99)
{
// 抵达目标附近，则停止速度控制，改为位置控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
cout << "Quit... " << endl;
return 0;
}
//回调
ros::spinOnce();
if( flag_get_cmd == 0)
{
if(exec_num == 10)
{
cout << "Waiting for trajectory" << endl;
exec_num=0;
}
ros::Duration(0.5).sleep();
}else if (distance_to_goal < MIN_DIS)
{
// 抵达目标附近，则停止速度控制，改为位置控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = goal.pose.position.x;
Command_Now.Reference_State.position_ref[1] = goal.pose.position.y;
Command_Now.Reference_State.position_ref[2] = goal.pose.position.z;
Command_Now.Reference_State.yaw_ref = desired_yaw;
command_pub.publish(Command_Now);
if(exec_num == 10)
{
cout << "Arrived the goal, waiting for a new goal... " << endl;
cout << "drone_pos: " << _DroneState.position[0] << " [m] "<< _DroneState.position[1] << " [m] "<< _DroneState.position[2] << " [m] "<<endl;
cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
exec_num=0;
}
flag_get_goal = 0;
while (flag_get_goal == 0)
{
ros::spinOnce();
ros::Duration(0.5).sleep();
}
}else
{
Fast_planner();
ros::Duration(0.05).sleep();
}
}
return 0;
}
void Fast_planner()
{
if (control_yaw_flag)
{
// 根据速度大小决定是否更新期望偏航角， 更新采用平滑滤波的方式，系数可调
// fastplanner航向策略仍然可以进一步优化
if( sqrt( fast_planner_cmd.velocity_ref[1]* fast_planner_cmd.velocity_ref[1]
+ fast_planner_cmd.velocity_ref[0]* fast_planner_cmd.velocity_ref[0]) > 0.05 )
{
float next_desired_yaw_vel = atan2( fast_planner_cmd.velocity_ref[1] ,
fast_planner_cmd.velocity_ref[0]);
float next_desired_yaw_pos = atan2( fast_planner_cmd.position_ref[1] - _DroneState.position[1],
fast_planner_cmd.position_ref[0] - _DroneState.position[0]);
if(next_desired_yaw_pos > 0.8)
{
next_desired_yaw_pos = 0.8;
}
if(next_desired_yaw_pos < -0.8)
{
next_desired_yaw_pos = -0.8;
}
desired_yaw = (0.92*desired_yaw + 0.04*next_desired_yaw_pos + 0.04*next_desired_yaw_vel );
}
}else
{
desired_yaw = 0.0;
}
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State = fast_planner_cmd;
Command_Now.Reference_State.yaw_ref = desired_yaw;
command_pub.publish(Command_Now);
}

66
src/UAV/Modules/mission/planning_mission/pub_goal.cpp
Unescape View File

@ -1,66 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
//topic 头文件
#include <geometry_msgs/PoseStamped.h>
using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "pub_goal");
ros::NodeHandle nh("~");
//【发布】目标点
ros::Publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/planning/goal", 10);
float x,y,z;
geometry_msgs::PoseStamped goal;
int flag;
cout << "Please choose 2D or 3D (0 for 2D, 1 for 3D):"<<endl;
cin >> flag;
while(ros::ok())
{
// Waiting for input
cout << "Please input the goal position:"<<endl;
cout << "goal - x [m] : "<< endl;
cin >> x;
cout << "goal - y [m] : "<< endl;
cin >> y;
if(flag == 1)
{
cout << "goal - z [m] : "<< endl;
cin >> z;
}else if(flag == 0)
{
z = 1.0;
}
goal.header.stamp =ros::Time::now();
goal.header.frame_id = "map";
goal.pose.position.x = x;
goal.pose.position.y = y;
goal.pose.position.z = z;
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;
goal.pose.orientation.z = 0.0;
goal.pose.orientation.w = 1.0;
goal_pub.publish(goal);
sleep(1.0);
}
return 0;
}

181
src/UAV/Modules/mission/planning_mission/pub_goal_from_qgc.cpp
Unescape View File

@ -1,181 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
//topic 头文件
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Pose.h>
#include <mavros_msgs/PositionTarget.h>
#include <eigen_conversions/eigen_msg.h>
#include <mavros/frame_tf.h>
#include <mavros_msgs/WaypointList.h>
#include <mavros_msgs/HomePosition.h>
#include <GeographicLib/Geocentric.hpp>
#include <sensor_msgs/NavSatFix.h>
using namespace std;
prometheus_msgs::DroneState _DroneState;
Eigen::Matrix3f R_Body_to_ENU;
// 航点列表回调函数
mavros_msgs::WaypointList waypoints;
void waypoints_cb(const mavros_msgs::WaypointList::ConstPtr& msg){
waypoints = *msg;
}
// GPS数据回调函数
Eigen::Vector3d current_gps;
void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
current_gps = { msg->latitude, msg->longitude, msg->altitude };
}
// 飞机本地位置的回调函数
geometry_msgs::PoseStamped local_pos;
Eigen::Vector3d current_local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
current_local_pos = mavros::ftf::to_eigen(msg->pose.position);
local_pos = *msg;
}
void timerCallback(const ros::TimerEvent & e)
{
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "pub_goal_from_qgc");
ros::NodeHandle nh("~");
//【订阅】航点
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state",10,drone_state_cb);
ros::Subscriber waypoint_sub = nh.subscribe<mavros_msgs::WaypointList>("/mavros/mission/waypoints", 100, waypoints_cb);
ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, local_pos_cb);
//【订阅】航点
ros::Subscriber gps_sub = nh.subscribe<sensor_msgs::NavSatFix>("/mavros/global_position/global",100,gps_cb);
//【发布】目标点
ros::Publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/planning/goal", 10);
ros::Rate rate(20.0);
// ros::Timer ros::NodeHandle::createTimer(ros::Duration 30,timerCallback,bool oneshot = false);
ros::Timer timer = nh.createTimer(ros::Duration(30),timerCallback);
printf("init ok!\n");
while(ros::ok())
{
while(ros::ok() && !_DroneState.connected)
{
ros::spinOnce();
rate.sleep();
}
printf("connected ok!\n");
//GPS航点信息转换为本地坐标系下的坐标
//*****************pose vextor 容器(将GPS位置转换为ENU坐标系下的位置)*************************//
std::vector<geometry_msgs::PoseStamped> pose;
//printf("wp size=%d\n", waypoints.waypoints.size());
// 处理GPS下的航点信息转成ENU坐标系下的期望位置信息
for( int index = 0; index < waypoints.waypoints.size(); index++)
{
//将大地坐标下的经纬度转换到地心坐标系的m，z轴指向北极，x轴零经度，零纬度。也就是ECEF坐标系
geometry_msgs::PoseStamped p;
//声明了一个类 earth类的实例化
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),GeographicLib::Constants::WGS84_f());
//GPS下的航点经纬高
Eigen::Vector3d goal_gps(waypoints.waypoints[index].x_lat,waypoints.waypoints[index].y_long,0);
// printf("%f %f \n", waypoints.waypoints[index].x_lat, waypoints.waypoints[index].y_long);
//将大地坐标系转换为地心坐标系
Eigen::Vector3d current_ecef;
earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(), current_ecef.x(), current_ecef.y(), current_ecef.z());
Eigen::Vector3d goal_ecef;
earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(), goal_ecef.x(), goal_ecef.y(), goal_ecef.z());
Eigen::Vector3d ecef_offset = goal_ecef - current_ecef;
Eigen::Vector3d enu_offset = mavros::ftf::transform_frame_ecef_enu(ecef_offset, current_gps);
//仿射变换
Eigen::Affine3d sp;
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ());
sp.translation() = current_local_pos + enu_offset;
sp.linear() = q.toRotationMatrix();
//*****************往vector容器中存数据*************************//
Eigen::Vector3d testv(sp.translation());
p.pose.position.x = testv[0];
p.pose.position.y = testv[1];
printf("%f %f \n", testv[0], testv[1]);
// printf("%f %f \n", p.pose.position.x, p.pose.position.y);
pose.push_back(p);
// printf("%f %f \n", pose[index].pose.position.x, pose[index].pose.position.y);
}
for(int i = 0; i < pose.size(); i++)
{
while (ros::ok()) {
while(ros::ok())
{
ros::spinOnce();
if(_DroneState.armed && _DroneState.connected && _DroneState.landed)
break;
rate.sleep();
}
if(!_DroneState.connected)
break;
if(fabs(local_pos.pose.position.x - pose[i].pose.position.x) < 1.0 &&
fabs(local_pos.pose.position.y - pose[i].pose.position.y) < 1.0)
{
break;
}
geometry_msgs::PoseStamped goal_get;
goal_get.header.stamp = ros::Time::now();
goal_get.header.frame_id = "map";
goal_get.pose.position.x = pose[i].pose.position.x;
goal_get.pose.position.y = pose[i].pose.position.y;
goal_get.pose.position.z = 1;
goal_get.pose.orientation.x = 0.0;
goal_get.pose.orientation.y = 0.0;
goal_get.pose.orientation.z = 0.0;
goal_get.pose.orientation.w = 1.0;
goal_pub.publish(goal_get);
}
}
printf("waypoints upload over!\n");
while(ros::ok())
{
ros::spinOnce();
rate.sleep();
}
}
return 0;
}

82
src/UAV/Modules/mission/planning_mission/pub_goal_swarm.cpp
Unescape View File

@ -1,82 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
//topic 头文件
#include <geometry_msgs/PoseStamped.h>
using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "pub_goal_swarm");
ros::NodeHandle nh("~");
//【发布】目标点
ros::Publisher goal_pub1 = nh.advertise<geometry_msgs::PoseStamped>("/uav1/prometheus/planning/goal", 10);
ros::Publisher goal_pub2 = nh.advertise<geometry_msgs::PoseStamped>("/uav2/prometheus/planning/goal", 10);
ros::Publisher goal_pub3 = nh.advertise<geometry_msgs::PoseStamped>("/uav3/prometheus/planning/goal", 10);
float x,y1,y2,y3,z;
geometry_msgs::PoseStamped goal1;
geometry_msgs::PoseStamped goal2;
geometry_msgs::PoseStamped goal3;
int flag;
cout << "Please choose 2D or 3D (0 for 2D, 1 for 3D):"<<endl;
cin >> flag;
while(ros::ok())
{
// Waiting for input
cout << "Please input the goal position:"<<endl;
cout << "goal - x [m] : "<< endl;
cin >> x;
cout << "goal - y_uav1 [m] : "<< endl;
cin >> y1;
cout << "goal - y_uav2 [m] : "<< endl;
cin >> y2;
cout << "goal - y_uav2 [m] : "<< endl;
cin >> y3;
if(flag == 1)
{
cout << "goal - z [m] : "<< endl;
cin >> z;
}else if(flag == 0)
{
z = 1.0;
}
goal1.header.stamp =ros::Time::now();
goal1.header.frame_id = "map";
goal1.pose.position.x = x;
goal1.pose.position.y = y1;
goal1.pose.position.z = z;
goal1.pose.orientation.x = 0.0;
goal1.pose.orientation.y = 0.0;
goal1.pose.orientation.z = 0.0;
goal1.pose.orientation.w = 1.0;
goal2 = goal1;
goal3 = goal1;
goal2.pose.position.y = y2;
goal3.pose.position.y = y3;
goal_pub1.publish(goal1);
goal_pub2.publish(goal2);
goal_pub3.publish(goal3);
sleep(1.0);
}
return 0;
}

413
src/UAV/Modules/mission/test/autonomous_landing_ukf.cpp
Unescape View File

@ -1,413 +0,0 @@
/***************************************************************************************************************************
* autonomous_landing.cpp
*
* Author: Qyp
*
* Update Time: 2020.1.12
*
* 说明: 目标追踪示例程序
* 1. 订阅目标位置(来自视觉的ros节点)
* 2. 追踪算法及追踪策略
* 3. 发布上层控制指令 (prometheus_msgs::ControlCommand)
***************************************************************************************************************************/
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include <tf/transform_datatypes.h>
#include <ukf_car.h>
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define LANDPAD_HEIGHT 0.99
#define NODE_NAME "autonomous_landing"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState _DroneState;
nav_msgs::Odometry GroundTruth;
Eigen::Matrix3f R_Body_to_ENU;
std_msgs::Bool flag_start;
//---------------------------------------Vision---------------------------------------------
Detection_result landpad_det;
Eigen::Vector3f pos_des_prev;
float kpx_land,kpy_land,kpz_land; //控制参数 - 比例参数
float start_point_x,start_point_y,start_point_z;
int debug_mode;
bool use_ukf;
bool moving_target;
Eigen::VectorXd state_fusion;
Eigen::Vector3f camera_offset;
//---------------------------------------Track---------------------------------------------
float distance_to_setpoint;
float distance_thres;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void landpad_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
landpad_det.object_name = "landpad";
landpad_det.Detection_info = *msg;
landpad_det.pos_body_frame[0] = - landpad_det.Detection_info.position[1] + DOWN_CAMERA_OFFSET_X;
landpad_det.pos_body_frame[1] = - landpad_det.Detection_info.position[0] + DOWN_CAMERA_OFFSET_Y;
landpad_det.pos_body_frame[2] = - landpad_det.Detection_info.position[2] + DOWN_CAMERA_OFFSET_Z;
landpad_det.pos_body_enu_frame = R_Body_to_ENU * landpad_det.pos_body_frame;
//若已知降落板高度，则无需使用深度信息。
landpad_det.pos_body_enu_frame[2] = LANDPAD_HEIGHT - _DroneState.position[2];
landpad_det.pos_enu_frame[0] = _DroneState.position[0] + landpad_det.pos_body_enu_frame[0];
landpad_det.pos_enu_frame[1] = _DroneState.position[1] + landpad_det.pos_body_enu_frame[1];
landpad_det.pos_enu_frame[2] = _DroneState.position[2] + landpad_det.pos_body_enu_frame[2];
// landpad_det.att_enu_frame[2] = _DroneState.attitude[2] + Detection_raw.attitude[2];
landpad_det.att_enu_frame[2] = 0.0;
if(landpad_det.Detection_info.detected)
{
landpad_det.num_regain++;
landpad_det.num_lost = 0;
}else
{
landpad_det.num_regain = 0;
landpad_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(landpad_det.num_lost > VISION_THRES)
{
landpad_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(landpad_det.num_regain > VISION_THRES)
{
landpad_det.is_detected = true;
}
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
}
void switch_cb(const std_msgs::Bool::ConstPtr& msg)
{
flag_start = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "autonomous_landing");
ros::NodeHandle nh("~");
//节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
//【订阅】降落板与无人机的相对位置及相对偏航角 单位：米 单位：弧度
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpad_det_cb);
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, groundtruth_cb);
ros::Subscriber switch_sub = nh.subscribe<std_msgs::Bool>("/prometheus/switch/landing", 10, switch_cb);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//追踪距离阈值
nh.param<float>("distance_thres", distance_thres, 0.2);
//是否使用UKF
nh.param<bool>("use_ukf", use_ukf, false);
//目标运动或静止
nh.param<bool>("moving_target", moving_target, false);
// DEBUG 模式
nh.param<int>("debug_mode", debug_mode, 0);
//追踪控制参数
nh.param<float>("kpx_land", kpx_land, 0.1);
nh.param<float>("kpy_land", kpy_land, 0.1);
nh.param<float>("kpz_land", kpz_land, 0.1);
nh.param<float>("start_point_x", start_point_x, 0.0);
nh.param<float>("start_point_y", start_point_y, 0.0);
nh.param<float>("start_point_z", start_point_z, 2.0);
//ukf用于估计目标运动状态，此处假设目标为恒定转弯速率和速度模型（CTRV）模型
UKF_CAR UKF_CAR;
//打印现实检查参数
printf_param();
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
// 起飞
cout<<"[autonomous_landing]: "<<"Takeoff to predefined position."<<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Takeoff to predefined position.");
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
while( _DroneState.position[2] < 0.3)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(5.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point_x;
Command_Now.Reference_State.position_ref[1] = start_point_y;
Command_Now.Reference_State.position_ref[2] = start_point_z;
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 先读取一些飞控的数据
for(int i=0;i<10;i++)
{
ros::spinOnce();
rate.sleep();
}
pos_des_prev[0] = _DroneState.position[0];
pos_des_prev[1] = _DroneState.position[1];
pos_des_prev[2] = _DroneState.position[2];
ros::Duration(3.0).sleep();
while (ros::ok())
{
//回调
ros::spinOnce();
if(use_ukf)
{
//UKF
prometheus_msgs::DetectionInfo Detection_ENU;
Detection_ENU.position[0] = landpad_det.pos_enu_frame[0];
Detection_ENU.position[1] = landpad_det.pos_enu_frame[1];
Detection_ENU.position[2] = landpad_det.pos_enu_frame[2];
Detection_ENU.attitude[2] = landpad_det.att_enu_frame[2];
state_fusion = UKF_CAR.Run(Detection_ENU,0.05);
Eigen::Vector3f target_pos_fusion;
landpad_det.pos_body_enu_frame[0] = state_fusion[0] - _DroneState.position[0];
landpad_det.pos_body_enu_frame[1] = state_fusion[1] - _DroneState.position[1];
landpad_det.pos_body_enu_frame[2] = state_fusion[2] - _DroneState.position[2];
//landpad_det.pos_body_enu_frame[2] = landing_pad_height - _DroneState.position[2];
}
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
printf_result();
//判断是否满足降落条件
distance_to_setpoint = landpad_det.pos_body_enu_frame.norm();
if(distance_to_setpoint < distance_thres)
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
cout <<"[autonomous_landing]: Catched the Landing Pad, distance_to_setpoint : "<< distance_to_setpoint << " [m] " << endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Catched the Landing Pad.");
}else if(!landpad_det.is_detected)
{
Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
pos_des_prev[0] = _DroneState.position[0];
pos_des_prev[1] = _DroneState.position[1];
pos_des_prev[2] = _DroneState.position[2];
cout <<"[autonomous_landing]: Lost the Landing Pad. "<< endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Lost the Landing Pad.");
}else if(abs(landpad_det.pos_body_enu_frame[2]) < 0.3)
{
cout <<"[autonomous_landing]: Reach the lowest height. "<< endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Reach the lowest height.");
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
}else
{
cout <<"[autonomous_landing]: Tracking the Landing Pad, distance_to_setpoint : "<< distance_to_setpoint << " [m] " << endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Tracking the Landing Pad.");
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS; //xy velocity z position
Eigen::Vector3f vel_command;
if(moving_target)
{
vel_command[0] = 1.0 + kpx_land * (landpad_det.pos_body_enu_frame[0] + 0.1);
}else{
vel_command[0] = kpx_land * landpad_det.pos_body_enu_frame[0];
}
vel_command[1] = kpy_land * landpad_det.pos_body_enu_frame[1];
vel_command[2] = kpz_land * landpad_det.pos_body_enu_frame[2];
for (int i=0; i<3; i++)
{
Command_Now.Reference_State.position_ref[i] = pos_des_prev[i] + vel_command[i]* 0.05;
}
// 机体系速度控制有些bug
// Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
// Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
// Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL; //xy velocity z position
// Eigen::Vector3f vel_command;
// vel_command[0] = 1.0 + kpx_land * (pos_body_frame[0] + 0.1);
// vel_command[1] = kpy_land * pos_body_frame[1];
// vel_command[2] = kpz_land * pos_body_frame[2];
// for (int i=0; i<3; i++)
// {
// Command_Now.Reference_State.velocity_ref[i] = vel_command[i];
// }
Command_Now.Reference_State.yaw_ref = 0.0;
for (int i=0; i<3; i++)
{
pos_des_prev[i] = Command_Now.Reference_State.position_ref[i];
}
}
//Publish
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
if (debug_mode == 0)
{
command_pub.publish(Command_Now);
}
rate.sleep();
}
return 0;
}
void printf_result()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<"<< endl;
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(landpad_det.is_detected)
{
cout << "is_detected: ture" <<endl;
}else
{
cout << "is_detected: false" <<endl;
}
cout << "Detection_raw(pos): " << landpad_det.pos_body_frame[0] << " [m] "<< landpad_det.pos_body_frame[1] << " [m] "<< landpad_det.pos_body_frame[2] << " [m] "<<endl;
cout << "Detection_raw(yaw): " << landpad_det.Detection_info.yaw_error/3.1415926 *180 << " [deg] "<<endl;
if(use_ukf)
{
// cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Before UKF<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
// cout << "Detection_ENU(pos): " << Detection_ENU.position[0] << " [m] "<< Detection_ENU.position[1] << " [m] "<< Detection_ENU.position[2] << " [m] "<<endl;
// cout << "Detection_ENU(yaw): " << Detection_ENU.attitude[2]/3.1415926 *180 << " [du] "<<endl;
// cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>After UKF<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
// cout << "State_fusion(pos): " << state_fusion[0] << " [m] "<< state_fusion[1] << " [m] "<< state_fusion[2] << " [m] "<<endl;
// cout << "State_fusion(vel): " << state_fusion[2] << " [m/s] "<<endl;
// cout << "State_fusion(yaw): " << state_fusion[3]/3.1415926 *180 << " [deg] "<< state_fusion[4]/3.1415926 *180 << " [deg/s] "<<endl;
}else
{
cout << "Detection_ENU(pos): " << landpad_det.pos_enu_frame[0] << " [m] "<< landpad_det.pos_enu_frame[1] << " [m] "<< landpad_det.pos_enu_frame[2] << " [m] "<<endl;
cout << "Detection_ENU(yaw): " << landpad_det.att_enu_frame[2]/3.1415926 *180 << " [deg] "<<endl;
}
if (debug_mode == 1)
{
cout << "Ground_truth(pos): " << GroundTruth.pose.pose.position.x << " [m] "<< GroundTruth.pose.pose.position.y << " [m] "<< GroundTruth.pose.pose.position.z << " [m] "<<endl;
cout << "Detection_ENU(pos): " << landpad_det.pos_enu_frame[0] << " [m] "<< landpad_det.pos_enu_frame[1] << " [m] "<< landpad_det.pos_enu_frame[2] << " [m] "<<endl;
cout << "Detection_ENU(yaw): " << landpad_det.att_enu_frame[2]/3.1415926 *180 << " [deg] "<<endl;
}
tf::Quaternion quat;
tf::quaternionMsgToTF(GroundTruth.pose.pose.orientation, quat);
double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
cout << "Ground_truth(yaw): " << yaw/3.1415926 *180 << " [deg] "<<endl;
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Land Control State<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "pos_des: " << Command_Now.Reference_State.position_ref[0] << " [m] "<< Command_Now.Reference_State.position_ref[1] << " [m] "<< Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "distance_thres : "<< distance_thres << endl;
cout << "kpx_land : "<< kpx_land << endl;
cout << "kpy_land : "<< kpy_land << endl;
cout << "kpz_land : "<< kpz_land << endl;
cout << "start_point_x : "<< start_point_x << endl;
cout << "start_point_y : "<< start_point_y << endl;
cout << "start_point_z : "<< start_point_z << endl;
}

44
src/UAV/Modules/mission/test/fake_detectioninfo.cpp
Unescape View File

@ -1,44 +0,0 @@
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
//topic 头文件
#include <prometheus_msgs/DetectionInfo.h>
using namespace std;
prometheus_msgs::DetectionInfo Detection_Info;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "fake_detectioninfo");
ros::NodeHandle nh("~");
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher detection_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/target", 10);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
while(ros::ok())
{
Detection_Info.header.stamp = ros::Time::now();
cout << "Please input the detection info: "<<endl;
cout << "position[0]: "<<endl;
cin >> Detection_Info.position[0];
cout << "position[1]: "<<endl;
cin >> Detection_Info.position[1];
cout << "position[2]: "<<endl;
cin >> Detection_Info.position[2];
cout << "attitude[2]: "<<endl;
cin >> Detection_Info.attitude[2];
Detection_Info.detected = true;
Detection_Info.yaw_error = Detection_Info.attitude[2];
detection_pub.publish(Detection_Info);
}
return 0;
}

54
src/UAV/Modules/mission/test/mission_cmd_pub.cpp
Unescape View File

@ -1,54 +0,0 @@
//ros头文件
#include <ros/ros.h>
#include <iostream>
#include <mission_utils.h>
#include "message_utils.h"
//topic 头文件
#include <geometry_msgs/PoseStamped.h>
using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "mission_cmd_pub");
ros::NodeHandle nh("~");
//【发布】目标点
ros::Publisher mission_cmd_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/mission/cmd", 10);
float x,y,z;
geometry_msgs::PoseStamped mission_cmd;
while(ros::ok())
{
// Waiting for input
cout << "Please input the mission_cmd:"<<endl;
cout << "mission_cmd - x [m] : "<< endl;
cin >> x;
cout << "mission_cmd - y [m] : "<< endl;
cin >> y;
mission_cmd.header.stamp =ros::Time::now();
mission_cmd.header.frame_id = "map";
mission_cmd.pose.position.x = x;
mission_cmd.pose.position.y = y;
mission_cmd.pose.position.z = 0;
mission_cmd.pose.orientation.x = 0.0;
mission_cmd.pose.orientation.y = 0.0;
mission_cmd.pose.orientation.z = 0.0;
mission_cmd.pose.orientation.w = 1.0;
mission_cmd_pub.publish(mission_cmd);
sleep(1.0);
}
return 0;
}

557
src/UAV/Modules/mission/test/pad_tracking.cpp
Unescape View File

@ -1,557 +0,0 @@
//ROS 头文件
#include <ros/ros.h>
#include <iostream>
#include <sensor_msgs/Range.h>
#include <mission_utils.h>
#include "message_utils.h"
using namespace std;
using namespace Eigen;
#define NODE_NAME "pad_tracking"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
bool hold_mode; // 悬停模式，用于测试检测精度
bool sim_mode; // 选择Gazebo仿真模式 或 真实实验模式
bool use_pad_height; // 是否使用降落板绝对高度
float pad_height;
string message;
bool tfmini_flag;
//-----　laser
sensor_msgs::Range tfmini_data;
std_msgs::Bool vision_switch;
geometry_msgs::PoseStamped mission_cmd;
float start_point[3]; // 起始降落位置
float camera_offset[3];
bool moving_target;
float target_vel_xy[2]; // 目标移动速度 enu坐标系 单位：m/s
std_msgs::Bool flag_start;
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState _DroneState;
Eigen::Matrix3f R_Body_to_ENU;
//---------------------------------------Vision---------------------------------------------
Detection_result landpad_det;
float kp_land[3]; //控制参数 - 比例参数
//---------------------------------------Track---------------------------------------------
// 五种状态机
enum EXEC_STATE
{
WAITING_RESULT,
TRACKING,
LOST,
LANDING,
};
EXEC_STATE exec_state;
float distance_to_pad;
float arm_height_to_ground;
float arm_distance_to_pad;
nav_msgs::Odometry GroundTruth;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param(); //打印各项参数以供检查
void printf_result();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void landpad_det_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
landpad_det.object_name = "landpad";
landpad_det.Detection_info = *msg;
// 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 相机安装误差 在mission_utils.h中设置
landpad_det.pos_body_frame[0] = - landpad_det.Detection_info.position[1] + camera_offset[0];
landpad_det.pos_body_frame[1] = - landpad_det.Detection_info.position[0] + camera_offset[1];
landpad_det.pos_body_frame[2] = - landpad_det.Detection_info.position[2] + camera_offset[2];
// 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
landpad_det.pos_body_enu_frame = R_Body_to_ENU * landpad_det.pos_body_frame;
if(tfmini_flag)
{
// 使用TFmini测得高度
landpad_det.pos_body_enu_frame[2] = - tfmini_data.range;
}else
{
// 若已知降落板高度，则无需使用深度信息。
landpad_det.pos_body_enu_frame[2] = - _DroneState.position[2];
}
// 机体惯性系 -> 惯性系
landpad_det.pos_enu_frame[0] = _DroneState.position[0] + landpad_det.pos_body_enu_frame[0];
landpad_det.pos_enu_frame[1] = _DroneState.position[1] + landpad_det.pos_body_enu_frame[1];
landpad_det.pos_enu_frame[2] = _DroneState.position[2] + landpad_det.pos_body_enu_frame[2];
landpad_det.att_enu_frame[2] = 0.0;
if(landpad_det.Detection_info.detected)
{
landpad_det.num_regain++;
landpad_det.num_lost = 0;
}else
{
landpad_det.num_regain = 0;
landpad_det.num_lost++;
}
// 当连续一段时间无法检测到目标时，认定目标丢失
if(landpad_det.num_lost > VISION_THRES)
{
landpad_det.is_detected = false;
}
// 当连续一段时间检测到目标时，认定目标得到
if(landpad_det.num_regain > VISION_THRES)
{
landpad_det.is_detected = true;
}
}
void groundtruth_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
GroundTruth = *msg;
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);
}
void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg)
{
tfmini_data = *msg;
}
void mission_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
mission_cmd = *msg;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
ros::init(argc, argv, "pad_tracking");
ros::NodeHandle nh("~");
//节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
ros::Rate rate(20.0);
//【订阅】降落板与无人机的相对位置及相对偏航角 单位：米 单位：弧度
// 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
// 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标
ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/ellipse_det", 10, landpad_det_cb);
//【订阅】无人机高度(由tfmini测量得到)
ros::Subscriber tfmini_sub = nh.subscribe<sensor_msgs::Range>("/prometheus/tfmini_range", 10, tfmini_cb);
//【订阅】无人机状态
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【订阅】地面真值，此信息仅做比较使用 不强制要求提供
ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, groundtruth_cb);
//【订阅】用于中断任务，直接降落
ros::Subscriber mission_sub = nh.subscribe<geometry_msgs::PoseStamped>("/prometheus/mission/cmd", 10, mission_cb);
// 【发布】 视觉模块开关量
ros::Publisher vision_switch_pub = nh.advertise<std_msgs::Bool>("/prometheus/switch/ellipse_det", 10);
//【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//强制上锁高度
nh.param<float>("arm_height_to_ground", arm_height_to_ground, 0.2);
//强制上锁距离
nh.param<float>("arm_distance_to_pad", arm_distance_to_pad, 0.2);
// 悬停模式 - 仅用于观察检测结果
nh.param<bool>("hold_mode", hold_mode, false);
// 仿真模式 - 区别在于是否自动切换offboard模式
nh.param<bool>("sim_mode", sim_mode, false);
// 是否使用tfmini_data
nh.param<bool>("tfmini_flag", tfmini_flag, false);
//追踪控制参数
nh.param<float>("kpx_land", kp_land[0], 0.1);
nh.param<float>("kpy_land", kp_land[1], 0.1);
nh.param<float>("kpz_land", kp_land[2], 0.1);
nh.param<float>("start_point_x", start_point[0], 0.0);
nh.param<float>("start_point_y", start_point[1], 0.0);
nh.param<float>("start_point_z", start_point[2], 1.0);
nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
nh.param<float>("camera_offset_z", camera_offset[2], 0.0);
//目标运动或静止
nh.param<bool>("moving_target", moving_target, false);
nh.param<float>("target_vel_x", target_vel_xy[0], 0.0);
nh.param<float>("target_vel_y", target_vel_xy[1], 0.0);
printf_param();
//固定的浮点显示
cout.setf(ios::fixed);
//setprecision(n) 设显示小数精度为n位
cout<<setprecision(2);
//左对齐
cout.setf(ios::left);
// 强制显示小数点
cout.setf(ios::showpoint);
// 强制显示符号
cout.setf(ios::showpos);
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
if(sim_mode)
{
// Waiting for input
int start_flag = 0;
while(start_flag == 0)
{
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> start_flag;
}
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}else
{
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
cout << "Waiting for the offboard mode"<<endl;
ros::Duration(1.0).sleep();
ros::spinOnce();
}
}
// 起飞
cout<<"[autonomous_landing]: "<<"Takeoff to predefined position."<<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Takeoff to predefined position.");
while( _DroneState.position[2] < 0.5)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = start_point[0];
Command_Now.Reference_State.position_ref[1] = start_point[1];
Command_Now.Reference_State.position_ref[2] = start_point[2];
Command_Now.Reference_State.yaw_ref = 0;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
// 等待
ros::Duration(3.0).sleep();
exec_state = EXEC_STATE::WAITING_RESULT;
while (ros::ok())
{
//回调
ros::spinOnce();
static int printf_num = 0;
printf_num++;
// 此处是为了控制打印频率
if(printf_num > 20)
{
if(exec_state == TRACKING)
{
// 正常追踪
char message_chars[256];
sprintf(message_chars, "Tracking the Landing Pad, distance_to_the_pad : %f [m] .", distance_to_pad);
message = message_chars;
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
}
if(sim_mode)
{
printf_result();
}
printf_num = 0;
}
// 接收到中断指令，直接降落
if(mission_cmd.pose.position.x == 99)
{
exec_state = LANDING;
}
// 接收到hold转降落指令,将设置hold模式为false
if(mission_cmd.pose.position.x == 88)
{
hold_mode = false;
}
switch (exec_state)
{
case WAITING_RESULT:
{
if(landpad_det.is_detected)
{
exec_state = TRACKING;
message = "Get the detection result.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
vision_switch.data = true;
vision_switch_pub.publish(vision_switch);
message = "Waiting for the detection result.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
ros::Duration(1.0).sleep();
break;
}
case TRACKING:
{
// 丢失,进入LOST状态
if(!landpad_det.is_detected && !hold_mode)
{
exec_state = LOST;
message = "Lost the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 抵达上锁点,进入LANDING
distance_to_pad = landpad_det.pos_body_enu_frame.norm();
//　达到降落距离，上锁降落
if(distance_to_pad < arm_distance_to_pad)
{
exec_state = LANDING;
message = "Catched the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
//　达到最低高度，上锁降落
else if(abs(landpad_det.pos_body_enu_frame[2]) < arm_height_to_ground)
{
exec_state = LANDING;
message = "Reach the lowest height.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 机体系速度控制
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL; //xy velocity z position
for (int i=0; i<3; i++)
{
Command_Now.Reference_State.velocity_ref[i] = kp_land[i] * landpad_det.pos_body_enu_frame[i];
}
Command_Now.Reference_State.yaw_ref = 0.0;
//Publish
//
if(moving_target)
{
Command_Now.Reference_State.velocity_ref[0] += target_vel_xy[0];
Command_Now.Reference_State.velocity_ref[1] += target_vel_xy[1];
}
if (!hold_mode)
{
command_pub.publish(Command_Now);
}
break;
}
case LOST:
{
static int lost_time = 0;
lost_time ++ ;
// 重新获得信息,进入TRACKING
if(landpad_det.is_detected)
{
exec_state = TRACKING;
lost_time = 0;
message = "Regain the Landing Pad.";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
// 首先是悬停等待 尝试得到图像, 然后重回初始点
if(lost_time < 10.0)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
ros::Duration(0.4).sleep();
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
Command_Now.Reference_State.velocity_ref[0] = 0.0;
Command_Now.Reference_State.velocity_ref[1] = 0.0;
Command_Now.Reference_State.velocity_ref[2] = 0.1;
Command_Now.Reference_State.yaw_ref = 0;
// 如果上升超过原始高度，则认为任务失败，则直接降落
if(_DroneState.position[2] >= start_point[2])
{
exec_state = LANDING;
lost_time = 0;
message = "Mission failed, landing... ";
cout << message <<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
break;
}
}
command_pub.publish(Command_Now);
break;
}
case LANDING:
{
if(sim_mode) //？？
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Disarm;
//Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}else
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
command_pub.publish(Command_Now);
}
ros::Duration(1.0).sleep();
break;
}
}
rate.sleep();
}
return 0;
}
void printf_result()
{
cout << ">>>>>>>>>>>>>>>>>>>>>>Autonomous Landing Mission<<<<<<<<<<<<<<<<<<<"<< endl;
switch (exec_state)
{
case WAITING_RESULT:
cout << "exec_state: WAITING_RESULT" <<endl;
break;
case TRACKING:
cout << "exec_state: TRACKING" <<endl;
break;
case LOST:
cout << "exec_state: LOST" <<endl;
break;
case LANDING:
cout << "exec_state: LANDING" <<endl;
break;
}
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
if(landpad_det.is_detected)
{
cout << "is_detected: ture" <<endl;
}else
{
cout << "is_detected: false" <<endl;
}
cout << "Target_pos (body): " << landpad_det.pos_body_frame[0] << " [m] "<< landpad_det.pos_body_frame[1] << " [m] "<< landpad_det.pos_body_frame[2] << " [m] "<<endl;
cout << "Target_pos (body_enu): " << landpad_det.pos_body_enu_frame[0] << " [m] "<< landpad_det.pos_body_enu_frame[1] << " [m] "<< landpad_det.pos_body_enu_frame[2] << " [m] "<<endl;
if( exec_state == TRACKING)
{
// 正常追踪
cout <<"Tracking the Landing Pad, distance_to_the_pad : "<< distance_to_pad << " [m] " << endl;
}
#if DEBUG_MODE == 1
cout << "Ground_truth(pos): " << GroundTruth.pose.pose.position.x << " [m] "<< GroundTruth.pose.pose.position.y << " [m] "<< GroundTruth.pose.pose.position.z << " [m] "<<endl;
cout << "Detection_ENU(pos): " << landpad_det.pos_enu_frame[0] << " [m] "<< landpad_det.pos_enu_frame[1] << " [m] "<< landpad_det.pos_enu_frame[2] << " [m] "<<endl;
cout << "Detection_ENU(yaw): " << landpad_det.att_enu_frame[2]/3.1415926 *180 << " [deg] "<<endl;
#endif
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Land Control State<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "vel_cmd: " << Command_Now.Reference_State.velocity_ref[0] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[1] << " [m/s] "<< Command_Now.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
}
void printf_param()
{
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
cout << "hold_mode : "<< hold_mode << endl;
cout << "sim_mode : "<< sim_mode << endl;
cout << "use_pad_height : "<< use_pad_height << endl;
cout << "arm_distance_to_pad : "<< arm_distance_to_pad << endl;
cout << "arm_height_to_ground : "<< arm_height_to_ground << endl;
cout << "kpx_land : "<< kp_land[0] << endl;
cout << "kpy_land : "<< kp_land[1] << endl;
cout << "kpz_land : "<< kp_land[2] << endl;
cout << "start_point_x : "<< start_point[0] << endl;
cout << "start_point_y : "<< start_point[1] << endl;
cout << "start_point_z : "<< start_point[2] << endl;
cout << "camera_offset_x : "<< camera_offset[0] << endl;
cout << "camera_offset_y : "<< camera_offset[1] << endl;
cout << "camera_offset_z : "<< camera_offset[2] << endl;
cout << "moving_target : "<< moving_target << endl;
cout << "target_vel_x : "<< target_vel_xy[0] << endl;
cout << "target_vel_y : "<< target_vel_xy[1] << endl;
}

18
src/UAV/Modules/mission/test/pad_tracking.yaml
Unescape View File

@ -1,18 +0,0 @@
##强制上锁高度
arm_height_to_ground: 0.2
##强制上锁距离
arm_distance_to_pad: 0.55
##降落追踪 的比例参数
kpx_land: 0.35
kpy_land: 0.35
kpz_land: 0.1
##降落起始点 静止目标建议起飞点：[1,2,6] 移动目标建议起飞点[-5,0,5]
start_point_x: 1
start_point_y: 2
start_point_z: 4.5

312
src/UAV/Modules/mission/waypoint_tracking/waypoint_tracking.cpp
Unescape View File

@ -1,312 +0,0 @@
/***************************************************************************************************************************
* waypoint_tracking.cpp
*
* Author: Qyp
*
* Update Time: 2020.1.12
*
* 说明: 航点追踪
***************************************************************************************************************************/
#include <ros/ros.h>
#include <iostream>
#include "mission_utils.h"
#include "message_utils.h"
#include <std_msgs/Bool.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/PositionReference.h>
using namespace std;
# define TIME_OUT 20.0
# define THRES_DISTANCE 0.15
# define NODE_NAME "waypoint_tracking"
bool sim_mode;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
prometheus_msgs::ControlCommand Command_Now;
prometheus_msgs::DroneState _DroneState; //无人机状态量
Eigen::Vector3f drone_pos;
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
drone_pos[0] = _DroneState.position[0];
drone_pos[1] = _DroneState.position[1];
drone_pos[2] = _DroneState.position[2];
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "waypoint_tracking");
ros::NodeHandle nh("~");
// 频率 [1hz]
ros::Rate rate(1.0);
//【订阅】无人机当前状态
// 本话题来自根据需求自定px4_pos_estimator.cpp
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher move_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 【发布】用于地面站显示的提示消息
ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
ros::Time time_begin;
float time_sec;
float distance;
Eigen::Vector3f point1;
Eigen::Vector3f point2;
Eigen::Vector3f point3;
Eigen::Vector3f point4;
Eigen::Vector3f point5;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// 仿真模式 - 区别在于是否自动切换offboard模式
nh.param<bool>("sim_mode", sim_mode, true);
nh.param<float>("point1_x", point1[0], 0.0);
nh.param<float>("point1_y", point1[1], 0.0);
nh.param<float>("point1_z", point1[2], 0.0);
nh.param<float>("point2_x", point2[0], 0.0);
nh.param<float>("point2_y", point2[1], 0.0);
nh.param<float>("point2_z", point2[2], 0.0);
nh.param<float>("point3_x", point3[0], 0.0);
nh.param<float>("point3_y", point3[1], 0.0);
nh.param<float>("point3_z", point3[2], 0.0);
nh.param<float>("point4_x", point4[0], 0.0);
nh.param<float>("point4_y", point4[1], 0.0);
nh.param<float>("point4_z", point4[2], 0.0);
nh.param<float>("point5_x", point5[0], 0.0);
nh.param<float>("point5_y", point5[1], 0.0);
nh.param<float>("point5_z", point5[2], 0.0);
// 这一步是为了程序运行前检查一下参数是否正确
// 输入1,继续，其他，退出程序
cout << "point1: " << "[ "<<point1[0]<< "," <<point1[1]<<","<<point1[2]<<" ]"<<endl;
cout << "point2: " << "[ "<<point2[0]<< "," <<point2[1]<<","<<point2[2]<<" ]"<<endl;
cout << "point3: " << "[ "<<point3[0]<< "," <<point3[1]<<","<<point3[2]<<" ]"<<endl;
cout << "point4: " << "[ "<<point4[0]<< "," <<point4[1]<<","<<point4[2]<<" ]"<<endl;
cout << "point5: " << "[ "<<point5[0]<< "," <<point5[1]<<","<<point5[2]<<" ]"<<endl;
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = 0;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = 0;
Command_Now.Reference_State.position_ref[1] = 0;
Command_Now.Reference_State.position_ref[2] = 0;
Command_Now.Reference_State.velocity_ref[0] = 0;
Command_Now.Reference_State.velocity_ref[1] = 0;
Command_Now.Reference_State.velocity_ref[2] = 0;
Command_Now.Reference_State.acceleration_ref[0] = 0;
Command_Now.Reference_State.acceleration_ref[1] = 0;
Command_Now.Reference_State.acceleration_ref[2] = 0;
Command_Now.Reference_State.yaw_ref = 0;
if(sim_mode)
{
// Waiting for input
int check_flag;
cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
cin >> check_flag;
if(check_flag != 1)
{
return -1;
}
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
move_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(2.0).sleep();
ros::spinOnce();
}
}else
{
while(ros::ok() && _DroneState.mode != "OFFBOARD")
{
cout<<"[waypoint_tracking]: "<<"Please arm and switch to OFFBOARD mode."<<endl;
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Please arm and switch to OFFBOARD mode.");
ros::Duration(1.0).sleep();
ros::spinOnce();
}
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主程序<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//takeoff
cout<<"[waypoint_tracking]: "<<"Takeoff."<<endl;
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Takeoff;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
move_pub.publish(Command_Now);
sleep(10.0);
//第一个目标点，左下角
cout<<"[waypoint_tracking]: "<<"Moving to Point 1."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Moving to Point 1.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = point1[0];
Command_Now.Reference_State.position_ref[1] = point1[1];
Command_Now.Reference_State.position_ref[2] = point1[2];
Command_Now.Reference_State.yaw_ref = 0;
move_pub.publish(Command_Now);
time_begin = ros::Time::now();
distance = cal_distance(drone_pos,point1);
time_sec = 0;
while(distance > THRES_DISTANCE && time_sec < TIME_OUT)
{
ros::Time time_now = ros::Time::now();
time_sec = time_now.sec-time_begin.sec;
distance = cal_distance(drone_pos,point1);
cout<<"[waypoint_tracking]: "<<"Distance to waypoint: "<<distance<< "[m], used " <<time_sec << "[s]."<<endl;
ros::spinOnce();
rate.sleep();
}
//第二个目标点，左上角
cout<<"[waypoint_tracking]: "<<"Moving to Point 2."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Moving to Point 2.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = point2[0];
Command_Now.Reference_State.position_ref[1] = point2[1];
Command_Now.Reference_State.position_ref[2] = point2[2];
Command_Now.Reference_State.yaw_ref = 0;
move_pub.publish(Command_Now);
time_begin = ros::Time::now();
distance = cal_distance(drone_pos,point2);
time_sec = 0;
while(distance > THRES_DISTANCE && time_sec < TIME_OUT)
{
ros::Time time_now = ros::Time::now();
time_sec = time_now.sec-time_begin.sec;
distance = cal_distance(drone_pos,point2);
cout<<"[waypoint_tracking]: "<<"Distance to waypoint: "<<distance<< "[m], used " <<time_sec << "[s]."<<endl;
ros::spinOnce();
rate.sleep();
}
//第三个目标点，右上角
cout<<"[waypoint_tracking]: "<<"Moving to Point 3."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Moving to Point 3.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = point3[0];
Command_Now.Reference_State.position_ref[1] = point3[1];
Command_Now.Reference_State.position_ref[2] = point3[2];
Command_Now.Reference_State.yaw_ref = 0;
move_pub.publish(Command_Now);
time_begin = ros::Time::now();
distance = cal_distance(drone_pos,point3);
time_sec = 0;
while(distance > THRES_DISTANCE && time_sec < TIME_OUT)
{
ros::Time time_now = ros::Time::now();
time_sec = time_now.sec-time_begin.sec;
distance = cal_distance(drone_pos,point3);
cout<<"[waypoint_tracking]: "<<"Distance to waypoint: "<<distance<< "[m], used " <<time_sec << "[s]."<<endl;
ros::spinOnce();
rate.sleep();
}
//第四个目标点，右下角
cout<<"[waypoint_tracking]: "<<"Moving to Point 4."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Moving to Point 4.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = point4[0];
Command_Now.Reference_State.position_ref[1] = point4[1];
Command_Now.Reference_State.position_ref[2] = point4[2];
Command_Now.Reference_State.yaw_ref = 0;
move_pub.publish(Command_Now);
time_begin = ros::Time::now();
distance = cal_distance(drone_pos,point4);
time_sec = 0;
while(distance > THRES_DISTANCE && time_sec < TIME_OUT)
{
ros::Time time_now = ros::Time::now();
time_sec = time_now.sec-time_begin.sec;
distance = cal_distance(drone_pos,point4);
cout<<"[waypoint_tracking]: "<<"Distance to waypoint: "<<distance<< "[m], used " <<time_sec << "[s]."<<endl;
ros::spinOnce();
rate.sleep();
}
//第五个目标点，回到起点
cout<<"[waypoint_tracking]: "<<"Moving to Point 5."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Moving to Point 5.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_Now.Reference_State.position_ref[0] = point5[0];
Command_Now.Reference_State.position_ref[1] = point5[1];
Command_Now.Reference_State.position_ref[2] = point5[2];
Command_Now.Reference_State.yaw_ref = 0;
move_pub.publish(Command_Now);
time_begin = ros::Time::now();
distance = cal_distance(drone_pos,point5);
time_sec = 0;
while(distance > THRES_DISTANCE && time_sec < TIME_OUT)
{
ros::Time time_now = ros::Time::now();
time_sec = time_now.sec-time_begin.sec;
distance = cal_distance(drone_pos,point5);
cout<<"[waypoint_tracking]: "<<"Distance to waypoint: "<<distance<< "[m], used " <<time_sec << "[s]."<<endl;
ros::spinOnce();
rate.sleep();
}
//降落
cout<<"[waypoint_tracking]: "<<"Landing."<<endl;
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Landing.");
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Land;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
move_pub.publish(Command_Now);
sleep(1.0);
return 0;
}

129
src/UAV/Modules/motion_planning/RapidQuad/CMakeLists.txt
Unescape View File

@ -1,129 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(prometheus_rapid_quad)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
geometry_msgs
nav_msgs
sensor_msgs
mavros
std_msgs
std_srvs
tf
tf2_ros
tf2_eigen
prometheus_msgs
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
################################################
## Declare ROS messages, services and actions ##
################################################
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs
nav_msgs
sensor_msgs
std_msgs
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
../common/include
)
###############################
## 声明可执行cpp文件 ##
###############################
add_library(RapidTrajectoryGenerator include/RapidTrajectoryGenerator.cpp)
add_library(SingleAxisTrajectory include/SingleAxisTrajectory.cpp)
#add_library(KeyboardEvent src/lib/KeyboardEvent.cpp)
###### Main File ##########
add_executable(main_node src/main_node.cpp)
add_dependencies(main_node prometheus_rapid_quad_gencpp)
target_link_libraries(main_node ${catkin_LIBRARIES})
target_link_libraries(main_node RapidTrajectoryGenerator)
target_link_libraries(main_node SingleAxisTrajectory)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_rapid_quad.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

218
src/UAV/Modules/motion_planning/RapidQuad/include/RapidTrajectoryGenerator.cpp
Unescape View File

@ -1,218 +0,0 @@
/*!
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#include "RapidTrajectoryGenerator.h"
#include "RootFinder/quartic.hpp"
#include <algorithm>
#include <limits>
using namespace RapidQuadrocopterTrajectoryGenerator;
RapidTrajectoryGenerator::RapidTrajectoryGenerator(const Vec3 x0, const Vec3 v0, const Vec3 a0, const Vec3 gravity)
{
//initialise each axis:
Reset();
for(int i=0; i<3; i++) _axis[i].SetInitialState(x0[i],v0[i],a0[i]);
_grav = gravity;
}
void RapidTrajectoryGenerator::SetGoalPosition(const Vec3 in)
{
for(unsigned i=0;i<3;i++) SetGoalPositionInAxis(i, in[i]);
}
void RapidTrajectoryGenerator::SetGoalVelocity(const Vec3 in)
{
for(int i=0;i<3;i++) SetGoalVelocityInAxis(i, in[i]);
}
void RapidTrajectoryGenerator::SetGoalAcceleration(const Vec3 in)
{
for(int i=0;i<3;i++) SetGoalAccelerationInAxis(i, in[i]);
}
void RapidTrajectoryGenerator::Reset(void)
{
for(int i=0; i<3; i++)
{
_axis[i].Reset();
}
_tf = 0;
}
//Generate the trajectory:
void RapidTrajectoryGenerator::Generate(const double timeToFinish)
{
_tf = timeToFinish;
for(int i=0;i<3;i++)
{
_axis[i].GenerateTrajectory(_tf);
}
}
RapidTrajectoryGenerator::InputFeasibilityResult RapidTrajectoryGenerator::CheckInputFeasibilitySection(double fminAllowed, double fmaxAllowed, double wmaxAllowed, double t1, double t2, double minTimeSection)
{
if (t2 - t1 < minTimeSection) return InputIndeterminable;
//test the acceleration at the two limits:
if (std::max(GetThrust(t1), GetThrust(t2)) > fmaxAllowed) return InputInfeasibleThrustHigh;
if (std::min(GetThrust(t1), GetThrust(t2)) < fminAllowed) return InputInfeasibleThrustLow;
double fminSqr = 0;
double fmaxSqr = 0;
double jmaxSqr = 0;
//Test the limits of the box we're putting around the trajectory:
for (int i = 0; i < 3; i++)
{
double amin, amax;
_axis[i].GetMinMaxAcc(amin, amax, t1, t2);
//distance from zero thrust point in this axis
double v1 = amin - _grav[i]; //left
double v2 = amax - _grav[i]; //right
//definitely infeasible:
if (std::max(pow(v1, 2), pow(v2, 2)) > pow(fmaxAllowed, 2)) return InputInfeasibleThrustHigh;
if (v1 * v2 < 0)
{
//sign of acceleration changes, so we've gone through zero
fminSqr += 0;
}
else
{
fminSqr += pow(std::min(fabs(v1), fabs(v2)), 2);
}
fmaxSqr += pow(std::max(fabs(v1), fabs(v2)), 2);
jmaxSqr += _axis[i].GetMaxJerkSquared(t1, t2);
}
double fmin = sqrt(fminSqr);
double fmax = sqrt(fmaxSqr);
double wBound;
if (fminSqr > 1e-6) wBound = sqrt(jmaxSqr / fminSqr);//the 1e-6 is a divide-by-zero protection
else wBound = std::numeric_limits<double>::max();
//definitely infeasible:
if (fmax < fminAllowed) return InputInfeasibleThrustLow;
if (fmin > fmaxAllowed) return InputInfeasibleThrustHigh;
//possibly infeasible:
if (fmin < fminAllowed || fmax > fmaxAllowed || wBound > wmaxAllowed)
{ //indeterminate: must check more closely:
double tHalf = (t1 + t2) / 2;
InputFeasibilityResult r1 = CheckInputFeasibilitySection(fminAllowed, fmaxAllowed, wmaxAllowed, t1, tHalf, minTimeSection);
if(r1 == InputFeasible)
{
//continue with second half
return CheckInputFeasibilitySection(fminAllowed, fmaxAllowed, wmaxAllowed, tHalf, t2, minTimeSection);
}
//first section is already infeasible, or indeterminate:
return r1;
}
//definitely feasible:
return InputFeasible;
}
RapidTrajectoryGenerator::InputFeasibilityResult RapidTrajectoryGenerator::CheckInputFeasibility(double fminAllowed, double fmaxAllowed, double wmaxAllowed, double minTimeSection)
{
//required thrust limits along trajectory
double t1 = 0;
double t2 = _tf;
return CheckInputFeasibilitySection(fminAllowed, fmaxAllowed, wmaxAllowed, t1, t2, minTimeSection);
}
RapidTrajectoryGenerator::StateFeasibilityResult RapidTrajectoryGenerator::CheckPositionFeasibility(Vec3 boundaryPoint, Vec3 boundaryNormal)
{
//Ensure that the normal is a unit vector:
boundaryNormal = boundaryNormal.GetUnitVector();
//first, we will build the polynomial describing the velocity of the a
//quadrocopter in the direction of the normal. Then we will solve for
//the zeros of this, which give us the times when the position is at a
//critical point. Then we evaluate the position at these points, and at
//the trajectory beginning and end, to see whether we are feasible.
//need to check that the trajectory stays inside the safe box throughout the flight:
//the coefficients of the quartic equation: x(t) = c[0]t**4 + c[1]*t**3 + c[2]*t**2 + c[3]*t + c[4]
double c[5] = { 0, 0, 0, 0, 0 };
for(int dim=0; dim<3; dim++)
{
c[0] += boundaryNormal[dim]*_axis[dim].GetParamAlpha() / 24.0; //t**4
c[1] += boundaryNormal[dim]*_axis[dim].GetParamBeta() / 6.0; //t**3
c[2] += boundaryNormal[dim]*_axis[dim].GetParamGamma() / 2.0; //t**2
c[3] += boundaryNormal[dim]*_axis[dim].GetInitialAcceleration(); //t
c[4] += boundaryNormal[dim]*_axis[dim].GetInitialVelocity(); //1
}
//Solve the roots (we prepend the times 0 and tf):
double roots[6];
roots[0] = 0;
roots[1] = _tf;
size_t rootCount;
if(fabs(c[0]) > 1e-6)
{
rootCount = magnet::math::quarticSolve(c[1] / c[0], c[2] / c[0], c[3] / c[0], c[4] / c[0], roots[2], roots[3], roots[4], roots[5]);
}
else
{
rootCount = magnet::math::cubicSolve(c[2] / c[1], c[3] / c[1], c[4] / c[1], roots[2], roots[3], roots[4]);
}
for(unsigned i=0; i<(rootCount+2); i++)
{
//don't evaluate points outside the domain
if(roots[i] < 0) continue;
if(roots[i] > _tf) continue;
if((GetPosition(roots[i]) - boundaryPoint).Dot(boundaryNormal) <= 0)
{
//touching, or on the wrong side of, the boundary!
return StateInfeasible;
}
}
return StateFeasible;
}
Vec3 RapidTrajectoryGenerator::GetOmega(double t, double timeStep) const
{
//Calculates the body rates necessary at time t, to rotate the normal vector.
//The result is coordinated in the world frame, i.e. would have to be rotated into a
//body frame.
const Vec3 n0 = GetNormalVector(t);
const Vec3 n1 = GetNormalVector(t + timeStep);
const Vec3 crossProd = n0.Cross(n1); //direction of omega, in inertial axes
if (crossProd.GetNorm2()) return (acos(n0.Dot(n1))/timeStep)*crossProd.GetUnitVector();
else return Vec3(0, 0, 0);
}

187
src/UAV/Modules/motion_planning/RapidQuad/include/RapidTrajectoryGenerator.h
Unescape View File

@ -1,187 +0,0 @@
/*!
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "SingleAxisTrajectory.h"
#include "Vec3.h"
namespace RapidQuadrocopterTrajectoryGenerator
{
//! A quadrocopter state interception trajectory.
/*!
* A quadrocopter state interception trajectory. The trajectory starts at a
* state defined by the vehicle's position, velocity, and acceleration. The
* acceleration can be calculated directly from the quadrocopter's attitude
* and thrust value. The trajectory duration is fixed, and given by the user.
*
* The trajectory goal state can include any combination of components from
* the quadrocopter's position, velocity, and acceleration. The acceleration
* allows to encode the direction of the quadrocopter's thrust at the end time.
*
* The trajectories are generated without consideration for any constraints,
* and are optimal with respect to the integral of the jerk squared (which is
* equivalent to an upper bound on a product of the inputs).
*
* The trajectories can then be tested with respect to input constraints
* (thrust/body rates) with an efficient, recursive algorithm. Whether linear
* combinations of states along the trajectory remain within some bounds can
* also be tested efficiently.
*
* For more information, please see the publication `A computationally efficient motion primitive for quadrocopter trajectory generation', avaible here: http://www.mwm.im/research/publications/
*
* NOTE: in the publication, axes are 1-indexed, while here they are
* zero-indexed.
*/
class RapidTrajectoryGenerator
{
public:
enum InputFeasibilityResult
{
InputFeasible = 0,//!<The trajectory is input feasible
InputIndeterminable = 1,//!<Cannot determine whether the trajectory is feasible with respect to the inputs
InputInfeasibleThrustHigh = 2,//!<Trajectory is infeasible, failed max. thrust test first
InputInfeasibleThrustLow = 3,//!<Trajectory is infeasible, failed min. thrust test first
InputInfeasibleRates = 4,//!<Trajectory is infeasible, failed max. rates test first
};
enum StateFeasibilityResult
{
StateFeasible = 0,//!<The trajectory is feasible w.r.t. the test
StateInfeasible = 1,//!<Trajectory is infeasible
};
//! Constructor, user must define initial state, and the direction of gravity.
RapidTrajectoryGenerator(const Vec3 x0, const Vec3 v0, const Vec3 a0, const Vec3 gravity);
//set the final state for all axes:
//! Fix the full position at the end time (see also the per-axis functions).
void SetGoalPosition(const Vec3 in);
//! Fix the full velocity at the end time (see also the per-axis functions).
void SetGoalVelocity(const Vec3 in);
//! Fix the full acceleration at the end time (see also the per-axis functions).
void SetGoalAcceleration(const Vec3 in);
//set final state per axis:
//! Fix the position at the end time in one axis. If not set, it is left free.
void SetGoalPositionInAxis(const unsigned axNum, const double in){_axis[axNum].SetGoalPosition(in);};
//! Fix the velocity at the end time in one axis. If not set, it is left free.
void SetGoalVelocityInAxis(const unsigned axNum, const double in){_axis[axNum].SetGoalVelocity(in);};
//! Fix the acceleration at the end time in one axis. If not set, it is left free.
void SetGoalAccelerationInAxis(const unsigned axNum, const double in){_axis[axNum].SetGoalAcceleration(in);};
//! Reset the trajectory, clearing any end state constraints.
void Reset(void);
/*! Calculate the optimal trajectory of duration `timeToGo`.
*
* Calculate the full trajectory, for all the parameters defined so far.
* @param timeToGo The trajectory duration, in [s].
*/
void Generate(const double timeToGo);
/*! Test the trajectory for input feasibility.
*
* Test whether the inputs required along the trajectory are within the allowable limits.
* Note that the test either
* - proves feasibility,
* - proves infeasibility,
* - fails to prove anything ("indeterminate")
*
* The user must also specify a minimumTimeSection, which then determines the
* precision of tests (and thus limit the number of recursion steps).
*
* Refer to the paper for a full discussion on these tests.
*
* Note that if the result is not feasible, the result is that of the first
* section which tested infeasible/indeterminate.
*
* @param fminAllowed Minimum thrust value inputs allowed [m/s**2].
* @param fmaxAllowed Maximum thrust value inputs allowed [m/s**2].
* @param wmaxAllowed Maximum body rates input allowed [rad/s].
* @param minTimeSection Minimum time section to test during the recursion [s].
* @return an instance of InputFeasibilityResult.
*/
InputFeasibilityResult CheckInputFeasibility(double fminAllowed, double fmaxAllowed, double wmaxAllowed, double minTimeSection);
/*! Test the trajectory for position feasibility.
*
* Test whether the position flown along the trajectory is feasible with
* respect to a user defined boundary (modelled as a plane). The plane is
* defined by a point lying on the plane, and the normal of the plane.
*
* No test is done for degenerate normals (of the form (0,0,0)).
*
* @param boundaryPoint The coordinates of any point on the plane defining the boundary.
* @param boundaryNormal A vector pointing in the allowable direction, away from the boundary.
* @return An instance of StateFeasibilityResult.
*/
StateFeasibilityResult CheckPositionFeasibility(Vec3 boundaryPoint, Vec3 boundaryNormal);
//! Return the jerk along the trajectory at time t
Vec3 GetJerk(double t) const {return Vec3(_axis[0].GetJerk(t), _axis[1].GetJerk(t), _axis[2].GetJerk(t));};
//! Return the acceleration along the trajectory at time t
Vec3 GetAcceleration(double t) const {return Vec3(_axis[0].GetAcceleration(t), _axis[1].GetAcceleration(t), _axis[2].GetAcceleration(t));};
//! Return the velocity along the trajectory at time t
Vec3 GetVelocity(double t) const {return Vec3(_axis[0].GetVelocity(t), _axis[1].GetVelocity(t), _axis[2].GetVelocity(t));};
//! Return the position along the trajectory at time t
Vec3 GetPosition(double t) const {return Vec3(_axis[0].GetPosition(t), _axis[1].GetPosition(t), _axis[2].GetPosition(t));};
//! Return the quadrocopter's normal vector along the trajectory at time t
Vec3 GetNormalVector(double t) const {return (GetAcceleration(t) - _grav).GetUnitVector();};
//! Return the quadrocopter's thrust input along the trajectory at time t
double GetThrust(double t) const {return (GetAcceleration(t) - _grav).GetNorm2();};
/*! Return the quadrocopter's body rates along the trajectory at time t
*
* Returns the required body rates along the trajectory. These are expressed
* in the world frame (in which the trajectory was planned). To convert them
* to (p,q,r), this needs to be rotated by the quadrocopter's attitude.
*
* The rates are calculated by taking the rates required to rotate the
* quadrocopter's normal at time `t` to that at time `t+dt`. Therefore, if
* the trajectory is used as implicit MPC control law, the value dt should
* correspond to the controller period.
*
* @param t Time along the trajectory to be evaluated [s].
* @param timeStep The timestep size for the finite differencing [s].
* @return The body rates, expressed in the inertial frame [rad/s]
*/
Vec3 GetOmega(double t, double timeStep) const;
//! Return the total cost of the trajectory.
double GetCost(void) const { return _axis[0].GetCost() + _axis[1].GetCost() + _axis[2].GetCost();};
//! Return the parameter defining the trajectory.
double GetAxisParamAlpha(int i) const{return _axis[i].GetParamAlpha();};
//! Return the parameter defining the trajectory.
double GetAxisParamBeta(int i) const{return _axis[i].GetParamBeta();};
//! Return the parameter defining the trajectory.
double GetAxisParamGamma(int i) const{return _axis[i].GetParamGamma();};
private:
//! Test a section of the trajectory for input feasibility (recursion).
InputFeasibilityResult CheckInputFeasibilitySection(double fminAllowed, double fmaxAllowed, double wmaxAllowed, double t1, double t2, double minTimeSection);
SingleAxisTrajectory _axis[3];//!<The axes along the single trajectories
Vec3 _grav;//!<gravity in the frame of the trajectory
double _tf; //!<trajectory end time [s]
};
};//namespace

273
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/cubic.hpp
Unescape View File

@ -1,273 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Slightly modified the original files from https://www.marcusbannerman.co.uk/index.php/downloads.html?func=finishdown&id=20
Mark W. Mueller (mwm@mwm.im)
Changes:
- undef max
- some readibility tweaks.
*/
#pragma once
#include <cmath>
#include <limits>
#include "quadratic.hpp"
//WinDef.h defines a max() macro, which screws up compilation here...
#ifdef max
#undef max
#endif
namespace magnet
{
namespace math
{
namespace detail
{
inline void
cubicNewtonRootPolish(const double& p, const double& q, const double& r, double& root, size_t iterations)
{
for (size_t it = 0; it < iterations; ++it)
{
double error = ((root + p) * root + q) * root + r;
double derivative = (3.0 * root + 2 * p) * root + q;
if ((error == 0) || derivative == 0)
return;
root -= error / derivative;
}
}
}
//Please read http://linus.it.uts.edu.au/~don/pubs/solving.html
//For solving cubics like x^3 + p * x^2 + q * x + r == 0
//This function always returns root1 >= root2 >= root3!
inline size_t
cubicSolve(const double& p, const double& q, const double& r, double& root1, double& root2, double& root3)
{
static const double maxSqrt = std::sqrt(std::numeric_limits<double>::max());
// static const double maxCubeRoot = std::pow(std::numeric_limits<double>::max(), 1.0 / 3.0); //Unused
if (r == 0)
{
//no constant term, so divide by x and the result is a
//quadratic, but we must include the trivial x = 0 root
if (quadraticSolve(p, q, root1, root2))
{
root3 = 0;
if (root1 < root2)
std::swap(root1, root2);
if (root2 < 0)
{
std::swap(root2, root3);
if (root1 < 0)
std::swap(root1, root2);
}
return 3;
}
else
{
root1 = 0;
return 1;
}
}
if ((p == 0) && (q == 0))
{
//Special case
//Equation is x^3 == -r
root1 = root2 = root3 = std::pow(-r, 1.0 / 3.0);
return 3;
}
if ((p > maxSqrt) || (p < -maxSqrt))
{
//Equation limits to x^3 + p * x^2 == 0
root1 = -p;
return 1;
}
if (q > maxSqrt)
{
//Special case, if q is large and the root is -r/q,
//The x^3 term is negligble, and all other terms cancel.
root1 = -r / q;
return 1;
}
if (q < -maxSqrt)
{
//Special case, equation is x^3 + q x == 0
root1 = -std::sqrt(-q);
return 1;
}
if ((r > maxSqrt) || (r < -maxSqrt))
{
//Another special case
//Equation is x^3 == -r
root1 = -std::pow(r, 1.0 / 3.0);
return 1;
}
double v = r + (2.0 * p * p / 9.0 - q) * (p / 3.0);
if ((v > maxSqrt) || (v < -maxSqrt))
{
root1 = -p;
return 1;
}
double uo3 = q / 3.0 - p * p / 9.0;
double u2o3 = uo3 + uo3;
if ((u2o3 > maxSqrt) || (u2o3 < -maxSqrt))
{
if (p == 0)
{
if (q > 0)
{
root1 = -r / q;
return 1;
}
if (q < 0)
{
root1 = -std::sqrt(-q);
return 1;
}
root1 = 0;
return 1;
}
root1 = -q / p;
return 1;
}
double uo3sq4 = u2o3 * u2o3;
if (uo3sq4 > maxSqrt)
{
if (p == 0)
{
if (q > 0)
{
root1 = -r / q;
return 1;
}
if (q < 0)
{
root1 = -std::sqrt(-q);
return 1;
}
root1 = 0;
return 1;
}
root1 = -q / p;
return 1;
}
double j = (uo3sq4 * uo3) + v * v;
if (j > 0)
{ //Only one root (but this test can be wrong due to a
//catastrophic cancellation in j
//(i.e. (uo3sq4 * uo3) == v * v)
double w = std::sqrt(j);
//double mcube;
if (v < 0)
root1 = std::pow(0.5 * (w - v), 1.0 / 3.0) - (uo3) * std::pow(2.0 / (w - v), 1.0 / 3.0) - p / 3.0;
else
root1 = uo3 * std::pow(2.0 / (w + v), 1.0 / 3.0) - std::pow(0.5 * (w + v), 1.0 / 3.0) - p / 3.0;
//We now polish the root up before we use it in other calculations
detail::cubicNewtonRootPolish(p, q, r, root1, 15);
//We double check that there are no more roots by using a
//quadratic formula on the factored problem, this helps when
//the j test is wrong due to numerical error.
//We have a choice of either -r/root1, or q -
//(p+root1)*root1 for the constant term of the quadratic.
//
//The division one usually results in more accurate roots
//when it finds them but fails to detect real roots more
//often than the multiply.
if (quadraticSolve(p + root1, -r / root1, root2, root3))
return 3;
//
//However, the multiply detects roots where there are none,
//the division does not. So we must either accept missing
//roots or extra roots, here we choose missing roots
//
//if (quadSolve(q-(p+root1)*root1, p + root1, 1.0, root2, root3))
// return 3;
return 1;
}
if (uo3 >= 0)
{ //Multiple root detected
root1 = root2 = root3 = std::pow(v, 1.0 / 3.0) - p / 3.0;
return 3;
}
double muo3 = -uo3;
double s;
if (muo3 > 0)
{
s = std::sqrt(muo3);
if (p > 0)
s = -s;
}
else
s = 0;
double scube = s * muo3;
if (scube == 0)
{
root1 = -p / 3.0;
return 1;
}
double t = -v / (scube + scube);
double k = std::acos(t) / 3.0;
double cosk = std::cos(k);
root1 = (s + s) * cosk - p / 3.0;
double sinsqk = 1.0 - cosk * cosk;
if (sinsqk < 0)
return 1;
double rt3sink = std::sqrt(3.) * std::sqrt(sinsqk);
root2 = s * (-cosk + rt3sink) - p / 3.0;
root3 = s * (-cosk - rt3sink) - p / 3.0;
detail::cubicNewtonRootPolish(p, q, r, root1, 15);
detail::cubicNewtonRootPolish(p, q, r, root2, 15);
detail::cubicNewtonRootPolish(p, q, r, root3, 15);
return 3;
}
}
}

144
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quadratic.hpp
Unescape View File

@ -1,144 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <cmath>
#include <stdexcept>
namespace magnet {
namespace math {
//Solve a quadratic of the form x^2 + B x + C == 0
inline bool quadraticSolve(const double& B, const double& C,
double& root1, double& root2)
{
double discriminant = (B * B) - (4 * C);
//Cannot do imaginary numbers, yet
if (discriminant < 0) return false;
//This avoids a cancellation of errors. See
//http://en.wikipedia.org/wiki/Quadratic_equation#Floating_point_implementation
double t = -0.5 * ( B + ((B < 0) ? -1 : 1) * std::sqrt(discriminant));
root1 = t;
if(t==0) root2 = 0;
else root2 = C / t;
return true;
}
inline bool quadSolve(const double& C, const double& B, const double& A,
double& root1, double& root2)
{
// Contingency: if A = 0, not a quadratic = linear
if(A == 0)
{
//If B is zero then we have a NaN
if(B == 0) return false;
root1 = -1.0 * C / B;
root2 = root1;
}
double discriminant = (B * B) - (4 * A * C);
//Cannot do imaginary numbers, yet
if (discriminant < 0) return false;
//This avoids a cancellation of errors. See
//http://en.wikipedia.org/wiki/Quadratic_equation#Floating_point_implementation
double t = -0.5 * ( B + ((B < 0) ? -1 : 1) * std::sqrt(discriminant));
root1 = t / A;
root2 = C / t;
return true;
}
typedef enum {
ROOT_SMALLEST_EITHER = 1,
ROOT_SMALLEST_POSITIVE = 2,
ROOT_SMALLEST_NEGATIVE = 4,
ROOT_LARGEST_EITHER = 8,
ROOT_LARGEST_POSITIVE = 16,
ROOT_LARGEST_NEGATIVE = 32
} rootTypeEnum;
template<rootTypeEnum rootType>
inline bool quadSolve(const double& C, const double& B, const double& A, double& ans)
{
double root1(0), root2(0);
if (!quadSolve(C,B,A,root1,root2)) return false;
switch (rootType)
{
case ROOT_SMALLEST_EITHER:
ans = (fabs(root1) < fabs(root2)) ? root1 : root2;
break;
case ROOT_LARGEST_EITHER:
ans = (fabs(root1) < fabs(root2)) ? root2 : root1;
case ROOT_LARGEST_NEGATIVE:
if (root1 < 0 && root2 < 0)
ans = ((root1 < root2) ? root1 : root2);
else if (root1 < 0)
ans = root1;
else if (root2 < 0)
ans = root2;
else
return false;
break;
case ROOT_SMALLEST_NEGATIVE:
if (root1 < 0 && root2 < 0)
ans = ((root1 < root2) ? root2 : root1);
else if (root1 < 0)
ans = root1;
else if (root2 < 0)
ans = root2;
else
return false;
break;
case ROOT_LARGEST_POSITIVE:
if (root1 > 0 && root2 > 0)
ans = ((root1 > root2) ? root1 : root2);
else if (root1 > 0)
ans = root1;
else if (root2 > 0)
ans = root2;
else
return false;
break;
case ROOT_SMALLEST_POSITIVE:
if (root1 > 0 && root2 > 0)
ans = ((root1 > root2) ? root2 : root1);
else if (root1 > 0)
ans = root1;
else if (root2 > 0)
ans = root2;
else
return false;
break;
default:
throw std::runtime_error("Unknown root selected");
break;
}
return true;
}
}
}

117
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic.hpp
Unescape View File

@ -1,117 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "quartic_yacfraid.hpp"
#include "quartic_descartes.hpp"
#include "quartic_neumark.hpp"
#include "quartic_ferrari.hpp"
namespace magnet {
namespace math {
namespace detail {
inline void quarticNewtonRootPolish(const double& a, const double& b, const double& c, const double& d,
double& root, size_t iterations)
{
for (size_t it = 0; it < iterations; ++it)
{
double error = (((root + a)*root + b) * root + c) * root + d;
double derivative = ((4 * root + 3 * a) * root + 2 * b) * root + c;
if ((error == 0) || derivative == 0) return;
root -= error / derivative;
}
}
}
//Solves quartics of the form x^4 + a x^3 + b x^2 + c x + d ==0
inline size_t quarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
static const double maxSqrt = std::sqrt(std::numeric_limits<double>::max());
if (std::abs(a) > maxSqrt)
yacfraidQuarticSolve(a,b,c,d,root1,root2,root3,root4);
if (d == 0)
{//Solve a cubic with a trivial root of 0
root1 = 0;
return 1 + cubicSolve(a, b, c, root2, root3, root4);
}
if ((a == 0) && (c== 0))
{//We have a biquadratic
double quadRoot1,quadRoot2;
if (quadSolve(d,b,1, quadRoot1, quadRoot2))
{
if (quadRoot1 < quadRoot2) std::swap(quadRoot1,quadRoot2);
if (quadRoot1 < 0)
return 0;
root1 = std::sqrt(quadRoot1);
root2 = -std::sqrt(quadRoot1);
if (quadRoot2 < 0)
return 2;
root3 = std::sqrt(quadRoot2);
root4 = -std::sqrt(quadRoot2);
return 4;
}
else
return 0;
}
//Now we have to resort to some dodgy formulae!
size_t k = 0, nr;
if (a < 0.0) k += 2;
if (b < 0.0) k += 1;
if (c < 0.0) k += 8;
if (d < 0.0) k += 4;
switch (k)
{
case 3 :
case 9 :
nr = ferrariQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
case 5 :
nr = descartesQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
case 15 :
//This algorithm is stable if we flip the sign of the roots
nr = descartesQuarticSolve(-a,b,-c,d,root1,root2,root3,root4);
root1 *=-1; root2 *=-1; root3 *=-1; root4 *=-1;
break;
default:
nr = neumarkQuarticSolve(a,b,c,d,root1,root2,root3,root4);
//nr = ferrariQuarticSolve(a,b,c,d,root1,root2,root3,root4);
//nr = yacfraidQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
}
if (nr) detail::quarticNewtonRootPolish(a, b, c, d, root1, 15);
if (nr>1) detail::quarticNewtonRootPolish(a, b, c, d, root2, 15);
if (nr>2) detail::quarticNewtonRootPolish(a, b, c, d, root3, 15);
if (nr>3) detail::quarticNewtonRootPolish(a, b, c, d, root4, 15);
return nr;
}
}
}

99
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic_descartes.hpp
Unescape View File

@ -1,99 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cubic.hpp"
namespace magnet {
namespace math {
inline size_t
descartesQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
double rts[4];
double worst3[3];
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
int j, n4[4];
double v1[4],v2[4],v3[4];
double k,y;
double p,q,r;
double e0,e1,e2;
double g,h;
double asq;
double ainv4;
double e1invk;
asq = a*a;
e2 = b - asq * (3.0/8.0);
e1 = c + a*(asq*0.125 - b*0.5);
e0 = d + asq*(b*0.0625 - asq*(3.0/256.0)) - a*c*0.25;
p = 2.0*e2;
q = e2*e2 - 4.0*e0;
r = -e1*e1;
size_t n3 = cubicSolve(p,q,r,v3[0],v3[1],v3[2]);
for (size_t j3 = 0; j3 < n3; ++j3)
{
y = v3[j3];
if (y <= 0.0)
n4[j3] = 0;
else
{
k = std::sqrt(y);
ainv4 = a*0.25;
e1invk = e1/k;
g = (y + e2 + e1invk)*0.5;
h = (y + e2 - e1invk)*0.5 ;
bool n1 = quadSolve( g, -k, 1.0, v1[0], v1[1]);
bool n2 = quadSolve( h, k, 1.0, v2[0], v2[1]);
qrts[0][j3] = v1[0] - ainv4;
qrts[1][j3] = v1[1] - ainv4;
qrts[n1*2][j3] = v2[0] - ainv4;
qrts[n1*2+1][j3] = v2[1] - ainv4;
n4[j3]= n1*2 + n2*2;
} /* y>=0 */
for (j = 0; j < n4[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts, n4[j3]);
} /* j3 loop */
size_t j3 = 0;
if (n3 != 1)
{
if ((n4[1] > n4[j3]) ||
((worst3[1] < worst3[j3] ) && (n4[1] == n4[j3]))) j3 = 1;
if ((n4[2] > n4[j3]) ||
((worst3[2] < worst3[j3] ) && (n4[2] == n4[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return (n4[j3]);
}
}
}

75
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic_error.hpp
Unescape View File

@ -1,75 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Slightly modified the original files from https://www.marcusbannerman.co.uk/index.php/downloads.html?func=finishdown&id=20
Mark W. Mueller (mwm@mwm.im)
Changes:
- Removed boost code
- some readibility tweaks.
*/
#pragma once
#include <cmath>
namespace magnet {
namespace math {
inline double quarticError(const double& a, const double& b, const double& c,
const double& d, const double roots[4], const size_t rootCount) {
// boost::array<double, 4> errors;
double errors[4];
for (size_t root = 0; root < rootCount; ++root) {
const double value = (((roots[root] + a) * roots[root] + b)
* roots[root] + c) * roots[root] + d;
if (value == 0) {
errors[root] = 0;
continue;
}
const double deriv = ((4 * roots[root] + 3 * a) * roots[root] + 2 * b)
* roots[root] + c;
if (deriv != 0)
errors[root] = std::abs(value / deriv);
else {
const double secDeriv = (12 * roots[root] + 6 * a) * roots[root]
+ 2 * b;
if (secDeriv != 0)
errors[root] = std::sqrt(std::abs(value / secDeriv));
else {
const double thirdDeriv = 24 * roots[root] + 6 * a;
if (thirdDeriv != 0)
errors[root] = std::pow(std::abs(value / thirdDeriv),
1.0 / 3.0);
else
errors[root] = std::sqrt(std::sqrt(std::abs(value) / 24));
}
}
}
double err = 0;
for (unsigned i = 0; i < rootCount; i++) {
if(errors[i]>err) err = errors[i];
}
return err;
// return *std::max_element(errors.begin(), errors.begin() + rootCount);
}
}
}

177
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic_ferrari.hpp
Unescape View File

@ -1,177 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Slightly modified the original files from https://www.marcusbannerman.co.uk/index.php/downloads.html?func=finishdown&id=20
Mark W. Mueller (mwm@mwm.im)
Changes:
- some readibility tweaks (also removes an eclipse warning about braces).
*/
#pragma once
#include "cubic.hpp"
namespace magnet {
namespace math {
inline size_t
ferrariQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
double rts[4];
double worst3[3];
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
int j;
int n4[4];
double asqinv4;
double ainv2;
double d4;
double yinv2;
double v1[4],v2[4],v3[4];
double p,q,r;
double y;
double e,f,esq,fsq,ef;
double g,gg,h,hh;
ainv2 = a*0.5;
asqinv4 = ainv2*ainv2;
d4 = d*4.0 ;
p = b;
q = a*c-d4;
r = (asqinv4 - b)*d4 + c*c;
size_t n3 = cubicSolve(p,q,r,v3[0],v3[1],v3[2]);
for (size_t j3 = 0; j3 < n3; ++j3)
{
y = v3[j3];
yinv2 = y*0.5;
esq = asqinv4 - b - y;
fsq = yinv2*yinv2 - d;
if ((esq < 0.0) && (fsq < 0.0))
n4[j3] = 0;
else
{
ef = -(0.25*a*y + 0.5*c);
if ( ((a > 0.0)&&(y > 0.0)&&(c > 0.0))
|| ((a > 0.0)&&(y < 0.0)&&(c < 0.0))
|| ((a < 0.0)&&(y > 0.0)&&(c < 0.0))
|| ((a < 0.0)&&(y < 0.0)&&(c > 0.0))
|| (a == 0.0)||(y == 0.0)||(c == 0.0))
/* use ef - */
{
if ((b < 0.0)&&(y < 0.0))
{
e = sqrt(esq);
f = ef/e;
}
else if (d < 0.0)
{
f = sqrt(fsq);
e = ef/f;
}
else
{
if (esq > 0.0)
e = sqrt(esq);
else
e = 0.0;
if (fsq > 0.0)
f = sqrt(fsq);
else
f = 0.0;
if (ef < 0.0)
f = -f;
}
}
else
/* use esq and fsq - */
{
if (esq > 0.0)
e = sqrt(esq);
else
e = 0.0;
if (fsq > 0.0)
f = sqrt(fsq);
else
f = 0.0;
if (ef < 0.0)
f = -f;
}
/* note that e >= 0.0 */
g = ainv2 - e;
gg = ainv2 + e;
if ( ((b > 0.0)&&(y > 0.0))
|| ((b < 0.0)&&(y < 0.0)) )
{
if (((a > 0.0) && (e > 0.0))
|| ((a < 0.0) && (e < 0.0) ))
g = (b + y)/gg;
else
if (((a > 0.0) && (e < 0.0))
|| ((a < 0.0) && (e > 0.0) ))
gg = (b + y)/g;
}
hh = -yinv2 + f;
h = -yinv2 - f;
if ( ((f > 0.0)&&(y < 0.0))
|| ((f < 0.0)&&(y > 0.0)) )
h = d/hh;
else
if ( ((f < 0.0)&&(y < 0.0))
|| ((f > 0.0)&&(y > 0.0)) )
hh = d/h;
bool n1 = quadSolve(hh,gg,1.0,v1[0],v1[1]);
bool n2 = quadSolve(h,g,1.0,v2[0],v2[1]);
n4[j3] = n1*2+n2*2;
qrts[0][j3] = v1[0];
qrts[1][j3] = v1[1];
qrts[n1*2+0][j3] = v2[0];
qrts[n1*2+1][j3] = v2[1];
}
for (j = 0; j < n4[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts, n4[j3]);
} /* j3 loop */
size_t j3 = 0;
if (n3 != 1)
{
if ((n4[1] > n4[j3]) ||
((worst3[1] < worst3[j3] ) && (n4[1] == n4[j3]))) j3 = 1;
if ((n4[2] > n4[j3]) ||
((worst3[2] < worst3[j3] ) && (n4[2] == n4[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return (n4[j3]);
}
}
}

179
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic_neumark.hpp
Unescape View File

@ -1,179 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cubic.hpp"
namespace magnet {
namespace math {
//solve the quartic equation -
//x**4 + a*x**3 + b*x**2 + c*x + d = 0
inline size_t
neumarkQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
double rts[4];
double worst3[3];
double y,g,gg,h,hh,gdis,gdisrt,hdis,hdisrt,g1,g2,h1,h2;
double bmy,gerr,herr,y4,bmysq;
double v1[4],v2[4],v3[4];
double hmax,gmax;
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
double asq = a * a;
double d4 = d * 4.0;
double p = -b * 2.0;
double q = b * b + a * c - d4;
double r = (c - a*b)*c + asq*d;
size_t cubicRoots = cubicSolve(p,q,r,v3[0],v3[1],v3[2]);
size_t nQuarticRoots[3];
for (size_t j3 = 0; j3 < cubicRoots; ++j3)
{
y = v3[j3];
bmy = b - y;
y4 = y * 4.0;
bmysq = bmy*bmy;
gdis = asq - y4;
hdis = bmysq - d4;
if ((gdis < 0.0) || (hdis < 0.0))
nQuarticRoots[j3] = 0;
else
{
g1 = a * 0.5;
h1 = bmy* 0.5;
gerr = asq + y4;
herr = hdis;
if (d > 0.0)
herr = bmysq + d4;
if ((y < 0.0) || (herr*gdis > gerr*hdis))
{
gdisrt = sqrt(gdis);
g2 = gdisrt*0.5;
if (gdisrt != 0.0)
h2 = (a*h1 - c)/gdisrt;
else
h2 = 0.0;
}
else
{
hdisrt = std::sqrt(hdis);
h2 = hdisrt*0.5;
if (hdisrt != 0.0)
g2 = (a*h1 - c)/hdisrt;
else
g2 = 0.0;
}
/*
note that in the following, the tests ensure non-zero
denominators -
*/
h = h1 - h2;
hh = h1 + h2;
hmax = hh;
if (hmax < 0.0)
hmax = -hmax;
if (hmax < h)
hmax = h;
if (hmax < -h)
hmax = -h;
if ((h1 > 0.0)&&(h2 > 0.0))
h = d/hh;
if ((h1 < 0.0)&&(h2 < 0.0))
h = d/hh;
if ((h1 > 0.0)&&(h2 < 0.0))
hh = d/h;
if ((h1 < 0.0)&&(h2 > 0.0))
hh = d/h;
if (h > hmax)
h = hmax;
if (h < -hmax)
h = -hmax;
if (hh > hmax)
hh = hmax;
if (hh < -hmax)
hh = -hmax;
g = g1 - g2;
gg = g1 + g2;
gmax = gg;
if (gmax < 0.0)
gmax = -gmax;
if (gmax < g)
gmax = g;
if (gmax < -g)
gmax = -g;
if ((g1 > 0.0)&&(g2 > 0.0))
g = y/gg;
if ((g1 < 0.0)&&(g2 < 0.0))
g = y/gg;
if ((g1 > 0.0)&&(g2 < 0.0))
gg = y/g;
if ((g1 < 0.0)&&(g2 > 0.0))
gg = y/g;
if (g > gmax)
g = gmax;
if (g < -gmax)
g = -gmax;
if (gg > gmax)
gg = gmax;
if (gg < -gmax)
gg = -gmax;
size_t n1 = quadSolve(hh,gg, 1.0,v1[0],v1[1]);
size_t n2 = quadSolve(h,g,1.0,v2[0],v2[1]);
nQuarticRoots[j3] = 2*n1 + 2*n2;
qrts[0][j3] = v1[0];
qrts[1][j3] = v1[1];
qrts[2*n1+0][j3] = v2[0];
qrts[2*n1+1][j3] = v2[1];
}
for (size_t j = 0; j < nQuarticRoots[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts,nQuarticRoots[j3]);
} /* j3 loop */
size_t j3 = 0;
if (cubicRoots > 1)
{
if ((nQuarticRoots[1] > nQuarticRoots[j3]) ||
((worst3[1] < worst3[j3] ) && (nQuarticRoots[1] == nQuarticRoots[j3]))) j3 = 1;
if ((nQuarticRoots[2] > nQuarticRoots[j3]) ||
((worst3[2] < worst3[j3] ) && (nQuarticRoots[2] == nQuarticRoots[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return nQuarticRoots[j3];
}
}
}

167
src/UAV/Modules/motion_planning/RapidQuad/include/RootFinder/quartic_yacfraid.hpp
Unescape View File

@ -1,167 +0,0 @@
/* DYNAMO:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2010 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cubic.hpp"
#include "quartic_error.hpp"
namespace magnet {
namespace math {
inline size_t
yacfraidQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
int j;
double y;
double v3[4];
double det0,det1,det2,det3;
double det0rt,det1rt,det2rt,det3rt;
double e,f,g,h,k;
double fsq,gsq,hsq,invk;
double P,Q,R,U;
double rts[4];
double worst3[3];
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
double asq = a * a;
double acu = a * asq;
double b4 = b * 4.0;
size_t n3 = 0;
int n4[4];
P = asq * b - b4 * b + 2.0 * a * c + 16.0 * d ;
Q = asq * c - b4 * c + 8.0 * a * d;
R = asq * d - c * c;
U = acu - b4 * a + 8.0 * c;
n4[0] = 0;
asq = a*a;
acu = a*asq;
b4 = b*4.0;
n3 = 0;
P = asq*b - b4*b + 2.0*a*c + 16.0*d ;
Q = asq*c - b4*c + 8.0*a*d;
R = asq*d - c*c ;
U = acu - b4*a + 8.0*c;
n4[0] = 0;
if (U == 0.0)
{
if (P == 0.0)
{
det0 = 3.0*asq - 8.0*b;
if (det0 < 0.0)
goto done;
det0rt = sqrt(det0);
qrts[0][0] = (-a + det0rt)*0.25;
qrts[1][0] = qrts[0][0];
qrts[2][0] = (-a - det0rt)*0.25;
qrts[3][0] = qrts[2][0];
n4[0] = 4;
goto done;
} /* P=0 */
else
{
det1 = asq*asq - 8.0*asq*b + 16.0*b*b - 64.0*d;
if (det1 < 0.0)
goto done;;
n4[0] = 0;
det1rt = sqrt(det1);
det2 = 3.0*asq - 8.0*b + 2.0*det1rt;
if (det2 >= 0.0)
{
det2rt = sqrt(det2);
qrts[0][0] = (-a + det2rt)*0.25;
qrts[1][0] = (-a - det2rt)*0.25;
n4[0] = 2;
}
det3 = 3.0*asq - 8.0*b - 2.0*det1rt;
if (det3 >= 0.0)
{
det3rt = sqrt(det3);
qrts[n4[0]++][0] = (-a + det3rt)*0.25;
qrts[n4[0]++][0] = (-a - det3rt)*0.25;
}
goto done;
} /* P<>0 */
}
n3 = cubicSolve(P/U,Q/U,R/U,v3[0], v3[1], v3[2]);
for (size_t j3 = 0; j3 < n3; ++j3)
{
y = v3[j3];
j = 0;
k = a + 4.0*y;
if (k == 0.0)
goto donej3;
invk = 1.0/k;
e = (acu - 4.0*c - 2.0*a*b + (6.0*asq - 16.0*b)*y)*invk;
fsq = (acu + 8.0*c - 4.0*a*b)*invk;
if (fsq < 0.0)
goto donej3;
f = sqrt(fsq);
gsq = 2.0*(e + f*k);
hsq = 2.0*(e - f*k);
if (gsq >= 0.0)
{
g = sqrt(gsq);
qrts[j++][j3] = (-a - f - g)*0.25;
qrts[j++][j3] = (-a - f + g)*0.25;
}
if (hsq >= 0.0)
{
h = sqrt(hsq);
qrts[j++][j3] = (-a + f - h)*0.25;
qrts[j++][j3] = (-a + f + h)*0.25;
}
donej3:
n4[j3] = j;
for (j = 0; j < n4[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts, n4[j3]);
} /* j3 loop */
done:
size_t j3 = 0;
if (n3 > 1)
{
if ((n4[1] > n4[j3]) ||
((worst3[1] < worst3[j3] ) && (n4[1] == n4[j3]))) j3 = 1;
if ((n4[2] > n4[j3]) ||
((worst3[2] < worst3[j3] ) && (n4[2] == n4[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return (n4[j3]);
}
}
}

197
src/UAV/Modules/motion_planning/RapidQuad/include/SingleAxisTrajectory.cpp
Unescape View File

@ -1,197 +0,0 @@
/*!
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#include "SingleAxisTrajectory.h"
#include <math.h>
#include <algorithm>//For min/max
#include <limits>//for max double
using namespace RapidQuadrocopterTrajectoryGenerator;
SingleAxisTrajectory::SingleAxisTrajectory(void)
:_a(0),_b(0),_g(0),_cost(std::numeric_limits<double>::max())
{
Reset();
}
/*!
* Resets the goal state constraints and the cost.
*/
void SingleAxisTrajectory::Reset(void)
{
_posGoalDefined = _velGoalDefined = _accGoalDefined = false;
_cost = std::numeric_limits<double>::max();
_accPeakTimes.initialised = false;
}
/*!
* Calculate the coefficients that define a trajectory. This solves, in closed form
* the optimal trajectory for some given set of goal constraints.
* Before calling this,
* - define the initial state
* - define the relevant parts of the trajectory goal state.
*
* @param Tf the trajectory duration [s]
* @see SetInitialState()
* @see SetGoalPosition()
* @see SetGoalVelocity()
* @see SetGoalAcceleration()
*/
void SingleAxisTrajectory::GenerateTrajectory(const double Tf)
{
//define starting position:
double delta_a = _af - _a0;
double delta_v = _vf - _v0 - _a0*Tf;
double delta_p = _pf - _p0 - _v0*Tf - 0.5*_a0*Tf*Tf;
//powers of the end time:
const double T2 = Tf*Tf;
const double T3 = T2*Tf;
const double T4 = T3*Tf;
const double T5 = T4*Tf;
//solve the trajectories, depending on what's constrained:
if(_posGoalDefined && _velGoalDefined && _accGoalDefined)
{
_a = ( 60*T2*delta_a - 360*Tf*delta_v + 720* 1*delta_p)/T5;
_b = (-24*T3*delta_a + 168*T2*delta_v - 360*Tf*delta_p)/T5;
_g = ( 3*T4*delta_a - 24*T3*delta_v + 60*T2*delta_p)/T5;
}
else if(_posGoalDefined && _velGoalDefined)
{
_a = (-120*Tf*delta_v + 320* delta_p)/T5;
_b = ( 72*T2*delta_v - 200*Tf*delta_p)/T5;
_g = ( -12*T3*delta_v + 40*T2*delta_p)/T5;
}
else if(_posGoalDefined && _accGoalDefined)
{
_a = (-15*T2*delta_a + 90* delta_p)/(2*T5);
_b = ( 15*T3*delta_a - 90*Tf*delta_p)/(2*T5);
_g = (- 3*T4*delta_a + 30*T2*delta_p)/(2*T5);
}
else if(_velGoalDefined && _accGoalDefined)
{
_a = 0;
_b = ( 6*Tf*delta_a - 12* delta_v)/T3;
_g = (-2*T2*delta_a + 6*Tf*delta_v)/T3;
}
else if(_posGoalDefined)
{
_a = 20*delta_p/T5;
_b = -20*delta_p/T4;
_g = 10*delta_p/T3;
}
else if(_velGoalDefined)
{
_a = 0;
_b =-3*delta_v/T3;
_g = 3*delta_v/T2;
}
else if(_accGoalDefined)
{
_a = 0;
_b = 0;
_g = delta_a/Tf;
}
else
{//Nothing to do!
_a = _b = _g = 0;
}
//Calculate the cost:
_cost = _g*_g + _b*_g*Tf + _b*_b*T2/3.0 + _a*_g*T2/3.0 + _a*_b*T3/4.0 + _a*_a*T4/20.0;
}
/*!
* Calculate the extrema of the acceleration trajectory. This is made relatively
* easy by the fact that the acceleration is a cubic polynomial, so we only need
* to solve for the roots of a quadratic.
* @param aMinOut [out] The minimum acceleration in the segment
* @param aMaxOut [out] The maximum acceleration in the segment
* @param t1 [in] The start time of the segment
* @param t2 [in] The end time of the segment
*/
void SingleAxisTrajectory::GetMinMaxAcc(double &aMinOut, double &aMaxOut, double t1, double t2)
{
if(!_accPeakTimes.initialised)
{
//calculate the roots of the polynomial
if(_a)
{//solve a quadratic for t
double det = _b*_b - 2*_g*_a;
if(det<0)
{//no real roots
_accPeakTimes.t[0] = 0;
_accPeakTimes.t[1] = 0;
}
else
{
_accPeakTimes.t[0] = (-_b + sqrt(det))/_a;
_accPeakTimes.t[1] = (-_b - sqrt(det))/_a;
}
}
else
{//solve linear equation: _g + _b*t == 0:
if(_b) _accPeakTimes.t[0] = -_g/_b;
else _accPeakTimes.t[0] = 0;
_accPeakTimes.t[1] = 0;
}
_accPeakTimes.initialised = 1;
}
//Evaluate the acceleration at the boundaries of the period:
aMinOut = std::min(GetAcceleration(t1), GetAcceleration(t2));
aMaxOut = std::max(GetAcceleration(t1), GetAcceleration(t2));
//Evaluate at the maximum/minimum times:
for(int i=0; i<2; i++)
{
if(_accPeakTimes.t[i] <= t1) continue;
if(_accPeakTimes.t[i] >= t2) continue;
aMinOut = std::min(aMinOut, GetAcceleration(_accPeakTimes.t[i]));
aMaxOut = std::max(aMaxOut, GetAcceleration(_accPeakTimes.t[i]));
}
}
/*!
* Calculate the maximum jerk squared along the trajectory. This is made easy
* because the jerk is quadratic, so we need to evaluate at most three points.
* @param t1 [in] The start time of the segment
* @param t2 [in] The end time of the segment
* @return the maximum jerk
*/
double SingleAxisTrajectory::GetMaxJerkSquared(double t1, double t2)
{
double jMaxSqr = std::max(pow(GetJerk(t1),2),pow(GetJerk(t2),2));
if(_a)
{
double tMax = -1;
tMax = -_b/_a;
if(tMax>t1 && tMax<t2)
{
jMaxSqr = std::max(pow(GetJerk(tMax),2),jMaxSqr);
}
}
return jMaxSqr;
}

102
src/UAV/Modules/motion_planning/RapidQuad/include/SingleAxisTrajectory.h
Unescape View File

@ -1,102 +0,0 @@
/**
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
namespace RapidQuadrocopterTrajectoryGenerator
{
//! An axis along one spatial direction
/*!
* For more information, please refer to Section II of the publication.
*/
class SingleAxisTrajectory
{
public:
//! Constructor, calls Reset() function.
SingleAxisTrajectory(void);
//! Define the trajectory's initial state (position, velocity, acceleration), at time zero
void SetInitialState(const double pos0, const double vel0, const double acc0){_p0=pos0; _v0=vel0; _a0=acc0; Reset();};
//! Define the trajectory's final position (if you don't call this, it is left free)
void SetGoalPosition(const double posf) {_posGoalDefined = true; _pf = posf;};
//! Define the trajectory's final velocity (if you don't call this, it is left free)
void SetGoalVelocity(const double velf) {_velGoalDefined = true; _vf = velf;};
//! Define the trajectory's final acceleration (if you don't call this, it is left free)
void SetGoalAcceleration(const double accf){_accGoalDefined = true; _af = accf;};
//! Generate the trajectory, from the defined initial state to the defined components of the final state.
void GenerateTrajectory(const double timeToGo);
//! Resets the cost, coefficients, and goal state constraints. Does *not* reset the initial state
void Reset(void);
//! Returns the jerk at time t
double GetJerk(double t) const{return _g + _b*t + (1/2.0)*_a*t*t;};
//! Returns the acceleration at time t
double GetAcceleration(double t) const{return _a0 + _g*t + (1/2.0)*_b*t*t + (1/6.0)*_a*t*t*t;};
//! Returns the velocity at time t
double GetVelocity(double t) const{return _v0 + _a0*t + (1/2.0)*_g*t*t + (1/6.0)*_b*t*t*t + (1/24.0)*_a*t*t*t*t;};
//! Returns the position at time t
double GetPosition(double t) const{return _p0 + _v0*t + (1/2.0)*_a0*t*t + (1/6.0)*_g*t*t*t + (1/24.0)*_b*t*t*t*t + (1/120.0)*_a*t*t*t*t*t;};
//! Calculate the extrema of the acceleration trajectory over a section
void GetMinMaxAcc(double &aMinOut, double &aMaxOut, double t1, double t2);
//! Calculate the extrema of the jerk squared over a section
double GetMaxJerkSquared(double t1, double t2);
//! Get the parameters defining the trajectory
double GetParamAlpha(void) const {return _a;};
//! Get the parameters defining the trajectory
double GetParamBeta (void) const {return _b;};
//! Get the parameters defining the trajectory
double GetParamGamma(void) const {return _g;};
//! Get the parameters defining the trajectory
double GetInitialAcceleration(void) const {return _a0;};
//! Get the parameters defining the trajectory
double GetInitialVelocity(void) const {return _v0;};
//! Get the parameters defining the trajectory
double GetInitialPosition(void) const {return _p0;};
//! Get the trajectory cost value
double GetCost(void) const{return _cost;};
private:
double _p0, _v0, _a0;//!< The initial state (position, velocity, acceleration)
double _pf, _vf, _af;//!< The goal state (position, velocity, acceleration)
bool _posGoalDefined, _velGoalDefined, _accGoalDefined;//!< The components of the goal state defined to be fixed (position, velocity, acceleration)
double _a, _b, _g;//!< The three coefficients that define the trajectory
double _cost;//!< The trajectory cost, J
struct
{
double t[2];
bool initialised;
} _accPeakTimes;//!< The times at which the acceleration has minimum/maximum
};
};//namespace

93
src/UAV/Modules/motion_planning/RapidQuad/include/Vec3.h
Unescape View File

@ -1,93 +0,0 @@
/*!
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <assert.h>
#include <math.h>
#include <limits>
//! 3D vector class
/*!
* A 3D vector class, to make passing arguments easier, and allow easy addition etc. of vectors.
* Could be readily replaced with arrays of doubles, but this would complicate some of the code.
*/
class Vec3
{
public:
double x,y,z; //!< the three components of the vector
Vec3(void):x(std::numeric_limits<double>::quiet_NaN()),y(std::numeric_limits<double>::quiet_NaN()),z(std::numeric_limits<double>::quiet_NaN()){};//!<Initialises all members to NaN
Vec3(double xin,double yin,double zin):x(xin),y(yin),z(zin){};//!< Initialise vector
//!Getter function, index 0 <-> x, 1 <-> y, 2 <-> z
inline double operator[](int i) const
{
switch(i)
{
case 0: return x;
case 1: return y;
case 2: return z;
break;
}
//we're doing something wrong if we get here
assert(0);
return std::numeric_limits<double>::quiet_NaN();
}
//!Setter function, index 0 <-> x, 1 <-> y, 2 <-> z
inline double & operator[](int i)
{
switch(i)
{
case 0: return x;
case 1: return y;
case 2: return z;
break;
}
//we're doing something wrong if we get here
assert(0);
//fail loudly:
x = y = z = std::numeric_limits<double>::quiet_NaN();
return x;
}
//!Calculate the dot product of two vectors
inline double Dot(const Vec3 rhs) const {return x*rhs.x + y*rhs.y + z*rhs.z;};
//!Calculate the cross product of two vectors
inline Vec3 Cross(const Vec3 rhs) const { return Vec3(y*rhs.z - z*rhs.y, z*rhs.x - x*rhs.z, x*rhs.y - y*rhs.x);};
//!Calculate the Euclidean norm of the vector, squared (= sum of squared elements).
inline double GetNorm2Squared(void) const {return this->Dot(*this);};
//!Calculate the Euclidean norm of the vector.
inline double GetNorm2(void) const {return sqrt(GetNorm2Squared());};
//!Get the unit vector pointing along the same direction as this vector. Will fail for zero vectors.
Vec3 GetUnitVector(void) const {return (*this)/this->GetNorm2();};
inline Vec3 operator+(const Vec3 rhs)const {return Vec3(x+rhs.x,y+rhs.y,z+rhs.z);};
inline Vec3 operator-(const Vec3 rhs)const {return Vec3(x-rhs.x,y-rhs.y,z-rhs.z);};
inline Vec3 operator/(const double rhs)const {return Vec3(x/rhs,y/rhs,z/rhs);};
};
//Some operator overloading:
inline Vec3 operator*(const double lhs, const Vec3 rhs) {return Vec3(lhs*rhs.x, lhs*rhs.y, lhs*rhs.z);};
inline Vec3 operator*(const Vec3 lhs, const double rhs) {return Vec3(lhs.x*rhs, lhs.y*rhs, lhs.z*rhs);};
inline Vec3 operator/(const Vec3 lhs, const double rhs) {return Vec3(lhs.x/rhs, lhs.y/rhs, lhs.z/rhs);};

28
src/UAV/Modules/motion_planning/RapidQuad/package.xml
Unescape View File

@ -1,28 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>prometheus_rapid_quad</name>
<version>0.0.0</version>
<description>The prometheus_rapid_quad package</description>
<maintainer email="fatmoonqyp@126.com">Yuhua Qi</maintainer>
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
<license>TODO</license>
<url type="website">http://www.amovauto.com/</url>
<url type="repository">https://github.com/potato77/prometheus_control.git</url>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>std_msgs</exec_depend>
<export>
</export>
</package>

104
src/UAV/Modules/motion_planning/RapidQuad/src/Demo.cpp
Unescape View File

@ -1,104 +0,0 @@
/*!
* Rapid trajectory generation for quadrocopters
*
* Copyright 2014 by Mark W. Mueller <mwm@mwm.im>
*
* This code is free software: you can redistribute
* it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This code is distributed in the hope that it will
* be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the code. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
using namespace std;
#include "RapidTrajectoryGenerator.h"
using namespace RapidQuadrocopterTrajectoryGenerator;
//Two simple helper function to make testing easier
const char* GetInputFeasibilityResultName(RapidTrajectoryGenerator::InputFeasibilityResult fr)
{
switch(fr)
{
case RapidTrajectoryGenerator::InputFeasible: return "Feasible";
case RapidTrajectoryGenerator::InputIndeterminable: return "Indeterminable";
case RapidTrajectoryGenerator::InputInfeasibleThrustHigh: return "InfeasibleThrustHigh";
case RapidTrajectoryGenerator::InputInfeasibleThrustLow: return "InfeasibleThrustLow";
case RapidTrajectoryGenerator::InputInfeasibleRates: return "InfeasibleRates";
}
return "Unknown!";
};
const char* GetStateFeasibilityResultName(RapidTrajectoryGenerator::StateFeasibilityResult fr)
{
switch(fr)
{
case RapidTrajectoryGenerator::StateFeasible: return "Feasible";
case RapidTrajectoryGenerator::StateInfeasible: return "Infeasible";
}
return "Unknown!";
};
int main(void)
{
//Define the trajectory starting state:
Vec3 pos0 = Vec3(0, 0, 2); //position
Vec3 vel0 = Vec3(0, 0, 0); //velocity
Vec3 acc0 = Vec3(0, 0, 0); //acceleration
//define the goal state:
Vec3 posf = Vec3(1, 0, 1); //position
Vec3 velf = Vec3(0, 0, 1); //velocity
Vec3 accf = Vec3(0, 0, 0); //acceleration
//define the duration:
double Tf = 1.3;
double fmin = 5;//[m/s**2]
double fmax = 25;//[m/s**2]
double wmax = 20;//[rad/s]
double minTimeSec = 0.02;//[s]
//Define how gravity lies in our coordinate system
Vec3 gravity = Vec3(0,0,-9.81);//[m/s**2]
//Define the state constraints. We'll only check that we don't fly into the floor:
Vec3 floorPos = Vec3(0,0,0);//any point on the boundary
Vec3 floorNormal = Vec3(0,0,1);//we want to be in this direction of the boundary
RapidTrajectoryGenerator traj(pos0, vel0, acc0, gravity);
traj.SetGoalPosition(posf);
traj.SetGoalVelocity(velf);
traj.SetGoalAcceleration(accf);
// Note: if you'd like to leave some states free, you can encode it like below.
// Here we would be leaving the velocity in `x` (axis 0) free:
//
// traj.SetGoalVelocityInAxis(1,velf[1]);
// traj.SetGoalVelocityInAxis(2,velf[2]);
traj.Generate(Tf);
for(int i = 0; i < 3; i++)
{
cout << "Axis #" << i << "\n";
cout << "\talpha = " << traj.GetAxisParamAlpha(i);
cout << "\tbeta = " << traj.GetAxisParamBeta(i);
cout << "\tgamma = " << traj.GetAxisParamGamma(i);
cout << "\n";
}
cout << "Total cost = " << traj.GetCost() << "\n";
cout << "Input feasible? " << GetInputFeasibilityResultName(traj.CheckInputFeasibility(fmin,fmax,wmax,minTimeSec)) << "\n";
cout << "Position feasible? " << GetStateFeasibilityResultName(traj.CheckPositionFeasibility(floorPos, floorNormal)) << "\n";
return 0;
}

266
src/UAV/Modules/motion_planning/RapidQuad/src/main_node.cpp
Unescape View File

@ -1,266 +0,0 @@
//头文件
#include <ros/ros.h>
#include <iostream>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/PositionReference.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Path.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
// #include "message_utils.h"
#include "RapidTrajectoryGenerator.h"
using namespace std;
using namespace RapidQuadrocopterTrajectoryGenerator;
#define NODE_NAME "motion_planning"
prometheus_msgs::ControlCommand Command_Now; //发送给控制模块 [px4_pos_controller.cpp]的命令
prometheus_msgs::DroneState _DroneState; //无人机状态量
geometry_msgs::PoseStamped goal;
Vec3 pos0 = Vec3(0, 0, 2); //position
Vec3 vel0 = Vec3(0, 0, 0); //velocity
Vec3 acc0 = Vec3(0, 0, 0); //acceleration
//define the goal state:
Vec3 posf = Vec3(1, 0, 1); //position
Vec3 velf = Vec3(0, 0, 1); //velocity
Vec3 accf = Vec3(0, 0, 0); //acceleration
int flag_get_goal = 0;
float time_trajectory;
//define the duration: TF不能打死
double Tf = 5.0;
float Alpha[3],Beta[3],Gamma[3];
nav_msgs::Path Optimal_Path;
ros::Publisher ref_tra_pub;
//Two simple helper function to make testing easier
const char* GetInputFeasibilityResultName(RapidTrajectoryGenerator::InputFeasibilityResult fr)
{
switch(fr)
{
case RapidTrajectoryGenerator::InputFeasible: return "Feasible";
case RapidTrajectoryGenerator::InputIndeterminable: return "Indeterminable";
case RapidTrajectoryGenerator::InputInfeasibleThrustHigh: return "InfeasibleThrustHigh";
case RapidTrajectoryGenerator::InputInfeasibleThrustLow: return "InfeasibleThrustLow";
case RapidTrajectoryGenerator::InputInfeasibleRates: return "InfeasibleRates";
}
return "Unknown!";
};
const char* GetStateFeasibilityResultName(RapidTrajectoryGenerator::StateFeasibilityResult fr)
{
switch(fr)
{
case RapidTrajectoryGenerator::StateFeasible: return "Feasible";
case RapidTrajectoryGenerator::StateInfeasible: return "Infeasible";
}
return "Unknown!";
};
void goal_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
goal = *msg;
flag_get_goal = 1;
cout << "Get a new goal!"<<endl;
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
_DroneState = *msg;
}
void pub_ref_trajectory();
int main(int argc, char **argv)
{
ros::init(argc, argv, "main_node");
ros::NodeHandle nh("~");
ros::Rate rate(20.0);
//　能用，但距离使用还差前端　和　后端约束
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);
//【订阅】目标点
ros::Subscriber goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/prometheus/planning/goal", 10, goal_cb);
// 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
ref_tra_pub = nh.advertise<nav_msgs::Path>("/prometheus/motion_planning/ref_trajectory", 10);
int start_flag;
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Motion Planning Mission<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
cout << "Please enter 1 for takeoff"<<endl;
cin >> start_flag;
// 起飞
if( start_flag == 1)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
Command_Now.Command_ID = 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.yaw_ref = 999;
command_pub.publish(Command_Now);
cout << "Switch to OFFBOARD and arm ..."<<endl;
ros::Duration(3.0).sleep();
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Takeoff;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
command_pub.publish(Command_Now);
cout << "Takeoff ..."<<endl;
ros::Duration(3.0).sleep();
ros::spinOnce();
}
double fmin = 5;//[m/s**2]
double fmax = 25;//[m/s**2]
double wmax = 20;//[rad/s]
double minTimeSec = 0.02;//[s]
//Define how gravity lies in our coordinate system
Vec3 gravity = Vec3(0,0,-9.81);//[m/s**2]
//Define the state constraints. We'll only check that we don't fly into the floor:
Vec3 floorPos = Vec3(0,0,0);//any point on the boundary
Vec3 floorNormal = Vec3(0,0,1);//we want to be in this direction of the boundary
while (ros::ok())
{
flag_get_goal = 0;
ros::spinOnce();
if(flag_get_goal == 1)
{
//Define the trajectory starting state:
pos0 = Vec3(_DroneState.position[0], _DroneState.position[1], _DroneState.position[2]);
vel0 = Vec3(_DroneState.velocity[0], _DroneState.velocity[1], _DroneState.velocity[2]);
acc0 = Vec3(0, 0, 0); //acceleration
posf = Vec3(goal.pose.position.x, goal.pose.position.y, goal.pose.position.z);
velf = Vec3(0.0, 0.0, 0.0);
accf = Vec3(0, 0, 0); //acceleration
RapidTrajectoryGenerator traj(pos0, vel0, acc0, gravity);
traj.SetGoalPosition(posf);
traj.SetGoalVelocity(velf);
traj.SetGoalAcceleration(accf);
// Note: if you'd like to leave some states free, you can encode it like below.
// Here we would be leaving the velocity in `x` (axis 0) free:
//
// traj.SetGoalVelocityInAxis(1,velf[1]);
// traj.SetGoalVelocityInAxis(2,velf[2]);
//　根据目标点重新预估时间,希望平均速度为1m/s
Tf = sqrt( pow(posf[0] - pos0[0], 2) + pow(posf[1] - pos0[1], 2) + pow(posf[2] - pos0[2], 2)) / 1.0;
traj.Generate(Tf);
for(int i = 0; i < 3; i++)
{
Alpha[i] = traj.GetAxisParamAlpha(i);
Beta[i] = traj.GetAxisParamBeta(i);
Gamma[i] = traj.GetAxisParamGamma(i);
cout << "Axis #" << i << "\n";
cout << "\tAlpha = " << Alpha[i];
cout << "\tBeta = " << Beta[i];
cout << "\tGamma = " << Gamma[i];
cout << "\n";
}
cout << "Tf = " << Tf << " [s] " << endl;
cout << "Total cost = " << traj.GetCost() << "\n";
cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
cout << "Input feasible? " << GetInputFeasibilityResultName(traj.CheckInputFeasibility(fmin,fmax,wmax,minTimeSec)) << "\n";
cout << "Position feasible? " << GetStateFeasibilityResultName(traj.CheckPositionFeasibility(floorPos, floorNormal)) << "\n";
pub_ref_trajectory();
time_trajectory = 0.0;
while(time_trajectory < Tf)
{
Command_Now.header.stamp = ros::Time::now();
Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
Command_Now.Command_ID = Command_Now.Command_ID + 1;
Command_Now.source = NODE_NAME;
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY;
Command_Now.Reference_State.time_from_start = time_trajectory;
for(int i = 0; i < 3; i++)
{
//时间戳怎么解决？
Command_Now.Reference_State.position_ref[i] = 1/120.0 * Alpha[i] * pow(time_trajectory,5) + 1/24.0 * Beta[i] * pow(time_trajectory,4) + 1/6.0 * Gamma[i] * pow(time_trajectory,3) + 1/2.0 * acc0[i] * pow(time_trajectory,2) + vel0[i] * time_trajectory + pos0[i];
Command_Now.Reference_State.velocity_ref[i] = 1/24.0 * Alpha[i] * pow(time_trajectory,4) + 1/6.0 * Beta[i] * pow(time_trajectory,3) + 1/2.0 * Gamma[i] * pow(time_trajectory,2) + acc0[i] * time_trajectory + vel0[i];
Command_Now.Reference_State.acceleration_ref[i] = 1/6.0 * Alpha[i] * pow(time_trajectory,3) + 1/2.0 * Beta[i] * pow(time_trajectory,2) + Gamma[i] * time_trajectory + acc0[i];
}
Command_Now.Reference_State.yaw_ref = 0.0;
command_pub.publish(Command_Now);
time_trajectory = time_trajectory + 0.01;
ros::Duration(0.01).sleep();
// cout << "Motion Planning Result:"<<endl;
// cout << "Trajectory tracking: "<< time_trajectory << " / " << Tf << " [ s ]" <<endl;
// cout << "desired_point: " << Command_Now.Reference_State.position_ref[0] << " [m] "
// << Command_Now.Reference_State.position_ref[1] << " [m] "
// << Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
// cout << "goal_pos: " << goal.pose.position.x << " [m] "<< goal.pose.position.y << " [m] "<< goal.pose.position.z << " [m] "<<endl;
}
}
}
return 0;
}
void pub_ref_trajectory()
{
Optimal_Path.header.frame_id = "world";
Optimal_Path.header.stamp = ros::Time::now();
Optimal_Path.poses.clear();
time_trajectory = 0.0;
int k = 0;
while(time_trajectory < Tf)
{
geometry_msgs::PoseStamped way_point;
way_point.header.frame_id = "world";
way_point.pose.position.x = 1/120.0 * Alpha[0] * pow(time_trajectory,5) + 1/24.0 * Beta[0] * pow(time_trajectory,4) + 1/6.0 * Gamma[0] * pow(time_trajectory,3) + 1/2.0 * acc0[0] * pow(time_trajectory,2) + vel0[0] * time_trajectory + pos0[0];
way_point.pose.position.y = 1/120.0 * Alpha[1] * pow(time_trajectory,5) + 1/24.0 * Beta[1] * pow(time_trajectory,4) + 1/6.0 * Gamma[1] * pow(time_trajectory,3) + 1/2.0 * acc0[1] * pow(time_trajectory,2) + vel0[1] * time_trajectory + pos0[1];
way_point.pose.position.z = 1/120.0 * Alpha[2] * pow(time_trajectory,5) + 1/24.0 * Beta[2] * pow(time_trajectory,4) + 1/6.0 * Gamma[2] * pow(time_trajectory,3) + 1/2.0 * acc0[2] * pow(time_trajectory,2) + vel0[2] * time_trajectory + pos0[2];
Optimal_Path.poses.push_back(way_point);
time_trajectory = time_trajectory + 0.01;
}
ref_tra_pub.publish(Optimal_Path);
}

185
src/UAV/Modules/object_circlex_detection/CMakeLists.txt
Unescape View File

@ -1,185 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(prometheus_detection_circlex)
add_definitions(-DCPU_ONLY=1)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp roslib std_msgs sensor_msgs image_transport cv_bridge prometheus_msgs
)
set(CAFFE_INCLUDEDIR spire_caffe/include spire_caffe/build/src)
set(CAFFE_LINK_LIBRARAY spire_caffe/build/lib)
# SET(CMAKE_MODULE_PATH /home/imav/jario_ws/aruco-2.0.19/build)
# SET(ARUCO_INCLUDE_DIRS /home/imav/jario_ws/aruco-2.0.19/src)
# SET(CMAKE_MODULE_PATH /home/imav/jario_ws/opencv-3.3.1/build)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
find_package(OpenCV REQUIRED)
# FIND_PACKAGE(aruco REQUIRED)
add_compile_options(-std=c++11)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"expected
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES ros_caffe
# CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
/usr/include
spire_caffe/build/include
)
include_directories(${CAFFE_INCLUDEDIR})
## Declare a cpp library
# add_library(ros_caffe
# src/${PROJECT_NAME}/ros_caffe.cpp
# )
## Declare a cpp executable
# add_executable(ros_caffe_node src/ros_caffe_node.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(ros_caffe_node ros_caffe_generate_messages_cpp)
## Specify libraries to link a library or executable target against
# target_link_libraries(ros_caffe_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ros_caffe ros_caffe_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_caffe.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
file(GLOB "FILES" "src/*.cpp")
set(SRCS src/fly_detector.cpp ${FILES})
link_directories(${CAFFE_LINK_LIBRARAY})
add_executable(fly_detector ${SRCS})
target_link_libraries(fly_detector ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} caffe glog)

180
src/UAV/Modules/object_circlex_detection/include/derror.h
Unescape View File

@ -1,180 +0,0 @@
#ifndef DERRORH
#define DERRORH
#include <cstdlib>
#include <utility>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <fstream>
#include <math.h>
#include "string"
#include <time.h>
#include <queue>
#include <vector>
using namespace std;
class DERROR
{
public:
//构造函数
DERROR(void)
{
error = 0;
error_list.push_back(make_pair(0.0,0.0));
//D_input = 0;
// P_Out = 0;
// I_Out = 0;
D_Out = 0;
Output = 0;
// Kf = 0;
// start_intergrate_flag = 0;
}
//float D_input, D_input0;
// float Kp; //参数P
// float Ki; //参数I
// float Kd; //参数D
// float Kf; //微分项的滤波增益
float error; //误差量 = 实际值 - 期望值
float delta_time; //时间间隔dt
float cur_time;
vector<pair<float, float>> error_list; //误差表,用作计算微分项 平滑窗口 [2nd data, 1st time]
// float P_Out; //P环节输出
// float I_Out; //I环节输出
float D_Out; //D环节输出
float Output; //输出
// bool start_intergrate_flag; //是否积分标志[进入offboard(启控)后,才开始积分]
// float Imax; //积分上限
// float Output_max; //输出最大值
float errThres; //误差死区(if error<errThres, error<0)
//设置PID参数函数[Kp Ki Kd Kf]
// void setPID(float p_value, float i_value, float d_value, float f_value);
//输入 误差 和 当前时间
bool add_error(float input_error, float curtime);
void show_error(void);
//PID输出结果
void derror_output(void);
//设置积分上限 控制量最大值 误差死区
// void set_sat(float i_max, float con_max, float thres)
// {
// Output_max = con_max;
// Imax = i_max;
// errThres = thres;
// }
//饱和函数
// float satfunc(float data, float Max, float Thres)
// {
// if (abs(data)<Thres)
// return 0;
// else if(abs(data)>Max){
// return (data>0)?Max:-Max;
// }
// else{
// return data;
// }
// }
};
//void PID::setPID(float p_value, float i_value, float d_value, float f_value)
//{
// Kp = p_value;
// Ki = i_value;
// Kd = d_value;
// Kf = f_value;
//}
void DERROR::derror_output(void)
{
// P_Out = Kp * error; //P环节输出值
// I_Out = I_Out + Ki *error*delta_time; //I环节输出值
// I_Out = satfunc(I_Out, Imax, 0); //I环节限幅[I_Out<=Imax]
// if(start_intergrate_flag == 0)
// {
// I_Out = 0;
// }
if (error_list.size() < 3)
{
D_Out = 0; //initiral process
}
else if(error_list.size()<7)
{
vector<pair<float, float>>::reverse_iterator initial_error_k = error_list.rbegin();
vector<pair<float, float>>::iterator initial_error_k_initial = error_list.begin();
vector<pair<float, float>>::iterator initial_error_k_n = initial_error_k_initial+1;
D_Out = (initial_error_k->second - initial_error_k_n->second)/delta_time;
}
else
{
vector<pair<float, float>>::reverse_iterator error_k = error_list.rbegin();
vector<pair<float, float>>::iterator error_k_n = error_list.begin();
D_Out = (error_k->second - error_k_n->second)/delta_time;
}
Output = D_Out;
// Output = satfunc(Output, Output_max, errThres);
}
void DERROR::show_error(void){
// cout <<"error_size "<< error_list.size()<<" delta_time " << delta_time <<endl;
cout << "delta_time: " << delta_time << " cur_time: " << cur_time << " error_list.rbegin() " << error_list.rbegin()->first << " error_list.begin() " << error_list.begin()->first << " error: "<< error_list.begin()->second << error_list.rbegin()->second << " Output: ["<<Output<<"] "<<std::endl;
}
bool DERROR::add_error(float input_error, float curtime)
{
error = input_error;
cur_time=curtime;
if(error_list.size()<6)
{
delta_time = 0.16;
}
else{
delta_time = curtime - error_list.begin()->first;
// cout<< "1 "<<"last time "<< error_list.end()->first <<"delta_time: "<< delta_time<<endl;
}
if(error_list.size()<6){
error_list.push_back(make_pair(curtime, error));
}
else{
// cout<<"*---------*"<<endl;
vector<pair<float, float>>::iterator k_beg = error_list.begin();
error_list.erase(k_beg);
std::pair<float,float > p1(curtime, error);
// error_list.emplace_back(std::move(p1));
error_list.emplace_back(p1);
// float tmp_first = error_list.end()->first;
/*error_list.end()->first = curtime;
error_list.end()->second = error;*/
// printf("time: %f, last time: %f\n", curtime, error_list.rbegin()->first);
}
return true;
}
#endif

461
src/UAV/Modules/object_circlex_detection/include/findx.h
Unescape View File

@ -1,461 +0,0 @@
#ifndef __FIND_X__
#define __FIND_X__
#include <iostream>
#include <math.h>
#include <string>
#include "opencv2/opencv.hpp"
using namespace std;
using namespace cv;
typedef std::vector<cv::Point> PointsVector;
typedef std::vector<PointsVector> ContoursVector;
struct PointValue
{
Point p;
int value;
Point p1;
float rad;
float length;
};
bool SortPointValue(const PointValue &p1, const PointValue &p2)
{ //从大到小
return p1.value > p2.value;
}
float GetLineRad(Point &p1, Point &p2)
{
float rad;
Point2f p = p2 - p1;
rad = atan2(p.y, p.x);
return rad;
}
float GetLineRad(Vec4i &vec)
{
Point2i p1, p2;
p1.x = vec[0];
p1.y = vec[1];
p2.x = vec[2];
p2.y = vec[3];
return GetLineRad(p1, p2);
}
float GetRad(Point &p1, Point &p2)
{
float rad;
Point2f p = p2 - p1;
rad = atan2(p.y, p.x);
if (rad < 0)
rad = rad + CV_PI;
return rad;
}
float GetVertRad(float rad)
{
float r = rad - CV_PI / 2;
if (rad < 0)
rad = rad + CV_PI;
}
float O2PI(float rad)
{
return rad >= 0 ? rad : (rad + CV_PI);
}
bool IsIn(float p1, float p2, float distance)
{
float dis = abs(p2 - p1);
if (dis < distance)
return true;
else
return false;
}
float GetDistance(Point p1, Point p2)
{
Point p = p1 - p2;
return sqrt(p.dot(p));
}
float GetDistance(int x1, int y1, int x2, int y2)
{
Point p1, p2;
p1.x = x1;
p1.y = y1;
p2.x = x2;
p2.y = y2;
return GetDistance(p1, p2);
}
int Limiter(int number, int min, int max)
{
if (number > max)
return max;
if (number < min)
return min;
return number;
}
void Limiter(Point &p, Mat &m)
{
p.x = Limiter(p.x, 0, m.cols - 1);
p.y = Limiter(p.y, 0, m.rows - 1);
}
Point Limiter(int x, int y, Mat &m)
{
Point lp;
lp.x = Limiter(x, 0, m.cols - 1);
lp.y = Limiter(y, 0, m.rows - 1);
return lp;
}
Point GetPoint(Point &p1, Point &p2, float length, Mat &limit)
{
float k = GetLineRad(p1, p2);
Point p;
p.x = p1.x + length * cos(k);
p.y = p1.y + length * sin(k);
Limiter(p, limit);
return p;
}
void int2str(const int &int_temp, string &string_temp)
{
stringstream stream;
stream << int_temp;
string_temp = stream.str(); //此处也可以用 stream>>string_temp
}
void Divide(const Point &beichu, float n, Point &ret)
{
ret.x = beichu.x / n;
ret.y = beichu.y / n;
}
float point2Line(Point &p1, Point &lp1, Point &lp2)
{
float a, b, c, dis;
// 化简两点式为一般式
// 两点式公式为(y - y1)/(x - x1) = (y2 - y1)/ (x2 - x1)
// 化简为一般式为(y2 - y1)x + (x1 - x2)y + (x2y1 - x1y2) = 0
// A = y2 - y1
// B = x1 - x2
// C = x2y1 - x1y2
a = lp2.y - lp1.y;
b = lp1.x - lp2.x;
c = lp2.x * lp1.y - lp1.x * lp2.y;
// 距离公式为d = |A*x0 + B*y0 + C|/√(A^2 + B^2)
dis = abs(a * p1.x + b * p1.y + c) / sqrt(a * a + b * b);
return dis;
}
int GetGray(Mat &m, Point &p, int s = 3)
{
Point plu = Limiter(p.x - s, p.y - s, m); //点左上
Point prd = Limiter(p.x + s, p.y + s, m); //点右下
Range row(plu.y, prd.y);
Range col(plu.x, prd.x);
// Range row(Limiter(p.y-3,0,m.rows-1),Limiter(p.y+3,0,m.rows-1));
// Range col(Limiter(p.x-3,0,m.cols-1),Limiter(p.x+3,0,m.cols-1));
Mat poi = m(row, col);
Scalar me = mean(poi);
return me[0];
}
bool findX(Mat &img, Point &p)
{
Mat grayImg, bwImg;
//设置静态变量，加快计算速度
static Mat dilateElement, transMask, transImg;
if (dilateElement.empty())
{
int dilation_size = 1;
dilateElement = getStructuringElement(MORPH_ELLIPSE,
Size(2 * dilation_size + 1, 2 * dilation_size + 1),
Point(dilation_size, dilation_size));
}
Size transSize(60, 60);
if (transMask.empty())
{
transMask = Mat(transSize.height, transSize.width, CV_8UC1);
transMask = Scalar(0);
rectangle(transMask, Point(19, 19), Point(59, 59), Scalar(255), -1);
}
static vector<Point2f> trans2d;
if (trans2d.size() == 0)
{
trans2d.push_back(Point2f(0, 0));
trans2d.push_back(Point2f(59, 0));
trans2d.push_back(Point2f(59, 59));
trans2d.push_back(Point2f(0, 59));
}
vector<Vec4i> lines;
ContoursVector linesEx, possibleLines;
vector<PointValue> resultPoints;
cvtColor(img, grayImg, COLOR_RGB2GRAY);
//
GaussianBlur(grayImg, grayImg, Size(11, 11), 0); //可能非最佳参数
//imshow("gauss blur",grayImg);
Canny(grayImg, bwImg, 70, 150); //可能非最佳参数
//imshow("canny",bwImg);
//腐蚀
dilate(bwImg, bwImg, dilateElement);
//imshow("canny",bwImg);
int64 t1, t2, t3;
t1 = getTickCount();
HoughLinesP(bwImg, lines, 2, CV_PI / 360, 60, 30, 3); //可能非最佳参数...慢
t2 = getTickCount();
int disAsOne = 6;
Point pi[2], pj[2];
for (int i = 0; i < lines.size(); i++)
{
pi[0].x = lines.at(i)[0];
pi[0].y = lines.at(i)[1];
pi[1].x = lines.at(i)[2];
pi[1].y = lines.at(i)[3];
//line(img,pi[0],pi[1],Scalar(255,255,0),2);
float rad1 = O2PI(GetLineRad(lines.at(i)));
for (int j = i + 1; j < lines.size(); j++)
{
if (i == j)
continue;
pj[0].x = lines.at(j)[0];
pj[0].y = lines.at(j)[1];
pj[1].x = lines.at(j)[2];
pj[1].y = lines.at(j)[3];
float rad2 = O2PI(GetLineRad(lines.at(j)));
if (IsIn(abs(rad1 - rad2), CV_PI / 2, CV_PI / 3))
{
float dp[4];
dp[0] = GetDistance(pi[0], pj[0]);
dp[1] = GetDistance(pi[0], pj[1]);
dp[2] = GetDistance(pi[1], pj[0]);
dp[3] = GetDistance(pi[1], pj[1]);
for (int k = 0; k < 4; k++)
{
if (dp[k] < disAsOne)
{
PointsVector line;
line.push_back(pi[1 - k / 2]);
Point pij;
Divide(pi[k / 2] + pj[k % 2], 2, pij);
line.push_back(pij);
line.push_back(pj[1 - k % 2]);
linesEx.push_back(line);
break;
}
}
}
}
}
bool linesToRemove[linesEx.size()];
for (int i = 0; i < linesEx.size(); i++)
{
for (int j = i + 1; j < linesEx.size(); j++)
{
if (GetDistance(linesEx.at(i).at(1), linesEx.at(j).at(1)) < disAsOne)
{
linesToRemove[j] = true;
}
}
}
for (int i = 0; i < linesEx.size(); i++)
{
if (!linesToRemove[i])
possibleLines.push_back(linesEx.at(i));
}
//polylines(img,possibleLines,false,Scalar(0,0,255),2);
vector<Point2f> fpoints;
for (int i = 0; i < possibleLines.size(); i++)
{
fpoints.clear();
Point p[10];
p[0] = possibleLines.at(i).at(0);
p[1] = possibleLines.at(i).at(1);
p[2] = possibleLines.at(i).at(2);
float len = GetDistance(p[0], p[1]);
//cout<<len<<endl;
len = len * 0.4;
p[3] = GetPoint(p[1], p[0], len * 2 / 3, grayImg);
p[4] = GetPoint(p[1], p[2], len * 2 / 3, grayImg);
Divide((p[3] + p[4]), 2, p[5]);
p[6] = GetPoint(p[1], p[5], len * 2 / 3 * sqrt(2), grayImg);
p[7] = GetPoint(p[6], p[3], len, grayImg);
p[9] = GetPoint(p[6], p[4], len, grayImg);
p[8] = GetPoint(p[5], p[1], len * 2 / 3 * sqrt(2), grayImg);
// p[3]=GetPoint(p[1],p[0],40,grayImg);
// p[4]=GetPoint(p[1],p[2],40,grayImg);
// p[5]=(p[3]+p[4])/2;
// p[6]=GetPoint(p[1],p[5],40*sqrt(2),grayImg);
// p[7]=GetPoint(p[6],p[3],60,grayImg);
// p[8]=GetPoint(p[5],p[1],40*sqrt(2),grayImg);
// p[9]=GetPoint(p[6],p[4],60,grayImg);
fpoints.push_back(p[8]);
fpoints.push_back(p[9]);
fpoints.push_back(p[6]);
fpoints.push_back(p[7]);
Mat trans = getPerspectiveTransform(fpoints, trans2d);
warpPerspective(grayImg, transImg, trans, transSize);
Scalar s1 = mean(transImg, transMask);
Scalar s2 = mean(transImg, 255 - transMask);
if (s1[0] - s2[0] > 80) ///////////////////////////
{
PointValue pv;
pv.p = p[1];
pv.value = 0;
pv.p1 = p[5];
pv.length = len * 1.2;
//pv.p2=p[4];
resultPoints.push_back(pv);
}
// vector<Point> pp;
// pp.push_back(p[8]);
// pp.push_back(p[9]);
// pp.push_back(p[6]);
// pp.push_back(p[7]);
// if(s1[0]-s2[0]>80)
// {
// circle(img,p[1],4,Scalar(0,0,255),-1);
// polylines(img,pp,true,Scalar(0,255,255));
// }
// else
// {
// polylines(img,pp,true,Scalar(255,255,255));
// }
}
for (int i = 0; i < resultPoints.size(); i++)
{
Point i1 = resultPoints.at(i).p - resultPoints.at(i).p1;
float radi = atan2(i1.y, i1.x);
for (int j = i + 1; j < resultPoints.size(); j++)
{
//if(i==j)continue;
Point ij = resultPoints.at(j).p - resultPoints.at(i).p;
float radij = atan2(ij.y, ij.x);
if (IsIn(abs(radi - radij), 0, CV_PI / 3))
{
Point j1 = resultPoints.at(j).p - resultPoints.at(j).p1;
float radj = atan2(j1.y, j1.x);
Point ji = resultPoints.at(i).p - resultPoints.at(j).p;
float radji = atan2(ji.y, ji.x);
if (IsIn(abs(radj - radji), 0, CV_PI / 3))
{
resultPoints.at(i).value++;
resultPoints.at(j).value++;
resultPoints.at(i).rad = radi;
resultPoints.at(j).rad = radj;
}
}
}
}
// cout<<resultPoints.size()<<endl;
for (int i = 0; i < resultPoints.size(); i++)
{
for (int j = i + 1; j < resultPoints.size(); j++)
{
if (i == j)
continue;
if (IsIn(
abs(resultPoints.at(i).rad - resultPoints.at(j).rad),
CV_PI,
CV_PI / 18))
{
float dij = point2Line(resultPoints.at(j).p, resultPoints.at(i).p, resultPoints.at(i).p1);
float dji = point2Line(resultPoints.at(i).p, resultPoints.at(j).p, resultPoints.at(j).p1);
float len = GetDistance(resultPoints.at(i).p, resultPoints.at(j).p);
if ((dij < len / 3) && (dji < len / 3))
{
Divide(resultPoints.at(i).p + resultPoints.at(j).p, 2, p);
float gray = GetGray(grayImg, p);
float back = GetGray(grayImg, p, 100);
if (gray < back)
return true;
}
}
}
}
// sort(resultPoints.begin(),resultPoints.end(),SortPointValue);
// if(resultPoints.size()>4)
// {
// resultPoints.erase(resultPoints.begin()+4,resultPoints.end());
// }
t3 = getTickCount();
//cout<<"hough:"<<(t2-t1)/getTickFrequency()<<"\tother:"<<(t3-t2)/getTickFrequency()<<endl;
// for(int i=0;i<resultPoints.size();i++)
// {
// if(resultPoints.at(i).value>0)
// {
// circle(img,resultPoints.at(i).p,4,Scalar(0,0,255),-1);
// }
// }
//string ss;
//int2str(resultPoints.size(),ss);
//putText(img,ss,Point(400,320),FONT_HERSHEY_SIMPLEX ,12,Scalar(0,0,225),2);
// cout<<lines.size()/3<<"\t"<<
// linesEx.size()<<"\t"<<
// possibleLines.size()<<"\t"<<
// resultPoints.size()<<"\t"<<endl;
//cout<<resultPoints.size()<<endl;
// switch (resultPoints.size()) {
// case 4:
// Divide(resultPoints.at(0).p+
// resultPoints.at(1).p+
// resultPoints.at(2).p+
// resultPoints.at(3).p,4,p);
// return true;
// break;
// case 3:
// for(int i=0;i<3;i++)
// {
// if(IsIn(
// abs(resultPoints.at(i).rad-resultPoints.at((i+1)%3).rad),
// CV_PI,
// CV_PI/4)
// )
// {
// Divide((resultPoints.at(i).p+resultPoints.at((i+1)%3).p),2,p);
// return true;
// }
// }
// return false;
// break;
//// case 2:
//// if(IsIn(abs(resultPoints.at(0).rad-resultPoints.at(1).rad),CV_PI,CV_PI/4))
//// {
//// p=(resultPoints.at(0).p+resultPoints.at(1).p)/2;
//// return true;
//// }
//// else
//// {
//// float radx=(resultPoints.at(0).rad+resultPoints.at(1).rad)/2;
//// if(abs(resultPoints.at(0).rad-resultPoints.at(1).rad)>CV_PI)
//// {
//// radx=radx+CV_PI;
//// }
//// Point xx=(resultPoints.at(0).p+resultPoints.at(1).p)/2;
//// Point xdis=(resultPoints.at(0).p-resultPoints.at(1).p)/2;
//// float xd=sqrt(xdis.dot(xdis));
//// p.x=xx.x+xd*cos(radx);
//// p.y=xx.y+xd*sin(radx);
//// return true;
//// }
// default:
// return false;
// break;
// }
return false;
}
#endif

61
src/UAV/Modules/object_circlex_detection/package.xml
Unescape View File

@ -1,61 +0,0 @@
<?xml version="1.0"?>
<package>
<name>prometheus_detection_circlex</name>
<version>0.0.0</version>
<description>The prometheus_detection_circlex package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="jariof@foxmail.com">jario</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ros_caffe</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>opencv2</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>prometheus_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>opencv2</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>prometheus_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

2335
src/UAV/Modules/object_circlex_detection/spire_caffe/.Doxyfile
View File

File diff suppressed because it is too large Load Diff

98
src/UAV/Modules/object_circlex_detection/spire_caffe/.gitignore vendored
Unescape View File

@ -1,98 +0,0 @@
## General
# Compiled Object files
*.slo
*.lo
*.o
*.cuo
# Compiled Dynamic libraries
*.so
*.dylib
# Compiled Static libraries
*.lai
*.la
*.a
# Compiled protocol buffers
*.pb.h
*.pb.cc
*_pb2.py
# Compiled python
*.pyc
# Compiled MATLAB
*.mex*
# IPython notebook checkpoints
.ipynb_checkpoints
# Editor temporaries
*.swp
*~
# Sublime Text settings
*.sublime-workspace
*.sublime-project
# Eclipse Project settings
*.*project
.settings
# QtCreator files
*.user
# PyCharm files
.idea
# Visual Studio Code files
.vscode
# OSX dir files
.DS_Store
## Caffe
# User's build configuration
Makefile.config
# Data and models are either
# 1. reference, and not casually committed
# 2. custom, and live on their own unless they're deliberated contributed
data/*
models/*
*.caffemodel.h5
*.solverstate
*.solverstate.h5
*.binaryproto
*leveldb
*lmdb
# build, distribute, and bins (+ python proto bindings)
build
.build_debug/*
.build_release/*
distribute/*
*.testbin
*.bin
python/caffe/proto/
cmake_build
.cmake_build
# Generated documentation
docs/_site
docs/_includes
docs/gathered
_site
doxygen
docs/dev
# LevelDB files
*.sst
*.ldb
LOCK
LOG*
CURRENT
MANIFEST-*

67
src/UAV/Modules/object_circlex_detection/spire_caffe/.travis.yml
Unescape View File

@ -1,67 +0,0 @@
dist: trusty
sudo: required
language: cpp
compiler: gcc
env:
global:
- NUM_THREADS=4
matrix:
# Use a build matrix to test many builds in parallel
# envvar defaults:
# WITH_CMAKE: false
# WITH_PYTHON3: false
# WITH_IO: true
# WITH_CUDA: false
# WITH_CUDNN: false
- BUILD_NAME="default-make"
# - BUILD_NAME="python3-make" WITH_PYTHON3=true
- BUILD_NAME="no-io-make" WITH_IO=false
- BUILD_NAME="cuda-make" WITH_CUDA=true
- BUILD_NAME="cudnn-make" WITH_CUDA=true WITH_CUDNN=true
- BUILD_NAME="default-cmake" WITH_CMAKE=true
- BUILD_NAME="python3-cmake" WITH_CMAKE=true WITH_PYTHON3=true
- BUILD_NAME="no-io-cmake" WITH_CMAKE=true WITH_IO=false
- BUILD_NAME="cuda-cmake" WITH_CMAKE=true WITH_CUDA=true
- BUILD_NAME="cudnn-cmake" WITH_CMAKE=true WITH_CUDA=true WITH_CUDNN=true
cache:
apt: true
directories:
- ~/protobuf3
before_install:
- source ./scripts/travis/defaults.sh
install:
- sudo -E ./scripts/travis/install-deps.sh
- ./scripts/travis/setup-venv.sh ~/venv
- source ~/venv/bin/activate
- ./scripts/travis/install-python-deps.sh
before_script:
- ./scripts/travis/configure.sh
script:
- ./scripts/travis/build.sh
- ./scripts/travis/test.sh
notifications:
# Emails are sent to the committer's git-configured email address by default,
# but only if they have access to the repository. To enable Travis on your
# public fork of Caffe, just go to travis-ci.org and flip the switch on for
# your Caffe fork. To configure your git email address, use:
# git config --global user.email me@example.com
email:
on_success: always
on_failure: always
# IRC notifications disabled by default.
# Uncomment next 5 lines to send notifications to chat.freenode.net#caffe
# irc:
# channels:
# - "chat.freenode.net#caffe"
# template:
# - "%{repository}/%{branch} (%{commit} - %{author}): %{message}"

96
src/UAV/Modules/object_circlex_detection/spire_caffe/CMakeLists.txt
Unescape View File

@ -1,96 +0,0 @@
cmake_minimum_required(VERSION 2.8.7)
if(POLICY CMP0046)
cmake_policy(SET CMP0046 NEW)
endif()
if(POLICY CMP0054)
cmake_policy(SET CMP0054 NEW)
endif()
# ---[ Caffe project
project(Caffe C CXX)
# ---[ Caffe version
set(CAFFE_TARGET_VERSION "1.0.0-rc4" CACHE STRING "Caffe logical version")
set(CAFFE_TARGET_SOVERSION "1.0.0-rc4" CACHE STRING "Caffe soname version")
add_definitions(-DCAFFE_VERSION=${CAFFE_TARGET_VERSION})
# Jario Edit Start
add_compile_options(-std=c++11)
# Jario Edit End
# ---[ Using cmake scripts and modules
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules)
include(ExternalProject)
include(cmake/Utils.cmake)
include(cmake/Targets.cmake)
include(cmake/Misc.cmake)
include(cmake/Summary.cmake)
include(cmake/ConfigGen.cmake)
# ---[ Options
caffe_option(CPU_ONLY "Build Caffe without CUDA support" ON) # TODO: rename to USE_CUDA
caffe_option(USE_CUDNN "Build Caffe with cuDNN library support" OFF)
caffe_option(USE_NCCL "Build Caffe with NCCL library support" OFF)
caffe_option(BUILD_SHARED_LIBS "Build shared libraries" ON)
caffe_option(BUILD_python "Build Python wrapper" ON)
set(python_version "2" CACHE STRING "Specify which Python version to use")
caffe_option(BUILD_matlab "Build Matlab wrapper" OFF IF UNIX OR APPLE)
caffe_option(BUILD_docs "Build documentation" ON IF UNIX OR APPLE)
caffe_option(BUILD_python_layer "Build the Caffe Python layer" ON)
caffe_option(USE_OPENCV "Build with OpenCV support" ON)
caffe_option(USE_LEVELDB "Build with levelDB" ON)
caffe_option(USE_LMDB "Build with lmdb" ON)
caffe_option(ALLOW_LMDB_NOLOCK "Allow MDB_NOLOCK when reading LMDB files (only if necessary)" OFF)
# ---[ Dependencies
include(cmake/Dependencies.cmake)
# ---[ Flags
if(UNIX OR APPLE)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -Wall")
endif()
caffe_set_caffe_link()
if(USE_libstdcpp)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libstdc++")
message("-- Warning: forcing libstdc++ (controlled by USE_libstdcpp option in cmake)")
endif()
add_definitions(-DGTEST_USE_OWN_TR1_TUPLE)
# ---[ Warnings
caffe_warnings_disable(CMAKE_CXX_FLAGS -Wno-sign-compare -Wno-uninitialized)
# ---[ Config generation
configure_file(cmake/Templates/caffe_config.h.in "${PROJECT_BINARY_DIR}/caffe_config.h")
# ---[ Includes
set(Caffe_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/include)
include_directories(${Caffe_INCLUDE_DIR} ${PROJECT_BINARY_DIR})
include_directories(BEFORE src) # This is needed for gtest.
# ---[ Subdirectories
add_subdirectory(src/gtest)
add_subdirectory(src/caffe)
add_subdirectory(tools)
add_subdirectory(examples)
add_subdirectory(python)
add_subdirectory(matlab)
add_subdirectory(docs)
# ---[ Linter target
add_custom_target(lint COMMAND ${CMAKE_COMMAND} -P ${PROJECT_SOURCE_DIR}/cmake/lint.cmake)
# ---[ pytest target
if(BUILD_python)
add_custom_target(pytest COMMAND python${python_version} -m unittest discover -s caffe/test WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/python )
add_dependencies(pytest pycaffe)
endif()
# ---[ Configuration summary
caffe_print_configuration_summary()
# ---[ Export configs generation
caffe_generate_export_configs()

30
src/UAV/Modules/object_circlex_detection/spire_caffe/CONTRIBUTING.md
Unescape View File

@ -1,30 +0,0 @@
# Contributing
## Issues
Specific Caffe design and development issues, bugs, and feature requests are maintained by GitHub Issues.
_Please do not post usage, installation, or modeling questions, or other requests for help to Issues._
Use the [caffe-users list](https://groups.google.com/forum/#!forum/caffe-users) instead. This helps developers maintain a clear, uncluttered, and efficient view of the state of Caffe.
When reporting a bug, it's most helpful to provide the following information, where applicable:
* What steps reproduce the bug?
* Can you reproduce the bug using the latest [master](https://github.com/BVLC/caffe/tree/master), compiled with the `DEBUG` make option?
* What hardware and operating system/distribution are you running?
* If the bug is a crash, provide the backtrace (usually printed by Caffe; always obtainable with `gdb`).
Try to give your issue a title that is succinct and specific. The devs will rename issues as needed to keep track of them.
## Pull Requests
Caffe welcomes all contributions.
See the [contributing guide](http://caffe.berkeleyvision.org/development.html) for details.
Briefly: read commit by commit, a PR should tell a clean, compelling story of _one_ improvement to Caffe. In particular:
* A PR should do one clear thing that obviously improves Caffe, and nothing more. Making many smaller PRs is better than making one large PR; review effort is superlinear in the amount of code involved.
* Similarly, each commit should be a small, atomic change representing one step in development. PRs should be made of many commits where appropriate.
* Please do rewrite PR history to be clean rather than chronological. Within-PR bugfixes, style cleanups, reversions, etc. should be squashed and should not appear in merged PR history.
* Anything nonobvious from the code should be explained in comments, commit messages, or the PR description, as appropriate.

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save