parent
3edd83c708
commit
8aca8a4d24
@ -1 +0,0 @@
|
||||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
@ -1,248 +0,0 @@
|
||||
#ifndef __GEOMETRY_UTILS_H
|
||||
#define __GEOMETRY_UTILS_H
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
/* clang-format off */
|
||||
namespace geometry_utils {
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t toRad(const Scalar_t& x) {
|
||||
return x / 180.0 * M_PI;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t toDeg(const Scalar_t& x) {
|
||||
return x * 180.0 / M_PI;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> rotx(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> R;
|
||||
R(0, 0) = 1.0;
|
||||
R(0, 1) = 0.0;
|
||||
R(0, 2) = 0.0;
|
||||
R(1, 0) = 0.0;
|
||||
R(1, 1) = std::cos(t);
|
||||
R(1, 2) = -std::sin(t);
|
||||
R(2, 0) = 0.0;
|
||||
R(2, 1) = std::sin(t);
|
||||
R(2, 2) = std::cos(t);
|
||||
|
||||
return R;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> roty(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> R;
|
||||
R(0, 0) = std::cos(t);
|
||||
R(0, 1) = 0.0;
|
||||
R(0, 2) = std::sin(t);
|
||||
R(1, 0) = 0.0;
|
||||
R(1, 1) = 1.0;
|
||||
R(1, 2) = 0;
|
||||
R(2, 0) = -std::sin(t);
|
||||
R(2, 1) = 0.0;
|
||||
R(2, 2) = std::cos(t);
|
||||
|
||||
return R;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> rotz(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> R;
|
||||
R(0, 0) = std::cos(t);
|
||||
R(0, 1) = -std::sin(t);
|
||||
R(0, 2) = 0.0;
|
||||
R(1, 0) = std::sin(t);
|
||||
R(1, 1) = std::cos(t);
|
||||
R(1, 2) = 0.0;
|
||||
R(2, 0) = 0.0;
|
||||
R(2, 1) = 0.0;
|
||||
R(2, 2) = 1.0;
|
||||
|
||||
return R;
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr_to_R(const Eigen::DenseBase<Derived>& ypr) {
|
||||
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
|
||||
typename Derived::Scalar c, s;
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> Rz = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
|
||||
typename Derived::Scalar y = ypr(0);
|
||||
c = cos(y);
|
||||
s = sin(y);
|
||||
Rz(0, 0) = c;
|
||||
Rz(1, 0) = s;
|
||||
Rz(0, 1) = -s;
|
||||
Rz(1, 1) = c;
|
||||
Rz(2, 2) = 1;
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> Ry = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
|
||||
typename Derived::Scalar p = ypr(1);
|
||||
c = cos(p);
|
||||
s = sin(p);
|
||||
Ry(0, 0) = c;
|
||||
Ry(2, 0) = -s;
|
||||
Ry(0, 2) = s;
|
||||
Ry(2, 2) = c;
|
||||
Ry(1, 1) = 1;
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> Rx = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
|
||||
typename Derived::Scalar r = ypr(2);
|
||||
c = cos(r);
|
||||
s = sin(r);
|
||||
Rx(1, 1) = c;
|
||||
Rx(2, 1) = s;
|
||||
Rx(1, 2) = -s;
|
||||
Rx(2, 2) = c;
|
||||
Rx(0, 0) = 1;
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> R = Rz * Ry * Rx;
|
||||
return R;
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> R_to_ypr(const Eigen::DenseBase<Derived>& R) {
|
||||
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> n = R.col(0);
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> o = R.col(1);
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> a = R.col(2);
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> ypr(3);
|
||||
typename Derived::Scalar y = atan2(n(1), n(0));
|
||||
typename Derived::Scalar p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
|
||||
typename Derived::Scalar r =
|
||||
atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
|
||||
ypr(0) = y;
|
||||
ypr(1) = p;
|
||||
ypr(2) = r;
|
||||
|
||||
return ypr;
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Quaternion<typename Derived::Scalar> ypr_to_quaternion(const Eigen::DenseBase<Derived>& ypr) {
|
||||
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
|
||||
const typename Derived::Scalar cy = cos(ypr(0) / 2.0);
|
||||
const typename Derived::Scalar sy = sin(ypr(0) / 2.0);
|
||||
const typename Derived::Scalar cp = cos(ypr(1) / 2.0);
|
||||
const typename Derived::Scalar sp = sin(ypr(1) / 2.0);
|
||||
const typename Derived::Scalar cr = cos(ypr(2) / 2.0);
|
||||
const typename Derived::Scalar sr = sin(ypr(2) / 2.0);
|
||||
|
||||
Eigen::Quaternion<typename Derived::Scalar> q;
|
||||
|
||||
q.w() = cr * cp * cy + sr * sp * sy;
|
||||
q.x() = sr * cp * cy - cr * sp * sy;
|
||||
q.y() = cr * sp * cy + sr * cp * sy;
|
||||
q.z() = cr * cp * sy - sr * sp * cy;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 1> quaternion_to_ypr(const Eigen::Quaternion<Scalar_t>& q_) {
|
||||
Eigen::Quaternion<Scalar_t> q = q_.normalized();
|
||||
|
||||
Eigen::Matrix<Scalar_t, 3, 1> ypr;
|
||||
ypr(2) = atan2(2 * (q.w() * q.x() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q.y() * q.y()));
|
||||
ypr(1) = asin(2 * (q.w() * q.y() - q.z() * q.x()));
|
||||
ypr(0) = atan2(2 * (q.w() * q.z() + q.x() * q.y()), 1 - 2 * (q.y() * q.y() + q.z() * q.z()));
|
||||
|
||||
return ypr;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion<Scalar_t>& q) {
|
||||
return quaternion_to_ypr(q)(0);
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Quaternion<Scalar_t> yaw_to_quaternion(Scalar_t yaw) {
|
||||
return Eigen::Quaternion<Scalar_t>(rotz(yaw));
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t normalize_angle(Scalar_t a) {
|
||||
int cnt = 0;
|
||||
while (true) {
|
||||
cnt++;
|
||||
|
||||
if (a < -M_PI) {
|
||||
a += M_PI * 2.0;
|
||||
} else if (a > M_PI) {
|
||||
a -= M_PI * 2.0;
|
||||
}
|
||||
|
||||
if (-M_PI <= a && a <= M_PI) {
|
||||
break;
|
||||
};
|
||||
|
||||
assert(cnt < 10 && "[geometry_utils/geometry_msgs] INVALID INPUT ANGLE");
|
||||
}
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t angle_add(Scalar_t a, Scalar_t b) {
|
||||
Scalar_t c = a + b;
|
||||
c = normalize_angle(c);
|
||||
assert(-M_PI <= c && c <= M_PI);
|
||||
return c;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t yaw_add(Scalar_t a, Scalar_t b) {
|
||||
return angle_add(a, b);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> get_skew_symmetric(const Eigen::DenseBase<Derived>& v) {
|
||||
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
|
||||
M.setZero();
|
||||
M(0, 1) = -v(2);
|
||||
M(0, 2) = v(1);
|
||||
M(1, 0) = v(2);
|
||||
M(1, 2) = -v(0);
|
||||
M(2, 0) = -v(1);
|
||||
M(2, 1) = v(0);
|
||||
return M;
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> from_skew_symmetric(const Eigen::DenseBase<Derived>& M) {
|
||||
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> v;
|
||||
v(0) = M(2, 1);
|
||||
v(1) = -M(2, 0);
|
||||
v(2) = M(1, 0);
|
||||
|
||||
assert(v.isApprox(Eigen::Matrix<typename Derived::Scalar, 3, 1>(-M(1, 2), M(0, 2), -M(0, 1))));
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
} // end of namespace geometry_utils
|
||||
/* clang-format on */
|
||||
|
||||
#endif
|
||||
@ -1,127 +0,0 @@
|
||||
#ifndef MATH_UTILS_H
|
||||
#define MATH_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;
|
||||
}
|
||||
|
||||
//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
|
||||
@ -1,92 +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
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
add_message_files(
|
||||
DIRECTORY msg
|
||||
FILES
|
||||
UAVState.msg
|
||||
MultiUAVState.msg
|
||||
UAVCommand.msg
|
||||
UAVControlState.msg
|
||||
UAVSetup.msg
|
||||
TextInfo.msg
|
||||
GlobalAruco.msg
|
||||
|
||||
ArucoInfo.msg
|
||||
MultiArucoInfo.msg
|
||||
DetectionInfo.msg
|
||||
MultiDetectionInfo.msg
|
||||
BoundingBox.msg
|
||||
BoundingBoxes.msg
|
||||
|
||||
SwarmCommand.msg
|
||||
FormationAssign.msg
|
||||
OffsetPose.msg
|
||||
GPSData.msg
|
||||
|
||||
#communication
|
||||
DetectionInfoSub.msg
|
||||
GimbalControl.msg
|
||||
GimbalState.msg
|
||||
MultiDetectionInfoSub.msg
|
||||
RheaCommunication.msg
|
||||
RheaGPS.msg
|
||||
RheaState.msg
|
||||
VisionDiff.msg
|
||||
WindowPosition.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
|
||||
)
|
||||
@ -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
|
||||
@ -1,8 +0,0 @@
|
||||
# 目标框相关信息
|
||||
string Class # 类别
|
||||
float64 probability # 置信度
|
||||
int64 xmin # 右上角
|
||||
int64 ymin
|
||||
int64 xmax # 坐下角
|
||||
int64 ymax
|
||||
|
||||
@ -1,3 +0,0 @@
|
||||
Header header
|
||||
Header image_header
|
||||
BoundingBox[] bounding_boxes
|
||||
@ -1,9 +0,0 @@
|
||||
#目标框的位置（主要斜对角两个点）
|
||||
float32 left
|
||||
float32 top
|
||||
float32 bot
|
||||
float32 right
|
||||
|
||||
|
||||
## TRACK TARGET(目标框ID)
|
||||
int32 trackIds
|
||||
@ -1,5 +0,0 @@
|
||||
#队形位置
|
||||
geometry_msgs/Point[] formation_poses
|
||||
|
||||
#位置点是否选取
|
||||
bool[] assigned
|
||||
@ -1,3 +0,0 @@
|
||||
float64 latitude
|
||||
float64 longitude
|
||||
float64 altitude
|
||||
@ -1,33 +0,0 @@
|
||||
Header header
|
||||
uint8 Id
|
||||
#control mode 0:nothong 1:angle 2:speed 3:home postion
|
||||
uint8 rpyMode
|
||||
uint8 manual = 1
|
||||
uint8 home = 2
|
||||
uint8 hold = 3 # 不控制
|
||||
uint8 fellow = 4 #fellow吊舱跟随无人机移动
|
||||
|
||||
uint8 roll
|
||||
uint8 yaw
|
||||
uint8 pitch
|
||||
|
||||
uint8 noCtl = 0
|
||||
uint8 velocityCtl = 1
|
||||
uint8 angleCtl = 2
|
||||
|
||||
float32 rValue # deg 单位
|
||||
float32 yValue # deg
|
||||
float32 pValue # deg
|
||||
|
||||
|
||||
# focus
|
||||
uint8 focusMode # 默认值
|
||||
uint8 focusStop = 1
|
||||
uint8 focusOut = 2
|
||||
uint8 focusIn = 3
|
||||
|
||||
# zoom
|
||||
uint8 zoomMode # 默认值
|
||||
uint8 zoomStop = 1
|
||||
uint8 zoomOut = 2
|
||||
uint8 zoomIn = 3
|
||||
@ -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
|
||||
@ -1,7 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 检测到的aruco码数量
|
||||
int32 num_arucos
|
||||
|
||||
## 每个aruco码的检测结果
|
||||
ArucoInfo[] aruco_infos
|
||||
@ -1,10 +0,0 @@
|
||||
Header header
|
||||
|
||||
## 检测到的目标数量
|
||||
int32 num_objs
|
||||
|
||||
## Detecting or Tracking (0:detect, 1:track)
|
||||
int32 detect_or_track
|
||||
|
||||
## 每个目标的检测结果
|
||||
DetectionInfo[] detection_infos
|
||||
@ -1,7 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
##
|
||||
int32 uav_num
|
||||
|
||||
## 从1开始
|
||||
UAVState[] uav_state_all
|
||||
@ -1,3 +0,0 @@
|
||||
uint8 uav_id
|
||||
float32 x
|
||||
float32 y
|
||||
@ -1,18 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
uint8 Mode #控制模式
|
||||
|
||||
##控制模式类型枚举
|
||||
uint8 Stop=0
|
||||
uint8 Forward=1
|
||||
uint8 Left=2
|
||||
uint8 Right=3
|
||||
uint8 Back=4
|
||||
uint8 CMD=5
|
||||
uint8 Waypoint=6
|
||||
|
||||
float64 linear
|
||||
float64 angular
|
||||
|
||||
## 航点
|
||||
RheaGPS[] waypoint
|
||||
@ -1,3 +0,0 @@
|
||||
float64 latitude
|
||||
float64 longitude
|
||||
float64 altitude
|
||||
@ -1,19 +0,0 @@
|
||||
std_msgs/Header header
|
||||
uint8 rhea_id
|
||||
|
||||
## 速度
|
||||
float64 linear
|
||||
float64 angular
|
||||
|
||||
## 航向角
|
||||
float64 yaw
|
||||
|
||||
## GPS
|
||||
float32 latitude #纬度
|
||||
float32 longitude #经度
|
||||
float32 altitude #高度
|
||||
|
||||
float32[3] position
|
||||
|
||||
## Status
|
||||
float32 battery_voltage
|
||||
@ -1,51 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 消息来源
|
||||
string source
|
||||
|
||||
## 编队套件数量
|
||||
uint8 swarm_num
|
||||
|
||||
uint8 swarm_location_source
|
||||
|
||||
# enum loc 定位来源枚举
|
||||
uint8 mocap = 0
|
||||
uint8 gps = 4
|
||||
uint8 rtk = 5
|
||||
uint8 uwb = 6
|
||||
|
||||
## 命令
|
||||
uint8 Swarm_CMD
|
||||
# enum CMD 控制模式枚举
|
||||
uint8 Ready=0
|
||||
uint8 Init=1
|
||||
uint8 Start=2
|
||||
uint8 Hold=3
|
||||
uint8 Stop=4
|
||||
uint8 Formation=5
|
||||
|
||||
uint8 Follow=11
|
||||
uint8 Search=12
|
||||
uint8 Attack=13
|
||||
|
||||
## 编队控制参考量
|
||||
float32[3] leader_pos
|
||||
float32[2] leader_vel
|
||||
float32 swarm_size
|
||||
uint8 swarm_shape
|
||||
uint8 One_column=0
|
||||
uint8 Triangle=1
|
||||
uint8 Square=2
|
||||
uint8 Circular=3
|
||||
|
||||
## 搜索控制参考量
|
||||
float32 target_area_x_min ## [m]
|
||||
float32 target_area_y_min ## [m]
|
||||
float32 target_area_x_max ## [m]
|
||||
float32 target_area_y_max ## [m]
|
||||
|
||||
## 攻击控制参考量
|
||||
float32[3] attack_target_pos ## [m]
|
||||
|
||||
#队形位置
|
||||
geometry_msgs/Point[] formation_poses
|
||||
@ -1,12 +0,0 @@
|
||||
#INFO:正常运行状况下反馈给地面站的信息,例如程序正常启动,状态切换的提示信息等.
|
||||
uint8 INFO=0
|
||||
#WARN:无人机或软件程序出现意外情况,依然能正常启动或继续执行任务,小概率会出现危险状况,例如无人机RTK无法维持退出到GPS,视觉跟踪目标突然丢失重新搜寻目标等.
|
||||
uint8 WARN=1
|
||||
#ERROR:无人机或软件程序出现重大意外情况,无法正常启动或继续执行任务,极有可能会出现危险状况,需要中断任务以及人为接管控制无人机,例如通信中断,无人机定位发散,ROS节点无法正常启动等.
|
||||
uint8 ERROR=2
|
||||
#FATAL:任务执行过程中,软件崩溃或无人机飞控崩溃导致无人机完全失控,需要迅速人为接管控制无人机降落减少炸机损失.
|
||||
uint8 FATAL=3
|
||||
|
||||
std_msgs/Header header
|
||||
uint8 MessageType
|
||||
string Message
|
||||
@ -1,38 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 控制命令的模式
|
||||
uint8 Agent_CMD
|
||||
# Agent_CMD 枚举
|
||||
uint8 Init_Pos_Hover=1 # home点上方悬停
|
||||
uint8 Current_Pos_Hover=2 # 当前位置上方悬停
|
||||
uint8 Land=3
|
||||
uint8 Move=4
|
||||
uint8 User_Mode1=5
|
||||
|
||||
## 移动命令下的子模式
|
||||
uint8 Move_mode
|
||||
## 移动命令下的子模式枚举
|
||||
uint8 XYZ_POS = 0 ### 惯性系定点控制
|
||||
uint8 XY_VEL_Z_POS = 1 ### 惯性系定高速度控制
|
||||
uint8 XYZ_VEL = 2 ### 惯性系速度控制
|
||||
uint8 XYZ_POS_BODY = 3 ### 机体系位置控制
|
||||
uint8 XYZ_VEL_BODY = 4 ### 机体系速度控制
|
||||
uint8 XY_VEL_Z_POS_BODY = 5 ### 机体系定高速度控制
|
||||
uint8 TRAJECTORY = 6 ### 轨迹追踪控制
|
||||
uint8 XYZ_ATT = 7 ### 姿态控制（来自外部控制器）
|
||||
uint8 LAT_LON_ALT = 8 ### 绝对坐标系下的经纬度
|
||||
|
||||
## 控制参考量
|
||||
float32[3] position_ref ## [m]
|
||||
float32[3] velocity_ref ## [m/s]
|
||||
float32[3] acceleration_ref ## [m/s^2]
|
||||
float32 yaw_ref ## [rad]
|
||||
bool Yaw_Rate_Mode ## True 代表控制偏航角速率
|
||||
float32 yaw_rate_ref ## [rad/s]
|
||||
float32[4] att_ref ## [rad] + [0-1]
|
||||
float64 latitude ## 无人机经度、纬度、高度
|
||||
float64 longitude ## 无人机经度、纬度、高度
|
||||
float64 altitude ## 无人机经度、纬度、高度
|
||||
|
||||
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
|
||||
uint32 Command_ID
|
||||
@ -1,23 +0,0 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 无人机编号
|
||||
uint8 uav_id
|
||||
|
||||
## 无人机控制状态
|
||||
uint8 control_state
|
||||
## 状态枚举
|
||||
uint8 INIT=0
|
||||
uint8 RC_POS_CONTROL=1
|
||||
uint8 COMMAND_CONTROL=2
|
||||
uint8 LAND_CONTROL=3
|
||||
|
||||
## 无人机控制器标志量
|
||||
uint8 pos_controller
|
||||
## 状态枚举
|
||||
uint8 PX4_ORIGIN=0
|
||||
uint8 PID=1
|
||||
uint8 UDE=2
|
||||
uint8 NE=3
|
||||
|
||||
# 无人机安全保护触发标志量
|
||||
bool failsafe
|
||||
@ -1,28 +0,0 @@
|
||||
uint8 Id
|
||||
uint8 detect
|
||||
|
||||
uint16 objectX # 左上角
|
||||
uint16 objectY # 左上角
|
||||
uint16 objectWidth
|
||||
uint16 objectHeight
|
||||
|
||||
uint16 frameWidth
|
||||
uint16 frameHeight
|
||||
|
||||
# Gimbal 跟踪pid
|
||||
float32 kp
|
||||
float32 ki
|
||||
float32 kd
|
||||
|
||||
int8 ctlMode # 0: yaw+pitch, 1: roll+pitch 3:混合(未实现)
|
||||
int8 yawPitch = 0
|
||||
int8 rollPitch = 1
|
||||
int8 mix=3
|
||||
|
||||
# 用于自动缩放
|
||||
float32 currSize #框选近大远小
|
||||
float32 maxSize
|
||||
float32 minSize #框选大小
|
||||
|
||||
float32 trackIgnoreError
|
||||
bool autoZoom
|
||||
@ -1,30 +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="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<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>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -1,50 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(quadrotor_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
nav_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
include
|
||||
)
|
||||
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
AuxCommand.msg
|
||||
Corrections.msg
|
||||
Gains.msg
|
||||
OutputData.msg
|
||||
PositionCommand.msg
|
||||
PPROutputData.msg
|
||||
Serial.msg
|
||||
SO3Command.msg
|
||||
StatusData.msg
|
||||
TRPYCommand.msg
|
||||
Odometry.msg
|
||||
PolynomialTrajectory.msg
|
||||
LQRTrajectory.msg
|
||||
Px4ctrlDebug.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS message_runtime
|
||||
)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
include_directories(${EIGEN3_INCLUDE_DIR})
|
||||
|
||||
|
||||
@ -1,5 +0,0 @@
|
||||
float64 current_yaw
|
||||
float64 kf_correction
|
||||
float64[2] angle_corrections# Trims for roll, pitch
|
||||
bool enable_motors
|
||||
bool use_external_yaw
|
||||
@ -1,2 +0,0 @@
|
||||
float64 kf_correction
|
||||
float64[2] angle_corrections
|
||||
@ -1,4 +0,0 @@
|
||||
float64 Kp
|
||||
float64 Kd
|
||||
float64 Kp_yaw
|
||||
float64 Kd_yaw
|
||||
@ -1,30 +0,0 @@
|
||||
Header header
|
||||
|
||||
# the trajectory id, starts from "1".
|
||||
uint32 trajectory_id
|
||||
|
||||
# the action command for trajectory server.
|
||||
uint32 ACTION_ADD = 1
|
||||
uint32 ACTION_ABORT = 2
|
||||
uint32 ACTION_WARN_START = 3
|
||||
uint32 ACTION_WARN_FINAL = 4
|
||||
uint32 ACTION_WARN_IMPOSSIBLE = 5
|
||||
uint32 action
|
||||
|
||||
# the weight coefficient of the control effort
|
||||
float64 r
|
||||
|
||||
# the yaw command
|
||||
float64 start_yaw
|
||||
float64 final_yaw
|
||||
|
||||
# the initial and final state
|
||||
float64[6] s0
|
||||
float64[3] ut
|
||||
|
||||
float64[6] sf
|
||||
|
||||
# the optimal arrival time
|
||||
float64 t_f
|
||||
|
||||
string debug_info
|
||||
@ -1,8 +0,0 @@
|
||||
uint8 STATUS_ODOM_VALID=0
|
||||
uint8 STATUS_ODOM_INVALID=1
|
||||
uint8 STATUS_ODOM_LOOPCLOSURE=2
|
||||
|
||||
nav_msgs/Odometry curodom
|
||||
nav_msgs/Odometry kfodom
|
||||
uint32 kfid
|
||||
uint8 status
|
||||
@ -1,12 +0,0 @@
|
||||
Header header
|
||||
uint16 loop_rate
|
||||
float64 voltage
|
||||
geometry_msgs/Quaternion orientation
|
||||
geometry_msgs/Vector3 angular_velocity
|
||||
geometry_msgs/Vector3 linear_acceleration
|
||||
float64 pressure_dheight
|
||||
float64 pressure_height
|
||||
geometry_msgs/Vector3 magnetic_field
|
||||
uint8[8] radio_channel
|
||||
#uint8[4] motor_rpm
|
||||
uint8 seq
|
||||
@ -1,16 +0,0 @@
|
||||
Header header
|
||||
uint16 quad_time
|
||||
float64 des_thrust
|
||||
float64 des_roll
|
||||
float64 des_pitch
|
||||
float64 des_yaw
|
||||
float64 est_roll
|
||||
float64 est_pitch
|
||||
float64 est_yaw
|
||||
float64 est_angvel_x
|
||||
float64 est_angvel_y
|
||||
float64 est_angvel_z
|
||||
float64 est_acc_x
|
||||
float64 est_acc_y
|
||||
float64 est_acc_z
|
||||
uint16[4] pwm
|
||||
@ -1,28 +0,0 @@
|
||||
Header header
|
||||
|
||||
# the trajectory id, starts from "1".
|
||||
uint32 trajectory_id
|
||||
|
||||
# the action command for trajectory server.
|
||||
uint32 ACTION_ADD = 1
|
||||
uint32 ACTION_ABORT = 2
|
||||
uint32 ACTION_WARN_START = 3
|
||||
uint32 ACTION_WARN_FINAL = 4
|
||||
uint32 ACTION_WARN_IMPOSSIBLE = 5
|
||||
uint32 action
|
||||
|
||||
# the order of trajectory.
|
||||
uint32 num_order
|
||||
uint32 num_segment
|
||||
|
||||
# the polynomial coecfficients of the trajectory.
|
||||
float64 start_yaw
|
||||
float64 final_yaw
|
||||
float64[] coef_x
|
||||
float64[] coef_y
|
||||
float64[] coef_z
|
||||
float64[] time
|
||||
float64 mag_coeff
|
||||
uint32[] order
|
||||
|
||||
string debug_info
|
||||
@ -1,22 +0,0 @@
|
||||
Header header
|
||||
geometry_msgs/Point position
|
||||
geometry_msgs/Vector3 velocity
|
||||
geometry_msgs/Vector3 acceleration
|
||||
geometry_msgs/Vector3 jerk
|
||||
float64 yaw
|
||||
float64 yaw_dot
|
||||
float64[3] kx
|
||||
float64[3] kv
|
||||
|
||||
uint32 trajectory_id
|
||||
|
||||
uint8 TRAJECTORY_STATUS_EMPTY = 0
|
||||
uint8 TRAJECTORY_STATUS_READY = 1
|
||||
uint8 TRAJECTORY_STATUS_COMPLETED = 3
|
||||
uint8 TRAJECTROY_STATUS_ABORT = 4
|
||||
uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5
|
||||
uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6
|
||||
uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7
|
||||
|
||||
# Its ID number will start from 1, allowing you comparing it with 0.
|
||||
uint8 trajectory_flag
|
||||
@ -1,37 +0,0 @@
|
||||
Header header
|
||||
|
||||
float64 des_p_x
|
||||
float64 des_p_y
|
||||
float64 des_p_z
|
||||
|
||||
float64 des_v_x
|
||||
float64 des_v_y
|
||||
float64 des_v_z
|
||||
|
||||
float64 fb_a_x
|
||||
float64 fb_a_y
|
||||
float64 fb_a_z
|
||||
|
||||
float64 des_a_x
|
||||
float64 des_a_y
|
||||
float64 des_a_z
|
||||
|
||||
float64 des_q_x
|
||||
float64 des_q_y
|
||||
float64 des_q_z
|
||||
float64 des_q_w
|
||||
|
||||
float64 des_thr
|
||||
float64 thr2acc
|
||||
float64 thr_scale_compensate
|
||||
float64 voltage
|
||||
|
||||
float64 err_axisang_x
|
||||
float64 err_axisang_y
|
||||
float64 err_axisang_z
|
||||
float64 err_axisang_ang
|
||||
|
||||
float64 fb_rate_x
|
||||
float64 fb_rate_y
|
||||
float64 fb_rate_z
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
Header header
|
||||
geometry_msgs/Vector3 force
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64[3] kR
|
||||
float64[3] kOm
|
||||
quadrotor_msgs/AuxCommand aux
|
||||
@ -1,13 +0,0 @@
|
||||
# Note: These constants need to be kept in sync with the types
|
||||
# defined in include/quadrotor_msgs/comm_types.h
|
||||
uint8 SO3_CMD = 115 # 's' in base 10
|
||||
uint8 TRPY_CMD = 112 # 'p' in base 10
|
||||
uint8 STATUS_DATA = 99 # 'c' in base 10
|
||||
uint8 OUTPUT_DATA = 100 # 'd' in base 10
|
||||
uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10
|
||||
uint8 PPR_GAINS = 103 # 'g'
|
||||
|
||||
Header header
|
||||
uint8 channel
|
||||
uint8 type # One of the types listed above
|
||||
uint8[] data
|
||||
@ -1,4 +0,0 @@
|
||||
Header header
|
||||
uint16 loop_rate
|
||||
float64 voltage
|
||||
uint8 seq
|
||||
@ -1,6 +0,0 @@
|
||||
Header header
|
||||
float32 thrust
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
quadrotor_msgs/AuxCommand aux
|
||||
@ -1,20 +0,0 @@
|
||||
<package>
|
||||
<name>quadrotor_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>quadrotor_msgs</description>
|
||||
<maintainer email="todo@todo.todo">Kartik Mohta</maintainer>
|
||||
<url>http://ros.org/wiki/quadrotor_msgs</url>
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<export>
|
||||
<cpp cflags="-I${prefix}/include"
|
||||
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@ -1,63 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(prometheus_communication_bridge)
|
||||
|
||||
## 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
|
||||
roscpp
|
||||
std_msgs
|
||||
std_srvs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
message_generation
|
||||
tf2_msgs
|
||||
visualization_msgs
|
||||
mavros
|
||||
mavros_msgs
|
||||
prometheus_msgs
|
||||
)
|
||||
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
std_srvs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
tf2_msgs
|
||||
visualization_msgs
|
||||
mavros_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
##CATKIN_DEPENDS roscpp std_msgs sensor_msgs
|
||||
CATKIN_DEPENDS
|
||||
message_runtime
|
||||
std_msgs
|
||||
std_srvs
|
||||
geometry_msgs
|
||||
mavros_msgs
|
||||
)
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include ${catkin_INCLUDE_DIRS}
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${PROJECT_SOURCE_DIR}/shard/include
|
||||
)
|
||||
|
||||
file(GLOB_RECURSE CURRENT_INCLUDE include/*.hpp include/*.h)
|
||||
file(GLOB_RECURSE CURRENT_SOURCE src/*.cpp)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
add_executable(communication_bridge ${CURRENT_SOURCE} ${CURRENT_INCLUDE})
|
||||
add_dependencies(communication_bridge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(communication_bridge ${catkin_LIBRARIES} ${PROJECT_SOURCE_DIR}/shard/libs/libcommunication.so boost_serialization)
|
||||
|
||||
@ -1,50 +0,0 @@
|
||||
#ifndef AUTONOMOUS_LANDING_TOPIC_HPP
|
||||
#define AUTONOMOUS_LANDING_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
|
||||
#include "gimbal_basic_topic.hpp"
|
||||
|
||||
#include "std_srvs/SetBool.h"
|
||||
#include "mavros_msgs/ParamSet.h"
|
||||
#include "prometheus_msgs/RheaState.h"
|
||||
|
||||
|
||||
class AutonomousLanding
|
||||
{
|
||||
public:
|
||||
AutonomousLanding(ros::NodeHandle &nh,Communication *communication);
|
||||
|
||||
~AutonomousLanding();
|
||||
|
||||
|
||||
void gimbalSearchServer(bool is);
|
||||
|
||||
void gimbalRecordVideoServer(bool is);
|
||||
|
||||
void gimbalTrackModeServer(bool is);
|
||||
|
||||
void gimbalParamSetServer(struct GimbalParamSet param_set);
|
||||
|
||||
void rheaStatePub(struct RheaState rhea_state);
|
||||
|
||||
private:
|
||||
|
||||
ros::Publisher ugv_state_pub_;
|
||||
|
||||
ros::ServiceClient gimbal_search_client_;
|
||||
ros::ServiceClient gimbal_record_video_client_;
|
||||
ros::ServiceClient gimbal_track_mode_client_;
|
||||
ros::ServiceClient param_set_client_;
|
||||
|
||||
struct GimbalState gimbal_state_;
|
||||
struct VisionDiff vision_diff_;
|
||||
|
||||
int robot_id;
|
||||
|
||||
Communication* communication_ = NULL;
|
||||
std::string multicast_udp_ip;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,39 +0,0 @@
|
||||
#ifndef GimbalBasic_HPP
|
||||
#define GimbalBasic_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
|
||||
#include "prometheus_msgs/GimbalState.h"
|
||||
#include "prometheus_msgs/VisionDiff.h"
|
||||
#include "prometheus_msgs/WindowPosition.h"
|
||||
#include "prometheus_msgs/GimbalControl.h"
|
||||
|
||||
class GimbalBasic
|
||||
{
|
||||
public:
|
||||
GimbalBasic(ros::NodeHandle &nh,Communication *communication);
|
||||
~GimbalBasic();
|
||||
|
||||
void stateCb(const prometheus_msgs::GimbalState::ConstPtr &msg);
|
||||
|
||||
void trackCb(const prometheus_msgs::VisionDiff::ConstPtr &msg);
|
||||
|
||||
void gimbalWindowPositionPub(struct WindowPosition window_position);
|
||||
|
||||
void gimbalControlPub(struct GimbalControl gimbal_control);
|
||||
|
||||
protected:
|
||||
ros::Subscriber gimbal_state_sub_;
|
||||
ros::Subscriber vision_diff_sub_;
|
||||
ros::Publisher window_position_pub_;
|
||||
ros::Publisher gimbal_control_pub_;
|
||||
|
||||
struct GimbalState gimbal_state_;
|
||||
struct VisionDiff vision_diff_;
|
||||
|
||||
Communication* communication_ = NULL;
|
||||
std::string multicast_udp_ip;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,26 +0,0 @@
|
||||
#ifndef OBJECT_TRACKING_TOPIC_HPP
|
||||
#define OBJECT_TRACKING_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
#include "prometheus_msgs/MultiDetectionInfoSub.h"
|
||||
#include "prometheus_msgs/DetectionInfoSub.h"
|
||||
|
||||
class ObjectTracking
|
||||
{
|
||||
public:
|
||||
ObjectTracking(ros::NodeHandle &nh,Communication *communication);
|
||||
~ObjectTracking();
|
||||
|
||||
void multiDetectionInfoCb(const prometheus_msgs::MultiDetectionInfoSub::ConstPtr &msg);
|
||||
|
||||
|
||||
private:
|
||||
Communication* communication_ = NULL;
|
||||
|
||||
std::string multicast_udp_ip;
|
||||
|
||||
ros::Subscriber multi_detection_info_sub_;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,90 +0,0 @@
|
||||
#ifndef SWARM_CONTROL_TOPIC_HPP
|
||||
#define SWARM_CONTROL_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
#include "uav_basic_topic.hpp"
|
||||
|
||||
#include "std_msgs/Bool.h"
|
||||
|
||||
#include "prometheus_msgs/MultiUAVState.h"
|
||||
#include "prometheus_msgs/SwarmCommand.h"
|
||||
#include "prometheus_msgs/UAVState.h"
|
||||
#include "prometheus_msgs/OffsetPose.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
//订阅: /prometheus/formation_assign/result
|
||||
//发布: /Prometheus/formation_assign/info
|
||||
|
||||
struct MultiUAVState
|
||||
{
|
||||
int uav_num;
|
||||
std::vector<UAVState> uav_state_all;
|
||||
};
|
||||
|
||||
class SwarmControl//: public UAVBasic
|
||||
{
|
||||
public:
|
||||
//真机构造
|
||||
SwarmControl(ros::NodeHandle &nh, int id, int swarm_num,Communication *communication);
|
||||
|
||||
//仿真构造
|
||||
SwarmControl(ros::NodeHandle &nh, int swarm_num,Communication *communication);
|
||||
|
||||
~SwarmControl();
|
||||
|
||||
void init(ros::NodeHandle &nh, int swarm_num,int id = 1);
|
||||
|
||||
//更新全部飞机数据
|
||||
void updateAllUAVState(struct UAVState uav_state);
|
||||
|
||||
//【发布】集群控制指令
|
||||
void swarmCommandPub(struct SwarmCommand swarm_command);
|
||||
|
||||
//【发布】连接是否失效
|
||||
void communicationStatePub(bool communication);
|
||||
|
||||
void communicationStatePub(bool communication,int id);
|
||||
|
||||
//【发布】所有无人机状态
|
||||
void allUAVStatePub(struct MultiUAVState m_multi_uav_state);
|
||||
|
||||
|
||||
void closeTopic();
|
||||
|
||||
inline struct MultiUAVState getMultiUAVState(){return this->multi_uav_state_;};
|
||||
|
||||
inline prometheus_msgs::UAVState getUAVStateMsg(){return this->uav_state_msg_;};
|
||||
|
||||
|
||||
private:
|
||||
|
||||
struct MultiUAVState multi_uav_state_;
|
||||
|
||||
Communication *communication_ = NULL;
|
||||
|
||||
prometheus_msgs::UAVState uav_state_msg_;
|
||||
|
||||
|
||||
//集群全部uav 状态
|
||||
ros::Publisher all_uav_state_pub_;
|
||||
//控制指令
|
||||
ros::Publisher swarm_command_pub_;
|
||||
//连接是否失效
|
||||
ros::Publisher communication_state_pub_;
|
||||
|
||||
|
||||
//仿真
|
||||
std::vector<ros::Publisher> simulation_communication_state_pub;
|
||||
|
||||
bool is_simulation_;
|
||||
|
||||
std::string udp_ip, multicast_udp_ip;
|
||||
|
||||
std::string user_type_ = "";
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,75 +0,0 @@
|
||||
#ifndef UAV_BASIC_TOPIC_HPP
|
||||
#define UAV_BASIC_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
#include "prometheus_msgs/UAVState.h"
|
||||
#include "prometheus_msgs/TextInfo.h"
|
||||
#include "prometheus_msgs/OffsetPose.h"
|
||||
#include "prometheus_msgs/UAVCommand.h"
|
||||
#include "prometheus_msgs/UAVSetup.h"
|
||||
#include "prometheus_msgs/UAVControlState.h"
|
||||
|
||||
class UAVBasic
|
||||
{
|
||||
public:
|
||||
UAVBasic();
|
||||
|
||||
UAVBasic(ros::NodeHandle &nh,int id,Communication *communication);
|
||||
|
||||
~UAVBasic();
|
||||
|
||||
inline int getRobotId(){return robot_id;};
|
||||
|
||||
void stateCb(const prometheus_msgs::UAVState::ConstPtr &msg);
|
||||
|
||||
//【回调】uav反馈信息
|
||||
void textInfoCb(const prometheus_msgs::TextInfo::ConstPtr &msg);
|
||||
|
||||
//【订阅】偏移量
|
||||
void offsetPoseCb(const prometheus_msgs::OffsetPose::ConstPtr &msg);
|
||||
|
||||
void controlStateCb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
|
||||
|
||||
struct UAVState getUAVState();
|
||||
|
||||
void uavCmdPub(struct UAVCommand uav_cmd);
|
||||
|
||||
void uavSetupPub(struct UAVSetup uav_setup);
|
||||
|
||||
void setTimeStamp(uint time);
|
||||
|
||||
uint getTimeStamp();
|
||||
|
||||
private:
|
||||
ros::Subscriber uav_state_sub_;
|
||||
|
||||
//反馈信息
|
||||
ros::Subscriber text_info_sub_;
|
||||
//控制状态
|
||||
ros::Subscriber uav_control_state_sub_;
|
||||
//偏移量订阅
|
||||
ros::Subscriber offset_pose_sub_;
|
||||
|
||||
ros::Publisher uav_cmd_pub_;
|
||||
|
||||
ros::Publisher uav_setup_pub_;
|
||||
|
||||
int current_mode_;
|
||||
|
||||
int robot_id;
|
||||
|
||||
struct UAVState uav_state_;
|
||||
struct TextInfo text_info_;
|
||||
struct UAVControlState uav_control_state_;
|
||||
|
||||
prometheus_msgs::OffsetPose offset_pose_;
|
||||
|
||||
Communication *communication_ = NULL;
|
||||
|
||||
std::string multicast_udp_ip;
|
||||
|
||||
uint time_stamp_ = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,63 +0,0 @@
|
||||
#ifndef UGV_BASIC_TOPIC_HPP
|
||||
#define UGV_BASIC_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "communication.hpp"
|
||||
#include "prometheus_msgs/RheaCommunication.h"
|
||||
#include "prometheus_msgs/RheaState.h"
|
||||
#include "prometheus_msgs/RheaGPS.h"
|
||||
using namespace std;
|
||||
|
||||
class UGVBasic
|
||||
{
|
||||
public:
|
||||
UGVBasic(ros::NodeHandle &nh,Communication *communication);
|
||||
|
||||
~UGVBasic();
|
||||
|
||||
// void scanCb(const sensor_msgs::LaserScan::ConstPtr &msg);
|
||||
|
||||
// void scanMatchedPoints2Cb(const sensor_msgs::PointCloud2::ConstPtr &msg);
|
||||
|
||||
// void tfCb(const tf2_msgs::TFMessage::ConstPtr &msg);
|
||||
|
||||
// void tfStaticCb(const tf2_msgs::TFMessage::ConstPtr &msg);
|
||||
|
||||
// void constraintListCb(const visualization_msgs::MarkerArray::ConstPtr &msg);
|
||||
|
||||
// void landmarkPosesListCb(const visualization_msgs::MarkerArray::ConstPtr &msg);
|
||||
|
||||
// void trajectoryNodeListCb(const visualization_msgs::MarkerArray::ConstPtr &msg);
|
||||
|
||||
void rheaControlPub(struct RheaControl rhea_control);
|
||||
|
||||
void rheaStateCb(const prometheus_msgs::RheaState::ConstPtr &msg);
|
||||
|
||||
void setTimeStamp(uint time);
|
||||
|
||||
uint getTimeStamp();
|
||||
|
||||
private:
|
||||
//rviz
|
||||
ros::Subscriber scan_matched_points2_sub_;
|
||||
ros::Subscriber scan_sub_;
|
||||
ros::Subscriber tf_static_sub_;
|
||||
ros::Subscriber tf_sub_;
|
||||
ros::Subscriber constraint_list_sub_;
|
||||
ros::Subscriber landmark_poses_list_sub_;
|
||||
ros::Subscriber trajectory_node_list_sub_;
|
||||
//
|
||||
ros::Publisher rhea_control_pub_;
|
||||
ros::Subscriber rhea_state_sub_;
|
||||
|
||||
ros::Subscriber cmd_vel_sub_;
|
||||
|
||||
Communication* communication_ = NULL;
|
||||
|
||||
std::string udp_ip;
|
||||
std::string multicast_udp_ip;
|
||||
|
||||
uint time_stamp_ = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,34 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_communication_bridge</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The ground_station_bridge module</description>
|
||||
|
||||
<maintainer email="Amov@gmail.com">Amov</maintainer>
|
||||
<license>Aomv</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>std_srvs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_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>
|
||||
<build_depend>mavros_msgs</build_depend>
|
||||
<build_export_depend>mavros_msgs</build_export_depend>
|
||||
<exec_depend>mavros_msgs</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,98 +0,0 @@
|
||||
#ifndef COMUNICATION_HPP
|
||||
#define COMUNICATION_HPP
|
||||
|
||||
#include <unistd.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#include "CRC.h"
|
||||
#include "Struct.hpp"
|
||||
|
||||
|
||||
#define BUF_LEN 1024 * 10 // 1024*10 bytes
|
||||
#define SERV_PORT 20168
|
||||
|
||||
typedef unsigned char byte;
|
||||
|
||||
enum Send_Mode
|
||||
{
|
||||
TCP = 1,
|
||||
UDP = 2
|
||||
};
|
||||
|
||||
class Communication
|
||||
{
|
||||
public:
|
||||
Communication();
|
||||
~Communication();
|
||||
|
||||
void init(int id, int udp_port, int tcp_port, int tcp_heart_port);
|
||||
|
||||
//编码
|
||||
template <typename T>
|
||||
int encodeMsg(int8_t send_mode, T msg);
|
||||
|
||||
//解码
|
||||
int decodeMsg(char *buff);
|
||||
|
||||
//根据传入的struct返回对应的MSG_ID
|
||||
template <typename T>
|
||||
uint8_t getMsgId(T msg);
|
||||
|
||||
template <typename T>
|
||||
T add(T num1,T num2);
|
||||
|
||||
// UDP client
|
||||
int connectToUdpMulticast(const char *ip, const int port);
|
||||
|
||||
// TCP client 返回套接字
|
||||
int connectToDrone(const char *ip, const int port);
|
||||
|
||||
void sendMsgByUdp(int msg_len, std::string target_ip);
|
||||
|
||||
void sendMsgByUdp(int msg_len, const char* udp_msg ,std::string target_ip, int port);
|
||||
|
||||
void sendMsgByTcp(int msg_len, std::string target_ip);
|
||||
|
||||
// TCP server
|
||||
int waitConnectionFromGroundStation(const int port);
|
||||
|
||||
// UDP server
|
||||
int waitConnectionFromMulticast(const int port);
|
||||
|
||||
unsigned short checksum(char *buff, int len);
|
||||
|
||||
protected:
|
||||
int ROBOT_ID = 0;
|
||||
|
||||
// tcp/udp
|
||||
struct sockaddr_in tcp_addr, udp_addr;
|
||||
int tcp_send_sock, udp_send_sock, server_fd, udp_fd, recv_sock, udp_socket, rviz_socket, UDP_PORT, TCP_PORT, TCP_HEARTBEAT_PORT;
|
||||
char udp_send_buf[BUF_LEN], udp_recv_buf[BUF_LEN], tcp_send_buf[BUF_LEN], tcp_recv_buf[BUF_LEN];
|
||||
std::string udp_ip, multicast_udp_ip;
|
||||
|
||||
int try_connect_num = 0, disconnect_num = 0;
|
||||
|
||||
public:
|
||||
struct SwarmCommand recv_swarm_command_;
|
||||
struct UAVState recv_uav_state_;
|
||||
struct ConnectState recv_connect_state_;
|
||||
struct GimbalControl recv_gimbal_control_;
|
||||
struct ModeSelection recv_mode_selection_;
|
||||
struct GimbalService recv_gimbal_service_;
|
||||
struct WindowPosition recv_window_position_;
|
||||
struct RheaControl recv_rhea_control_;
|
||||
struct GimbalParamSet recv_param_set_;
|
||||
struct RheaState recv_rhea_state_;
|
||||
struct ImageData recv_image_data_;
|
||||
struct UAVCommand recv_uav_cmd_;
|
||||
struct UAVSetup recv_uav_setup_;
|
||||
struct TextInfo recv_text_info_;
|
||||
struct GimbalState recv_gimbal_state_;
|
||||
struct VisionDiff recv_vision_diff_;
|
||||
};
|
||||
|
||||
#endif
|
||||
Binary file not shown.
@ -1,76 +0,0 @@
|
||||
#include "autonomous_landing_topic.hpp"
|
||||
|
||||
AutonomousLanding::AutonomousLanding(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<int>("ROBOT_ID", robot_id, 0);
|
||||
nh.param<std::string>("multicast_udp_ip", multicast_udp_ip, "224.0.0.88");
|
||||
|
||||
this->communication_ = communication;
|
||||
|
||||
//prefix.c_str() + std::to_string(robot_id) +
|
||||
//【服务】是否开启搜索
|
||||
this->gimbal_search_client_ = nh.serviceClient<std_srvs::SetBool>("/gimbal/search");
|
||||
//【服务】是否开启视频录制
|
||||
this->gimbal_record_video_client_ = nh.serviceClient<std_srvs::SetBool>("/gimbal/record_video");
|
||||
//【服务】是否自动反馈(真实IMU速度)
|
||||
this->gimbal_track_mode_client_ = nh.serviceClient<std_srvs::SetBool>("/gimbal/track_mode");
|
||||
//【服务】自主降落参数配置
|
||||
this->param_set_client_ = nh.serviceClient<mavros_msgs::ParamSet>("/autonomous_landing/ParamSet");
|
||||
//【发布】无人车数据
|
||||
this->ugv_state_pub_ = nh.advertise<prometheus_msgs::RheaState>("/ugv1/prometheus/state", 1000);
|
||||
|
||||
|
||||
};
|
||||
|
||||
AutonomousLanding::~AutonomousLanding()
|
||||
{
|
||||
// delete this->communication_;
|
||||
};
|
||||
|
||||
void AutonomousLanding::gimbalSearchServer(bool is)
|
||||
{
|
||||
std_srvs::SetBool set_bool;
|
||||
set_bool.request.data = is;
|
||||
|
||||
this->gimbal_search_client_.call(set_bool);
|
||||
}
|
||||
|
||||
void AutonomousLanding::gimbalRecordVideoServer(bool is)
|
||||
{
|
||||
std_srvs::SetBool set_bool;
|
||||
set_bool.request.data = is;
|
||||
this->gimbal_record_video_client_.call(set_bool);
|
||||
}
|
||||
|
||||
void AutonomousLanding::gimbalTrackModeServer(bool is)
|
||||
{
|
||||
std_srvs::SetBool set_bool;
|
||||
set_bool.request.data = is;
|
||||
this->gimbal_track_mode_client_.call(set_bool);
|
||||
}
|
||||
|
||||
void AutonomousLanding::gimbalParamSetServer(struct GimbalParamSet param_set)
|
||||
{
|
||||
mavros_msgs::ParamSet srv;
|
||||
srv.request.param_id = param_set.param_id;
|
||||
srv.request.value.real = param_set.real;
|
||||
this->param_set_client_.call(srv);
|
||||
}
|
||||
|
||||
void AutonomousLanding::rheaStatePub(struct RheaState rhea_state)
|
||||
{
|
||||
prometheus_msgs::RheaState rhea_state_;
|
||||
rhea_state_.rhea_id = rhea_state.rhea_id;
|
||||
rhea_state_.angular = rhea_state.angular;
|
||||
rhea_state_.linear = rhea_state.linear;
|
||||
rhea_state_.yaw = rhea_state.yaw;
|
||||
rhea_state_.latitude = rhea_state.latitude;
|
||||
rhea_state_.longitude = rhea_state.longitude;
|
||||
rhea_state_.battery_voltage = rhea_state.battery_voltage;
|
||||
rhea_state_.altitude = rhea_state.altitude;
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
rhea_state_.position[i] = rhea_state.position[i];
|
||||
}
|
||||
this->ugv_state_pub_.publish(rhea_state_);
|
||||
}
|
||||
@ -1,93 +0,0 @@
|
||||
#include "gimbal_basic_topic.hpp"
|
||||
|
||||
GimbalBasic::GimbalBasic(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("multicast_udp_ip", multicast_udp_ip, "224.0.0.88");
|
||||
this->communication_ = communication;
|
||||
//【订阅】吊舱状态数据
|
||||
this->gimbal_state_sub_ = nh.subscribe("/gimbal/state", 10, &GimbalBasic::stateCb, this);
|
||||
//【订阅】跟踪误差
|
||||
this->vision_diff_sub_ = nh.subscribe("/gimbal/track", 10, &GimbalBasic::trackCb, this);
|
||||
//【发布】框选 点击 目标
|
||||
this->window_position_pub_ = nh.advertise<prometheus_msgs::WindowPosition>("/detection/bbox_draw", 1000);
|
||||
//【发布】吊舱控制
|
||||
this->gimbal_control_pub_ = nh.advertise<prometheus_msgs::GimbalControl>("/gimbal/control", 1000);
|
||||
}
|
||||
|
||||
GimbalBasic::~GimbalBasic()
|
||||
{
|
||||
// delete this->communication_;
|
||||
}
|
||||
|
||||
void GimbalBasic::stateCb(const prometheus_msgs::GimbalState::ConstPtr &msg)
|
||||
{
|
||||
this->gimbal_state_.Id = msg->Id;
|
||||
this->gimbal_state_.feedbackMode = msg->feedbackMode;
|
||||
this->gimbal_state_.mode = msg->mode;
|
||||
this->gimbal_state_.isRecording = msg->isRecording;
|
||||
this->gimbal_state_.zoomState = msg->zoomState;
|
||||
this->gimbal_state_.zoomVal = msg->zoomVal;
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
this->gimbal_state_.imuAngle[i] = msg->imuAngle[i];
|
||||
this->gimbal_state_.rotorAngle[i] = msg->rotorAngle[i];
|
||||
this->gimbal_state_.imuAngleVel[i] = msg->imuAngleVel[i];
|
||||
this->gimbal_state_.rotorAngleTarget[i] = msg->rotorAngleTarget[i];
|
||||
}
|
||||
//发送到组播地址
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,this->gimbal_state_),multicast_udp_ip);
|
||||
}
|
||||
|
||||
void GimbalBasic::trackCb(const prometheus_msgs::VisionDiff::ConstPtr &msg)
|
||||
{
|
||||
this->vision_diff_.id = msg->Id;
|
||||
this->vision_diff_.detect = msg->detect;
|
||||
this->vision_diff_.objectX = msg->objectX;
|
||||
this->vision_diff_.objectY = msg->objectY;
|
||||
this->vision_diff_.objectWidth = msg->objectWidth;
|
||||
this->vision_diff_.objectHeight = msg->objectHeight;
|
||||
this->vision_diff_.frameWidth = msg->frameWidth;
|
||||
this->vision_diff_.frameHeight = msg->frameHeight;
|
||||
this->vision_diff_.kp = msg->kp;
|
||||
this->vision_diff_.ki = msg->ki;
|
||||
this->vision_diff_.kd = msg->kd;
|
||||
this->vision_diff_.ctlMode = msg->ctlMode;
|
||||
this->vision_diff_.currSize = msg->currSize;
|
||||
this->vision_diff_.maxSize = msg->maxSize;
|
||||
this->vision_diff_.minSize = msg->minSize;
|
||||
this->vision_diff_.trackIgnoreError = msg->trackIgnoreError;
|
||||
this->vision_diff_.autoZoom = msg->autoZoom;
|
||||
//发送到组播地址
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,this->vision_diff_),multicast_udp_ip);
|
||||
}
|
||||
|
||||
void GimbalBasic::gimbalWindowPositionPub(struct WindowPosition window_position)
|
||||
{
|
||||
prometheus_msgs::WindowPosition window_position_;
|
||||
window_position_.mode = window_position.mode;
|
||||
window_position_.track_id = window_position.track_id;
|
||||
window_position_.origin_x = window_position.origin_x;
|
||||
window_position_.origin_y = window_position.origin_y;
|
||||
window_position_.height = window_position.height;
|
||||
window_position_.width = window_position.width;
|
||||
window_position_.window_position_x = window_position.window_position_x;
|
||||
window_position_.window_position_y = window_position.window_position_y;
|
||||
this->window_position_pub_.publish(window_position_);
|
||||
}
|
||||
|
||||
void GimbalBasic::gimbalControlPub(struct GimbalControl gimbal_control)
|
||||
{
|
||||
prometheus_msgs::GimbalControl gimbal_control_;
|
||||
gimbal_control_.Id = gimbal_control.Id;
|
||||
gimbal_control_.rpyMode = gimbal_control.rpyMode;
|
||||
gimbal_control_.roll = gimbal_control.roll;
|
||||
gimbal_control_.yaw = gimbal_control.yaw;
|
||||
gimbal_control_.pitch = gimbal_control.pitch;
|
||||
gimbal_control_.rValue = gimbal_control.rValue;
|
||||
gimbal_control_.yValue = gimbal_control.yValue;
|
||||
gimbal_control_.pValue = gimbal_control.pValue;
|
||||
gimbal_control_.focusMode = gimbal_control.focusMode;
|
||||
gimbal_control_.zoomMode = gimbal_control.zoomMode;
|
||||
//发布话题
|
||||
this->gimbal_control_pub_.publish(gimbal_control_);
|
||||
}
|
||||
@ -1,15 +0,0 @@
|
||||
#include "communication_bridge.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "ground_station_bridge");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
printf("\033[1;32m---->[ground_station_bridge] start running\n\033[0m");
|
||||
|
||||
CommunicationBridge communication_bridge_(nh);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,30 +0,0 @@
|
||||
#include "object_tracking_topic.hpp"
|
||||
|
||||
ObjectTracking::ObjectTracking(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("multicast_udp_ip", this->multicast_udp_ip, "224.0.0.88");
|
||||
this->communication_ = communication;
|
||||
this->multi_detection_info_sub_ = nh.subscribe("/deepsort_ros/object_detection_result", 10, &ObjectTracking::multiDetectionInfoCb, this);
|
||||
}
|
||||
ObjectTracking::~ObjectTracking()
|
||||
{
|
||||
delete this->communication_;
|
||||
}
|
||||
|
||||
void ObjectTracking::multiDetectionInfoCb(const prometheus_msgs::MultiDetectionInfoSub::ConstPtr &msg)
|
||||
{
|
||||
struct MultiDetectionInfo multi_detection_info;
|
||||
multi_detection_info.mode = msg->mode;
|
||||
multi_detection_info.num_objs = msg->num_objs;
|
||||
for(int i = 0; i < msg->num_objs; i++)
|
||||
{
|
||||
struct DetectionInfo detection_info;
|
||||
detection_info.left = msg->detection_infos[i].left;
|
||||
detection_info.top = msg->detection_infos[i].top;
|
||||
detection_info.bot = msg->detection_infos[i].bot;
|
||||
detection_info.right = msg->detection_infos[i].right;
|
||||
detection_info.trackIds = msg->detection_infos[i].trackIds;
|
||||
multi_detection_info.detection_infos.push_back(detection_info);
|
||||
}
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,multi_detection_info),this->multicast_udp_ip);
|
||||
}
|
||||
@ -1,214 +0,0 @@
|
||||
#include "swarm_control_topic.hpp"
|
||||
|
||||
//真机构造
|
||||
SwarmControl::SwarmControl(ros::NodeHandle &nh, int id, int swarm_num,Communication *communication) // : UAVBasic(nh, Mode::SWARMCONTROL)
|
||||
{
|
||||
std::cout << "集群 真机模式" << std::endl;
|
||||
this->communication_ = communication;
|
||||
is_simulation_ = false;
|
||||
init(nh, swarm_num, id);
|
||||
|
||||
//【发布】集群控制指令
|
||||
this->swarm_command_pub_ = nh.advertise<prometheus_msgs::SwarmCommand>("/prometheus/swarm_command", 1000);
|
||||
//【发布】连接是否失效
|
||||
this->communication_state_pub_ = nh.advertise<std_msgs::Bool>("/uav" + std::to_string(id) + "/prometheus/communication_state", 10);
|
||||
//【发布】所有无人机状态
|
||||
this->all_uav_state_pub_ = nh.advertise<prometheus_msgs::MultiUAVState>("/prometheus/all_uav_state", 1000);
|
||||
}
|
||||
|
||||
//仿真构造
|
||||
SwarmControl::SwarmControl(ros::NodeHandle &nh, int swarm_num,Communication *communication)
|
||||
{
|
||||
std::cout << "集群 仿真模式" << std::endl;
|
||||
this->communication_ = communication;
|
||||
is_simulation_ = true;
|
||||
init(nh, swarm_num);
|
||||
|
||||
for (int i = 1; i <= swarm_num; i++)
|
||||
{
|
||||
//连接是否失效
|
||||
ros::Publisher simulation_communication_state = nh.advertise<std_msgs::Bool>("/uav" + std::to_string(i) + "/prometheus/communication_state", 10);
|
||||
simulation_communication_state_pub.push_back(simulation_communication_state);
|
||||
}
|
||||
|
||||
//【发布】所有无人机状态
|
||||
this->all_uav_state_pub_ = nh.advertise<prometheus_msgs::MultiUAVState>("/prometheus/all_uav_state", 1000);
|
||||
//【发布】集群控制指令
|
||||
this->swarm_command_pub_ = nh.advertise<prometheus_msgs::SwarmCommand>("/prometheus/swarm_command", 1000);
|
||||
}
|
||||
|
||||
SwarmControl::~SwarmControl()
|
||||
{
|
||||
// delete this->communication_;
|
||||
// this->communication_ = nullptr;
|
||||
}
|
||||
|
||||
void SwarmControl::init(ros::NodeHandle &nh, int swarm_num, int id)
|
||||
{
|
||||
nh.param<std::string>("ground_stationt_ip", udp_ip, "127.0.0.1");
|
||||
nh.param<std::string>("multicast_udp_ip", multicast_udp_ip, "224.0.0.88");
|
||||
for (int i = 1; i <= swarm_num; i++)
|
||||
{
|
||||
struct UAVState uav_state;
|
||||
uav_state.uav_id = i;
|
||||
// uav_state.state = UAVState::State::unknown;
|
||||
uav_state.location_source = UAVState::LocationSource::MOCAP;
|
||||
uav_state.gps_status = 0;
|
||||
uav_state.mode = "";
|
||||
uav_state.connected = false;
|
||||
uav_state.armed = false;
|
||||
uav_state.odom_valid = false;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
uav_state.position[j] = 0;
|
||||
uav_state.velocity[j] = 0;
|
||||
uav_state.attitude[j] = 0;
|
||||
uav_state.attitude_rate[j] = 0;
|
||||
}
|
||||
uav_state.latitude = 0;
|
||||
uav_state.longitude = 0;
|
||||
uav_state.altitude = 0;
|
||||
|
||||
uav_state.attitude_q.x = 0;
|
||||
uav_state.attitude_q.y = 0;
|
||||
uav_state.attitude_q.z = 0;
|
||||
uav_state.attitude_q.w = 0;
|
||||
uav_state.battery_state = 0;
|
||||
uav_state.battery_percetage = 0;
|
||||
this->multi_uav_state_.uav_state_all.push_back(uav_state);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//更新全部飞机数据
|
||||
void SwarmControl::updateAllUAVState(struct UAVState uav_state)
|
||||
{
|
||||
//更新数据
|
||||
for(int i = 0; i < this->multi_uav_state_.uav_state_all.size(); i++)
|
||||
{
|
||||
if(this->multi_uav_state_.uav_state_all[i].uav_id == uav_state.uav_id)
|
||||
{
|
||||
this->multi_uav_state_.uav_state_all[i] = uav_state;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//【发布】集群控制指令
|
||||
void SwarmControl::swarmCommandPub(struct SwarmCommand swarm_command)
|
||||
{
|
||||
//进行发布
|
||||
prometheus_msgs::SwarmCommand m_swarm_command;
|
||||
m_swarm_command.source = swarm_command.source;
|
||||
m_swarm_command.Swarm_CMD = swarm_command.Swarm_CMD;
|
||||
m_swarm_command.swarm_location_source = swarm_command.swarm_location_source;
|
||||
m_swarm_command.swarm_num = swarm_command.swarm_num;
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
m_swarm_command.leader_pos[i] = swarm_command.leader_pos[i];
|
||||
m_swarm_command.leader_vel[i] = swarm_command.leader_vel[i];
|
||||
}
|
||||
m_swarm_command.leader_pos[2] = swarm_command.leader_pos[2];
|
||||
|
||||
m_swarm_command.swarm_size = swarm_command.swarm_size;
|
||||
m_swarm_command.swarm_shape = swarm_command.swarm_shape;
|
||||
m_swarm_command.target_area_x_min = swarm_command.target_area_x_min;
|
||||
m_swarm_command.target_area_y_min = swarm_command.target_area_y_min;
|
||||
m_swarm_command.target_area_x_max = swarm_command.target_area_x_max;
|
||||
m_swarm_command.target_area_y_max = swarm_command.target_area_y_max;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_swarm_command.attack_target_pos[i] = swarm_command.attack_target_pos[i];
|
||||
};
|
||||
for(int i = 0; i < swarm_command.formation_poses.size() ; i++)
|
||||
{
|
||||
geometry_msgs::Point point;
|
||||
point.x = swarm_command.formation_poses[i].x;
|
||||
point.y = swarm_command.formation_poses[i].y;
|
||||
point.z = swarm_command.formation_poses[i].z;
|
||||
m_swarm_command.formation_poses.push_back(point);
|
||||
}
|
||||
this->swarm_command_pub_.publish(m_swarm_command);
|
||||
}
|
||||
|
||||
//【发布】连接是否失效
|
||||
void SwarmControl::communicationStatePub(bool communication)
|
||||
{
|
||||
std_msgs::Bool communication_state;
|
||||
communication_state.data = communication;
|
||||
this->communication_state_pub_.publish(communication_state);
|
||||
}
|
||||
|
||||
void SwarmControl::communicationStatePub(bool communication, int id)
|
||||
{
|
||||
std_msgs::Bool communication_state;
|
||||
communication_state.data = communication;
|
||||
//this->communication_state_pub_.publish(communication_state);
|
||||
this->simulation_communication_state_pub[id].publish(communication_state);
|
||||
}
|
||||
|
||||
//【发布】所有无人机状态
|
||||
void SwarmControl::allUAVStatePub(struct MultiUAVState m_multi_uav_state)
|
||||
{
|
||||
prometheus_msgs::MultiUAVState multi_uav_state;
|
||||
multi_uav_state.uav_num = 0;
|
||||
|
||||
for (auto it = m_multi_uav_state.uav_state_all.begin(); it != m_multi_uav_state.uav_state_all.end(); it++)
|
||||
{
|
||||
prometheus_msgs::UAVState uav_state;
|
||||
uav_state.uav_id = (*it).uav_id;
|
||||
// uav_state.state = (*it).state;
|
||||
uav_state.mode = (*it).mode;
|
||||
uav_state.connected = (*it).connected;
|
||||
uav_state.armed = (*it).armed;
|
||||
uav_state.odom_valid = (*it).odom_valid;
|
||||
uav_state.location_source = (*it).location_source;
|
||||
uav_state.gps_status = (*it).gps_status;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
uav_state.position[i] = (*it).position[i];
|
||||
uav_state.velocity[i] = (*it).velocity[i];
|
||||
uav_state.attitude[i] = (*it).attitude[i];
|
||||
uav_state.attitude_rate[i] = (*it).attitude_rate[i];
|
||||
};
|
||||
uav_state.latitude = (*it).latitude;
|
||||
uav_state.longitude = (*it).longitude;
|
||||
uav_state.altitude = (*it).altitude;
|
||||
|
||||
uav_state.attitude_q.x = (*it).attitude_q.x;
|
||||
uav_state.attitude_q.y = (*it).attitude_q.y;
|
||||
uav_state.attitude_q.z = (*it).attitude_q.z;
|
||||
uav_state.attitude_q.w = (*it).attitude_q.w;
|
||||
uav_state.battery_state = (*it).battery_state;
|
||||
uav_state.battery_percetage = (*it).battery_percetage;
|
||||
|
||||
multi_uav_state.uav_num++;
|
||||
multi_uav_state.uav_state_all.push_back(uav_state);
|
||||
}
|
||||
|
||||
//发布话题
|
||||
this->all_uav_state_pub_.publish(multi_uav_state);
|
||||
}
|
||||
|
||||
void SwarmControl::closeTopic()
|
||||
{
|
||||
// if(is_simulation_)
|
||||
// {
|
||||
//auto it = simulation_communication_state_pub.begin();
|
||||
//std::cout << "size():"<<simulation_communication_state_pub.;
|
||||
// for(auto it = ; i < simulation_communication_state_pub.size();i++)
|
||||
// {
|
||||
// std::cout << " i 1: " << i << std::endl;
|
||||
// simulation_communication_state_pub[i].shutdown();
|
||||
// std::cout << " i 2: " << i << std::endl;
|
||||
// }
|
||||
// std::cout << "close 2" << std::endl;
|
||||
// }else
|
||||
// {
|
||||
// this->communication_state_pub_.shutdown();
|
||||
// }
|
||||
|
||||
// this->all_uav_state_pub_.shutdown();
|
||||
// this->swarm_command_pub_.shutdown();
|
||||
}
|
||||
@ -1,147 +0,0 @@
|
||||
#include "uav_basic_topic.hpp"
|
||||
|
||||
UAVBasic::UAVBasic()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
UAVBasic::UAVBasic(ros::NodeHandle &nh,int id,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("multicast_udp_ip", multicast_udp_ip, "224.0.0.88");
|
||||
this->robot_id = id;
|
||||
this->offset_pose_.x = 0;
|
||||
this->offset_pose_.y = 0;
|
||||
this->communication_ = communication;
|
||||
|
||||
//【订阅】uav状态信息
|
||||
this->uav_state_sub_ = nh.subscribe("/uav" + std::to_string(this->robot_id) + "/prometheus/state", 10, &UAVBasic::stateCb, this);
|
||||
//【订阅】uav反馈信息
|
||||
this->text_info_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/text_info", 10, &UAVBasic::textInfoCb, this);
|
||||
//【订阅】uav控制状态信息
|
||||
this->uav_control_state_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/control_state", 10, &UAVBasic::controlStateCb, this);
|
||||
//【订阅】偏移量
|
||||
this->offset_pose_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/offset_pose", 10, &UAVBasic::offsetPoseCb, this);
|
||||
//【发布】底层控制指令(-> uav_control.cpp)
|
||||
this->uav_cmd_pub_ = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + std::to_string(id) + "/prometheus/command", 1);
|
||||
//【发布】mavros接口调用指令(-> uav_control.cpp)
|
||||
this->uav_setup_pub_ = nh.advertise<prometheus_msgs::UAVSetup>("/uav" + std::to_string(this->robot_id) + "/prometheus/setup", 1);
|
||||
}
|
||||
|
||||
UAVBasic::~UAVBasic()
|
||||
{
|
||||
// delete this->communication_;
|
||||
}
|
||||
|
||||
void UAVBasic::stateCb(const prometheus_msgs::UAVState::ConstPtr &msg)
|
||||
{
|
||||
this->uav_state_.uav_id = msg->uav_id;
|
||||
// this->uav_state_.state = msg->state;
|
||||
this->uav_state_.location_source = msg->location_source;
|
||||
this->uav_state_.gps_status = msg->gps_status;
|
||||
this->uav_state_.mode = msg->mode;
|
||||
this->uav_state_.connected = msg->connected;
|
||||
this->uav_state_.armed = msg->armed;
|
||||
this->uav_state_.odom_valid = msg->odom_valid;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
this->uav_state_.velocity[i] = msg->velocity[i];
|
||||
this->uav_state_.attitude[i] = msg->attitude[i];
|
||||
this->uav_state_.attitude_rate[i] = msg->attitude_rate[i];
|
||||
};
|
||||
this->uav_state_.position[0] = msg->position[0] + offset_pose_.x;
|
||||
this->uav_state_.position[1] = msg->position[1] + offset_pose_.y;
|
||||
this->uav_state_.position[2] = msg->position[2];
|
||||
|
||||
this->uav_state_.latitude = msg->latitude;
|
||||
this->uav_state_.longitude = msg->longitude;
|
||||
this->uav_state_.altitude = msg->altitude;
|
||||
|
||||
this->uav_state_.attitude_q.x = msg->attitude_q.x;
|
||||
this->uav_state_.attitude_q.y = msg->attitude_q.y;
|
||||
this->uav_state_.attitude_q.z = msg->attitude_q.z;
|
||||
this->uav_state_.attitude_q.w = msg->attitude_q.w;
|
||||
this->uav_state_.battery_state = msg->battery_state;
|
||||
this->uav_state_.battery_percetage = msg->battery_percetage;
|
||||
|
||||
//发送到组播地址
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->uav_state_), multicast_udp_ip);
|
||||
setTimeStamp(msg->header.stamp.sec);
|
||||
}
|
||||
|
||||
//【回调】uav反馈信息
|
||||
void UAVBasic::textInfoCb(const prometheus_msgs::TextInfo::ConstPtr &msg)
|
||||
{
|
||||
this->text_info_.sec = msg->header.stamp.sec;
|
||||
this->text_info_.MessageType = msg->MessageType;
|
||||
this->text_info_.Message = msg->Message;
|
||||
|
||||
//发送到组播地址
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->text_info_), multicast_udp_ip);
|
||||
}
|
||||
|
||||
//【订阅】偏移量
|
||||
void UAVBasic::offsetPoseCb(const prometheus_msgs::OffsetPose::ConstPtr &msg)
|
||||
{
|
||||
this->offset_pose_.x = msg->x;
|
||||
this->offset_pose_.y = msg->y;
|
||||
}
|
||||
|
||||
void UAVBasic::controlStateCb(const prometheus_msgs::UAVControlState::ConstPtr &msg)
|
||||
{
|
||||
this->uav_control_state_.uav_id = msg->uav_id;
|
||||
this->uav_control_state_.control_state = msg->control_state;
|
||||
this->uav_control_state_.pos_controller = msg->pos_controller;
|
||||
this->uav_control_state_.failsafe = msg->failsafe;
|
||||
|
||||
//发送到组播地址
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->uav_control_state_), multicast_udp_ip);
|
||||
}
|
||||
|
||||
struct UAVState UAVBasic::getUAVState()
|
||||
{
|
||||
return this->uav_state_;
|
||||
}
|
||||
|
||||
void UAVBasic::uavCmdPub(struct UAVCommand uav_cmd)
|
||||
{
|
||||
prometheus_msgs::UAVCommand uav_cmd_;
|
||||
uav_cmd_.Agent_CMD = uav_cmd.Agent_CMD;
|
||||
uav_cmd_.Move_mode = uav_cmd.Move_mode;
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
uav_cmd_.position_ref[i] = uav_cmd.position_ref[i];
|
||||
uav_cmd_.velocity_ref[i] = uav_cmd.velocity_ref[i];
|
||||
uav_cmd_.acceleration_ref[i] = uav_cmd.acceleration_ref[i];
|
||||
uav_cmd_.att_ref[i] = uav_cmd.att_ref[i];
|
||||
}
|
||||
uav_cmd_.att_ref[3] = uav_cmd.att_ref[3];
|
||||
uav_cmd_.yaw_ref = uav_cmd.yaw_ref;
|
||||
uav_cmd_.Yaw_Rate_Mode = uav_cmd.Yaw_Rate_Mode;
|
||||
uav_cmd_.yaw_rate_ref = uav_cmd.yaw_rate_ref;
|
||||
uav_cmd_.Command_ID = uav_cmd.Command_ID;
|
||||
uav_cmd_.latitude = uav_cmd.latitude;
|
||||
uav_cmd_.longitude = uav_cmd.longitude;
|
||||
uav_cmd_.altitude = uav_cmd.altitude;
|
||||
this->uav_cmd_pub_.publish(uav_cmd_);
|
||||
}
|
||||
|
||||
void UAVBasic::uavSetupPub(struct UAVSetup uav_setup)
|
||||
{
|
||||
prometheus_msgs::UAVSetup uav_setup_;
|
||||
uav_setup_.cmd = uav_setup.cmd;
|
||||
uav_setup_.arming = uav_setup.arming;
|
||||
uav_setup_.control_state = uav_setup.control_state;
|
||||
uav_setup_.px4_mode = uav_setup.px4_mode;
|
||||
this->uav_setup_pub_.publish(uav_setup_);
|
||||
}
|
||||
|
||||
void UAVBasic::setTimeStamp(uint time)
|
||||
{
|
||||
this->time_stamp_ = time;
|
||||
}
|
||||
|
||||
uint UAVBasic::getTimeStamp()
|
||||
{
|
||||
return this->time_stamp_;
|
||||
}
|
||||
@ -1,118 +0,0 @@
|
||||
#include "ugv_basic_topic.hpp"
|
||||
|
||||
UGVBasic::UGVBasic(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
this->communication_ = communication;
|
||||
nh.param<std::string>("ground_stationt_ip", udp_ip, "127.0.0.1");
|
||||
nh.param<std::string>("multicast_udp_ip", multicast_udp_ip, "224.0.0.88");
|
||||
|
||||
//【订阅】rviz 点云
|
||||
// this->scan_matched_points2_sub_ = nh.subscribe("/prometheus/pcl_groundtruth", 10, &UGVBasic::scanMatchedPoints2Cb, this);
|
||||
// //【订阅】rviz 激光雷达
|
||||
// this->scan_sub_ = nh.subscribe("/scan", 10, &UGVBasic::scanCb, this);
|
||||
// //【订阅】rviz tf_static
|
||||
// this->tf_static_sub_ = nh.subscribe("/tf_static", 10, &UGVBasic::tfCb, this);
|
||||
// //【订阅】rviz tf
|
||||
// this->tf_sub_ = nh.subscribe("/tf", 10, &UGVBasic::tfCb, this);
|
||||
//【订阅】rviz constraint_list
|
||||
//this->constraint_list_sub_
|
||||
//【订阅】rviz landmark_poses_list
|
||||
//this->landmark_poses_list_sub_
|
||||
//【订阅】rviz trajectory_node_list
|
||||
//this->trajectory_node_list_sub_
|
||||
this->rhea_control_pub_ = nh.advertise<prometheus_msgs::RheaCommunication>("/rhea_command/control", 1000);
|
||||
this->rhea_state_sub_ = nh.subscribe("/rhea_command/state", 10, &UGVBasic::rheaStateCb, this);
|
||||
}
|
||||
|
||||
UGVBasic::~UGVBasic()
|
||||
{
|
||||
// delete this->communication_;
|
||||
}
|
||||
|
||||
// void UGVBasic::scanCb(const sensor_msgs::LaserScan::ConstPtr &msg)
|
||||
// {
|
||||
// sensor_msgs::LaserScan laser_scan = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(laser_scan), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::scanMatchedPoints2Cb(const sensor_msgs::PointCloud2::ConstPtr &msg)
|
||||
// {
|
||||
// sensor_msgs::PointCloud2 scan_matched_points2 = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(scan_matched_points2), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::tfCb(const tf2_msgs::TFMessage::ConstPtr &msg)
|
||||
// {
|
||||
// tf2_msgs::TFMessage tf = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(tf), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::tfStaticCb(const tf2_msgs::TFMessage::ConstPtr &msg)
|
||||
// {
|
||||
// tf2_msgs::TFMessage tf_static = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(tf_static, MsgId::UGVTFSTATIC), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::constraintListCb(const visualization_msgs::MarkerArray::ConstPtr &msg)
|
||||
// {
|
||||
// visualization_msgs::MarkerArray constraint_list = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(constraint_list), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::landmarkPosesListCb(const visualization_msgs::MarkerArray::ConstPtr &msg)
|
||||
// {
|
||||
// visualization_msgs::MarkerArray landmark_poses_list = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(landmark_poses_list, MsgId::UGVMARKERARRAYLANDMARK), udp_ip);
|
||||
// }
|
||||
|
||||
// void UGVBasic::trajectoryNodeListCb(const visualization_msgs::MarkerArray::ConstPtr &msg)
|
||||
// {
|
||||
// visualization_msgs::MarkerArray trajectory_node_list = *msg;
|
||||
// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(trajectory_node_list, MsgId::UGVMARKERARRAYTRAJECTORY), udp_ip);
|
||||
// }
|
||||
|
||||
void UGVBasic::rheaControlPub(struct RheaControl rhea_control)
|
||||
{
|
||||
prometheus_msgs::RheaCommunication rhea_control_;
|
||||
rhea_control_.Mode = rhea_control.Mode;
|
||||
rhea_control_.linear = rhea_control.linear;
|
||||
rhea_control_.angular = rhea_control.angular;
|
||||
for(int i = 0; i < rhea_control.waypoint.size(); i++)
|
||||
{
|
||||
prometheus_msgs::RheaGPS rhea_gps;
|
||||
rhea_gps.altitude = rhea_control.waypoint[i].altitude;
|
||||
rhea_gps.latitude = rhea_control.waypoint[i].latitude;
|
||||
rhea_gps.longitude = rhea_control.waypoint[i].longitude;
|
||||
rhea_control_.waypoint.push_back(rhea_gps);
|
||||
}
|
||||
this->rhea_control_pub_.publish(rhea_control_);
|
||||
}
|
||||
|
||||
void UGVBasic::rheaStateCb(const prometheus_msgs::RheaState::ConstPtr &msg)
|
||||
{
|
||||
struct RheaState rhea_state;
|
||||
rhea_state.rhea_id = msg->rhea_id;
|
||||
rhea_state.linear = msg->linear;
|
||||
rhea_state.angular = msg->angular;
|
||||
rhea_state.yaw = msg->yaw;
|
||||
rhea_state.latitude = msg->latitude;
|
||||
rhea_state.longitude = msg->longitude;
|
||||
rhea_state.battery_voltage = msg->battery_voltage;
|
||||
rhea_state.altitude = msg->altitude;
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
rhea_state.position[i] = msg->position[i];
|
||||
}
|
||||
this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,rhea_state),multicast_udp_ip);
|
||||
setTimeStamp(msg->header.stamp.sec);
|
||||
}
|
||||
|
||||
void UGVBasic::setTimeStamp(uint time)
|
||||
{
|
||||
this->time_stamp_ = time;
|
||||
}
|
||||
|
||||
uint UGVBasic::getTimeStamp()
|
||||
{
|
||||
return this->time_stamp_;
|
||||
}
|
||||
@ -1 +0,0 @@
|
||||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
@ -1,45 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(bspline_opt)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
ADD_COMPILE_OPTIONS(-std=c++11 )
|
||||
ADD_COMPILE_OPTIONS(-std=c++14 )
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
visualization_msgs
|
||||
plan_env
|
||||
cv_bridge
|
||||
traj_utils
|
||||
path_searching
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES bspline_opt
|
||||
CATKIN_DEPENDS plan_env
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library( bspline_opt
|
||||
src/uniform_bspline.cpp
|
||||
src/bspline_optimizer.cpp
|
||||
src/gradient_descent_optimizer.cpp
|
||||
)
|
||||
target_link_libraries( bspline_opt
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
@ -1,213 +0,0 @@
|
||||
#ifndef _BSPLINE_OPTIMIZER_H_
|
||||
#define _BSPLINE_OPTIMIZER_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <path_searching/dyn_a_star.h>
|
||||
#include <bspline_opt/uniform_bspline.h>
|
||||
#include <plan_env/grid_map.h>
|
||||
#include <plan_env/obj_predictor.h>
|
||||
#include <ros/ros.h>
|
||||
#include "bspline_opt/lbfgs.hpp"
|
||||
#include <traj_utils/plan_container.hpp>
|
||||
|
||||
// Gradient and elasitc band optimization
|
||||
|
||||
// Input: a signed distance field and a sequence of points
|
||||
// Output: the optimized sequence of points
|
||||
// The format of points: N x 3 matrix, each row is a point
|
||||
namespace ego_planner
|
||||
{
|
||||
|
||||
class ControlPoints
|
||||
{
|
||||
public:
|
||||
double clearance;
|
||||
int size;
|
||||
Eigen::MatrixXd points;
|
||||
std::vector<std::vector<Eigen::Vector3d>> base_point; // The point at the statrt of the direction vector (collision point)
|
||||
std::vector<std::vector<Eigen::Vector3d>> direction; // Direction vector, must be normalized.
|
||||
std::vector<bool> flag_temp; // A flag that used in many places. Initialize it everytime before using it.
|
||||
// std::vector<bool> occupancy;
|
||||
|
||||
void resize(const int size_set)
|
||||
{
|
||||
size = size_set;
|
||||
|
||||
base_point.clear();
|
||||
direction.clear();
|
||||
flag_temp.clear();
|
||||
// occupancy.clear();
|
||||
|
||||
points.resize(3, size_set);
|
||||
base_point.resize(size);
|
||||
direction.resize(size);
|
||||
flag_temp.resize(size);
|
||||
// occupancy.resize(size);
|
||||
}
|
||||
|
||||
void segment(ControlPoints &buf, const int start, const int end)
|
||||
{
|
||||
|
||||
if (start < 0 || end >= size || points.rows() != 3)
|
||||
{
|
||||
ROS_ERROR("Wrong segment index! start=%d, end=%d", start, end);
|
||||
return;
|
||||
}
|
||||
|
||||
buf.resize(end - start + 1);
|
||||
buf.points = points.block(0, start, 3, end - start + 1);
|
||||
buf.clearance = clearance;
|
||||
buf.size = end - start + 1;
|
||||
for (int i = start; i <= end; i++)
|
||||
{
|
||||
buf.base_point[i - start] = base_point[i];
|
||||
buf.direction[i - start] = direction[i];
|
||||
|
||||
// if ( buf.base_point[i - start].size() > 1 )
|
||||
// {
|
||||
// ROS_ERROR("buf.base_point[i - start].size()=%d, base_point[i].size()=%d", buf.base_point[i - start].size(), base_point[i].size());
|
||||
// }
|
||||
}
|
||||
|
||||
// cout << "RichInfoOneSeg_temp, insede" << endl;
|
||||
// for ( int k=0; k<buf.size; k++ )
|
||||
// if ( buf.base_point[k].size() > 0 )
|
||||
// {
|
||||
// cout << "###" << buf.points.col(k).transpose() << endl;
|
||||
// for (int k2 = 0; k2 < buf.base_point[k].size(); k2++)
|
||||
// {
|
||||
// cout << " " << buf.base_point[k][k2].transpose() << " @ " << buf.direction[k][k2].transpose() << endl;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
};
|
||||
|
||||
class BsplineOptimizer
|
||||
{
|
||||
|
||||
public:
|
||||
BsplineOptimizer() {}
|
||||
~BsplineOptimizer() {}
|
||||
|
||||
/* main API */
|
||||
void setEnvironment(const GridMap::Ptr &map);
|
||||
void setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj);
|
||||
void setParam(ros::NodeHandle &nh);
|
||||
Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd &points, const double &ts,
|
||||
const int &cost_function, int max_num_id, int max_time_id);
|
||||
|
||||
/* helper function */
|
||||
|
||||
// required inputs
|
||||
void setControlPoints(const Eigen::MatrixXd &points);
|
||||
void setBsplineInterval(const double &ts);
|
||||
void setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr);
|
||||
void setDroneId(const int drone_id);
|
||||
|
||||
// optional inputs
|
||||
void setGuidePath(const vector<Eigen::Vector3d> &guide_pt);
|
||||
void setWaypoints(const vector<Eigen::Vector3d> &waypts,
|
||||
const vector<int> &waypt_idx); // N-2 constraints at most
|
||||
void setLocalTargetPt(const Eigen::Vector3d local_target_pt) { local_target_pt_ = local_target_pt; };
|
||||
|
||||
void optimize();
|
||||
|
||||
ControlPoints getControlPoints() { return cps_; };
|
||||
|
||||
AStar::Ptr a_star_;
|
||||
std::vector<Eigen::Vector3d> ref_pts_;
|
||||
|
||||
std::vector<ControlPoints> distinctiveTrajs(vector<std::pair<int, int>> segments);
|
||||
std::vector<std::pair<int, int>> initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init = true);
|
||||
bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts); // must be called after initControlPoints()
|
||||
bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts);
|
||||
bool BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points);
|
||||
|
||||
inline int getOrder(void) { return order_; }
|
||||
inline double getSwarmClearance(void) { return swarm_clearance_; }
|
||||
|
||||
private:
|
||||
GridMap::Ptr grid_map_;
|
||||
fast_planner::ObjPredictor::Ptr moving_objs_;
|
||||
SwarmTrajData *swarm_trajs_{NULL}; // Can not use shared_ptr and no need to free
|
||||
int drone_id_;
|
||||
|
||||
enum FORCE_STOP_OPTIMIZE_TYPE
|
||||
{
|
||||
DONT_STOP,
|
||||
STOP_FOR_REBOUND,
|
||||
STOP_FOR_ERROR
|
||||
} force_stop_type_;
|
||||
|
||||
// main input
|
||||
// Eigen::MatrixXd control_points_; // B-spline control points, N x dim
|
||||
double bspline_interval_; // B-spline knot span
|
||||
Eigen::Vector3d end_pt_; // end of the trajectory
|
||||
// int dim_; // dimension of the B-spline
|
||||
//
|
||||
vector<Eigen::Vector3d> guide_pts_; // geometric guiding path points, N-6
|
||||
vector<Eigen::Vector3d> waypoints_; // waypts constraints
|
||||
vector<int> waypt_idx_; // waypts constraints index
|
||||
//
|
||||
int max_num_id_, max_time_id_; // stopping criteria
|
||||
int cost_function_; // used to determine objective function
|
||||
double start_time_; // global time for moving obstacles
|
||||
|
||||
/* optimization parameters */
|
||||
int order_; // bspline degree
|
||||
double lambda1_; // jerk smoothness weight
|
||||
double lambda2_, new_lambda2_; // distance weight
|
||||
double lambda3_; // feasibility weight
|
||||
double lambda4_; // curve fitting
|
||||
|
||||
int a;
|
||||
//
|
||||
double dist0_, swarm_clearance_; // safe distance
|
||||
double max_vel_, max_acc_; // dynamic limits
|
||||
|
||||
int variable_num_; // optimization variables
|
||||
int iter_num_; // iteration of the solver
|
||||
Eigen::VectorXd best_variable_; //
|
||||
double min_cost_; //
|
||||
|
||||
Eigen::Vector3d local_target_pt_;
|
||||
|
||||
#define INIT_min_ellip_dist_ 123456789.0123456789
|
||||
double min_ellip_dist_;
|
||||
|
||||
ControlPoints cps_;
|
||||
|
||||
/* cost function */
|
||||
/* calculate each part of cost function with control points q as input */
|
||||
|
||||
static double costFunction(const std::vector<double> &x, std::vector<double> &grad, void *func_data);
|
||||
void combineCost(const std::vector<double> &x, vector<double> &grad, double &cost);
|
||||
|
||||
// q contains all control points
|
||||
void calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, bool falg_use_jerk = true);
|
||||
void calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
|
||||
void calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
|
||||
void calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost);
|
||||
void calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
|
||||
void calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
|
||||
void calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
|
||||
bool check_collision_and_rebound(void);
|
||||
|
||||
static int earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls);
|
||||
static double costFunctionRebound(void *func_data, const double *x, double *grad, const int n);
|
||||
static double costFunctionRefine(void *func_data, const double *x, double *grad, const int n);
|
||||
|
||||
bool rebound_optimize(double &final_cost);
|
||||
bool refine_optimize();
|
||||
void combineCostRebound(const double *x, double *grad, double &f_combine, const int n);
|
||||
void combineCostRefine(const double *x, double *grad, double &f_combine, const int n);
|
||||
|
||||
/* for benckmark evaluation only */
|
||||
public:
|
||||
typedef unique_ptr<BsplineOptimizer> Ptr;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} // namespace ego_planner
|
||||
#endif
|
||||
@ -1,52 +0,0 @@
|
||||
#ifndef _GRADIENT_DESCENT_OPT_H_
|
||||
#define _GRADIENT_DESCENT_OPT_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
using namespace std;
|
||||
|
||||
class GradientDescentOptimizer
|
||||
{
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
|
||||
|
||||
typedef double (*objfunDef)(const Eigen::VectorXd &x, Eigen::VectorXd &grad, bool &force_return, void *data);
|
||||
enum RESULT
|
||||
{
|
||||
FIND_MIN,
|
||||
FAILED,
|
||||
RETURN_BY_ORDER,
|
||||
REACH_MAX_ITERATION
|
||||
};
|
||||
|
||||
GradientDescentOptimizer(int v_num, objfunDef objf, void *f_data)
|
||||
{
|
||||
variable_num_ = v_num;
|
||||
objfun_ = objf;
|
||||
f_data_ = f_data;
|
||||
};
|
||||
|
||||
void set_maxiter(int limit) { iter_limit_ = limit; }
|
||||
void set_maxeval(int limit) { invoke_limit_ = limit; }
|
||||
void set_xtol_rel(double xtol_rel) { xtol_rel_ = xtol_rel; }
|
||||
void set_xtol_abs(double xtol_abs) { xtol_abs_ = xtol_abs; }
|
||||
void set_min_grad(double min_grad) { min_grad_ = min_grad; }
|
||||
|
||||
RESULT optimize(Eigen::VectorXd &x_init_optimal, double &opt_f);
|
||||
|
||||
private:
|
||||
int variable_num_{0};
|
||||
int iter_limit_{1e10};
|
||||
int invoke_limit_{1e10};
|
||||
double xtol_rel_;
|
||||
double xtol_abs_;
|
||||
double min_grad_;
|
||||
double time_limit_;
|
||||
void *f_data_;
|
||||
objfunDef objfun_;
|
||||
};
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,80 +0,0 @@
|
||||
#ifndef _UNIFORM_BSPLINE_H_
|
||||
#define _UNIFORM_BSPLINE_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace ego_planner
|
||||
{
|
||||
// An implementation of non-uniform B-spline with different dimensions
|
||||
// It also represents uniform B-spline which is a special case of non-uniform
|
||||
class UniformBspline
|
||||
{
|
||||
private:
|
||||
// control points for B-spline with different dimensions.
|
||||
// Each row represents one single control point
|
||||
// The dimension is determined by column number
|
||||
// e.g. B-spline with N points in 3D space -> Nx3 matrix
|
||||
Eigen::MatrixXd control_points_;
|
||||
|
||||
int p_, n_, m_; // p degree, n+1 control points, m = n+p+1
|
||||
Eigen::VectorXd u_; // knots vector
|
||||
double interval_; // knot span \delta t
|
||||
|
||||
Eigen::MatrixXd getDerivativeControlPoints();
|
||||
|
||||
double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_; // physical limits and time adjustment ratio
|
||||
|
||||
public:
|
||||
UniformBspline() {}
|
||||
UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
|
||||
~UniformBspline();
|
||||
|
||||
Eigen::MatrixXd get_control_points(void) { return control_points_; }
|
||||
|
||||
// initialize as an uniform B-spline
|
||||
void setUniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
|
||||
|
||||
// get / set basic bspline info
|
||||
|
||||
void setKnot(const Eigen::VectorXd &knot);
|
||||
Eigen::VectorXd getKnot();
|
||||
Eigen::MatrixXd getControlPoint();
|
||||
double getInterval();
|
||||
bool getTimeSpan(double &um, double &um_p);
|
||||
|
||||
// compute position / derivative
|
||||
|
||||
Eigen::VectorXd evaluateDeBoor(const double &u); // use u \in [up, u_mp]
|
||||
inline Eigen::VectorXd evaluateDeBoorT(const double &t) { return evaluateDeBoor(t + u_(p_)); } // use t \in [0, duration]
|
||||
UniformBspline getDerivative();
|
||||
|
||||
// 3D B-spline interpolation of points in point_set, with boundary vel&acc
|
||||
// constraints
|
||||
// input : (K+2) points with boundary vel/acc; ts
|
||||
// output: (K+6) control_pts
|
||||
static void parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
|
||||
const vector<Eigen::Vector3d> &start_end_derivative,
|
||||
Eigen::MatrixXd &ctrl_pts);
|
||||
|
||||
/* check feasibility, adjust time */
|
||||
|
||||
void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance);
|
||||
bool checkFeasibility(double &ratio, bool show = false);
|
||||
void lengthenTime(const double &ratio);
|
||||
|
||||
/* for performance evaluation */
|
||||
|
||||
double getTimeSum();
|
||||
double getLength(const double &res = 0.01);
|
||||
double getJerk();
|
||||
void getMeanAndMaxVel(double &mean_v, double &max_v);
|
||||
void getMeanAndMaxAcc(double &mean_a, double &max_a);
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
} // namespace ego_planner
|
||||
#endif
|
||||
@ -1,77 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>bspline_opt</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The bspline_opt package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</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 multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/bspline_opt</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>plan_env</build_depend>
|
||||
<build_depend>path_searching</build_depend>
|
||||
<build_depend>traj_utils</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>plan_env</build_export_depend>
|
||||
<build_export_depend>path_searching</build_export_depend>
|
||||
<build_export_depend>traj_utils</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>plan_env</exec_depend>
|
||||
<exec_depend>path_searching</exec_depend>
|
||||
<exec_depend>traj_utils</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,94 +0,0 @@
|
||||
#include <bspline_opt/gradient_descent_optimizer.h>
|
||||
|
||||
#define RESET "\033[0m"
|
||||
#define RED "\033[31m"
|
||||
|
||||
GradientDescentOptimizer::RESULT
|
||||
GradientDescentOptimizer::optimize(Eigen::VectorXd &x_init_optimal, double &opt_f)
|
||||
{
|
||||
if (min_grad_ < 1e-10)
|
||||
{
|
||||
cout << RED << "min_grad_ is invalid:" << min_grad_ << RESET << endl;
|
||||
return FAILED;
|
||||
}
|
||||
if (iter_limit_ <= 2)
|
||||
{
|
||||
cout << RED << "iter_limit_ is invalid:" << iter_limit_ << RESET << endl;
|
||||
return FAILED;
|
||||
}
|
||||
|
||||
void *f_data = f_data_;
|
||||
int iter = 2;
|
||||
int invoke_count = 2;
|
||||
bool force_return;
|
||||
Eigen::VectorXd x_k(x_init_optimal), x_kp1(x_init_optimal.rows());
|
||||
double cost_k, cost_kp1, cost_min;
|
||||
Eigen::VectorXd grad_k(x_init_optimal.rows()), grad_kp1(x_init_optimal.rows());
|
||||
|
||||
cost_k = objfun_(x_k, grad_k, force_return, f_data);
|
||||
if (force_return)
|
||||
return RETURN_BY_ORDER;
|
||||
cost_min = cost_k;
|
||||
double max_grad = max(abs(grad_k.maxCoeff()), abs(grad_k.minCoeff()));
|
||||
constexpr double MAX_MOVEMENT_AT_FIRST_ITERATION = 0.1; // meter
|
||||
double alpha0 = max_grad < MAX_MOVEMENT_AT_FIRST_ITERATION ? 1.0 : (MAX_MOVEMENT_AT_FIRST_ITERATION / max_grad);
|
||||
x_kp1 = x_k - alpha0 * grad_k;
|
||||
cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
|
||||
if (force_return)
|
||||
return RETURN_BY_ORDER;
|
||||
if (cost_min > cost_kp1)
|
||||
cost_min = cost_kp1;
|
||||
|
||||
/*** start iteration ***/
|
||||
while (++iter <= iter_limit_ && invoke_count <= invoke_limit_)
|
||||
{
|
||||
Eigen::VectorXd s = x_kp1 - x_k;
|
||||
Eigen::VectorXd y = grad_kp1 - grad_k;
|
||||
double alpha = s.dot(y) / y.dot(y);
|
||||
if (isnan(alpha) || isinf(alpha))
|
||||
{
|
||||
cout << RED << "step size invalid! alpha=" << alpha << RESET << endl;
|
||||
return FAILED;
|
||||
}
|
||||
|
||||
if (iter % 2) // to aviod copying operations
|
||||
{
|
||||
do
|
||||
{
|
||||
x_k = x_kp1 - alpha * grad_kp1;
|
||||
cost_k = objfun_(x_k, grad_k, force_return, f_data);
|
||||
invoke_count++;
|
||||
if (force_return)
|
||||
return RETURN_BY_ORDER;
|
||||
alpha *= 0.5;
|
||||
} while (cost_k > cost_kp1 - 1e-4 * alpha * grad_kp1.transpose() * grad_kp1); // Armijo condition
|
||||
|
||||
if (grad_k.norm() < min_grad_)
|
||||
{
|
||||
opt_f = cost_k;
|
||||
return FIND_MIN;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
do
|
||||
{
|
||||
x_kp1 = x_k - alpha * grad_k;
|
||||
cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
|
||||
invoke_count++;
|
||||
if (force_return)
|
||||
return RETURN_BY_ORDER;
|
||||
alpha *= 0.5;
|
||||
} while (cost_kp1 > cost_k - 1e-4 * alpha * grad_k.transpose() * grad_k); // Armijo condition
|
||||
|
||||
if (grad_kp1.norm() < min_grad_)
|
||||
{
|
||||
opt_f = cost_kp1;
|
||||
return FIND_MIN;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
opt_f = iter_limit_ % 2 ? cost_k : cost_kp1;
|
||||
return REACH_MAX_ITERATION;
|
||||
}
|
||||
@ -1,377 +0,0 @@
|
||||
#include "bspline_opt/uniform_bspline.h"
|
||||
#include <ros/ros.h>
|
||||
|
||||
namespace ego_planner
|
||||
{
|
||||
|
||||
UniformBspline::UniformBspline(const Eigen::MatrixXd &points, const int &order,
|
||||
const double &interval)
|
||||
{
|
||||
setUniformBspline(points, order, interval);
|
||||
}
|
||||
|
||||
UniformBspline::~UniformBspline() {}
|
||||
|
||||
void UniformBspline::setUniformBspline(const Eigen::MatrixXd &points, const int &order,
|
||||
const double &interval)
|
||||
{
|
||||
control_points_ = points;
|
||||
p_ = order;
|
||||
interval_ = interval;
|
||||
|
||||
n_ = points.cols() - 1;
|
||||
m_ = n_ + p_ + 1;
|
||||
|
||||
u_ = Eigen::VectorXd::Zero(m_ + 1);
|
||||
for (int i = 0; i <= m_; ++i)
|
||||
{
|
||||
|
||||
if (i <= p_)
|
||||
{
|
||||
u_(i) = double(-p_ + i) * interval_;
|
||||
}
|
||||
else if (i > p_ && i <= m_ - p_)
|
||||
{
|
||||
u_(i) = u_(i - 1) + interval_;
|
||||
}
|
||||
else if (i > m_ - p_)
|
||||
{
|
||||
u_(i) = u_(i - 1) + interval_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UniformBspline::setKnot(const Eigen::VectorXd &knot) { this->u_ = knot; }
|
||||
|
||||
Eigen::VectorXd UniformBspline::getKnot() { return this->u_; }
|
||||
|
||||
bool UniformBspline::getTimeSpan(double &um, double &um_p)
|
||||
{
|
||||
if (p_ > u_.rows() || m_ - p_ > u_.rows())
|
||||
return false;
|
||||
|
||||
um = u_(p_);
|
||||
um_p = u_(m_ - p_);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd UniformBspline::getControlPoint() { return control_points_; }
|
||||
|
||||
Eigen::VectorXd UniformBspline::evaluateDeBoor(const double &u)
|
||||
{
|
||||
|
||||
double ub = min(max(u_(p_), u), u_(m_ - p_));
|
||||
|
||||
// determine which [ui,ui+1] lay in
|
||||
int k = p_;
|
||||
while (true)
|
||||
{
|
||||
if (u_(k + 1) >= ub)
|
||||
break;
|
||||
++k;
|
||||
}
|
||||
|
||||
/* deBoor's alg */
|
||||
vector<Eigen::VectorXd> d;
|
||||
for (int i = 0; i <= p_; ++i)
|
||||
{
|
||||
d.push_back(control_points_.col(k - p_ + i));
|
||||
// cout << d[i].transpose() << endl;
|
||||
}
|
||||
|
||||
for (int r = 1; r <= p_; ++r)
|
||||
{
|
||||
for (int i = p_; i >= r; --i)
|
||||
{
|
||||
double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
|
||||
// cout << "alpha: " << alpha << endl;
|
||||
d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
|
||||
}
|
||||
}
|
||||
|
||||
return d[p_];
|
||||
}
|
||||
|
||||
// Eigen::VectorXd UniformBspline::evaluateDeBoorT(const double& t) {
|
||||
// return evaluateDeBoor(t + u_(p_));
|
||||
// }
|
||||
|
||||
Eigen::MatrixXd UniformBspline::getDerivativeControlPoints()
|
||||
{
|
||||
// The derivative of a b-spline is also a b-spline, its order become p_-1
|
||||
// control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
|
||||
Eigen::MatrixXd ctp(control_points_.rows(), control_points_.cols() - 1);
|
||||
for (int i = 0; i < ctp.cols(); ++i)
|
||||
{
|
||||
ctp.col(i) =
|
||||
p_ * (control_points_.col(i + 1) - control_points_.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
|
||||
}
|
||||
return ctp;
|
||||
}
|
||||
|
||||
UniformBspline UniformBspline::getDerivative()
|
||||
{
|
||||
Eigen::MatrixXd ctp = getDerivativeControlPoints();
|
||||
UniformBspline derivative(ctp, p_ - 1, interval_);
|
||||
|
||||
/* cut the first and last knot */
|
||||
Eigen::VectorXd knot(u_.rows() - 2);
|
||||
knot = u_.segment(1, u_.rows() - 2);
|
||||
derivative.setKnot(knot);
|
||||
|
||||
return derivative;
|
||||
}
|
||||
|
||||
double UniformBspline::getInterval() { return interval_; }
|
||||
|
||||
void UniformBspline::setPhysicalLimits(const double &vel, const double &acc, const double &tolerance)
|
||||
{
|
||||
limit_vel_ = vel;
|
||||
limit_acc_ = acc;
|
||||
limit_ratio_ = 1.1;
|
||||
feasibility_tolerance_ = tolerance;
|
||||
}
|
||||
|
||||
bool UniformBspline::checkFeasibility(double &ratio, bool show)
|
||||
{
|
||||
bool fea = true;
|
||||
|
||||
Eigen::MatrixXd P = control_points_;
|
||||
int dimension = control_points_.rows();
|
||||
|
||||
/* check vel feasibility and insert points */
|
||||
double max_vel = -1.0;
|
||||
double enlarged_vel_lim = limit_vel_ * (1.0 + feasibility_tolerance_) + 1e-4;
|
||||
for (int i = 0; i < P.cols() - 1; ++i)
|
||||
{
|
||||
Eigen::VectorXd vel = p_ * (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
|
||||
|
||||
if (fabs(vel(0)) > enlarged_vel_lim || fabs(vel(1)) > enlarged_vel_lim ||
|
||||
fabs(vel(2)) > enlarged_vel_lim)
|
||||
{
|
||||
|
||||
if (show)
|
||||
cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
|
||||
fea = false;
|
||||
|
||||
for (int j = 0; j < dimension; ++j)
|
||||
{
|
||||
max_vel = max(max_vel, fabs(vel(j)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* acc feasibility */
|
||||
double max_acc = -1.0;
|
||||
double enlarged_acc_lim = limit_acc_ * (1.0 + feasibility_tolerance_) + 1e-4;
|
||||
for (int i = 0; i < P.cols() - 2; ++i)
|
||||
{
|
||||
|
||||
Eigen::VectorXd acc = p_ * (p_ - 1) *
|
||||
((P.col(i + 2) - P.col(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
|
||||
(P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
|
||||
(u_(i + p_ + 1) - u_(i + 2));
|
||||
|
||||
if (fabs(acc(0)) > enlarged_acc_lim || fabs(acc(1)) > enlarged_acc_lim ||
|
||||
fabs(acc(2)) > enlarged_acc_lim)
|
||||
{
|
||||
|
||||
if (show)
|
||||
cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
|
||||
fea = false;
|
||||
|
||||
for (int j = 0; j < dimension; ++j)
|
||||
{
|
||||
max_acc = max(max_acc, fabs(acc(j)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
|
||||
|
||||
return fea;
|
||||
}
|
||||
|
||||
void UniformBspline::lengthenTime(const double &ratio)
|
||||
{
|
||||
int num1 = 5;
|
||||
int num2 = getKnot().rows() - 1 - 5;
|
||||
|
||||
double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
|
||||
double t_inc = delta_t / double(num2 - num1);
|
||||
for (int i = num1 + 1; i <= num2; ++i)
|
||||
u_(i) += double(i - num1) * t_inc;
|
||||
for (int i = num2 + 1; i < u_.rows(); ++i)
|
||||
u_(i) += delta_t;
|
||||
}
|
||||
|
||||
// void UniformBspline::recomputeInit() {}
|
||||
|
||||
void UniformBspline::parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
|
||||
const vector<Eigen::Vector3d> &start_end_derivative,
|
||||
Eigen::MatrixXd &ctrl_pts)
|
||||
{
|
||||
if (ts <= 0)
|
||||
{
|
||||
cout << "[B-spline]:time step error." << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (point_set.size() <= 3)
|
||||
{
|
||||
cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (start_end_derivative.size() != 4)
|
||||
{
|
||||
cout << "[B-spline]:derivatives error." << endl;
|
||||
}
|
||||
|
||||
int K = point_set.size();
|
||||
|
||||
// write A
|
||||
Eigen::Vector3d prow(3), vrow(3), arow(3);
|
||||
prow << 1, 4, 1;
|
||||
vrow << -1, 0, 1;
|
||||
arow << 1, -2, 1;
|
||||
|
||||
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
|
||||
|
||||
for (int i = 0; i < K; ++i)
|
||||
A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
|
||||
|
||||
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
|
||||
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
|
||||
|
||||
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
|
||||
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
|
||||
|
||||
//cout << "A" << endl << A << endl << endl;
|
||||
|
||||
// write b
|
||||
Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
|
||||
for (int i = 0; i < K; ++i)
|
||||
{
|
||||
bx(i) = point_set[i](0);
|
||||
by(i) = point_set[i](1);
|
||||
bz(i) = point_set[i](2);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
bx(K + i) = start_end_derivative[i](0);
|
||||
by(K + i) = start_end_derivative[i](1);
|
||||
bz(K + i) = start_end_derivative[i](2);
|
||||
}
|
||||
|
||||
// solve Ax = b
|
||||
Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
|
||||
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
|
||||
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
|
||||
|
||||
// convert to control pts
|
||||
ctrl_pts.resize(3, K + 2);
|
||||
ctrl_pts.row(0) = px.transpose();
|
||||
ctrl_pts.row(1) = py.transpose();
|
||||
ctrl_pts.row(2) = pz.transpose();
|
||||
|
||||
// cout << "[B-spline]: parameterization ok." << endl;
|
||||
}
|
||||
|
||||
double UniformBspline::getTimeSum()
|
||||
{
|
||||
double tm, tmp;
|
||||
if (getTimeSpan(tm, tmp))
|
||||
return tmp - tm;
|
||||
else
|
||||
return -1.0;
|
||||
}
|
||||
|
||||
double UniformBspline::getLength(const double &res)
|
||||
{
|
||||
double length = 0.0;
|
||||
double dur = getTimeSum();
|
||||
Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n;
|
||||
for (double t = res; t <= dur + 1e-4; t += res)
|
||||
{
|
||||
p_n = evaluateDeBoorT(t);
|
||||
length += (p_n - p_l).norm();
|
||||
p_l = p_n;
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
double UniformBspline::getJerk()
|
||||
{
|
||||
UniformBspline jerk_traj = getDerivative().getDerivative().getDerivative();
|
||||
|
||||
Eigen::VectorXd times = jerk_traj.getKnot();
|
||||
Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint();
|
||||
int dimension = ctrl_pts.rows();
|
||||
|
||||
double jerk = 0.0;
|
||||
for (int i = 0; i < ctrl_pts.cols(); ++i)
|
||||
{
|
||||
for (int j = 0; j < dimension; ++j)
|
||||
{
|
||||
jerk += (times(i + 1) - times(i)) * ctrl_pts(j, i) * ctrl_pts(j, i);
|
||||
}
|
||||
}
|
||||
|
||||
return jerk;
|
||||
}
|
||||
|
||||
void UniformBspline::getMeanAndMaxVel(double &mean_v, double &max_v)
|
||||
{
|
||||
UniformBspline vel = getDerivative();
|
||||
double tm, tmp;
|
||||
vel.getTimeSpan(tm, tmp);
|
||||
|
||||
double max_vel = -1.0, mean_vel = 0.0;
|
||||
int num = 0;
|
||||
for (double t = tm; t <= tmp; t += 0.01)
|
||||
{
|
||||
Eigen::VectorXd vxd = vel.evaluateDeBoor(t);
|
||||
double vn = vxd.norm();
|
||||
|
||||
mean_vel += vn;
|
||||
++num;
|
||||
if (vn > max_vel)
|
||||
{
|
||||
max_vel = vn;
|
||||
}
|
||||
}
|
||||
|
||||
mean_vel = mean_vel / double(num);
|
||||
mean_v = mean_vel;
|
||||
max_v = max_vel;
|
||||
}
|
||||
|
||||
void UniformBspline::getMeanAndMaxAcc(double &mean_a, double &max_a)
|
||||
{
|
||||
UniformBspline acc = getDerivative().getDerivative();
|
||||
double tm, tmp;
|
||||
acc.getTimeSpan(tm, tmp);
|
||||
|
||||
double max_acc = -1.0, mean_acc = 0.0;
|
||||
int num = 0;
|
||||
for (double t = tm; t <= tmp; t += 0.01)
|
||||
{
|
||||
Eigen::VectorXd axd = acc.evaluateDeBoor(t);
|
||||
double an = axd.norm();
|
||||
|
||||
mean_acc += an;
|
||||
++num;
|
||||
if (an > max_acc)
|
||||
{
|
||||
max_acc = an;
|
||||
}
|
||||
}
|
||||
|
||||
mean_acc = mean_acc / double(num);
|
||||
mean_a = mean_acc;
|
||||
max_a = max_acc;
|
||||
}
|
||||
} // namespace ego_planner
|
||||
@ -1,130 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(drone_detect)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
ADD_COMPILE_OPTIONS(-std=c++11 )
|
||||
ADD_COMPILE_OPTIONS(-std=c++14 )
|
||||
## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
|
||||
## enforcing cleaner code.
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
roscpp
|
||||
sensor_msgs
|
||||
roslint
|
||||
cv_bridge
|
||||
message_filters
|
||||
)
|
||||
|
||||
## Find system libraries
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
###################################
|
||||
## 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 your 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
|
||||
## This is only necessary because Eigen3 sets a non-standard EIGEN3_INCLUDE_DIR variable
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
LIBRARIES
|
||||
CATKIN_DEPENDS
|
||||
roscpp
|
||||
sensor_msgs
|
||||
DEPENDS OpenCV Eigen Boost
|
||||
## find_package(Eigen3) provides a non standard EIGEN3_INCLUDE_DIR instead of Eigen3_INCLUDE_DIRS.
|
||||
## Therefore, the DEPEND does not work as expected and we need to add the directory to the INCLUDE_DIRS
|
||||
# Eigen3
|
||||
|
||||
## Boost is not part of the DEPENDS since it is only used in source files,
|
||||
## Dependees do not depend on Boost when they depend on this package.
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
# Set manually because Eigen sets a non standard INCLUDE DIR
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
# Set because Boost is an internal dependency, not transitive.
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
|
||||
)
|
||||
|
||||
## Declare cpp executables
|
||||
add_executable(${PROJECT_NAME}
|
||||
src/${PROJECT_NAME}_node.cpp
|
||||
src/drone_detector.cpp
|
||||
)
|
||||
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_11)
|
||||
|
||||
## Add dependencies to exported targets, like ROS msgs or srvs
|
||||
add_dependencies(${PROJECT_NAME}
|
||||
${catkin_EXPORTED_TARGETS}
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# Mark executables and/or libraries for installation
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
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"
|
||||
)
|
||||
|
||||
# Mark other files for installation
|
||||
install(
|
||||
DIRECTORY doc
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
# if(${CATKIN_ENABLE_TESTING})
|
||||
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
|
||||
# ## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test
|
||||
# test/test_drone_detector.cpp)
|
||||
|
||||
# target_link_libraries(${PROJECT_NAME}-test)
|
||||
# endif()
|
||||
|
||||
##########################
|
||||
## Static code analysis ##
|
||||
##########################
|
||||
|
||||
roslint_cpp()
|
||||
@ -1,29 +0,0 @@
|
||||
Software License Agreement (BSD License)
|
||||
|
||||
Copyright (c) 2017, Peter Fankhauser
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of Autonomous Systems Lab nor ETH Zurich nor the names
|
||||
of its contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@ -1,97 +0,0 @@
|
||||
# Drone Detect
|
||||
|
||||
## Overview
|
||||
|
||||
This is a package for detecting other drones in depth image and then calculating their true pose error in the world frame of the observer drone.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## Usage
|
||||
|
||||
You can launch the node alongside the main after you assigning the right topic name
|
||||
|
||||
```
|
||||
roslaunch drone_detect drone_detect.launch
|
||||
```
|
||||
|
||||
|
||||
|
||||
or add the following code in `run_in_sim.launch`
|
||||
|
||||
```xml
|
||||
<node pkg="drone_detect" type="drone_detect" name="drone_$(arg drone_id)_drone_detect" output="screen">
|
||||
<rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
|
||||
<rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
|
||||
<param name="my_id" value="$(arg drone_id)" />
|
||||
|
||||
<remap from="~odometry" to="/drone_$(arg drone_id)_$(arg odom_topic)"/>
|
||||
<remap from="~depth" to="/drone_$(arg drone_id)_pcl_render_node/depth"/>
|
||||
<remap from="~colordepth" to="/drone_$(arg drone_id)_pcl_render_node/colordepth"/>
|
||||
<remap from="~camera_pose" to="/drone_$(arg drone_id)_pcl_render_node/camera_pose"/>
|
||||
|
||||
<remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
|
||||
<remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
|
||||
<remap from="~drone2" to="/drone_2_$(arg odom_topic)"/>
|
||||
</node>
|
||||
```
|
||||
|
||||
|
||||
|
||||
## Config files
|
||||
|
||||
* **camera.yaml** : The camera intrinsics are stored in this file
|
||||
* **default.yaml**: Some parameters related to drone detection.
|
||||
|
||||
```yaml
|
||||
debug_flag: true
|
||||
# the proportion of the pixel threshold to the total pixels projected from the drone to the depth map
|
||||
estimate/pixel_ratio: 0.1
|
||||
# the radius of the pose errror sphere
|
||||
estimate/max_pose_error: 0.4
|
||||
# the width and height of the model of drone
|
||||
estimate/drone_width: 0.5
|
||||
estimate/drone_height: 0.1
|
||||
```
|
||||
|
||||
|
||||
|
||||
## Nodes
|
||||
|
||||
|
||||
#### Subscribed Topics
|
||||
|
||||
* **`/depth`** ([sensor_msgs::Image])
|
||||
|
||||
The depth image from pcl_render_node.
|
||||
|
||||
* **`/colordepth`**([sensor_msgs::Image])
|
||||
|
||||
The color image from pcl_render_node
|
||||
|
||||
* **`/camera_pose`** ([geometry_msgs::PoseStamped])
|
||||
|
||||
The camere pose from pcl_render_node
|
||||
|
||||
The above three topics are synchronized when in use, the callback function is **`rcvDepthColorCamPoseCallback`**
|
||||
|
||||
- **`/dronex_odom_sub_`** ([nav_msgs::Odometry])
|
||||
|
||||
The odometry of other drones
|
||||
|
||||
#### Published Topics
|
||||
|
||||
* **`/new_depth`** ([sensor_msgs::Image])
|
||||
|
||||
The new depth image after erasing the moving drones
|
||||
|
||||
* **`/new_colordepth`**([sensor_msgs::Image])
|
||||
|
||||
The color image with some debug marks
|
||||
|
||||
* **`/camera_pose_error`** ([geometry_msgs::PoseStamped])
|
||||
|
||||
The pose error of detected drones in world frame of the observer drone.
|
||||
@ -1,7 +0,0 @@
|
||||
cam_width: 640
|
||||
cam_height: 480
|
||||
cam_fx: 386.02557373046875
|
||||
cam_fy: 386.02557373046875
|
||||
cam_cx: 321.8554382324219
|
||||
cam_cy: 241.2396240234375
|
||||
|
||||
@ -1,5 +0,0 @@
|
||||
# debug_flag: true
|
||||
pixel_ratio: 0.1
|
||||
estimate/max_pose_error: 0.4
|
||||
estimate/drone_width: 0.5
|
||||
estimate/drone_height: 0.2
|
||||
|
Before Width: | Height: | Size: 364 KiB |
|
Before Width: | Height: | Size: 103 KiB |
@ -1,156 +0,0 @@
|
||||
#pragma once
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
#include "std_msgs/String.h"
|
||||
#include "std_msgs/Bool.h"
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
// synchronize topic
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <std_srvs/Trigger.h>
|
||||
|
||||
//include opencv and eigen
|
||||
#include <Eigen/Eigen>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
namespace detect {
|
||||
|
||||
const int max_drone_num_ = 3;
|
||||
|
||||
/*!
|
||||
* Main class for the node to handle the ROS interfacing.
|
||||
*/
|
||||
class DroneDetector
|
||||
{
|
||||
public:
|
||||
/*!
|
||||
* Constructor.
|
||||
* @param nodeHandle the ROS node handle.
|
||||
*/
|
||||
DroneDetector(ros::NodeHandle& nodeHandle);
|
||||
|
||||
/*!
|
||||
* Destructor.
|
||||
*/
|
||||
virtual ~DroneDetector();
|
||||
|
||||
void test();
|
||||
private:
|
||||
void readParameters();
|
||||
|
||||
// inline functions
|
||||
double getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2);
|
||||
double getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2);
|
||||
Eigen::Vector4d depth2Pos(int u, int v, float depth);
|
||||
Eigen::Vector4d depth2Pos(const Eigen::Vector2i &pixel, float depth) ;
|
||||
Eigen::Vector2i pos2Depth(const Eigen::Vector4d &pose_in_camera) ;
|
||||
bool isInSensorRange(const Eigen::Vector2i &pixel);
|
||||
|
||||
bool countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam);
|
||||
void detect(int drone_id, Eigen::Vector2i &true_pixel);
|
||||
|
||||
// subscribe callback function
|
||||
void rcvDepthColorCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
|
||||
const sensor_msgs::ImageConstPtr& color_img, \
|
||||
const geometry_msgs::PoseStampedConstPtr& camera_pose);
|
||||
|
||||
void rcvDepthCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
|
||||
const geometry_msgs::PoseStampedConstPtr& camera_pose);
|
||||
|
||||
void rcvMyOdomCallback(const nav_msgs::Odometry& odom);
|
||||
void rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img);
|
||||
|
||||
|
||||
void rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, const int drone_id);
|
||||
|
||||
void rcvDrone0OdomCallback(const nav_msgs::Odometry& odom);
|
||||
void rcvDrone1OdomCallback(const nav_msgs::Odometry& odom);
|
||||
void rcvDrone2OdomCallback(const nav_msgs::Odometry& odom);
|
||||
void rcvDroneXOdomCallback(const nav_msgs::Odometry& odom);
|
||||
|
||||
//! ROS node handle.
|
||||
ros::NodeHandle& nh_;
|
||||
|
||||
//! ROS topic subscriber.
|
||||
// depth, colordepth, camera_pos subscriber
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthColorImagePose;
|
||||
typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthColorImagePose>> SynchronizerDepthColorImagePose;
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthImagePose;
|
||||
typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthImagePose>> SynchronizerDepthImagePose;
|
||||
|
||||
// std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_img_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> colordepth_img_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> camera_pos_sub_;
|
||||
|
||||
SynchronizerDepthColorImagePose sync_depth_color_img_pose_;
|
||||
SynchronizerDepthImagePose sync_depth_img_pose_;
|
||||
// other drones subscriber
|
||||
ros::Subscriber drone0_odom_sub_, drone1_odom_sub_, drone2_odom_sub_, droneX_odom_sub_;
|
||||
std::string drone1_odom_topic_, drone2_odom_topic_;
|
||||
|
||||
ros::Subscriber my_odom_sub_, depth_img_sub_;
|
||||
bool has_odom_;
|
||||
nav_msgs::Odometry my_odom_;
|
||||
// ROS topic publisher
|
||||
// new_depth_img: erase the detected drones
|
||||
// new_colordepth_img: for debug
|
||||
ros::Publisher new_depth_img_pub_;
|
||||
ros::Publisher debug_depth_img_pub_;
|
||||
|
||||
// parameters
|
||||
//camera param
|
||||
int img_width_, img_height_;
|
||||
double fx_,fy_,cx_,cy_;
|
||||
|
||||
double max_pose_error_;
|
||||
double max_pose_error2_;
|
||||
double drone_width_, drone_height_;
|
||||
double pixel_ratio_;
|
||||
int pixel_threshold_;
|
||||
|
||||
// for debug
|
||||
bool debug_flag_;
|
||||
int debug_detect_result_[max_drone_num_];
|
||||
std::stringstream debug_img_text_[max_drone_num_];
|
||||
ros::Time debug_start_time_, debug_end_time_;
|
||||
|
||||
ros::Publisher debug_info_pub_;
|
||||
ros::Publisher drone_pose_err_pub_[max_drone_num_];
|
||||
|
||||
int my_id_;
|
||||
cv::Mat depth_img_, color_img_;
|
||||
|
||||
Eigen::Matrix4d cam2body_;
|
||||
Eigen::Matrix4d cam2world_;
|
||||
Eigen::Quaterniond cam2world_quat_;
|
||||
Eigen::Vector4d my_pose_world_;
|
||||
Eigen::Quaterniond my_attitude_world_;
|
||||
Eigen::Vector4d my_last_pose_world_;
|
||||
ros::Time my_last_odom_stamp_ = ros::TIME_MAX;
|
||||
ros::Time my_last_camera_stamp_ = ros::TIME_MAX;
|
||||
|
||||
Eigen::Matrix4d drone2world_[max_drone_num_];
|
||||
Eigen::Vector4d drone_pose_world_[max_drone_num_];
|
||||
Eigen::Quaterniond drone_attitude_world_[max_drone_num_];
|
||||
Eigen::Vector4d drone_pose_cam_[max_drone_num_];
|
||||
Eigen::Vector2i drone_ref_pixel_[max_drone_num_];
|
||||
|
||||
std::vector<Eigen::Vector2i> hit_pixels_[max_drone_num_];
|
||||
int valid_pixel_cnt_[max_drone_num_];
|
||||
|
||||
bool in_depth_[max_drone_num_] = {false};
|
||||
cv::Point searchbox_lu_[max_drone_num_], searchbox_rd_[max_drone_num_];
|
||||
cv::Point boundingbox_lu_[max_drone_num_], boundingbox_rd_[max_drone_num_];
|
||||
};
|
||||
|
||||
} /* namespace */
|
||||
@ -1,24 +0,0 @@
|
||||
<launch>
|
||||
<arg name="my_id" value="1"/>
|
||||
<arg name="odom_topic" value="/vins_estimator/imu_propagate"/>
|
||||
|
||||
<!-- Launch ROS Package Template Node -->
|
||||
<node pkg="drone_detect" type="drone_detect" name="test_drone_detect" output="screen">
|
||||
<rosparam command="load" file="$(find drone_detect)/config/camera.yaml" />
|
||||
<rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
|
||||
<param name="my_id" value="$(arg my_id)" />
|
||||
<param name="debug_flag" value="false" />
|
||||
<remap from="~odometry" to="$(arg odom_topic)"/>
|
||||
<remap from="~depth" to="/camera/depth/image_rect_raw"/>
|
||||
<!-- <remap from="~camera_pose" to="/drone_$(arg my_id)_pcl_render_node/camera_pose"/> -->
|
||||
|
||||
<!-- <remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
|
||||
<remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
|
||||
<remap from="~drone2" to="/drone_2_$(arg odom_topic)"/> -->
|
||||
|
||||
<!-- <remap from="~drone1" to="/test/dynamic0_odom"/> -->
|
||||
<!-- <remap from="~drone2" to="/test/dynamic1_odom"/> -->
|
||||
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,27 +0,0 @@
|
||||
<launch>
|
||||
|
||||
<!-- This launch file shows how to launch ROS nodes with default parameters and some user's
|
||||
custom parameters:
|
||||
* Default parameters are loaded form the file specified by 'default_param_file';
|
||||
* User's overlying parameters file path is specified by 'overlying_param_file', which can
|
||||
be set from the launch file that includes this file.
|
||||
|
||||
The user can overwrite even just a subset of the default parameters. Only parameters
|
||||
contained in 'overlying_param_file' will overwrite the corresponding default ones.
|
||||
This means that if the user does not specify some parameters, then the default ones
|
||||
will be loaded. -->
|
||||
|
||||
<!-- Default parameters. Note the usage of 'value', to avoid they can be wrongly changed. -->
|
||||
<arg name="default_param_file" value="$(dirname)/../config/default.yaml" />
|
||||
|
||||
<!-- User's parameters that can overly default ones: use default ones in case user does not specify them. -->
|
||||
<arg name="overlying_param_file" default="$(arg default_param_file)" />
|
||||
|
||||
<!-- Launch ROS Package Template node. -->
|
||||
<node pkg="ros_package_template" type="ros_package_template" name="ros_package_template" output="screen">
|
||||
<rosparam command="load" file="$(arg default_param_file)" />
|
||||
<!-- Overlay parameters if user specified them. They must be loaded after default parameters! -->
|
||||
<rosparam command="load" file="$(arg overlying_param_file)" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,20 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>drone_detect</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Detect other drones in depth image.</description>
|
||||
<maintainer email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</maintainer>
|
||||
<license>BSD</license>
|
||||
<author email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</author>
|
||||
|
||||
<!-- buildtool_depend: dependencies of the build process -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<!-- build_depend: dependencies only used in source files -->
|
||||
<build_depend>boost</build_depend>
|
||||
<!-- depend: build, export, and execution dependency -->
|
||||
<depend>eigen</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
<build_depend>roslint</build_depend>
|
||||
</package>
|
||||
@ -1,14 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include "drone_detector/drone_detector.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "drone_detect");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
detect::DroneDetector drone_detector(nh);
|
||||
drone_detector.test();
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
@ -1,10 +0,0 @@
|
||||
// gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
srand((int)time(0));
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
## EGO_planner_swarm
|
||||
|
||||
map_generator
|
||||
- 局部点云
|
||||
- 全局点云
|
||||
|
||||
uav_control
|
||||
- fake_odom
|
||||
- PX4
|
||||
|
||||
#### 情况讨论
|
||||
|
||||
- 局部点云情况
|
||||
- 对应真实情况:D435i等RGBD相机,三维激光雷达
|
||||
- map_generator生成全局点云,模拟得到局部点云信息
|
||||
- 无人机不需要搭载传感器
|
||||
- PX4动力学 & fake_odom模块
|
||||
|
||||
roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
|
||||
roslaunch ego_planner sitl_ego_planner_basic.launch
|
||||
|
||||
|
||||
## 运行
|
||||
|
||||
@ -1,42 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(path_searching)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
ADD_COMPILE_OPTIONS(-std=c++11 )
|
||||
ADD_COMPILE_OPTIONS(-std=c++14 )
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
visualization_msgs
|
||||
plan_env
|
||||
cv_bridge
|
||||
)
|
||||
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES path_searching
|
||||
CATKIN_DEPENDS plan_env
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library( path_searching
|
||||
src/dyn_a_star.cpp
|
||||
)
|
||||
target_link_libraries( path_searching
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
@ -1,115 +0,0 @@
|
||||
#ifndef _DYN_A_STAR_H_
|
||||
#define _DYN_A_STAR_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <ros/console.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <plan_env/grid_map.h>
|
||||
#include <queue>
|
||||
|
||||
constexpr double inf = 1 >> 20;
|
||||
struct GridNode;
|
||||
typedef GridNode *GridNodePtr;
|
||||
|
||||
struct GridNode
|
||||
{
|
||||
enum enum_state
|
||||
{
|
||||
OPENSET = 1,
|
||||
CLOSEDSET = 2,
|
||||
UNDEFINED = 3
|
||||
};
|
||||
|
||||
int rounds{0}; // Distinguish every call
|
||||
enum enum_state state
|
||||
{
|
||||
UNDEFINED
|
||||
};
|
||||
Eigen::Vector3i index;
|
||||
|
||||
double gScore{inf}, fScore{inf};
|
||||
GridNodePtr cameFrom{NULL};
|
||||
};
|
||||
|
||||
class NodeComparator
|
||||
{
|
||||
public:
|
||||
bool operator()(GridNodePtr node1, GridNodePtr node2)
|
||||
{
|
||||
return node1->fScore > node2->fScore;
|
||||
}
|
||||
};
|
||||
|
||||
class AStar
|
||||
{
|
||||
private:
|
||||
GridMap::Ptr grid_map_;
|
||||
|
||||
inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z);
|
||||
|
||||
double getDiagHeu(GridNodePtr node1, GridNodePtr node2);
|
||||
double getManhHeu(GridNodePtr node1, GridNodePtr node2);
|
||||
double getEuclHeu(GridNodePtr node1, GridNodePtr node2);
|
||||
inline double getHeu(GridNodePtr node1, GridNodePtr node2);
|
||||
|
||||
bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx);
|
||||
|
||||
inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const;
|
||||
inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const;
|
||||
|
||||
//bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos );
|
||||
|
||||
inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); }
|
||||
|
||||
std::vector<GridNodePtr> retrievePath(GridNodePtr current);
|
||||
|
||||
double step_size_, inv_step_size_;
|
||||
Eigen::Vector3d center_;
|
||||
Eigen::Vector3i CENTER_IDX_, POOL_SIZE_;
|
||||
const double tie_breaker_ = 1.0 + 1.0 / 10000;
|
||||
|
||||
std::vector<GridNodePtr> gridPath_;
|
||||
|
||||
GridNodePtr ***GridNodeMap_;
|
||||
std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> openSet_;
|
||||
|
||||
int rounds_{0};
|
||||
|
||||
public:
|
||||
typedef std::shared_ptr<AStar> Ptr;
|
||||
|
||||
AStar(){};
|
||||
~AStar();
|
||||
|
||||
void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size);
|
||||
|
||||
bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
|
||||
|
||||
std::vector<Eigen::Vector3d> getPath();
|
||||
};
|
||||
|
||||
inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2)
|
||||
{
|
||||
return tie_breaker_ * getDiagHeu(node1, node2);
|
||||
}
|
||||
|
||||
inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const
|
||||
{
|
||||
return ((index - CENTER_IDX_).cast<double>() * step_size_) + center_;
|
||||
};
|
||||
|
||||
inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const
|
||||
{
|
||||
idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast<int>() + CENTER_IDX_;
|
||||
|
||||
if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2))
|
||||
{
|
||||
ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2));
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,71 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>path_searching</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The path_searching package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</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 multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/path_searching</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>plan_env</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>plan_env</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>plan_env</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -1,260 +0,0 @@
|
||||
#include "path_searching/dyn_a_star.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
||||
AStar::~AStar()
|
||||
{
|
||||
for (int i = 0; i < POOL_SIZE_(0); i++)
|
||||
for (int j = 0; j < POOL_SIZE_(1); j++)
|
||||
for (int k = 0; k < POOL_SIZE_(2); k++)
|
||||
delete GridNodeMap_[i][j][k];
|
||||
}
|
||||
|
||||
void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
|
||||
{
|
||||
POOL_SIZE_ = pool_size;
|
||||
CENTER_IDX_ = pool_size / 2;
|
||||
|
||||
GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)];
|
||||
for (int i = 0; i < POOL_SIZE_(0); i++)
|
||||
{
|
||||
GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)];
|
||||
for (int j = 0; j < POOL_SIZE_(1); j++)
|
||||
{
|
||||
GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)];
|
||||
for (int k = 0; k < POOL_SIZE_(2); k++)
|
||||
{
|
||||
GridNodeMap_[i][j][k] = new GridNode;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
grid_map_ = occ_map;
|
||||
}
|
||||
|
||||
double AStar::getDiagHeu(GridNodePtr node1, GridNodePtr node2)
|
||||
{
|
||||
double dx = abs(node1->index(0) - node2->index(0));
|
||||
double dy = abs(node1->index(1) - node2->index(1));
|
||||
double dz = abs(node1->index(2) - node2->index(2));
|
||||
|
||||
double h = 0.0;
|
||||
int diag = min(min(dx, dy), dz);
|
||||
dx -= diag;
|
||||
dy -= diag;
|
||||
dz -= diag;
|
||||
|
||||
if (dx == 0)
|
||||
{
|
||||
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz);
|
||||
}
|
||||
if (dy == 0)
|
||||
{
|
||||
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz);
|
||||
}
|
||||
if (dz == 0)
|
||||
{
|
||||
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy);
|
||||
}
|
||||
return h;
|
||||
}
|
||||
|
||||
double AStar::getManhHeu(GridNodePtr node1, GridNodePtr node2)
|
||||
{
|
||||
double dx = abs(node1->index(0) - node2->index(0));
|
||||
double dy = abs(node1->index(1) - node2->index(1));
|
||||
double dz = abs(node1->index(2) - node2->index(2));
|
||||
|
||||
return dx + dy + dz;
|
||||
}
|
||||
|
||||
double AStar::getEuclHeu(GridNodePtr node1, GridNodePtr node2)
|
||||
{
|
||||
return (node2->index - node1->index).norm();
|
||||
}
|
||||
|
||||
vector<GridNodePtr> AStar::retrievePath(GridNodePtr current)
|
||||
{
|
||||
vector<GridNodePtr> path;
|
||||
path.push_back(current);
|
||||
|
||||
while (current->cameFrom != NULL)
|
||||
{
|
||||
current = current->cameFrom;
|
||||
path.push_back(current);
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
bool AStar::ConvertToIndexAndAdjustStartEndPoints(Vector3d start_pt, Vector3d end_pt, Vector3i &start_idx, Vector3i &end_idx)
|
||||
{
|
||||
if (!Coord2Index(start_pt, start_idx) || !Coord2Index(end_pt, end_idx))
|
||||
return false;
|
||||
|
||||
if (checkOccupancy(Index2Coord(start_idx)))
|
||||
{
|
||||
//ROS_WARN("Start point is insdide an obstacle.");
|
||||
do
|
||||
{
|
||||
start_pt = (start_pt - end_pt).normalized() * step_size_ + start_pt;
|
||||
if (!Coord2Index(start_pt, start_idx))
|
||||
return false;
|
||||
} while (checkOccupancy(Index2Coord(start_idx)));
|
||||
}
|
||||
|
||||
if (checkOccupancy(Index2Coord(end_idx)))
|
||||
{
|
||||
//ROS_WARN("End point is insdide an obstacle.");
|
||||
do
|
||||
{
|
||||
end_pt = (end_pt - start_pt).normalized() * step_size_ + end_pt;
|
||||
if (!Coord2Index(end_pt, end_idx))
|
||||
return false;
|
||||
} while (checkOccupancy(Index2Coord(end_idx)));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AStar::AstarSearch(const double step_size, Vector3d start_pt, Vector3d end_pt)
|
||||
{
|
||||
ros::Time time_1 = ros::Time::now();
|
||||
++rounds_;
|
||||
|
||||
step_size_ = step_size;
|
||||
inv_step_size_ = 1 / step_size;
|
||||
center_ = (start_pt + end_pt) / 2;
|
||||
|
||||
Vector3i start_idx, end_idx;
|
||||
if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx))
|
||||
{
|
||||
ROS_ERROR("Unable to handle the initial or end point, force return!");
|
||||
return false;
|
||||
}
|
||||
|
||||
// if ( start_pt(0) > -1 && start_pt(0) < 0 )
|
||||
// cout << "start_pt=" << start_pt.transpose() << " end_pt=" << end_pt.transpose() << endl;
|
||||
|
||||
GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)][start_idx(2)];
|
||||
GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)][end_idx(2)];
|
||||
|
||||
std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> empty;
|
||||
openSet_.swap(empty);
|
||||
|
||||
GridNodePtr neighborPtr = NULL;
|
||||
GridNodePtr current = NULL;
|
||||
|
||||
startPtr->index = start_idx;
|
||||
startPtr->rounds = rounds_;
|
||||
startPtr->gScore = 0;
|
||||
startPtr->fScore = getHeu(startPtr, endPtr);
|
||||
startPtr->state = GridNode::OPENSET; //put start node in open set
|
||||
startPtr->cameFrom = NULL;
|
||||
openSet_.push(startPtr); //put start in open set
|
||||
|
||||
endPtr->index = end_idx;
|
||||
|
||||
double tentative_gScore;
|
||||
|
||||
int num_iter = 0;
|
||||
while (!openSet_.empty())
|
||||
{
|
||||
num_iter++;
|
||||
current = openSet_.top();
|
||||
openSet_.pop();
|
||||
|
||||
// if ( num_iter < 10000 )
|
||||
// cout << "current=" << current->index.transpose() << endl;
|
||||
|
||||
if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1) && current->index(2) == endPtr->index(2))
|
||||
{
|
||||
// ros::Time time_2 = ros::Time::now();
|
||||
// printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
|
||||
// if((time_2 - time_1).toSec() > 0.1)
|
||||
// ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
|
||||
gridPath_ = retrievePath(current);
|
||||
return true;
|
||||
}
|
||||
current->state = GridNode::CLOSEDSET; //move current node from open set to closed set.
|
||||
|
||||
for (int dx = -1; dx <= 1; dx++)
|
||||
for (int dy = -1; dy <= 1; dy++)
|
||||
for (int dz = -1; dz <= 1; dz++)
|
||||
{
|
||||
if (dx == 0 && dy == 0 && dz == 0)
|
||||
continue;
|
||||
|
||||
Vector3i neighborIdx;
|
||||
neighborIdx(0) = (current->index)(0) + dx;
|
||||
neighborIdx(1) = (current->index)(1) + dy;
|
||||
neighborIdx(2) = (current->index)(2) + dz;
|
||||
|
||||
if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)];
|
||||
neighborPtr->index = neighborIdx;
|
||||
|
||||
bool flag_explored = neighborPtr->rounds == rounds_;
|
||||
|
||||
if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET)
|
||||
{
|
||||
continue; //in closed set.
|
||||
}
|
||||
|
||||
neighborPtr->rounds = rounds_;
|
||||
|
||||
if (checkOccupancy(Index2Coord(neighborPtr->index)))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double static_cost = sqrt(dx * dx + dy * dy + dz * dz);
|
||||
tentative_gScore = current->gScore + static_cost;
|
||||
|
||||
if (!flag_explored)
|
||||
{
|
||||
//discover a new node
|
||||
neighborPtr->state = GridNode::OPENSET;
|
||||
neighborPtr->cameFrom = current;
|
||||
neighborPtr->gScore = tentative_gScore;
|
||||
neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
|
||||
openSet_.push(neighborPtr); //put neighbor in open set and record it.
|
||||
}
|
||||
else if (tentative_gScore < neighborPtr->gScore)
|
||||
{ //in open set and need update
|
||||
neighborPtr->cameFrom = current;
|
||||
neighborPtr->gScore = tentative_gScore;
|
||||
neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
|
||||
}
|
||||
}
|
||||
ros::Time time_2 = ros::Time::now();
|
||||
if ((time_2 - time_1).toSec() > 0.2)
|
||||
{
|
||||
ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
ros::Time time_2 = ros::Time::now();
|
||||
|
||||
if ((time_2 - time_1).toSec() > 0.1)
|
||||
ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
vector<Vector3d> AStar::getPath()
|
||||
{
|
||||
vector<Vector3d> path;
|
||||
|
||||
for (auto ptr : gridPath_)
|
||||
path.push_back(Index2Coord(ptr->index));
|
||||
|
||||
reverse(path.begin(), path.end());
|
||||
return path;
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue